slam/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>
17 
18 namespace gtsam {
19 
20 // 2D Geometry
21 
25 
26 inline Point2_ transformTo(const Pose2_& x, const Point2_& p) {
27  return Point2_(x, &Pose2::transformTo, p);
28 }
29 
30 inline Double_ range(const Point2_& p, const Point2_& q) {
31  return Double_(Range<Point2, Point2>(), p, q);
32 }
33 
34 // 3D Geometry
35 
42 
43 inline Point3_ transformTo(const Pose3_& x, const Point3_& p) {
44  return Point3_(x, &Pose3::transformTo, p);
45 }
46 
47 inline Point3_ transformFrom(const Pose3_& x, const Point3_& p) {
48  return Point3_(x, &Pose3::transformFrom, p);
49 }
50 
51 inline Line3_ transformTo(const Pose3_ &wTc, const Line3_ &wL) {
52  Line3 (*f)(const Pose3 &, const Line3 &,
54  return Line3_(f, wTc, wL);
55 }
56 
57 inline Pose3_ transformPoseTo(const Pose3_& p, const Pose3_& q) {
58  return Pose3_(p, &Pose3::transformPoseTo, q);
59 }
60 
61 inline Point3_ normalize(const Point3_& a){
63  return Point3_(f, a);
64 }
65 
66 inline Point3_ cross(const Point3_& a, const Point3_& b) {
67  Point3 (*f)(const Point3 &, const Point3 &,
69  return Point3_(f, a, b);
70 }
71 
72 inline Double_ dot(const Point3_& a, const Point3_& b) {
73  double (*f)(const Point3 &, const Point3 &,
75  return Double_(f, a, b);
76 }
77 
78 namespace internal {
79 // define getter that returns value rather than reference
81  return pose.rotation(H);
82 }
83 
85  return pose.translation(H);
86 }
87 } // namespace internal
88 
89 inline Rot3_ rotation(const Pose3_& pose) {
90  return Rot3_(internal::rotation, pose);
91 }
92 
93 inline Point3_ translation(const Pose3_& pose) {
95 }
96 
97 inline Point3_ rotate(const Rot3_& x, const Point3_& p) {
98  return Point3_(x, &Rot3::rotate, p);
99 }
100 
101 inline Point3_ point3(const Unit3_& v) {
102  return Point3_(&Unit3::point3, v);
103 }
104 
105 inline Unit3_ rotate(const Rot3_& x, const Unit3_& p) {
106  return Unit3_(x, &Rot3::rotate, p);
107 }
108 
109 inline Point3_ unrotate(const Rot3_& x, const Point3_& p) {
110  return Point3_(x, &Rot3::unrotate, p);
111 }
112 
113 inline Unit3_ unrotate(const Rot3_& x, const Unit3_& p) {
114  return Unit3_(x, &Rot3::unrotate, p);
115 }
116 
119 }
120 
121 inline Unit3_ normal(const OrientedPlane3_& p) {
122  return Unit3_(&OrientedPlane3::normal, p);
123 }
124 
125 // Projection
126 
129 
131 inline Point2_ project(const Point3_& p_cam) {
133  return Point2_(f, p_cam);
134 }
135 
136 inline Point2_ project(const Unit3_& p_cam) {
138  return Point2_(f, p_cam);
139 }
140 
141 namespace internal {
142 // Helper template for project2 expression below
143 template <class CAMERA, class POINT>
146  return camera.project2(p, Dcam, Dpoint);
147 }
148 }
149 
150 template <class CAMERA, class POINT>
152  return Point2_(internal::project4<CAMERA, POINT>, camera_, p_);
153 }
154 
155 namespace internal {
156 // Helper template for project3 expression below
157 template <class CALIBRATION, class POINT>
158 inline Point2 project6(const Pose3& x, const POINT& p, const CALIBRATION& K,
161  return PinholeCamera<CALIBRATION>(x, K).project(p, Dpose, Dpoint, Dcal);
162 }
163 }
164 
165 template <class CALIBRATION, class POINT>
166 inline 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 
171 template <class CALIBRATION>
173  return Point2_(K, &CALIBRATION::uncalibrate, xy_hat);
174 }
175 
176 template <class CALIBRATION>
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 
191 template <typename T>
196 }
197 
198 } // \namespace gtsam
gtsam::internal::rotation
Rot3 rotation(const Pose3 &pose, OptionalJacobian< 3, 6 > H)
Definition: slam/expressions.h:80
H
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Definition: gnuplot_common_settings.hh:74
gtsam::logmap
gtsam::Expression< typename gtsam::traits< T >::TangentVector > logmap(const gtsam::Expression< T > &x1, const gtsam::Expression< T > &x2)
logmap
Definition: slam/expressions.h:192
Pose2.h
2D Pose
gtsam::project3
Point2_ project3(const Pose3_ &x, const Expression< POINT > &p, const Expression< CALIBRATION > &K)
Definition: slam/expressions.h:166
gtsam::translation
Point3_ translation(const Pose3_ &pose)
Definition: slam/expressions.h:93
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
gtsam::between
Expression< T > between(const Expression< T > &t1, const Expression< T > &t2)
Definition: nonlinear/expressions.h:17
gtsam::Double_
Expression< double > Double_
Definition: nonlinear/expressions.h:28
gtsam::Pose3::transformTo
Point3 transformTo(const Point3 &point, OptionalJacobian< 3, 6 > Hself={}, OptionalJacobian< 3, 3 > Hpoint={}) const
takes point in world coordinates and transforms it to Pose coordinates
Definition: Pose3.cpp:371
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
binary::p_cam
Point3_ p_cam(x, &Pose3::transformTo, p)
gtsam::Range< Point2, Point2 >
Definition: Point2.h:99
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
gtsam::Unit3::point3
Point3 point3(OptionalJacobian< 3, 2 > H={}) const
Return unit-norm Point3.
Definition: Unit3.cpp:144
gtsam::Point2_
Expression< Point2 > Point2_
Definition: slam/expressions.h:22
gtsam::point3
Point3_ point3(const Unit3_ &v)
Definition: slam/expressions.h:101
gtsam::internal::project4
Point2 project4(const CAMERA &camera, const POINT &p, OptionalJacobian< 2, CAMERA::dimension > Dcam, OptionalJacobian< 2, FixedDimension< POINT >::value > Dpoint)
Definition: slam/expressions.h:144
gtsam::Cal3_S2_
Expression< Cal3_S2 > Cal3_S2_
Definition: slam/expressions.h:127
gtsam::Pose2_
Expression< Pose2 > Pose2_
Definition: slam/expressions.h:24
gtsam::Rot3::unrotate
Point3 unrotate(const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
rotate point from world to rotated frame
Definition: Rot3.cpp:135
gtsam::PinholeBaseK::project
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
gtsam::Line3_
Expression< Line3 > Line3_
Definition: slam/expressions.h:40
uncalibrate
Point2 uncalibrate(const CAL &K, const Point2 &p, OptionalJacobian< 2, 5 > Dcal, OptionalJacobian< 2, 2 > Dp)
Definition: testExpression.cpp:37
OrientedPlane3.h
gtsam::range
Double_ range(const Point2_ &p, const Point2_ &q)
Definition: slam/expressions.h:30
gtsam::Expression
Definition: Expression.h:47
gtsam::project2
Point2_ project2(const Expression< CAMERA > &camera_, const Expression< POINT > &p_)
Definition: slam/expressions.h:151
gtsam::OrientedPlane3_
Expression< OrientedPlane3 > OrientedPlane3_
Definition: slam/expressions.h:41
gtsam::Pose3::transformPoseTo
Pose3 transformPoseTo(const Pose3 &wTb, OptionalJacobian< 6, 6 > Hself={}, OptionalJacobian< 6, 6 > HwTb={}) const
Definition: Pose3.cpp:338
gtsam::Unit3_
Expression< Unit3 > Unit3_
Definition: slam/expressions.h:37
gtsam::transformFrom
Point3_ transformFrom(const Pose3_ &x, const Point3_ &p)
Definition: slam/expressions.h:47
gtsam::normalize
static void normalize(Signature::Row &row)
Definition: Signature.cpp:88
gtsam::Rot2_
Expression< Rot2 > Rot2_
Definition: slam/expressions.h:23
expressions.h
Common expressions, both linear and non-linear.
gtsam::unrotate
Point3_ unrotate(const Rot3_ &x, const Point3_ &p)
Definition: slam/expressions.h:109
gtsam::rotate
Point3_ rotate(const Rot3_ &x, const Point3_ &p)
Definition: slam/expressions.h:97
gtsam::Rot3::rotate
Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Definition: Rot3M.cpp:148
gtsam::cross
Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2)
cross product
Definition: Point3.cpp:64
Eigen::numext::q
EIGEN_DEVICE_FUNC const Scalar & q
Definition: SpecialFunctionsImpl.h:1984
gtsam::transformTo
Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose, OptionalJacobian< 4, 4 > Dline)
Definition: Line3.cpp:94
x1
Pose3 x1
Definition: testPose3.cpp:663
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::Pose3
Definition: Pose3.h:37
gtsam::PinholeCamera
Definition: PinholeCamera.h:33
gtsam::OrientedPlane3::distance
double distance(OptionalJacobian< 1, 3 > H={}) const
Return the perpendicular distance to the origin.
Definition: OrientedPlane3.h:134
gtsam::OrientedPlane3::normal
Unit3 normal(OptionalJacobian< 2, 3 > H={}) const
Return the normal.
Definition: OrientedPlane3.h:128
gtsam::dot
double dot(const V1 &a, const V2 &b)
Definition: Vector.h:195
PinholeCamera.h
Base class for all pinhole cameras.
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::normal
Unit3_ normal(const OrientedPlane3_ &p)
Definition: slam/expressions.h:121
gtsam::uncalibrate
Point2_ uncalibrate(const Expression< CALIBRATION > &K, const Point2_ &xy_hat)
Definition: slam/expressions.h:172
gtsam::Pose2::transformTo
Point2 transformTo(const Point2 &point, OptionalJacobian< 2, 3 > Dpose={}, OptionalJacobian< 2, 2 > Dpoint={}) const
Definition: Pose2.cpp:207
gtsam::PinholeBase::Project
static Point2 Project(const Point3 &pc, OptionalJacobian< 2, 3 > Dpoint={})
Definition: CalibratedCamera.cpp:88
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
gtsam::b
const G & b
Definition: Group.h:79
gtsam::getPose
Pose3_ getPose(const Expression< PinholeCamera< CALIBRATION > > &cam)
Definition: slam/expressions.h:177
gtsam::Point3_
Expression< Point3 > Point3_
Definition: slam/expressions.h:36
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
Line3.h
4 dimensional manifold of 3D lines
gtsam
traits
Definition: SFMdata.h:40
gtsam::rotation
Rot3_ rotation(const Pose3_ &pose)
Definition: slam/expressions.h:89
gtsam::traits
Definition: Group.h:36
K
#define K
Definition: igam.h:8
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
gtsam::Line3
Definition: Line3.h:44
gtsam::internal::translation
Point3 translation(const Pose3 &pose, OptionalJacobian< 3, 6 > H)
Definition: slam/expressions.h:84
gtsam::Pose3::transformFrom
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
Definition: Pose3.cpp:347
p
float * p
Definition: Tutorial_Map_using.cpp:9
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::Rot3_
Expression< Rot3 > Rot3_
Definition: slam/expressions.h:38
gtsam::Pose3_
Expression< Pose3 > Pose3_
Definition: slam/expressions.h:39
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::internal::project6
Point2 project6(const Pose3 &x, const POINT &p, const CALIBRATION &K, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, 3 > Dpoint, OptionalJacobian< 2, CALIBRATION::dimension > Dcal)
Definition: slam/expressions.h:158
gtsam::distance
Double_ distance(const OrientedPlane3_ &p)
Definition: slam/expressions.h:117
gtsam::FixedDimension
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition: Manifold.h:161
internal
Definition: BandTriangularSolver.h:13
gtsam::Cal3Bundler_
Expression< Cal3Bundler > Cal3Bundler_
Definition: slam/expressions.h:128
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
camera
static const CalibratedCamera camera(kDefaultPose)
gtsam::transformPoseTo
Pose3_ transformPoseTo(const Pose3_ &p, const Pose3_ &q)
Definition: slam/expressions.h:57
gtsam::project
Point2_ project(const Point3_ &p_cam)
Expression version of PinholeBase::Project.
Definition: slam/expressions.h:131
Cal3Bundler.h
Calibration used by Bundler.
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:31