gtsam 4.1.1 gtsam
expressions.h
Go to the documentation of this file.
1
8#pragma once
9
16
17namespace gtsam {
18
19// 2D Geometry
20
21typedef Expression<Point2> Point2_;
22typedef Expression<Rot2> Rot2_;
23typedef Expression<Pose2> Pose2_;
24
25inline Point2_ transformTo(const Pose2_& x, const Point2_& p) {
26 return Point2_(x, &Pose2::transformTo, p);
27}
28
29// 3D Geometry
30
31typedef Expression<Point3> Point3_;
32typedef Expression<Unit3> Unit3_;
33typedef Expression<Rot3> Rot3_;
34typedef Expression<Pose3> Pose3_;
35typedef Expression<Line3> Line3_;
36
37inline Point3_ transformTo(const Pose3_& x, const Point3_& p) {
38 return Point3_(x, &Pose3::transformTo, p);
39}
40
41inline Point3_ transformFrom(const Pose3_& x, const Point3_& p) {
42 return Point3_(x, &Pose3::transformFrom, p);
43}
44
45inline Line3_ transformTo(const Pose3_ &wTc, const Line3_ &wL) {
46 Line3 (*f)(const Pose3 &, const Line3 &,
47 OptionalJacobian<4, 6>, OptionalJacobian<4, 4>) = &transformTo;
48 return Line3_(f, wTc, wL);
49}
50
51inline Point3_ cross(const Point3_& a, const Point3_& b) {
52 Point3 (*f)(const Point3 &, const Point3 &,
53 OptionalJacobian<3, 3>, OptionalJacobian<3, 3>) = &cross;
54 return Point3_(f, a, b);
55}
56
57inline Double_ dot(const Point3_& a, const Point3_& b) {
58 double (*f)(const Point3 &, const Point3 &,
59 OptionalJacobian<1, 3>, OptionalJacobian<1, 3>) = &dot;
60 return Double_(f, a, b);
61}
62
63namespace internal {
64// define getter that returns value rather than reference
65inline Rot3 rotation(const Pose3& pose, OptionalJacobian<3, 6> H) {
66 return pose.rotation(H);
67}
68} // namespace internal
69
70inline Rot3_ rotation(const Pose3_& pose) {
71 return Rot3_(internal::rotation, pose);
72}
73
74inline Point3_ rotate(const Rot3_& x, const Point3_& p) {
75 return Point3_(x, &Rot3::rotate, p);
76}
77
78inline Unit3_ rotate(const Rot3_& x, const Unit3_& p) {
79 return Unit3_(x, &Rot3::rotate, p);
80}
81
82inline Point3_ unrotate(const Rot3_& x, const Point3_& p) {
83 return Point3_(x, &Rot3::unrotate, p);
84}
85
86inline Unit3_ unrotate(const Rot3_& x, const Unit3_& p) {
87 return Unit3_(x, &Rot3::unrotate, p);
88}
89
90// Projection
91
92typedef Expression<Cal3_S2> Cal3_S2_;
93typedef Expression<Cal3Bundler> Cal3Bundler_;
94
96inline Point2_ project(const Point3_& p_cam) {
98 return Point2_(f, p_cam);
99}
100
101inline Point2_ project(const Unit3_& p_cam) {
102 Point2 (*f)(const Unit3&, OptionalJacobian<2, 2>) = &PinholeBase::Project;
103 return Point2_(f, p_cam);
104}
105
106namespace internal {
107// Helper template for project2 expression below
108template <class CAMERA, class POINT>
109Point2 project4(const CAMERA& camera, const POINT& p, OptionalJacobian<2, CAMERA::dimension> Dcam,
110 OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint) {
111 return camera.project2(p, Dcam, Dpoint);
112}
113}
114
115template <class CAMERA, class POINT>
116Point2_ project2(const Expression<CAMERA>& camera_, const Expression<POINT>& p_) {
117 return Point2_(internal::project4<CAMERA, POINT>, camera_, p_);
118}
119
120namespace internal {
121// Helper template for project3 expression below
122template <class CALIBRATION, class POINT>
123inline Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K,
124 OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint,
125 OptionalJacobian<2, 5> Dcal) {
126 return PinholeCamera<Cal3_S2>(x, K).project(p, Dpose, Dpoint, Dcal);
127}
128}
129
130template <class CALIBRATION, class POINT>
131inline Point2_ project3(const Pose3_& x, const Expression<POINT>& p,
132 const Expression<CALIBRATION>& K) {
133 return Point2_(internal::project6<CALIBRATION, POINT>, x, p, K);
134}
135
136template <class CALIBRATION>
137Point2_ uncalibrate(const Expression<CALIBRATION>& K, const Point2_& xy_hat) {
138 return Point2_(K, &CALIBRATION::uncalibrate, xy_hat);
139}
140
141
143// TODO(dellaert): Should work but fails because of a type deduction conflict.
144// template <typename T>
145// gtsam::Expression<typename gtsam::traits<T>::TangentVector> logmap(
146// const gtsam::Expression<T> &x1, const gtsam::Expression<T> &x2) {
147// return gtsam::Expression<typename gtsam::traits<T>::TangentVector>(
148// x1, &T::logmap, x2);
149// }
150
151template <typename T>
153 const gtsam::Expression<T> &x1, const gtsam::Expression<T> &x2) {
155 gtsam::traits<T>::Logmap, between(x1, x2));
156}
157
158} // \namespace gtsam
P rotate(const T &r, const P &pt)
rotation functions
Definition: lieProxies.h:47
The most common 5DOF 3D->2D calibration.
Base class for all pinhole cameras.
Calibration used by Bundler.
4 dimensional manifold of 3D lines
2D Pose
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::Expression< typename gtsam::traits< T >::TangentVector > logmap(const gtsam::Expression< T > &x1, const gtsam::Expression< T > &x2)
logmap
Definition: expressions.h:152
Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose, OptionalJacobian< 4, 4 > Dline)
Transform a line from world to camera frame.
Definition: Line3.cpp:94
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition: Point2.h:27
Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2)
cross product
Definition: Point3.cpp:63
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition: Point3.h:35
Point2_ project(const Point3_ &p_cam)
Expression version of PinholeBase::Project.
Definition: expressions.h:96
double dot(const V1 &a, const V2 &b)
Dot product.
Definition: Vector.h:194
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:39
static Point2 Project(const Point3 &pc, OptionalJacobian< 2, 3 > Dpoint=boost::none)
Project from 3D point in camera coordinates into image Does not throw a CheiralityException,...
Definition: CalibratedCamera.cpp:88
GTSAM_EXPORT Point2 transformTo(const Point2 &point, OptionalJacobian< 2, 3 > Dpose=boost::none, OptionalJacobian< 2, 2 > Dpoint=boost::none) const
Return point coordinates in pose coordinate frame.
Definition: Pose2.cpp:207
Point3 transformTo(const Point3 &point, OptionalJacobian< 3, 6 > Hself=boost::none, OptionalJacobian< 3, 3 > Hpoint=boost::none) const
takes point in world coordinates and transforms it to Pose coordinates
Definition: Pose3.cpp:358
Point3 transformFrom(const Point3 &point, OptionalJacobian< 3, 6 > Hself=boost::none, OptionalJacobian< 3, 3 > Hpoint=boost::none) const
takes point in Pose coordinates and transforms it to world coordinates
Definition: Pose3.cpp:342
Point3 unrotate(const Point3 &p, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
rotate point from world to rotated frame
Definition: Rot3.cpp:136
Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
rotate point from rotated coordinate frame to world
Definition: Rot3M.cpp:149
Expression class that supports automatic differentiation.
Definition: Expression.h:48
Common expressions, both linear and non-linear.