gtsam  4.1.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 
24 #include <gtsam/geometry/Point2.h>
25 #include <gtsam/geometry/Rot2.h>
26 #include <gtsam/base/Lie.h>
27 #include <gtsam/dllexport.h>
28 
29 namespace gtsam {
30 
36 class Pose2: public LieGroup<Pose2, 3> {
37 
38 public:
39 
41  typedef Rot2 Rotation;
42  typedef Point2 Translation;
43 
44 private:
45 
46  Rot2 r_;
47  Point2 t_;
48 
49 public:
50 
53 
55  Pose2() :
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 
98 
100  GTSAM_EXPORT void print(const std::string& s = "") const;
101 
103  GTSAM_EXPORT bool equals(const Pose2& pose, double tol = 1e-9) const;
104 
108 
110  inline static Pose2 identity() { return Pose2(); }
111 
113  GTSAM_EXPORT Pose2 inverse() const;
114 
116  inline Pose2 operator*(const Pose2& p2) const {
117  return Pose2(r_*p2.r(), t_ + r_*p2.t());
118  }
119 
123 
125  GTSAM_EXPORT static Pose2 Expmap(const Vector3& xi, ChartJacobian H = boost::none);
126 
128  GTSAM_EXPORT static Vector3 Logmap(const Pose2& p, ChartJacobian H = boost::none);
129 
134  GTSAM_EXPORT Matrix3 AdjointMap() const;
135 
137  inline Vector3 Adjoint(const Vector3& xi) const {
138  return AdjointMap()*xi;
139  }
140 
144  GTSAM_EXPORT static Matrix3 adjointMap(const Vector3& v);
145 
149  static Vector3 adjoint(const Vector3& xi, const Vector3& y) {
150  return adjointMap(xi) * y;
151  }
152 
156  static Vector3 adjointTranspose(const Vector3& xi, const Vector3& y) {
157  return adjointMap(xi).transpose() * y;
158  }
159 
160  // temporary fix for wrappers until case issue is resolved
161  static Matrix3 adjointMap_(const Vector3 &xi) { return adjointMap(xi);}
162  static Vector3 adjoint_(const Vector3 &xi, const Vector3 &y) { return adjoint(xi, y);}
163 
171  static inline Matrix3 wedge(double vx, double vy, double w) {
172  Matrix3 m;
173  m << 0.,-w, vx,
174  w, 0., vy,
175  0., 0., 0.;
176  return m;
177  }
178 
180  GTSAM_EXPORT static Matrix3 ExpmapDerivative(const Vector3& v);
181 
183  GTSAM_EXPORT static Matrix3 LogmapDerivative(const Pose2& v);
184 
185  // Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP
186  struct ChartAtOrigin {
187  GTSAM_EXPORT static Pose2 Retract(const Vector3& v, ChartJacobian H = boost::none);
188  GTSAM_EXPORT static Vector3 Local(const Pose2& r, ChartJacobian H = boost::none);
189  };
190 
191  using LieGroup<Pose2, 3>::inverse; // version with derivative
192 
196 
198  GTSAM_EXPORT Point2 transformTo(const Point2& point,
199  OptionalJacobian<2, 3> Dpose = boost::none,
200  OptionalJacobian<2, 2> Dpoint = boost::none) const;
201 
203  GTSAM_EXPORT Point2 transformFrom(const Point2& point,
204  OptionalJacobian<2, 3> Dpose = boost::none,
205  OptionalJacobian<2, 2> Dpoint = boost::none) const;
206 
208  inline Point2 operator*(const Point2& point) const { return transformFrom(point);}
209 
213 
215  inline double x() const { return t_.x(); }
216 
218  inline double y() const { return t_.y(); }
219 
221  inline double theta() const { return r_.theta(); }
222 
224  inline const Point2& t() const { return t_; }
225 
227  inline const Rot2& r() const { return r_; }
228 
230  inline const Point2& translation() const { return t_; }
231 
233  inline const Rot2& rotation() const { return r_; }
234 
236  GTSAM_EXPORT Matrix3 matrix() const;
237 
243  GTSAM_EXPORT Rot2 bearing(const Point2& point,
244  OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 2> H2=boost::none) const;
245 
251  GTSAM_EXPORT Rot2 bearing(const Pose2& pose,
252  OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 3> H2=boost::none) const;
253 
259  GTSAM_EXPORT double range(const Point2& point,
260  OptionalJacobian<1, 3> H1=boost::none,
261  OptionalJacobian<1, 2> H2=boost::none) const;
262 
268  GTSAM_EXPORT double range(const Pose2& point,
269  OptionalJacobian<1, 3> H1=boost::none,
270  OptionalJacobian<1, 3> H2=boost::none) const;
271 
275 
281  inline static std::pair<size_t, size_t> translationInterval() { return std::make_pair(0, 1); }
282 
288  static std::pair<size_t, size_t> rotationInterval() { return std::make_pair(2, 2); }
289 
291 
292  private:
293 
294  // Serialization function
295  friend class boost::serialization::access;
296  template<class Archive>
297  void serialize(Archive & ar, const unsigned int /*version*/) {
298  ar & BOOST_SERIALIZATION_NVP(t_);
299  ar & BOOST_SERIALIZATION_NVP(r_);
300  }
301 
302 public:
303  // Align for Point2, which is either derived from, or is typedef, of Vector2
305 }; // Pose2
306 
308 template <>
309 inline Matrix wedge<Pose2>(const Vector& xi) {
310  // NOTE(chris): Need eval() as workaround for Apple clang + avx2.
311  return Matrix(Pose2::wedge(xi(0),xi(1),xi(2))).eval();
312 }
313 
318 typedef std::pair<Point2,Point2> Point2Pair;
319 GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
320 
321 template <>
322 struct traits<Pose2> : public internal::LieGroup<Pose2> {};
323 
324 template <>
325 struct traits<const Pose2> : public internal::LieGroup<Pose2> {};
326 
327 // bearing and range traits, used in RangeFactor
328 template <typename T>
329 struct Bearing<Pose2, T> : HasBearing<Pose2, T, Rot2> {};
330 
331 template <typename T>
332 struct Range<Pose2, T> : HasRange<Pose2, T, double> {};
333 
334 } // namespace gtsam
335 
gtsam::Pose2::ExpmapDerivative
static GTSAM_EXPORT Matrix3 ExpmapDerivative(const Vector3 &v)
Derivative of Expmap.
Definition: Pose2.cpp:142
gtsam::Pose2::Expmap
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:60
gtsam::HasRange
Definition: BearingRange.h:193
BearingRange.h
Bearing-Range product.
gtsam::Rot2
Definition: Rot2.h:35
gtsam::Bearing
Definition: BearingRange.h:33
gtsam::Pose2::r
const Rot2 & r() const
rotation
Definition: Pose2.h:227
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::internal::LieGroup
Both LieGroupTraits and Testable.
Definition: Lie.h:228
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::Rot2::theta
double theta() const
return angle (RADIANS)
Definition: Rot2.h:183
gtsam::HasBearing
Definition: BearingRange.h:179
gtsam::Pose2::rotation
const Rot2 & rotation() const
rotation
Definition: Pose2.h:233
gtsam::Pose2::Pose2
Pose2()
default constructor = origin
Definition: Pose2.h:55
gtsam::Pose2::translation
const Point2 & translation() const
translation
Definition: Pose2.h:230
gtsam::Pose2::inverse
GTSAM_EXPORT Pose2 inverse() const
inverse
Definition: Pose2.cpp:195
gtsam::Pose2::range
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:247
gtsam::Pose2::Pose2
Pose2(const Rot2 &r, const Point2 &t)
construct from r,t
Definition: Pose2.h:78
gtsam::Pose2
Definition: Pose2.h:36
gtsam::Pose2::Logmap
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:76
gtsam::Pose2::Pose2
Pose2(const Vector &v)
Construct from canonical coordinates (Lie algebra)
Definition: Pose2.h:91
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::Pose2::theta
double theta() const
get theta
Definition: Pose2.h:221
gtsam::Pose2::x
double x() const
get x
Definition: Pose2.h:215
gtsam::Pose2::AdjointMap
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:120
gtsam::Point2Pair
std::pair< Point2, Point2 > Point2Pair
Calculate pose between a vector of 2D point correspondences (p,q) where q = Pose2::transformFrom(p) =...
Definition: Point2.h:38
gtsam::Pose2::LogmapDerivative
static GTSAM_EXPORT Matrix3 LogmapDerivative(const Pose2 &v)
Derivative of Logmap.
Definition: Pose2.cpp:173
gtsam::Pose2::t
const Point2 & t() const
translation
Definition: Pose2.h:224
gtsam::Pose2::Pose2
Pose2(double theta, const Point2 &t)
construct from rotation and translation
Definition: Pose2.h:73
gtsam::Pose2::operator*
Pose2 operator*(const Pose2 &p2) const
compose syntactic sugar
Definition: Pose2.h:116
gtsam::Pose2::adjointTranspose
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:156
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::Pose2::Pose2
Pose2(double x, double y, double theta)
construct from (x,y,theta)
Definition: Pose2.h:68
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
gtsam::Pose2::Pose2
Pose2(const Matrix &T)
Constructor from 3*3 matrix.
Definition: Pose2.h:81
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition: types.h:269
gtsam::Pose2::Rotation
Rot2 Rotation
Pose Concept requirements.
Definition: Pose2.h:41
gtsam::LieGroup
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition: Lie.h:36
gtsam::Range
Definition: BearingRange.h:39
Rot2.h
2D rotation
gtsam::wedge< Pose2 >
Matrix wedge< Pose2 >(const Vector &xi)
specialization for pose2 wedge function (generic template in Lie.h)
Definition: Pose2.h:309
Point2.h
2D Point
gtsam::Pose2::ChartAtOrigin
Definition: Pose2.h:186
gtsam::Pose2::wedge
static Matrix3 wedge(double vx, double vy, double w)
wedge for SE(2):
Definition: Pose2.h:171
gtsam::Pose2::transformFrom
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:212
gtsam::Pose2::Pose2
Pose2(const Pose2 &pose)
copy constructor
Definition: Pose2.h:60
gtsam::Pose2::y
double y() const
get y
Definition: Pose2.h:218
gtsam::Pose2::translationInterval
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:281
gtsam::Pose2::adjoint
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:149
gtsam::Pose2::Adjoint
Vector3 Adjoint(const Vector3 &xi) const
Apply AdjointMap to twist xi.
Definition: Pose2.h:137
gtsam::Pose2::bearing
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:222
gtsam::Pose2::adjointMap
static GTSAM_EXPORT Matrix3 adjointMap(const Vector3 &v)
Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19.
Definition: Pose2.cpp:131
gtsam::Pose2::operator*
Point2 operator*(const Point2 &point) const
syntactic sugar for transformFrom
Definition: Pose2.h:208
gtsam::Pose2::rotationInterval
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:288
gtsam::Pose2::equals
GTSAM_EXPORT bool equals(const Pose2 &pose, double tol=1e-9) const
assert equality up to a tolerance
Definition: Pose2.cpp:55
gtsam::Pose2::print
GTSAM_EXPORT void print(const std::string &s="") const
print with optional string
Definition: Pose2.cpp:50
Lie.h
Base class and basic functions for Lie types.
gtsam::Pose2::identity
static Pose2 identity()
identity for group operation
Definition: Pose2.h:110