1
2/**
3 * @file InvDepthCamera3.h
4 * @brief Inverse Depth Camera based on Civera09tro, Montiel06rss.
5 * Landmarks are initialized from the first camera observation with
6 * (x,y,z,theta,phi,inv_depth), where x,y,z are the coordinates of
7 * the camera
8 * @author Chris Beall
9 * @date Apr 14, 2012
10 */
11
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>
19#include <gtsam/geometry/PinholeCamera.h>
20#include <gtsam/nonlinear/NonlinearFactor.h>
21
22#if GTSAM_ENABLE_BOOST_SERIALIZATION
23#include <boost/serialization/nvp.hpp>
24#endif
25
26namespace gtsam {
27
28/**
29 * A pinhole camera class that has a Pose3 and a Calibration.
30 * @ingroup geometry
31 * \nosubgrouping
32 */
33template <class CALIBRATION>
34class InvDepthCamera3 {
35private:
36 Pose3 pose_; ///< The camera pose
37 std::shared_ptr<CALIBRATION> k_; ///< The fixed camera calibration
38
39public:
40
41 /// @name Standard Constructors
42 /// @{
43
44 /** default constructor */
45 InvDepthCamera3() {}
46
47 /** constructor with pose and calibration */
48 InvDepthCamera3(const Pose3& pose, const std::shared_ptr<CALIBRATION>& k) :
49 pose_(pose),k_(k) {}
50
51 /// @}
52 /// @name Standard Interface
53 /// @{
54
55 virtual ~InvDepthCamera3() {}
56
57 /// return pose
58 inline Pose3& pose() { return pose_; }
59
60 /// return calibration
61 inline const std::shared_ptr<CALIBRATION>& calibration() const { return k_; }
62
63 /// print
64 void print(const std::string& s = "") const {
65 pose_.print(s: "pose3");
66 k_.print("calibration");
67 }
68
69 /**
70 * Convert an inverse depth landmark to cartesian Point3
71 * @param pw first five parameters (x,y,z,theta,phi) of inv depth landmark
72 * @param inv inverse depth
73 * @return Point3
74 */
75 static gtsam::Point3 invDepthTo3D(const Vector5& pw, double rho) {
76 gtsam::Point3 ray_base(pw.segment(start: 0,n: 3));
77 double theta = pw(3), phi = pw(4);
78 gtsam::Point3 m(cos(x: theta)*cos(x: phi),sin(x: theta)*cos(x: phi),sin(x: phi));
79 return ray_base + m/rho;
80 }
81
82 /** project a point from world InvDepth parameterization to the image
83 * @param pw is a point in the world coordinate
84 * @param H1 is the jacobian w.r.t. [pose3 calibration]
85 * @param H2 is the jacobian w.r.t. inv_depth_landmark
86 */
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(start: 0,n: 3));
94 double theta = pw(3), phi = pw(4);
95 gtsam::Point3 m(cos(x: theta)*cos(x: phi),sin(x: theta)*cos(x: phi),sin(x: phi));
96 const gtsam::Point3 landmark = ray_base + m/rho;
97
98 gtsam::PinholeCamera<CALIBRATION> camera(pose_, *k_);
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(x: theta);
112 double sin_theta = sin(x: theta);
113 double cos_phi = cos(x: phi);
114 double sin_phi = sin(x: 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
154 /**
155 * backproject a 2-dimensional point to an Inverse Depth landmark
156 * useful for point initialization
157 */
158
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(point: pc);
164 const gtsam::Point3& pt = pose_.translation();
165 gtsam::Point3 ray = pw - pt;
166 double theta = atan2(y: ray.y(), x: ray.x()); // longitude
167 double phi = atan2(y: ray.z(), x: sqrt(x: ray.x()*ray.x()+ray.y()*ray.y()));
168 return std::make_pair(x&: (Vector5() << pt.x(),pt.y(),pt.z(), theta, phi).finished(),
169 y: double(1./depth));
170 }
171
172private:
173
174 /// @}
175 /// @name Advanced Interface
176 /// @{
177
178#if GTSAM_ENABLE_BOOST_SERIALIZATION
179 /** Serialization function */
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 /// @}
188}; // \class InvDepthCamera
189} // \namespace gtsam
190