gtsam  4.1.0
gtsam
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;
42  typedef Point3 Translation;
43 
44 private:
45 
46  Rot3 R_;
47  Point3 t_;
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,
78  OptionalJacobian<6, 3> HR = boost::none,
79  OptionalJacobian<6, 3> Ht = boost::none);
80 
86  static boost::optional<Pose3> Align(const std::vector<Point3Pair>& abPointPairs);
87 
91 
93  void print(const std::string& s = "") const;
94 
96  bool equals(const Pose3& pose, double tol = 1e-9) const;
97 
101 
103  static Pose3 identity() {
104  return Pose3();
105  }
106 
108  Pose3 inverse() const;
109 
111  Pose3 operator*(const Pose3& T) const {
112  return Pose3(R_ * T.R_, t_ + R_ * T.t_);
113  }
114 
118 
120  static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi = boost::none);
121 
123  static Vector6 Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose = boost::none);
124 
129  Matrix6 AdjointMap() const;
130 
135  Vector6 Adjoint(const Vector6& xi_b) const {
136  return AdjointMap() * xi_b;
137  }
138 
154  static Matrix6 adjointMap(const Vector6 &xi);
155 
159  static Vector6 adjoint(const Vector6 &xi, const Vector6 &y,
160  OptionalJacobian<6, 6> Hxi = boost::none);
161 
162  // temporary fix for wrappers until case issue is resolved
163  static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);}
164  static Vector6 adjoint_(const Vector6 &xi, const Vector6 &y) { return adjoint(xi, y);}
165 
169  static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y,
170  OptionalJacobian<6, 6> Hxi = boost::none);
171 
173  static Matrix6 ExpmapDerivative(const Vector6& xi);
174 
176  static Matrix6 LogmapDerivative(const Pose3& xi);
177 
178  // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP
179  struct ChartAtOrigin {
180  static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = boost::none);
181  static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = boost::none);
182  };
183 
193  static Matrix3 ComputeQforExpmapDerivative(
194  const Vector6& xi, double nearZeroThreshold = 1e-5);
195 
196  using LieGroup<Pose3, 6>::inverse; // version with derivative
197 
205  static Matrix wedge(double wx, double wy, double wz, double vx, double vy,
206  double vz) {
207  return (Matrix(4, 4) << 0., -wz, wy, vx, wz, 0., -wx, vy, -wy, wx, 0., vz, 0., 0., 0., 0.).finished();
208  }
209 
213 
221  Point3 transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself =
222  boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const;
223 
225  inline Point3 operator*(const Point3& point) const {
226  return transformFrom(point);
227  }
228 
236  Point3 transformTo(const Point3& point, OptionalJacobian<3, 6> Hself =
237  boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const;
238 
242 
244  const Rot3& rotation(OptionalJacobian<3, 6> Hself = boost::none) const;
245 
247  const Point3& translation(OptionalJacobian<3, 6> Hself = boost::none) const;
248 
250  double x() const {
251  return t_.x();
252  }
253 
255  double y() const {
256  return t_.y();
257  }
258 
260  double z() const {
261  return t_.z();
262  }
263 
265  Matrix4 matrix() const;
266 
272  Pose3 transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself = boost::none,
273  OptionalJacobian<6, 6> HaTb = boost::none) const;
274 
279  Pose3 transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself = boost::none,
280  OptionalJacobian<6, 6> HwTb = boost::none) const;
281 
287  double range(const Point3& point, OptionalJacobian<1, 6> Hself = boost::none,
288  OptionalJacobian<1, 3> Hpoint = boost::none) const;
289 
295  double range(const Pose3& pose, OptionalJacobian<1, 6> Hself = boost::none,
296  OptionalJacobian<1, 6> Hpose = boost::none) const;
297 
303  Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> Hself = boost::none,
304  OptionalJacobian<2, 3> Hpoint = boost::none) const;
305 
312  Unit3 bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself = boost::none,
313  OptionalJacobian<2, 6> Hpose = boost::none) const;
314 
318 
324  inline static std::pair<size_t, size_t> translationInterval() {
325  return std::make_pair(3, 5);
326  }
327 
333  static std::pair<size_t, size_t> rotationInterval() {
334  return std::make_pair(0, 2);
335  }
336 
338  GTSAM_EXPORT
339  friend std::ostream &operator<<(std::ostream &os, const Pose3& p);
340 
341  private:
343  friend class boost::serialization::access;
344  template<class Archive>
345  void serialize(Archive & ar, const unsigned int /*version*/) {
346  ar & BOOST_SERIALIZATION_NVP(R_);
347  ar & BOOST_SERIALIZATION_NVP(t_);
348  }
350 
351 #ifdef GTSAM_USE_QUATERNIONS
352  // Align if we are using Quaternions
353  public:
355 #endif
356 };
357 // Pose3 class
358 
366 template<>
367 inline Matrix wedge<Pose3>(const Vector& xi) {
368  return Pose3::wedge(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5));
369 }
370 
371 // Convenience typedef
372 using Pose3Pair = std::pair<Pose3, Pose3>;
373 
374 // For MATLAB wrapper
375 typedef std::vector<Pose3> Pose3Vector;
376 
377 template <>
378 struct traits<Pose3> : public internal::LieGroup<Pose3> {};
379 
380 template <>
381 struct traits<const Pose3> : public internal::LieGroup<Pose3> {};
382 
383 // bearing and range traits, used in RangeFactor
384 template <>
385 struct Bearing<Pose3, Point3> : HasBearing<Pose3, Point3, Unit3> {};
386 
387 template<>
388 struct Bearing<Pose3, Pose3> : HasBearing<Pose3, Pose3, Unit3> {};
389 
390 template <typename T>
391 struct Range<Pose3, T> : HasRange<Pose3, T, double> {};
392 
393 } // namespace gtsam
gtsam::HasRange
Definition: BearingRange.h:193
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
BearingRange.h
Bearing-Range product.
gtsam::Bearing
Definition: BearingRange.h:33
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::HasBearing
Definition: BearingRange.h:179
gtsam::Pose3::operator*
Pose3 operator*(const Pose3 &T) const
compose syntactic sugar
Definition: Pose3.h:111
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
gtsam::Pose3::Pose3
Pose3(const Rot3 &R, const Point3 &t)
Construct from R,t.
Definition: Pose3.h:63
gtsam::Pose2
Definition: Pose2.h:36
gtsam::Pose3::z
double z() const
get z
Definition: Pose3.h:260
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::Pose3::y
double y() const
get y
Definition: Pose3.h:255
gtsam::Pose3::Pose3
Pose3(const Pose3 &pose)
Copy constructor.
Definition: Pose3.h:58
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::Pose3::Pose3
Pose3()
Default constructor is origin.
Definition: Pose3.h:55
gtsam::Pose3::Pose3
Pose3(const Matrix &T)
Constructor from 4*4 matrix.
Definition: Pose3.h:71
gtsam::Pose3::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: Pose3.h:333
gtsam::Pose3::ChartAtOrigin
Definition: Pose3.h:179
Point3.h
3D Point
gtsam::Rot3
Definition: Rot3.h:59
gtsam::wedge< Pose3 >
Matrix wedge< Pose3 >(const Vector &xi)
wedge for Pose3:
Definition: Pose3.h:367
gtsam::Pose3::Rotation
Rot3 Rotation
Pose Concept requirements.
Definition: Pose3.h:41
gtsam::Pose3::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: Pose3.h:324
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition: types.h:269
gtsam::LieGroup
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition: Lie.h:36
gtsam::Range
Definition: BearingRange.h:39
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
gtsam::Pose3
Definition: Pose3.h:37
gtsam::transformTo
Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose, OptionalJacobian< 4, 4 > Dline)
Transform a line from world to camera frame.
Definition: Line3.cpp:94
gtsam::Pose3::x
double x() const
get x
Definition: Pose3.h:250
gtsam::Pose3::operator*
Point3 operator*(const Point3 &point) const
syntactic sugar for transformFrom
Definition: Pose3.h:225
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition: Point3.h:35
gtsam::Pose3::identity
static Pose3 identity()
identity for group operation
Definition: Pose3.h:103
gtsam::Pose3::wedge
static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz)
wedge for Pose3:
Definition: Pose3.h:205
Lie.h
Base class and basic functions for Lie types.
Rot3.h
3D rotation represented as a rotation matrix or quaternion
gtsam::Pose3::Adjoint
Vector6 Adjoint(const Vector6 &xi_b) const
FIXME Not tested - marked as incorrect.
Definition: Pose3.h:135