/*
This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.

Developer: Chunran Zheng <zhengcr@connect.hku.hk>

For commercial use, please contact me at <zhengcr@connect.hku.hk> or
Prof. Fu Zhang at <fuzhang@hku.hk>.

This file is subject to the terms and conditions outlined in the 'LICENSE' file,
which is included as part of this source code package.
*/

#ifndef LIVO_FRAME_H_
#define LIVO_FRAME_H_

#include <boost/noncopyable.hpp>
#include <vikit/abstract_camera.h>

class VisualPoint;
struct Feature;

typedef list<Feature *> Features;
typedef vector<cv::Mat> ImgPyr;

/// A frame saves the images (one per camera), the associated features and the
/// estimated per-camera poses. Multi-camera support (Omni-LIVO): all cameras
/// share one rigid body pose, but are stored per-camera here because each
/// camera has a different fixed extrinsic offset from that body pose.
class Frame : boost::noncopyable
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  static int frame_counter_;                //!< Counts the number of created frames. Used to set the unique id.
  int id_;                                   //!< Unique id of the frame.
  std::vector<vk::AbstractCamera *> cams_;   //!< Camera models, one per camera.
  std::vector<SE3<double>> T_f_w_;           //!< Transform (f)rame from (w)orld, one per camera.
  std::vector<SE3<double>> T_f_w_prior_;     //!< Transform (f)rame from (w)orld provided by the IMU prior, one per camera.
  std::vector<std::shared_ptr<cv::Mat>> imgs_shared_; //!< Shared ownership of each camera's image.
  std::vector<cv::Mat> imgs_;                //!< Images of the frame, one per camera (Mat headers referencing imgs_shared_).
  double timestamp_;
  Features fts_;                             //!< List of features in the image.

  Frame(const std::vector<vk::AbstractCamera *> &cams, std::vector<cv::Mat> &imgs, double timestamp);
  ~Frame();

  /// Initialize new frame and create image pyramid.
  void initFrame(std::vector<cv::Mat> &imgs);

  /// Return number of point observations.
  inline size_t nObs() const { return fts_.size(); }

  /// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c).
  inline Vector2d w2c(const Vector3d &xyz_w, int cam_id) const { return cams_[cam_id]->world2cam(T_f_w_[cam_id] * xyz_w); }

  /// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c) using the IMU prior pose.
  inline Vector2d w2c_prior(const Vector3d &xyz_w, int cam_id) const { return cams_[cam_id]->world2cam(T_f_w_prior_[cam_id] * xyz_w); }

  /// Transforms pixel coordinates (c) to frame unit sphere coordinates (f).
  inline Vector3d c2f(const double x, const double y, int cam_id) const { return cams_[cam_id]->cam2world(x, y); }

  /// Transforms point coordinates in world-frame (w) to camera-frame (f).
  inline Vector3d w2f(const Vector3d &xyz_w, int cam_id) const { return T_f_w_[cam_id] * xyz_w; }

  /// Transforms point from frame unit sphere (f) frame to world coordinate frame (w).
  inline Vector3d f2w(const Vector3d &f, int cam_id) const { return T_f_w_[cam_id].inverse() * f; }

  /// Return the pose of the frame in the (w)orld coordinate frame, for a given camera.
  inline Vector3d pos(int cam_id) const { return T_f_w_[cam_id].inverse().translation(); }
};

typedef std::shared_ptr<Frame> FramePtr;

/// Some helper functions for the frame object.
namespace frame_utils
{

/// Creates an image pyramid of half-sampled images.
void createImgPyramid(const cv::Mat &img_level_0, int n_levels, ImgPyr &pyr);

} // namespace frame_utils

#endif // LIVO_FRAME_H_
