Pose3.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 
17 // \callgraph
18 #pragma once
19 
20 #include <gtsam/config.h>
21 
23 #include <gtsam/geometry/Point3.h>
24 #include <gtsam/geometry/Rot3.h>
25 #include <gtsam/base/Lie.h>
26 
27 namespace gtsam {
28 
29 class Pose2;
30 // forward declare
31 
37 class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> {
38 public:
39 
41  typedef Rot3 Rotation;
43 
44 private:
45 
46  Rot3 R_;
48 
49 public:
50 
53 
55  Pose3() : R_(traits<Rot3>::Identity()), t_(traits<Point3>::Identity()) {}
56 
58  Pose3(const Pose3& pose) :
59  R_(pose.R_), t_(pose.t_) {
60  }
61 
63  Pose3(const Rot3& R, const Point3& t) :
64  R_(R), t_(t) {
65  }
66 
68  explicit Pose3(const Pose2& pose2);
69 
71  Pose3(const Matrix &T) :
72  R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0), T(2, 1),
73  T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {
74  }
75 
77  static Pose3 Create(const Rot3& R, const Point3& t,
79  OptionalJacobian<6, 3> Ht = {});
80 
86  static std::optional<Pose3> Align(const Point3Pairs& abPointPairs);
87 
88  // Version of Pose3::Align that takes 2 matrices.
89  static std::optional<Pose3> Align(const Matrix& a, const Matrix& b);
90 
94 
96  void print(const std::string& s = "") const;
97 
99  bool equals(const Pose3& pose, double tol = 1e-9) const;
100 
104 
106  static Pose3 Identity() {
107  return Pose3();
108  }
109 
111  Pose3 inverse() const;
112 
114  Pose3 operator*(const Pose3& T) const {
115  return Pose3(R_ * T.R_, t_ + R_ * T.t_);
116  }
117 
132  Pose3 interpolateRt(const Pose3& T, double t) const;
133 
137 
139  static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi = {});
140 
142  static Vector6 Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose = {});
143 
148  Matrix6 AdjointMap() const;
149 
156  Vector6 Adjoint(const Vector6& xi_b,
157  OptionalJacobian<6, 6> H_this = {},
158  OptionalJacobian<6, 6> H_xib = {}) const;
159 
161  Vector6 AdjointTranspose(const Vector6& x,
162  OptionalJacobian<6, 6> H_this = {},
163  OptionalJacobian<6, 6> H_x = {}) const;
164 
180  static Matrix6 adjointMap(const Vector6& xi);
181 
185  static Vector6 adjoint(const Vector6& xi, const Vector6& y,
186  OptionalJacobian<6, 6> Hxi = {},
187  OptionalJacobian<6, 6> H_y = {});
188 
189  // temporary fix for wrappers until case issue is resolved
190  static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);}
191  static Vector6 adjoint_(const Vector6 &xi, const Vector6 &y) { return adjoint(xi, y);}
192 
196  static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y,
197  OptionalJacobian<6, 6> Hxi = {},
198  OptionalJacobian<6, 6> H_y = {});
199 
201  static Matrix6 ExpmapDerivative(const Vector6& xi);
202 
204  static Matrix6 LogmapDerivative(const Pose3& xi);
205 
206  // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP
207  struct GTSAM_EXPORT ChartAtOrigin {
208  static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = {});
209  static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = {});
210  };
211 
221  static Matrix3 ComputeQforExpmapDerivative(
222  const Vector6& xi, double nearZeroThreshold = 1e-5);
223 
224  using LieGroup<Pose3, 6>::inverse; // version with derivative
225 
233  static Matrix wedge(double wx, double wy, double wz, double vx, double vy,
234  double vz) {
235  return (Matrix(4, 4) << 0., -wz, wy, vx, wz, 0., -wx, vy, -wy, wx, 0., vz, 0., 0., 0., 0.).finished();
236  }
237 
241 
250  {}, OptionalJacobian<3, 3> Hpoint = {}) const;
251 
257  Matrix transformFrom(const Matrix& points) const;
258 
260  inline Point3 operator*(const Point3& point) const {
261  return transformFrom(point);
262  }
263 
272  {}, OptionalJacobian<3, 3> Hpoint = {}) const;
273 
279  Matrix transformTo(const Matrix& points) const;
280 
284 
286  const Rot3& rotation(OptionalJacobian<3, 6> Hself = {}) const;
287 
289  const Point3& translation(OptionalJacobian<3, 6> Hself = {}) const;
290 
292  double x() const {
293  return t_.x();
294  }
295 
297  double y() const {
298  return t_.y();
299  }
300 
302  double z() const {
303  return t_.z();
304  }
305 
307  Matrix4 matrix() const;
308 
314  Pose3 transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself = {},
315  OptionalJacobian<6, 6> HaTb = {}) const;
316 
321  Pose3 transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself = {},
322  OptionalJacobian<6, 6> HwTb = {}) const;
323 
329  double range(const Point3& point, OptionalJacobian<1, 6> Hself = {},
330  OptionalJacobian<1, 3> Hpoint = {}) const;
331 
337  double range(const Pose3& pose, OptionalJacobian<1, 6> Hself = {},
338  OptionalJacobian<1, 6> Hpose = {}) const;
339 
345  Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> Hself = {},
346  OptionalJacobian<2, 3> Hpoint = {}) const;
347 
354  Unit3 bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself = {},
355  OptionalJacobian<2, 6> Hpose = {}) const;
356 
360 
366  inline static std::pair<size_t, size_t> translationInterval() {
367  return {3, 5};
368  }
369 
375  static std::pair<size_t, size_t> rotationInterval() {
376  return {0, 2};
377  }
378 
384  Pose3 slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = {},
385  OptionalJacobian<6, 6> Hy = {}) const;
386 
388  GTSAM_EXPORT
389  friend std::ostream &operator<<(std::ostream &os, const Pose3& p);
390 
391  private:
392 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
393 
394  friend class boost::serialization::access;
395  template<class Archive>
396  void serialize(Archive & ar, const unsigned int /*version*/) {
397  ar & BOOST_SERIALIZATION_NVP(R_);
398  ar & BOOST_SERIALIZATION_NVP(t_);
399  }
400 #endif
401 
403 #ifdef GTSAM_USE_QUATERNIONS
404  // Align if we are using Quaternions
405  public:
407 #endif
408 };
409 // Pose3 class
410 
418 template<>
419 inline Matrix wedge<Pose3>(const Vector& xi) {
420  return Pose3::wedge(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5));
421 }
422 
423 // Convenience typedef
424 using Pose3Pair = std::pair<Pose3, Pose3>;
425 using Pose3Pairs = std::vector<std::pair<Pose3, Pose3> >;
426 
427 // For MATLAB wrapper
428 typedef std::vector<Pose3> Pose3Vector;
429 
430 template <>
431 struct traits<Pose3> : public internal::LieGroup<Pose3> {};
432 
433 template <>
434 struct traits<const Pose3> : public internal::LieGroup<Pose3> {};
435 
436 // bearing and range traits, used in RangeFactor
437 template <>
438 struct Bearing<Pose3, Point3> : HasBearing<Pose3, Point3, Unit3> {};
439 
440 template<>
441 struct Bearing<Pose3, Pose3> : HasBearing<Pose3, Pose3, Unit3> {};
442 
443 template <typename T>
444 struct Range<Pose3, T> : HasRange<Pose3, T, double> {};
445 
446 } // namespace gtsam
gtsam::Pose3Pair
std::pair< Pose3, Pose3 > Pose3Pair
Definition: Pose3.h:424
gtsam::Pose3Vector
std::vector< Pose3 > Pose3Vector
Definition: Pose3.h:428
gtsam::Pose3::R_
Rot3 R_
Rotation gRp, between global and pose frame.
Definition: Pose3.h:46
gtsam::Point3Pairs
std::vector< Point3Pair > Point3Pairs
Definition: Point3.h:45
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))
inverse
const EIGEN_DEVICE_FUNC InverseReturnType inverse() const
Definition: ArrayCwiseUnaryOps.h:411
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::operator<<
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Definition: testGroup.cpp:109
screwPose2::xi
Vector xi
Definition: testPose2.cpp:148
gtsam::Pose3::rotationInterval
static std::pair< size_t, size_t > rotationInterval()
Definition: Pose3.h:375
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::Pose3::Pose3
Pose3(const Matrix &T)
Definition: Pose3.h:71
gtsam::Pose3::Pose3
Pose3(const Rot3 &R, const Point3 &t)
Definition: Pose3.h:63
gtsam::Pose3::operator*
Point3 operator*(const Point3 &point) const
Definition: Pose3.h:260
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
simple::pose2
static Pose3 pose2
Definition: testInitializePose3.cpp:58
Point3.h
3D Point
os
ofstream os("timeSchurFactors.csv")
gtsam::Pose3::t_
Point3 t_
Translation gPp, from global origin to pose frame origin.
Definition: Pose3.h:47
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::Pose3::Pose3
Pose3()
Definition: Pose3.h:55
gtsam::Pose3::Identity
static Pose3 Identity()
identity for group operation
Definition: Pose3.h:106
gtsam::Pose3::operator*
Pose3 operator*(const Pose3 &T) const
compose syntactic sugar
Definition: Pose3.h:114
Rot3.h
3D rotation represented as a rotation matrix or quaternion
gtsam::Pose3::adjoint_
static Vector6 adjoint_(const Vector6 &xi, const Vector6 &y)
Definition: Pose3.h:191
gtsam::range
Double_ range(const Point2_ &p, const Point2_ &q)
Definition: slam/expressions.h:30
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::transformFrom
Point3_ transformFrom(const Pose3_ &x, const Point3_ &p)
Definition: slam/expressions.h:47
gtsam::Pose3::wedge
static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz)
Definition: Pose3.h:233
vy
StridedVectorType vy(make_vector(y, *n, std::abs(*incy)))
gtsam::Pose3::Translation
Point3 Translation
Definition: Pose3.h:42
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::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::Pose3::y
double y() const
get y
Definition: Pose3.h:297
example2::aTb
Point3 aTb
Definition: testEssentialMatrixFactor.cpp:541
gtsam::Pose3Pairs
std::vector< std::pair< Pose3, Pose3 > > Pose3Pairs
Definition: Pose3.h:425
Eigen::Triplet< double >
gtsam::Range
Definition: BearingRange.h:41
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::b
const G & b
Definition: Group.h:79
gtsam::Pose3::ChartAtOrigin
Definition: Pose3.h:207
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
Lie.h
Base class and basic functions for Lie types.
gtsam
traits
Definition: chartTesting.h:28
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::Pose3::Pose3
Pose3(const Pose3 &pose)
Definition: Pose3.h:58
gtsam::HasRange
Definition: BearingRange.h:197
gtsam::Pose3::translationInterval
static std::pair< size_t, size_t > translationInterval()
Definition: Pose3.h:366
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::Pose3::x
double x() const
get x
Definition: Pose3.h:292
gtsam::LieGroup
Definition: Lie.h:37
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::Pose3::Rotation
Rot3 Rotation
Definition: Pose3.h:41
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
gtsam::transformPoseTo
Pose3_ transformPoseTo(const Pose3_ &p, const Pose3_ &q)
Definition: slam/expressions.h:57
align_3::t
Point2 t(10, 10)
Pose3
Definition: testDependencies.h:3
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
pybind_wrapper_test_script.other
other
Definition: pybind_wrapper_test_script.py:42
R
Rot2 R(Rot2::fromAngle(0.1))
gtsam::wedge< Pose3 >
Matrix wedge< Pose3 >(const Vector &xi)
Definition: Pose3.h:419
gtsam::Pose3::adjointMap_
static Matrix6 adjointMap_(const Vector6 &xi)
Definition: Pose3.h:190
gtsam::Pose3::z
double z() const
get z
Definition: Pose3.h:302
vx
StridedVectorType vx(make_vector(x, *n, std::abs(*incx)))
gtsam::Pose2
Definition: Pose2.h:39
HR
#define HR
Definition: mincover.c:26


gtsam
Author(s):
autogenerated on Thu Jun 13 2024 03:04:21