//////////////////////////////////////////////////////////////////////////////////////////////
/// \file ParseBA.hpp
/// \brief Parses a simple bundle adjustment problem (poses, landmarks and measurements)
///
/// \author Sean Anderson, ASRL
//////////////////////////////////////////////////////////////////////////////////////////////

#ifndef STEAM_PARSE_BUNDLE_ADJUSTMENT_HPP
#define STEAM_PARSE_BUNDLE_ADJUSTMENT_HPP

#include <stdio.h>
#include <iostream>

#include <Eigen/Core>
#include <lgmath.hpp>
#include <steam/evaluator/samples/StereoCameraErrorEval.hpp>

namespace steam {
namespace data {

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Structure used to store the bundle adjustment dataset
//////////////////////////////////////////////////////////////////////////////////////////////
struct SimpleBaDataset {

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Structure used to store a vehicle pose estimate
  //////////////////////////////////////////////////////////////////////////////////////////////
  struct Frame {
    unsigned int frameID;
    double time;
    Eigen::Matrix<double,6,1> pose_vec_k0;
    lgmath::se3::Transformation T_k0;
  };

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Structure used to store a landmark estimate
  //////////////////////////////////////////////////////////////////////////////////////////////
  struct Landmark {
    unsigned int landID;
    Eigen::Matrix<double,3,1> point;
  };

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Structure used to store camera intrinsics
  //////////////////////////////////////////////////////////////////////////////////////////////
  struct CameraIntrinsics {
    double b;
    double fu;
    double fv;
    double cu;
    double cv;
  };

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Structure used to store a stereo camera measurement (with assoc. frame and land IDs)
  //////////////////////////////////////////////////////////////////////////////////////////////
  struct StereoMeas {
    unsigned int frameID;
    unsigned int landID;
    double time;
    Eigen::Matrix<double,4,1> data; // ul vl ur vr
  };

  std::vector<Frame> frames_gt;     // ground-truth poses
  std::vector<Landmark> land_gt;    // ground-truth landmarks
  std::vector<Frame> frames_ic;     // initial condition of poses
  std::vector<Landmark> land_ic;    // initial condition of landmarks
  lgmath::se3::Transformation T_cv; // extrinsic calibration between cam and vehicle: T_cam_vehicle
  CameraIntrinsics camParams;       // camera intrinsics
  Eigen::Matrix4d noise;            // 4x4 stereo measurement noise covariance
  std::vector<StereoMeas> meas;     // stereo measurements [ul vl ur vr]'
};

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Function that parses a simple BA dataset
//////////////////////////////////////////////////////////////////////////////////////////////
SimpleBaDataset parseSimpleBaDataset(const std::string& file);

} // data
} // steam

#endif // STEAM_PARSE_BUNDLE_ADJUSTMENT_HPP
