gtsam 4.2.0
gtsam
expressions.h
Go to the documentation of this file.
1
8#pragma once
9
15#include <gtsam/geometry/OrientedPlane3.h>
17
18namespace gtsam {
19
20// 2D Geometry
21
22typedef Expression<Point2> Point2_;
23typedef Expression<Rot2> Rot2_;
24typedef Expression<Pose2> Pose2_;
25
26inline Point2_ transformTo(const Pose2_& x, const Point2_& p) {
27 return Point2_(x, &Pose2::transformTo, p);
28}
29
30inline Double_ range(const Point2_& p, const Point2_& q) {
31 return Double_(Range<Point2, Point2>(), p, q);
32}
33
34// 3D Geometry
35
36typedef Expression<Point3> Point3_;
37typedef Expression<Unit3> Unit3_;
38typedef Expression<Rot3> Rot3_;
39typedef Expression<Pose3> Pose3_;
40typedef Expression<Line3> Line3_;
41typedef Expression<OrientedPlane3> OrientedPlane3_;
42
43inline Point3_ transformTo(const Pose3_& x, const Point3_& p) {
44 return Point3_(x, &Pose3::transformTo, p);
45}
46
47inline Point3_ transformFrom(const Pose3_& x, const Point3_& p) {
48 return Point3_(x, &Pose3::transformFrom, p);
49}
50
51inline Line3_ transformTo(const Pose3_ &wTc, const Line3_ &wL) {
52 Line3 (*f)(const Pose3 &, const Line3 &,
53 OptionalJacobian<4, 6>, OptionalJacobian<4, 4>) = &transformTo;
54 return Line3_(f, wTc, wL);
55}
56
57inline Pose3_ transformPoseTo(const Pose3_& p, const Pose3_& q) {
58 return Pose3_(p, &Pose3::transformPoseTo, q);
59}
60
61inline Point3_ normalize(const Point3_& a){
62 Point3 (*f)(const Point3 &, OptionalJacobian<3, 3>) = &normalize;
63 return Point3_(f, a);
64}
65
66inline Point3_ cross(const Point3_& a, const Point3_& b) {
67 Point3 (*f)(const Point3 &, const Point3 &,
68 OptionalJacobian<3, 3>, OptionalJacobian<3, 3>) = &cross;
69 return Point3_(f, a, b);
70}
71
72inline Double_ dot(const Point3_& a, const Point3_& b) {
73 double (*f)(const Point3 &, const Point3 &,
74 OptionalJacobian<1, 3>, OptionalJacobian<1, 3>) = &dot;
75 return Double_(f, a, b);
76}
77
78namespace internal {
79// define getter that returns value rather than reference
80inline Rot3 rotation(const Pose3& pose, OptionalJacobian<3, 6> H) {
81 return pose.rotation(H);
82}
83
84inline Point3 translation(const Pose3& pose, OptionalJacobian<3, 6> H) {
85 return pose.translation(H);
86}
87} // namespace internal
88
89inline Rot3_ rotation(const Pose3_& pose) {
90 return Rot3_(internal::rotation, pose);
91}
92
93inline Point3_ translation(const Pose3_& pose) {
94 return Point3_(internal::translation, pose);
95}
96
97inline Point3_ rotate(const Rot3_& x, const Point3_& p) {
98 return Point3_(x, &Rot3::rotate, p);
99}
100
101inline Point3_ point3(const Unit3_& v) {
102 return Point3_(&Unit3::point3, v);
103}
104
105inline Unit3_ rotate(const Rot3_& x, const Unit3_& p) {
106 return Unit3_(x, &Rot3::rotate, p);
107}
108
109inline Point3_ unrotate(const Rot3_& x, const Point3_& p) {
110 return Point3_(x, &Rot3::unrotate, p);
111}
112
113inline Unit3_ unrotate(const Rot3_& x, const Unit3_& p) {
114 return Unit3_(x, &Rot3::unrotate, p);
115}
116
117inline Double_ distance(const OrientedPlane3_& p) {
118 return Double_(&OrientedPlane3::distance, p);
119}
120
121inline Unit3_ normal(const OrientedPlane3_& p) {
122 return Unit3_(&OrientedPlane3::normal, p);
123}
124
125// Projection
126
127typedef Expression<Cal3_S2> Cal3_S2_;
128typedef Expression<Cal3Bundler> Cal3Bundler_;
129
131inline Point2_ project(const Point3_& p_cam) {
133 return Point2_(f, p_cam);
134}
135
136inline Point2_ project(const Unit3_& p_cam) {
137 Point2 (*f)(const Unit3&, OptionalJacobian<2, 2>) = &PinholeBase::Project;
138 return Point2_(f, p_cam);
139}
140
141namespace internal {
142// Helper template for project2 expression below
143template <class CAMERA, class POINT>
144Point2 project4(const CAMERA& camera, const POINT& p, OptionalJacobian<2, CAMERA::dimension> Dcam,
145 OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint) {
146 return camera.project2(p, Dcam, Dpoint);
147}
148}
149
150template <class CAMERA, class POINT>
151Point2_ project2(const Expression<CAMERA>& camera_, const Expression<POINT>& p_) {
152 return Point2_(internal::project4<CAMERA, POINT>, camera_, p_);
153}
154
155namespace internal {
156// Helper template for project3 expression below
157template <class CALIBRATION, class POINT>
158inline Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K,
159 OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint,
160 OptionalJacobian<2, 5> Dcal) {
161 return PinholeCamera<Cal3_S2>(x, K).project(p, Dpose, Dpoint, Dcal);
162}
163}
164
165template <class CALIBRATION, class POINT>
166inline Point2_ project3(const Pose3_& x, const Expression<POINT>& p,
167 const Expression<CALIBRATION>& K) {
168 return Point2_(internal::project6<CALIBRATION, POINT>, x, p, K);
169}
170
171template <class CALIBRATION>
172Point2_ uncalibrate(const Expression<CALIBRATION>& K, const Point2_& xy_hat) {
173 return Point2_(K, &CALIBRATION::uncalibrate, xy_hat);
174}
175
176template <class CALIBRATION>
177inline Pose3_ getPose(const Expression<PinholeCamera<CALIBRATION> > & cam) {
178 return Pose3_(&PinholeCamera<CALIBRATION>::getPose, cam);
179}
180
181
183// TODO(dellaert): Should work but fails because of a type deduction conflict.
184// template <typename T>
185// gtsam::Expression<typename gtsam::traits<T>::TangentVector> logmap(
186// const gtsam::Expression<T> &x1, const gtsam::Expression<T> &x2) {
187// return gtsam::Expression<typename gtsam::traits<T>::TangentVector>(
188// x1, &T::logmap, x2);
189// }
190
191template <typename T>
193 const gtsam::Expression<T> &x1, const gtsam::Expression<T> &x2) {
195 gtsam::traits<T>::Logmap, between(x1, x2));
196}
197
198} // \namespace gtsam
P rotate(const T &r, const P &pt)
rotation functions
Definition: lieProxies.h:47
2D Pose
Base class for all pinhole cameras.
4 dimensional manifold of 3D lines
The most common 5DOF 3D->2D calibration.
Calibration used by Bundler.
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:192
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:64
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:36
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
Point3 normalize(const Point3 &p, OptionalJacobian< 3, 3 > H)
normalize, with optional Jacobian
Definition: Point3.cpp:52
Point2_ project(const Point3_ &p_cam)
Expression version of PinholeBase::Project.
Definition: expressions.h:131
double dot(const V1 &a, const V2 &b)
Dot product.
Definition: Vector.h:195
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:41
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
double distance(OptionalJacobian< 1, 3 > H=boost::none) const
Return the perpendicular distance to the origin.
Definition: OrientedPlane3.h:134
Unit3 normal(OptionalJacobian< 2, 3 > H=boost::none) const
Return the normal.
Definition: OrientedPlane3.h:128
const Pose3 & getPose(OptionalJacobian< 6, dimension > H) const
return pose, with derivative
Definition: PinholeCamera.h:169
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:371
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:347
Pose3 transformPoseTo(const Pose3 &wTb, OptionalJacobian< 6, 6 > Hself=boost::none, OptionalJacobian< 6, 6 > HwTb=boost::none) const
Assuming self == wTa, takes a pose wTb in world coordinates and transforms it to local coordinates aT...
Definition: Pose3.cpp:338
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
Point3 point3(OptionalJacobian< 3, 2 > H=boost::none) const
Return unit-norm Point3.
Definition: Unit3.cpp:144
Expression class that supports automatic differentiation.
Definition: Expression.h:48
Common expressions, both linear and non-linear.