InvDepthCamera3.h
Go to the documentation of this file.
1 
12 #pragma once
13 
14 #include <cmath>
15 #include <gtsam/base/Vector.h>
16 #include <gtsam/base/Matrix.h>
17 #include <gtsam/geometry/Point2.h>
18 #include <gtsam/geometry/Pose3.h>
21 
22 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
23 #include <boost/serialization/nvp.hpp>
24 #endif
25 
26 namespace gtsam {
27 
33 template <class CALIBRATION>
35 private:
37  std::shared_ptr<CALIBRATION> k_;
38 
39 public:
40 
43 
46 
48  InvDepthCamera3(const Pose3& pose, const std::shared_ptr<CALIBRATION>& k) :
49  pose_(pose),k_(k) {}
50 
54 
55  virtual ~InvDepthCamera3() {}
56 
58  inline Pose3& pose() { return pose_; }
59 
61  inline const std::shared_ptr<CALIBRATION>& calibration() const { return k_; }
62 
64  void print(const std::string& s = "") const {
65  pose_.print("pose3");
66  k_.print("calibration");
67  }
68 
75  static gtsam::Point3 invDepthTo3D(const Vector5& pw, double rho) {
76  gtsam::Point3 ray_base(pw.segment(0,3));
77  double theta = pw(3), phi = pw(4);
78  gtsam::Point3 m(cos(theta)*cos(phi),sin(theta)*cos(phi),sin(phi));
79  return ray_base + m/rho;
80  }
81 
87  inline gtsam::Point2 project(const Vector5& pw,
88  double rho,
89  OptionalJacobian<2,6> H1 = {},
90  OptionalJacobian<2,5> H2 = {},
91  OptionalJacobian<2,1> H3 = {}) const {
92 
93  gtsam::Point3 ray_base(pw.segment(0,3));
94  double theta = pw(3), phi = pw(4);
95  gtsam::Point3 m(cos(theta)*cos(phi),sin(theta)*cos(phi),sin(phi));
96  const gtsam::Point3 landmark = ray_base + m/rho;
97 
99 
100  if (!H1 && !H2 && !H3) {
101  gtsam::Point2 uv= camera.project(landmark);
102  return uv;
103  }
104  else {
105  gtsam::Matrix J2;
106  gtsam::Point2 uv= camera.project(landmark,H1, J2, {});
107  if (H1) {
108  *H1 = (*H1) * I_6x6;
109  }
110 
111  double cos_theta = cos(theta);
112  double sin_theta = sin(theta);
113  double cos_phi = cos(phi);
114  double sin_phi = sin(phi);
115  double rho2 = rho * rho;
116 
117  if (H2) {
118  double H11 = 1;
119  double H12 = 0;
120  double H13 = 0;
121  double H14 = -cos_phi*sin_theta/rho;
122  double H15 = -cos_theta*sin_phi/rho;
123 
124  double H21 = 0;
125  double H22 = 1;
126  double H23 = 0;
127  double H24 = cos_phi*cos_theta/rho;
128  double H25 = -sin_phi*sin_theta/rho;
129 
130  double H31 = 0;
131  double H32 = 0;
132  double H33 = 1;
133  double H34 = 0;
134  double H35 = cos_phi/rho;
135 
136  *H2 = J2 * (Matrix(3, 5) <<
137  H11, H12, H13, H14, H15,
138  H21, H22, H23, H24, H25,
139  H31, H32, H33, H34, H35).finished();
140  }
141  if(H3) {
142  double H16 = -cos_phi*cos_theta/rho2;
143  double H26 = -cos_phi*sin_theta/rho2;
144  double H36 = -sin_phi/rho2;
145  *H3 = J2 * (Matrix(3, 1) <<
146  H16,
147  H26,
148  H36).finished();
149  }
150  return uv;
151  }
152  }
153 
159  inline std::pair<Vector5, double> backproject(const gtsam::Point2& pi, const double depth) const {
160  const gtsam::Point2 pn = k_->calibrate(pi);
161  gtsam::Point3 pc(pn.x(), pn.y(), 1.0);
162  pc = pc/pc.norm();
163  gtsam::Point3 pw = pose_.transformFrom(pc);
164  const gtsam::Point3& pt = pose_.translation();
165  gtsam::Point3 ray = pw - pt;
166  double theta = atan2(ray.y(), ray.x()); // longitude
167  double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y()));
168  return std::make_pair((Vector5() << pt.x(),pt.y(),pt.z(), theta, phi).finished(),
169  double(1./depth));
170  }
171 
172 private:
173 
177 
178 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
179 
180  friend class boost::serialization::access;
181  template<class Archive>
182  void serialize(Archive & ar, const unsigned int /*version*/) {
183  ar & BOOST_SERIALIZATION_NVP(pose_);
184  ar & BOOST_SERIALIZATION_NVP(k_);
185  }
186 #endif
187 }; // \class InvDepthCamera
189 } // \namespace gtsam
Matrix3f m
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Definition: Pose3.cpp:308
Pose3 pose_
The camera pose.
std::string serialize(const T &input)
serializes to a string
int RealScalar int RealScalar int RealScalar * pc
static double depth
Vector2 Point2
Definition: Point2.h:32
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
const std::shared_ptr< CALIBRATION > & calibration() const
return calibration
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
Pose3 & pose()
return pose
static const Point3 pt(1.0, 2.0, 3.0)
std::shared_ptr< CALIBRATION > k_
The fixed camera calibration.
static Point3 landmark(0, 0, 5)
Base class for all pinhole cameras.
Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose={}, OptionalJacobian< 2, 3 > Dpoint={}, OptionalJacobian< 2, DimK > Dcal={}) const
project a 3D point from world coordinates into the image
Definition: PinholePose.h:112
std::pair< Vector5, double > backproject(const gtsam::Point2 &pi, const double depth) const
Point3 transformFrom(const Point3 &point, OptionalJacobian< 3, 6 > Hself={}, OptionalJacobian< 3, 3 > Hpoint={}) const
takes point in Pose coordinates and transforms it to world coordinates
Definition: Pose3.cpp:347
RealScalar s
static gtsam::Point3 invDepthTo3D(const Vector5 &pw, double rho)
AnnoyingScalar atan2(const AnnoyingScalar &y, const AnnoyingScalar &x)
traits
Definition: chartTesting.h:28
typedef and functions to augment Eigen&#39;s VectorXd
gtsam::Point2 project(const Vector5 &pw, double rho, OptionalJacobian< 2, 6 > H1={}, OptionalJacobian< 2, 5 > H2={}, OptionalJacobian< 2, 1 > H3={}) const
void print(const std::string &s="") const
print with optional string
Definition: Pose3.cpp:152
Non-linear factor base classes.
InvDepthCamera3(const Pose3 &pose, const std::shared_ptr< CALIBRATION > &k)
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
Vector3 Point3
Definition: Point3.h:38
static const CalibratedCamera camera(kDefaultPose)
2D Point
3D Pose
void print(const std::string &s="") const
print


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:23