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>
29 
30 #include <optional>
31 
32 namespace gtsam {
33 
39 class GTSAM_EXPORT Pose2: public LieGroup<Pose2, 3> {
40 
41 public:
42 
44  typedef Rot2 Rotation;
46 
47 private:
48 
51 
52 public:
53 
56 
58  Pose2() :
59  r_(traits<Rot2>::Identity()), t_(traits<Point2>::Identity()) {
60  }
61 
63  Pose2(const Pose2& pose) : r_(pose.r_), t_(pose.t_) {}
64 
71  Pose2(double x, double y, double theta) :
72  r_(Rot2::fromAngle(theta)), t_(x, y) {
73  }
74 
76  Pose2(double theta, const Point2& t) :
77  r_(Rot2::fromAngle(theta)), t_(t) {
78  }
79 
81  Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {}
82 
84  Pose2(const Matrix &T) :
85  r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) {
86  assert(T.rows() == 3 && T.cols() == 3);
87  }
88 
92 
94  Pose2(const Vector& v) : Pose2() {
95  *this = Expmap(v);
96  }
97 
105  static std::optional<Pose2> Align(const Point2Pairs& abPointPairs);
106 
107  // Version of Pose2::Align that takes 2 matrices.
108  static std::optional<Pose2> Align(const Matrix& a, const Matrix& b);
109 
113 
115  void print(const std::string& s = "") const;
116 
118  bool equals(const Pose2& pose, double tol = 1e-9) const;
119 
123 
125  inline static Pose2 Identity() { return Pose2(); }
126 
128  Pose2 inverse() const;
129 
131  inline Pose2 operator*(const Pose2& p2) const {
132  return Pose2(r_*p2.r(), t_ + r_*p2.t());
133  }
134 
138 
140  static Pose2 Expmap(const Vector3& xi, ChartJacobian H = {});
141 
143  static Vector3 Logmap(const Pose2& p, ChartJacobian H = {});
144 
149  Matrix3 AdjointMap() const;
150 
152  inline Vector3 Adjoint(const Vector3& xi) const {
153  return AdjointMap()*xi;
154  }
155 
159  static Matrix3 adjointMap(const Vector3& v);
160 
164  static Vector3 adjoint(const Vector3& xi, const Vector3& y) {
165  return adjointMap(xi) * y;
166  }
167 
171  static Vector3 adjointTranspose(const Vector3& xi, const Vector3& y) {
172  return adjointMap(xi).transpose() * y;
173  }
174 
175  // temporary fix for wrappers until case issue is resolved
176  static Matrix3 adjointMap_(const Vector3 &xi) { return adjointMap(xi);}
177  static Vector3 adjoint_(const Vector3 &xi, const Vector3 &y) { return adjoint(xi, y);}
178 
186  static inline Matrix3 wedge(double vx, double vy, double w) {
187  Matrix3 m;
188  m << 0.,-w, vx,
189  w, 0., vy,
190  0., 0., 0.;
191  return m;
192  }
193 
195  static Matrix3 ExpmapDerivative(const Vector3& v);
196 
198  static Matrix3 LogmapDerivative(const Pose2& v);
199 
200  // Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP
201  struct GTSAM_EXPORT ChartAtOrigin {
202  static Pose2 Retract(const Vector3& v, ChartJacobian H = {});
203  static Vector3 Local(const Pose2& r, ChartJacobian H = {});
204  };
205 
206  using LieGroup<Pose2, 3>::inverse; // version with derivative
207 
211 
214  OptionalJacobian<2, 3> Dpose = {},
215  OptionalJacobian<2, 2> Dpoint = {}) const;
216 
222  Matrix transformTo(const Matrix& points) const;
223 
226  OptionalJacobian<2, 3> Dpose = {},
227  OptionalJacobian<2, 2> Dpoint = {}) const;
228 
234  Matrix transformFrom(const Matrix& points) const;
235 
237  inline Point2 operator*(const Point2& point) const {
238  return transformFrom(point);
239  }
240 
244 
246  inline double x() const { return t_.x(); }
247 
249  inline double y() const { return t_.y(); }
250 
252  inline double theta() const { return r_.theta(); }
253 
255  inline const Point2& t() const { return t_; }
256 
258  inline const Rot2& r() const { return r_; }
259 
261  inline const Point2& translation(OptionalJacobian<2, 3> Hself={}) const {
262  if (Hself) {
263  *Hself = Matrix::Zero(2, 3);
264  (*Hself).block<2, 2>(0, 0) = rotation().matrix();
265  }
266  return t_;
267  }
268 
270  inline const Rot2& rotation(OptionalJacobian<1, 3> Hself={}) const {
271  if (Hself) *Hself << 0, 0, 1;
272  return r_;
273  }
274 
276  Matrix3 matrix() const;
277 
283  Rot2 bearing(const Point2& point,
284  OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 2> H2={}) const;
285 
291  Rot2 bearing(const Pose2& pose,
292  OptionalJacobian<1, 3> H1={}, OptionalJacobian<1, 3> H2={}) const;
293 
299  double range(const Point2& point,
300  OptionalJacobian<1, 3> H1={},
301  OptionalJacobian<1, 2> H2={}) const;
302 
308  double range(const Pose2& point,
309  OptionalJacobian<1, 3> H1={},
310  OptionalJacobian<1, 3> H2={}) const;
311 
315 
321  inline static std::pair<size_t, size_t> translationInterval() { return {0, 1}; }
322 
328  static std::pair<size_t, size_t> rotationInterval() { return {2, 2}; }
329 
331  GTSAM_EXPORT
332  friend std::ostream &operator<<(std::ostream &os, const Pose2& p);
333 
335 
336  private:
337 
338 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION //
339  // Serialization function
340  friend class boost::serialization::access;
341  template<class Archive>
342  void serialize(Archive & ar, const unsigned int /*version*/) {
343  ar & BOOST_SERIALIZATION_NVP(t_);
344  ar & BOOST_SERIALIZATION_NVP(r_);
345  }
346 #endif
347 
348 public:
349  // Align for Point2, which is either derived from, or is typedef, of Vector2
351 }; // Pose2
352 
354 template <>
355 inline Matrix wedge<Pose2>(const Vector& xi) {
356  // NOTE(chris): Need eval() as workaround for Apple clang + avx2.
357  return Matrix(Pose2::wedge(xi(0),xi(1),xi(2))).eval();
358 }
359 
360 // Convenience typedef
361 using Pose2Pair = std::pair<Pose2, Pose2>;
362 using Pose2Pairs = std::vector<Pose2Pair>;
363 
364 template <>
365 struct traits<Pose2> : public internal::LieGroup<Pose2> {};
366 
367 template <>
368 struct traits<const Pose2> : public internal::LieGroup<Pose2> {};
369 
370 // bearing and range traits, used in RangeFactor
371 template <typename T>
372 struct Bearing<Pose2, T> : HasBearing<Pose2, T, Rot2> {};
373 
374 template <typename T>
375 struct Range<Pose2, T> : HasRange<Pose2, T, double> {};
376 
377 } // namespace gtsam
378 
w
RowVector3d w
Definition: Matrix_resize_int.cpp:3
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::Pose2::Rotation
Rot2 Rotation
Definition: Pose2.h:44
gtsam::Pose2::Pose2
Pose2(const Matrix &T)
Definition: Pose2.h:84
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
inverse
const EIGEN_DEVICE_FUNC InverseReturnType inverse() const
Definition: ArrayCwiseUnaryOps.h:411
gtsam::Pose2::adjoint_
static Vector3 adjoint_(const Vector3 &xi, const Vector3 &y)
Definition: Pose2.h:177
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::Pose2::Adjoint
Vector3 Adjoint(const Vector3 &xi) const
Apply AdjointMap to twist xi.
Definition: Pose2.h:152
gtsam::operator<<
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Definition: testGroup.cpp:109
screwPose2::xi
Vector xi
Definition: testPose2.cpp:148
gtsam::Pose2::adjointMap_
static Matrix3 adjointMap_(const Vector3 &xi)
Definition: Pose2.h:176
gtsam::Pose2::rotation
const Rot2 & rotation(OptionalJacobian< 1, 3 > Hself={}) const
rotation
Definition: Pose2.h:270
gtsam::Pose2::t
const Point2 & t() const
translation
Definition: Pose2.h:255
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::Rot2::theta
double theta() const
Definition: Rot2.h:186
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
os
ofstream os("timeSchurFactors.csv")
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::Pose2::Translation
Point2 Translation
Definition: Pose2.h:45
Rot2.h
2D rotation
Point2.h
2D Point
gtsam::Pose2::translationInterval
static std::pair< size_t, size_t > translationInterval()
Definition: Pose2.h:321
gtsam::range
Double_ range(const Point2_ &p, const Point2_ &q)
Definition: slam/expressions.h:30
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:51
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
Expmap
Pose2_ Expmap(const Vector3_ &xi)
Definition: InverseKinematicsExampleExpressions.cpp:47
gtsam::HasBearing
Definition: BearingRange.h:183
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
gtsam::Pose2::operator*
Pose2 operator*(const Pose2 &p2) const
compose syntactic sugar
Definition: Pose2.h:131
gtsam::transformFrom
Point3_ transformFrom(const Pose3_ &x, const Point3_ &p)
Definition: slam/expressions.h:47
gtsam::Pose2::ChartAtOrigin
Definition: Pose2.h:201
vy
StridedVectorType vy(make_vector(y, *n, std::abs(*incy)))
gtsam::transformTo
Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose, OptionalJacobian< 4, 4 > Dline)
Definition: Line3.cpp:94
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition: Lie.h:229
gtsam::Pose2::Pose2
Pose2(const Pose2 &pose)
Definition: Pose2.h:63
gtsam::Pose2::r
const Rot2 & r() const
rotation
Definition: Pose2.h:258
gtsam::Pose2::Identity
static Pose2 Identity()
identity for group operation
Definition: Pose2.h:125
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
Eigen::Triplet< double >
gtsam::Range
Definition: BearingRange.h:41
gtsam::Pose2::Pose2
Pose2(const Vector &v)
Definition: Pose2.h:94
gtsam::equals
Definition: Testable.h:112
gtsam::Pose2::Pose2
Pose2(const Rot2 &r, const Point2 &t)
Definition: Pose2.h:81
y
Scalar * y
Definition: level1_cplx_impl.h:124
matrix
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
Definition: gtsam/3rdparty/Eigen/blas/common.h:110
gtsam::Pose2::theta
double theta() const
get theta
Definition: Pose2.h:252
std_optional_serialization.h
atan2
AnnoyingScalar atan2(const AnnoyingScalar &y, const AnnoyingScalar &x)
Definition: AnnoyingScalar.h:110
gtsam::b
const G & b
Definition: Group.h:79
gtsam::Rot2
Definition: Rot2.h:35
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
gtsam::Pose2::adjointTranspose
static Vector3 adjointTranspose(const Vector3 &xi, const Vector3 &y)
Definition: Pose2.h:171
gtsam::Pose2::Pose2
Pose2()
Definition: Pose2.h:58
gtsam::Pose2::translation
const Point2 & translation(OptionalJacobian< 2, 3 > Hself={}) const
translation
Definition: Pose2.h:261
Lie.h
Base class and basic functions for Lie types.
gtsam::Pose2::Pose2
Pose2(double theta, const Point2 &t)
Definition: Pose2.h:76
gtsam
traits
Definition: SFMdata.h:40
gtsam::rotation
Rot3_ rotation(const Pose3_ &pose)
Definition: slam/expressions.h:89
gtsam::traits
Definition: Group.h:36
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
gtsam::Pose2::x
double x() const
get x
Definition: Pose2.h:246
gtsam::Pose2::r_
Rot2 r_
Definition: Pose2.h:49
gtsam::HasRange
Definition: BearingRange.h:197
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::Pose2::rotationInterval
static std::pair< size_t, size_t > rotationInterval()
Definition: Pose2.h:328
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::Pose2::Pose2
Pose2(double x, double y, double theta)
Definition: Pose2.h:71
gtsam::Pose2::adjoint
static Vector3 adjoint(const Vector3 &xi, const Vector3 &y)
Definition: Pose2.h:164
gtsam::LieGroup
Definition: Lie.h:37
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Point2Pairs
std::vector< Point2Pair > Point2Pairs
Definition: Point2.h:38
gtsam::Pose2Pairs
std::vector< Pose2Pair > Pose2Pairs
Definition: Pose2.h:362
gtsam::Pose2::operator*
Point2 operator*(const Point2 &point) const
Definition: Pose2.h:237
gtsam::Pose2::wedge
static Matrix3 wedge(double vx, double vy, double w)
Definition: Pose2.h:186
BearingRange.h
Bearing-Range product.
gtsam::internal::Align
static Similarity2 Align(const Point2Pairs &d_abPointPairs, const Rot2 &aRb, const Point2Pair &centroids)
This method estimates the similarity transform from differences point pairs, given a known or estimat...
Definition: Similarity2.cpp:74
gtsam::Bearing
Definition: BearingRange.h:35
align_3::t
Point2 t(10, 10)
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
Definition: types.h:279
adjoint
void adjoint(const MatrixType &m)
Definition: adjoint.cpp:67
gtsam::wedge< Pose2 >
Matrix wedge< Pose2 >(const Vector &xi)
Definition: Pose2.h:355
gtsam::Pose2::y
double y() const
get y
Definition: Pose2.h:249
gtsam::Pose2::t_
Point2 t_
Definition: Pose2.h:50
gtsam::Pose2Pair
std::pair< Pose2, Pose2 > Pose2Pair
Definition: Pose2.h:361
vx
StridedVectorType vx(make_vector(x, *n, std::abs(*incx)))
gtsam::Pose2
Definition: Pose2.h:39


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:34:24