gtsam  4.1.0
gtsam
expressions.h
Go to the documentation of this file.
1 
8 #pragma once
9 
11 #include <gtsam/geometry/Pose2.h>
12 #include <gtsam/geometry/Cal3_S2.h>
14 #include <gtsam/geometry/Line3.h>
16 
17 namespace gtsam {
18 
19 // 2D Geometry
20 
21 typedef Expression<Point2> Point2_;
22 typedef Expression<Rot2> Rot2_;
23 typedef Expression<Pose2> Pose2_;
24 
25 inline Point2_ transformTo(const Pose2_& x, const Point2_& p) {
26  return Point2_(x, &Pose2::transformTo, p);
27 }
28 
29 // 3D Geometry
30 
31 typedef Expression<Point3> Point3_;
32 typedef Expression<Unit3> Unit3_;
33 typedef Expression<Rot3> Rot3_;
34 typedef Expression<Pose3> Pose3_;
35 typedef Expression<Line3> Line3_;
36 
37 inline Point3_ transformTo(const Pose3_& x, const Point3_& p) {
38  return Point3_(x, &Pose3::transformTo, p);
39 }
40 
41 inline Point3_ transformFrom(const Pose3_& x, const Point3_& p) {
42  return Point3_(x, &Pose3::transformFrom, p);
43 }
44 
45 inline 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 
51 namespace internal {
52 // define getter that returns value rather than reference
53 inline Rot3 rotation(const Pose3& pose, OptionalJacobian<3, 6> H) {
54  return pose.rotation(H);
55 }
56 } // namespace internal
57 
58 inline Rot3_ rotation(const Pose3_& pose) {
59  return Rot3_(internal::rotation, pose);
60 }
61 
62 inline Point3_ rotate(const Rot3_& x, const Point3_& p) {
63  return Point3_(x, &Rot3::rotate, p);
64 }
65 
66 inline Unit3_ rotate(const Rot3_& x, const Unit3_& p) {
67  return Unit3_(x, &Rot3::rotate, p);
68 }
69 
70 inline Point3_ unrotate(const Rot3_& x, const Point3_& p) {
71  return Point3_(x, &Rot3::unrotate, p);
72 }
73 
74 inline Unit3_ unrotate(const Rot3_& x, const Unit3_& p) {
75  return Unit3_(x, &Rot3::unrotate, p);
76 }
77 
78 // Projection
79 
80 typedef Expression<Cal3_S2> Cal3_S2_;
81 typedef Expression<Cal3Bundler> Cal3Bundler_;
82 
84 inline Point2_ project(const Point3_& p_cam) {
86  return Point2_(f, p_cam);
87 }
88 
89 inline Point2_ project(const Unit3_& p_cam) {
90  Point2 (*f)(const Unit3&, OptionalJacobian<2, 2>) = &PinholeBase::Project;
91  return Point2_(f, p_cam);
92 }
93 
94 namespace internal {
95 // Helper template for project2 expression below
96 template <class CAMERA, class POINT>
97 Point2 project4(const CAMERA& camera, const POINT& p, OptionalJacobian<2, CAMERA::dimension> Dcam,
98  OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint) {
99  return camera.project2(p, Dcam, Dpoint);
100 }
101 }
102 
103 template <class CAMERA, class POINT>
104 Point2_ project2(const Expression<CAMERA>& camera_, const Expression<POINT>& p_) {
105  return Point2_(internal::project4<CAMERA, POINT>, camera_, p_);
106 }
107 
108 namespace internal {
109 // Helper template for project3 expression below
110 template <class CALIBRATION, class POINT>
111 inline Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K,
112  OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint,
113  OptionalJacobian<2, 5> Dcal) {
114  return PinholeCamera<Cal3_S2>(x, K).project(p, Dpose, Dpoint, Dcal);
115 }
116 }
117 
118 template <class CALIBRATION, class POINT>
119 inline Point2_ project3(const Pose3_& x, const Expression<POINT>& p,
120  const Expression<CALIBRATION>& K) {
121  return Point2_(internal::project6<CALIBRATION, POINT>, x, p, K);
122 }
123 
124 template <class CALIBRATION>
125 Point2_ uncalibrate(const Expression<CALIBRATION>& K, const Point2_& xy_hat) {
126  return Point2_(K, &CALIBRATION::uncalibrate, xy_hat);
127 }
128 
129 } // \namespace gtsam
Line3.h
4 dimensional manifold of 3D lines
gtsam::Pose3::transformFrom
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:298
PinholeCamera.h
Base class for all pinhole cameras.
gtsam::Rot3::rotate
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
gtsam::Pose3::transformTo
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:314
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:39
gtsam::testing::rotate
P rotate(const T &r, const P &pt)
rotation functions
Definition: lieProxies.h:47
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::Rot3::unrotate
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
gtsam::project
Point2_ project(const Point3_ &p_cam)
Expression version of PinholeBase::Project.
Definition: expressions.h:84
Pose2.h
2D Pose
gtsam::Pose2::transformTo
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:201
gtsam::Point2
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
expressions.h
Common expressions, both linear and non-linear.
gtsam::PinholeBase::Project
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::Expression
Expression class that supports automatic differentiation.
Definition: Expression.h:49
gtsam::transformTo
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
gtsam::Point3
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
Cal3Bundler.h
Calibration used by Bundler.
Cal3_S2.h
The most common 5DOF 3D->2D calibration.