gtsam 4.2.0
gtsam
Pose2.h
Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
19// \callgraph
20
21#pragma once
22
25#include <gtsam/geometry/Rot2.h>
26#include <gtsam/base/Lie.h>
27#include <gtsam/dllexport.h>
28
29namespace gtsam {
30
36class Pose2: public LieGroup<Pose2, 3> {
37
38public:
39
41 typedef Rot2 Rotation;
42 typedef Point2 Translation;
43
44private:
45
46 Rot2 r_;
47 Point2 t_;
48
49public:
50
53
56 r_(traits<Rot2>::Identity()), t_(traits<Point2>::Identity()) {
57 }
58
60 Pose2(const Pose2& pose) : r_(pose.r_), t_(pose.t_) {}
61
68 Pose2(double x, double y, double theta) :
69 r_(Rot2::fromAngle(theta)), t_(x, y) {
70 }
71
73 Pose2(double theta, const Point2& t) :
74 r_(Rot2::fromAngle(theta)), t_(t) {
75 }
76
78 Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {}
79
81 Pose2(const Matrix &T) :
82 r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) {
83 assert(T.rows() == 3 && T.cols() == 3);
84 }
85
89
91 Pose2(const Vector& v) : Pose2() {
92 *this = Expmap(v);
93 }
94
102 static boost::optional<Pose2> Align(const Point2Pairs& abPointPairs);
103
104 // Version of Pose2::Align that takes 2 matrices.
105 static boost::optional<Pose2> Align(const Matrix& a, const Matrix& b);
106
110
112 GTSAM_EXPORT void print(const std::string& s = "") const;
113
115 GTSAM_EXPORT bool equals(const Pose2& pose, double tol = 1e-9) const;
116
120
122 inline static Pose2 Identity() { return Pose2(); }
123
125 GTSAM_EXPORT Pose2 inverse() const;
126
128 inline Pose2 operator*(const Pose2& p2) const {
129 return Pose2(r_*p2.r(), t_ + r_*p2.t());
130 }
131
135
137 GTSAM_EXPORT static Pose2 Expmap(const Vector3& xi, ChartJacobian H = boost::none);
138
140 GTSAM_EXPORT static Vector3 Logmap(const Pose2& p, ChartJacobian H = boost::none);
141
146 GTSAM_EXPORT Matrix3 AdjointMap() const;
147
149 inline Vector3 Adjoint(const Vector3& xi) const {
150 return AdjointMap()*xi;
151 }
152
156 GTSAM_EXPORT static Matrix3 adjointMap(const Vector3& v);
157
161 static Vector3 adjoint(const Vector3& xi, const Vector3& y) {
162 return adjointMap(xi) * y;
163 }
164
168 static Vector3 adjointTranspose(const Vector3& xi, const Vector3& y) {
169 return adjointMap(xi).transpose() * y;
170 }
171
172 // temporary fix for wrappers until case issue is resolved
173 static Matrix3 adjointMap_(const Vector3 &xi) { return adjointMap(xi);}
174 static Vector3 adjoint_(const Vector3 &xi, const Vector3 &y) { return adjoint(xi, y);}
175
183 static inline Matrix3 wedge(double vx, double vy, double w) {
184 Matrix3 m;
185 m << 0.,-w, vx,
186 w, 0., vy,
187 0., 0., 0.;
188 return m;
189 }
190
192 GTSAM_EXPORT static Matrix3 ExpmapDerivative(const Vector3& v);
193
195 GTSAM_EXPORT static Matrix3 LogmapDerivative(const Pose2& v);
196
197 // Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP
199 GTSAM_EXPORT static Pose2 Retract(const Vector3& v, ChartJacobian H = boost::none);
200 GTSAM_EXPORT static Vector3 Local(const Pose2& r, ChartJacobian H = boost::none);
201 };
202
203 using LieGroup<Pose2, 3>::inverse; // version with derivative
204
208
210 GTSAM_EXPORT Point2 transformTo(const Point2& point,
211 OptionalJacobian<2, 3> Dpose = boost::none,
212 OptionalJacobian<2, 2> Dpoint = boost::none) const;
213
219 Matrix transformTo(const Matrix& points) const;
220
222 GTSAM_EXPORT Point2 transformFrom(const Point2& point,
223 OptionalJacobian<2, 3> Dpose = boost::none,
224 OptionalJacobian<2, 2> Dpoint = boost::none) const;
225
231 Matrix transformFrom(const Matrix& points) const;
232
234 inline Point2 operator*(const Point2& point) const {
235 return transformFrom(point);
236 }
237
241
243 inline double x() const { return t_.x(); }
244
246 inline double y() const { return t_.y(); }
247
249 inline double theta() const { return r_.theta(); }
250
252 inline const Point2& t() const { return t_; }
253
255 inline const Rot2& r() const { return r_; }
256
258 inline const Point2& translation() const { return t_; }
259
261 inline const Rot2& rotation() const { return r_; }
262
264 GTSAM_EXPORT Matrix3 matrix() const;
265
271 GTSAM_EXPORT Rot2 bearing(const Point2& point,
272 OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 2> H2=boost::none) const;
273
279 GTSAM_EXPORT Rot2 bearing(const Pose2& pose,
280 OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 3> H2=boost::none) const;
281
287 GTSAM_EXPORT double range(const Point2& point,
288 OptionalJacobian<1, 3> H1=boost::none,
289 OptionalJacobian<1, 2> H2=boost::none) const;
290
296 GTSAM_EXPORT double range(const Pose2& point,
297 OptionalJacobian<1, 3> H1=boost::none,
298 OptionalJacobian<1, 3> H2=boost::none) const;
299
303
309 inline static std::pair<size_t, size_t> translationInterval() { return std::make_pair(0, 1); }
310
316 static std::pair<size_t, size_t> rotationInterval() { return std::make_pair(2, 2); }
317
319 GTSAM_EXPORT
320 friend std::ostream &operator<<(std::ostream &os, const Pose2& p);
321
323
324 private:
325
326 // Serialization function
327 friend class boost::serialization::access;
328 template<class Archive>
329 void serialize(Archive & ar, const unsigned int /*version*/) {
330 ar & BOOST_SERIALIZATION_NVP(t_);
331 ar & BOOST_SERIALIZATION_NVP(r_);
332 }
333
334public:
335 // Align for Point2, which is either derived from, or is typedef, of Vector2
337}; // Pose2
338
340template <>
341inline Matrix wedge<Pose2>(const Vector& xi) {
342 // NOTE(chris): Need eval() as workaround for Apple clang + avx2.
343 return Matrix(Pose2::wedge(xi(0),xi(1),xi(2))).eval();
344}
345
346#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
352GTSAM_EXPORT boost::optional<Pose2>
353GTSAM_DEPRECATED align(const Point2Pairs& pairs);
354#endif
355
356// Convenience typedef
357using Pose2Pair = std::pair<Pose2, Pose2>;
358using Pose2Pairs = std::vector<Pose2Pair>;
359
360template <>
361struct traits<Pose2> : public internal::LieGroup<Pose2> {};
362
363template <>
364struct traits<const Pose2> : public internal::LieGroup<Pose2> {};
365
366// bearing and range traits, used in RangeFactor
367template <typename T>
368struct Bearing<Pose2, T> : HasBearing<Pose2, T, Rot2> {};
369
370template <typename T>
371struct Range<Pose2, T> : HasRange<Pose2, T, double> {};
372
373} // namespace gtsam
374
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition: types.h:308
Base class and basic functions for Lie types.
2D rotation
2D Point
Bearing-Range product.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
Matrix wedge< Pose2 >(const Vector &xi)
specialization for pose2 wedge function (generic template in Lie.h)
Definition: Pose2.h:341
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
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition: Lie.h:37
Both LieGroupTraits and Testable.
Definition: Lie.h:229
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:41
Definition: BearingRange.h:34
Definition: BearingRange.h:40
Definition: BearingRange.h:180
Definition: BearingRange.h:194
A 2D pose (Point2,Rot2)
Definition: Pose2.h:36
GTSAM_EXPORT Rot2 bearing(const Point2 &point, OptionalJacobian< 1, 3 > H1=boost::none, OptionalJacobian< 1, 2 > H2=boost::none) const
Calculate bearing to a landmark.
Definition: Pose2.cpp:245
Pose2 operator*(const Pose2 &p2) const
compose syntactic sugar
Definition: Pose2.h:128
GTSAM_EXPORT void print(const std::string &s="") const
print with optional string
Definition: Pose2.cpp:50
const Rot2 & rotation() const
rotation
Definition: Pose2.h:261
GTSAM_EXPORT Matrix3 AdjointMap() const
Calculate Adjoint map Ad_pose is 3*3 matrix that when applied to twist xi , returns Ad_pose(xi)
Definition: Pose2.cpp:126
static GTSAM_EXPORT Matrix3 LogmapDerivative(const Pose2 &v)
Derivative of Logmap.
Definition: Pose2.cpp:179
double y() const
get y
Definition: Pose2.h:246
static GTSAM_EXPORT Vector3 Logmap(const Pose2 &p, ChartJacobian H=boost::none)
Log map at identity - return the canonical coordinates of this rotation.
Definition: Pose2.cpp:82
GTSAM_EXPORT Pose2 inverse() const
inverse
Definition: Pose2.cpp:201
GTSAM_EXPORT friend std::ostream & operator<<(std::ostream &os, const Pose2 &p)
Output stream operator.
Definition: Pose2.cpp:55
Point2 operator*(const Point2 &point) const
syntactic sugar for transformFrom
Definition: Pose2.h:234
Pose2(double x, double y, double theta)
construct from (x,y,theta)
Definition: Pose2.h:68
const Point2 & t() const
translation
Definition: Pose2.h:252
static std::pair< size_t, size_t > translationInterval()
Return the start and end indices (inclusive) of the translation component of the exponential map para...
Definition: Pose2.h:309
Pose2(double theta, const Point2 &t)
construct from rotation and translation
Definition: Pose2.h:73
double x() const
get x
Definition: Pose2.h:243
const Rot2 & r() const
rotation
Definition: Pose2.h:255
Pose2(const Vector &v)
Construct from canonical coordinates (Lie algebra)
Definition: Pose2.h:91
Vector3 Adjoint(const Vector3 &xi) const
Apply AdjointMap to twist xi.
Definition: Pose2.h:149
static Matrix3 wedge(double vx, double vy, double w)
wedge for SE(2):
Definition: Pose2.h:183
static std::pair< size_t, size_t > rotationInterval()
Return the start and end indices (inclusive) of the rotation component of the exponential map paramet...
Definition: Pose2.h:316
static GTSAM_EXPORT Matrix3 adjointMap(const Vector3 &v)
Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19.
Definition: Pose2.cpp:137
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
double theta() const
get theta
Definition: Pose2.h:249
static GTSAM_EXPORT Matrix3 ExpmapDerivative(const Vector3 &v)
Derivative of Expmap.
Definition: Pose2.cpp:148
GTSAM_EXPORT bool equals(const Pose2 &pose, double tol=1e-9) const
assert equality up to a tolerance
Definition: Pose2.cpp:61
Rot2 Rotation
Pose Concept requirements.
Definition: Pose2.h:41
Pose2()
default constructor = origin
Definition: Pose2.h:55
GTSAM_EXPORT double range(const Point2 &point, OptionalJacobian< 1, 3 > H1=boost::none, OptionalJacobian< 1, 2 > H2=boost::none) const
Calculate range to a landmark.
Definition: Pose2.cpp:270
GTSAM_EXPORT Point2 transformFrom(const Point2 &point, OptionalJacobian< 2, 3 > Dpose=boost::none, OptionalJacobian< 2, 2 > Dpoint=boost::none) const
Return point coordinates in global frame.
Definition: Pose2.cpp:226
static Vector3 adjointTranspose(const Vector3 &xi, const Vector3 &y)
The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.
Definition: Pose2.h:168
static GTSAM_EXPORT Pose2 Expmap(const Vector3 &xi, ChartJacobian H=boost::none)
Exponential map at identity - create a rotation from canonical coordinates .
Definition: Pose2.cpp:66
static Pose2 Identity()
identity for group operation
Definition: Pose2.h:122
static boost::optional< Pose2 > Align(const Point2Pairs &abPointPairs)
Create Pose2 by aligning two point pairs A pose aTb is estimated between pairs (a_point,...
Definition: Pose2.cpp:330
Pose2(const Rot2 &r, const Point2 &t)
construct from r,t
Definition: Pose2.h:78
static Vector3 adjoint(const Vector3 &xi, const Vector3 &y)
Action of the adjointMap on a Lie-algebra vector y, with optional derivatives.
Definition: Pose2.h:161
Pose2(const Pose2 &pose)
copy constructor
Definition: Pose2.h:60
const Point2 & translation() const
translation
Definition: Pose2.h:258
Pose2(const Matrix &T)
Constructor from 3*3 matrix.
Definition: Pose2.h:81
Definition: Pose2.h:198
Rotation matrix NOTE: the angle theta is in radians unless explicitly stated.
Definition: Rot2.h:36
double theta() const
return angle (RADIANS)
Definition: Rot2.h:187