/home/hauberg/Dokumenter/Capture/humim-tracker-0.1/src/ntk/geometry/pose_3d.h

Go to the documentation of this file.

#ifndef NTK_GEOMETRY_POSE3D_H
#define NTK_GEOMETRY_POSE3D_H

# include <ntk/core.h>

namespace ntk
{

class Pose3D
{
private:
  class PrivatePose3D;
  friend class PrivatePose3D;
  PrivatePose3D* impl;

public:
  Pose3D();
  ~Pose3D();

  Pose3D(const Pose3D& rhs);
  Pose3D& operator=(const Pose3D& rhs);

public:
  virtual void saveToYaml(cv::FileStorage& yaml) const;
  virtual void loadFromYaml(cv::FileNode yaml);

  void setCameraParametersFromOpencv(const cv::Mat1d& cv_matrix);

  void setCameraParameters(double fx, double fy, double cx, double cy, bool orthographic = false);

public:
  void toLeftCamera(const cv::Mat1d& intrinsics_matrix,
                    const cv::Mat1d& R,
                    const cv::Mat1d& T);

  void toRightCamera(const cv::Mat1d& cv_matrix,
                     const cv::Mat1d& R,
                     const cv::Mat1d& T);

  Pose3D computeDeltaPoseWith(const Pose3D& new_pose) const;

public:
  double focalX() const { return m_focal_x; }
  double focalY() const { return m_focal_y; }

  double imageCenterX() const { return m_image_center_x; }
  double imageCenterY() const { return m_image_center_y; }

  bool focalAreIdentical() const { return std::abs(m_focal_x - m_focal_y) <= 1e-5; }

  double meanFocal() const { return (m_focal_x + m_focal_y)/2.0; }

  bool isValid() const { return m_has_camera_params; }

  void setOrthographic(bool ortho);
  bool isOrthographic() const { return m_orthographic; }

public:
  const cv::Vec3f cvTranslation() const;

  const cv::Vec3f cvEulerRotation() const;

  const cv::Vec3f cvRodriguesRotation() const;

  const cv::Mat1f cvCameraTransform() const;

  cv::Mat1f cvInvCameraTransform() const;

  cv::Mat1f cvProjectionMatrix() const;

  cv::Mat1f cvInvProjectionMatrix() const;

public:
  void resetCameraTransform();

  void invert();

  void setCameraTransform(const cv::Mat1d& tvec, const cv::Mat1d& rvec);

  void setCameraTransform(const cv::Mat1f& H);

  void setCameraTransformFromCvFundamentalMatrix(const cv::Mat1f& F);

  void applyTransformBefore(const Pose3D& rhs_pose);
  void applyTransformBefore(const cv::Vec3f& cvTranslation, const cv::Vec3f& rotation_euler_angles);
  void applyTransformBefore(const cv::Vec3f& cvTranslation, const cv::Mat1d& rotation_matrix);
  void applyTransformBeforeRodrigues(const cv::Vec3f& cvTranslation, const cv::Vec3f& rotation_rodrigues);

  void applyTransformAfter(const Pose3D& rhs_pose);
  void applyTransformAfter(const cv::Vec3f& translation, const cv::Vec3f& rotation_euler_angles);
  void applyTransformAfter(const cv::Vec3f& translation, const cv::Mat1d& rotation_matrix);
  void applyTransformAfterRodrigues(const cv::Vec3f& translation, const cv::Vec3f& rotation_rodrigues);

public:
  cv::Point3f cameraTransform(const cv::Point3f& p) const;

  cv::Point3f invCameraTransform(const cv::Point3f& p) const;

  cv::Point3f projectToImage(const cv::Point3f& p) const;

  void projectToImage(const cv::Mat3f& voxels, const cv::Mat1b& mask, cv::Mat3f& pixels) const;

  cv::Point3f unprojectFromImage(const cv::Point2f& p, double depth) const;
  cv::Point3f unprojectFromImage(const cv::Point3f& p) const
  { return unprojectFromImage(cv::Point2f(p.x,p.y), p.z); }

  void unprojectFromImage(const cv::Mat1f& pixels, const cv::Mat1b& mask, cv::Mat3f& voxels) const;

private:
  double m_focal_x;
  double m_focal_y;
  double m_image_center_x;
  double m_image_center_y;
  bool m_has_camera_params;
  bool m_orthographic;
};

cv::Vec3f estimate_normal_from_depth(const cv::Mat1f& depth_yml, const Pose3D& pose, int r, int c);

cv::Vec3f camera_eye_vector(const Pose3D& pose, int r, int c);

cv::Vec3f compute_axis_angle_rotation(const cv::Vec3f& src, const cv::Vec3f& dst);

} // ntk

#endif // NTK_GEOMETRY_POSE3D_H