22 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION 23 #include <boost/serialization/nvp.hpp> 33 template <
class CALIBRATION>
37 std::shared_ptr<CALIBRATION> k_;
61 inline const std::shared_ptr<CALIBRATION>&
calibration()
const {
return k_; }
64 void print(
const std::string& s =
"")
const {
66 k_.print(
"calibration");
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;
94 double theta = pw(3), phi = pw(4);
95 gtsam::Point3 m(cos(theta)*cos(phi),sin(theta)*cos(phi),sin(phi));
100 if (!H1 && !H2 && !H3) {
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;
121 double H14 = -cos_phi*sin_theta/rho;
122 double H15 = -cos_theta*sin_phi/rho;
127 double H24 = cos_phi*cos_theta/rho;
128 double H25 = -sin_phi*sin_theta/rho;
134 double H35 = cos_phi/rho;
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();
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) <<
166 double theta = atan2(ray.y(), ray.x());
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(),
178 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION 180 friend class boost::serialization::access;
181 template<
class Archive>
182 void serialize(Archive & ar,
const unsigned int ) {
183 ar & BOOST_SERIALIZATION_NVP(pose_);
184 ar & BOOST_SERIALIZATION_NVP(k_);
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:113
Vector2 Point2
Definition: Point2.h:32
const std::shared_ptr< CALIBRATION > & calibration() const
return calibration
Definition: InvDepthCamera3.h:61
InvDepthCamera3()
Definition: InvDepthCamera3.h:45
Pose3 & pose()
return pose
Definition: InvDepthCamera3.h:58
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
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
Definition: InvDepthCamera3.h:159
static gtsam::Point3 invDepthTo3D(const Vector5 &pw, double rho)
Definition: InvDepthCamera3.h:75
typedef and functions to augment Eigen's MatrixXd
Definition: PinholeCamera.h:33
Definition: chartTesting.h:28
typedef and functions to augment Eigen's VectorXd
gtsam::Point2 project(const Vector5 &pw, double rho, OptionalJacobian< 2, 6 > H1={}, OptionalJacobian< 2, 5 > H2={}, OptionalJacobian< 2, 1 > H3={}) const
Definition: InvDepthCamera3.h:87
Definition: OptionalJacobian.h:38
Non-linear factor base classes.
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
InvDepthCamera3(const Pose3 &pose, const std::shared_ptr< CALIBRATION > &k)
Definition: InvDepthCamera3.h:48
void print(const std::string &s="") const
print with optional string
Definition: InvDepthCamera3.h:34
Vector3 Point3
Definition: Point3.h:38
void print(const std::string &s="") const
print
Definition: InvDepthCamera3.h:64