InvDepthCamera3.h

Go to the documentation of this file.

 
#pragma once
 
#include <gtsam/base/Matrix.h>
#include <gtsam/base/MatrixConstants.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
 
#include <cmath>
 
#if GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/nvp.hpp>
#endif
 
namespace gtsam {

template <class CALIBRATION>
class InvDepthCamera3 {
private:
  Pose3 pose_;                        
  std::shared_ptr<CALIBRATION> k_;  
 
public:


  InvDepthCamera3() {}

  InvDepthCamera3(const Pose3& pose, const std::shared_ptr<CALIBRATION>& k) :
    pose_(pose),k_(k) {}

 
  virtual ~InvDepthCamera3() {}

  inline Pose3& pose() {  return pose_; }

  inline const std::shared_ptr<CALIBRATION>& calibration() const {  return k_; }

  void print(const std::string& s = "") const {
    pose_.print("pose3");
    k_.print("calibration");
  }

  static gtsam::Point3 invDepthTo3D(const Vector5& pw, double rho) {
    gtsam::Point3 ray_base(pw.segment(0,3));
    double theta = pw(3), phi = pw(4);
    gtsam::Point3 m(cos(theta)*cos(phi),sin(theta)*cos(phi),sin(phi));
    return ray_base + m/rho;
  }

  inline gtsam::Point2 project(const Vector5& pw,
      double rho,
      OptionalJacobian<2,6> H1 = {},
      OptionalJacobian<2,5> H2 = {},
      OptionalJacobian<2,1> H3 = {}) const {
 
    gtsam::Point3 ray_base(pw.segment(0,3));
    double theta = pw(3), phi = pw(4);
    gtsam::Point3 m(cos(theta)*cos(phi),sin(theta)*cos(phi),sin(phi));
    const gtsam::Point3 landmark = ray_base + m/rho;
 
    gtsam::PinholeCamera<CALIBRATION> camera(pose_, *k_);
 
    if (!H1 && !H2 && !H3) {
      gtsam::Point2 uv= camera.project(landmark);
      return uv;
    }
    else {
      gtsam::Matrix J2;
      gtsam::Point2 uv= camera.project(landmark,H1, J2, {});
      if (H1) {
        *H1 = (*H1) * I_6x6;
      }
 
      double cos_theta = cos(theta);
      double sin_theta = sin(theta);
      double cos_phi = cos(phi);
      double sin_phi = sin(phi);
      double rho2 = rho * rho;
 
      if (H2) {
        double H11 = 1;
        double H12 = 0;
        double H13 = 0;
        double H14 = -cos_phi*sin_theta/rho;
        double H15 = -cos_theta*sin_phi/rho;
 
        double H21 = 0;
        double H22 = 1;
        double H23 = 0;
        double H24 = cos_phi*cos_theta/rho;
        double H25 = -sin_phi*sin_theta/rho;
 
        double H31 = 0;
        double H32 = 0;
        double H33 = 1;
        double H34 = 0;
        double H35 = cos_phi/rho;
 
        *H2 = J2 * Matrix{{H11, H12, H13, H14, H15},
                          {H21, H22, H23, H24, H25},
                          {H31, H32, H33, H34, H35}};
      }
      if(H3) {
        double H16 = -cos_phi*cos_theta/rho2;
        double H26 = -cos_phi*sin_theta/rho2;
        double H36 = -sin_phi/rho2;
        *H3 = J2 * Matrix{{H16}, {H26}, {H36}};
      }
      return uv;
    }
  }

 
  inline std::pair<Vector5, double> backproject(const gtsam::Point2& pi, const double depth) const {
    const gtsam::Point2 pn = k_->calibrate(pi);
    gtsam::Point3 pc(pn.x(), pn.y(), 1.0);
    pc = pc/pc.norm();
    gtsam::Point3 pw = pose_.transformFrom(pc);
    const gtsam::Point3& pt = pose_.translation();
    gtsam::Point3 ray = pw - pt;
    double theta = atan2(ray.y(), ray.x()); // longitude
    double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y()));
    return std::make_pair(Vector5{pt.x(), pt.y(), pt.z(), theta, phi},
                          double(1. / depth));
  }
 
private:

 
#if GTSAM_ENABLE_BOOST_SERIALIZATION
  friend class boost::serialization::access;
  template<class Archive>
  void serialize(Archive & ar, const unsigned int /*version*/) {
    ar & BOOST_SERIALIZATION_NVP(pose_);
    ar & BOOST_SERIALIZATION_NVP(k_);
  }
#endif
}; // \class InvDepthCamera
} // \namespace gtsam