gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
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
24#include <gtsam/geometry/Rot3.h>
25#include <gtsam/base/Lie.h>
26
27namespace gtsam {
28
29class Pose2;
30// forward declare
31
37class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> {
38public:
39
41 typedef Rot3 Rotation;
42 typedef Point3 Translation;
43
44private:
45
46 Rot3 R_;
47 Point3 t_;
48
49public:
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 Point3Pairs& abPointPairs);
87
88 // Version of Pose3::Align that takes 2 matrices.
89 static boost::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 = boost::none);
140
142 static Vector6 Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose = boost::none);
143
148 Matrix6 AdjointMap() const;
149
156 Vector6 Adjoint(const Vector6& xi_b,
157 OptionalJacobian<6, 6> H_this = boost::none,
158 OptionalJacobian<6, 6> H_xib = boost::none) const;
159
161 Vector6 AdjointTranspose(const Vector6& x,
162 OptionalJacobian<6, 6> H_this = boost::none,
163 OptionalJacobian<6, 6> H_x = boost::none) 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 = boost::none,
187 OptionalJacobian<6, 6> H_y = boost::none);
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 = boost::none,
198 OptionalJacobian<6, 6> H_y = boost::none);
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
208 static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = boost::none);
209 static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = boost::none);
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
249 Point3 transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself =
250 boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const;
251
257 Matrix transformFrom(const Matrix& points) const;
258
260 inline Point3 operator*(const Point3& point) const {
261 return transformFrom(point);
262 }
263
271 Point3 transformTo(const Point3& point, OptionalJacobian<3, 6> Hself =
272 boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const;
273
279 Matrix transformTo(const Matrix& points) const;
280
284
286 const Rot3& rotation(OptionalJacobian<3, 6> Hself = boost::none) const;
287
289 const Point3& translation(OptionalJacobian<3, 6> Hself = boost::none) 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 = boost::none,
315 OptionalJacobian<6, 6> HaTb = boost::none) const;
316
321 Pose3 transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself = boost::none,
322 OptionalJacobian<6, 6> HwTb = boost::none) const;
323
329 double range(const Point3& point, OptionalJacobian<1, 6> Hself = boost::none,
330 OptionalJacobian<1, 3> Hpoint = boost::none) const;
331
337 double range(const Pose3& pose, OptionalJacobian<1, 6> Hself = boost::none,
338 OptionalJacobian<1, 6> Hpose = boost::none) const;
339
345 Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> Hself = boost::none,
346 OptionalJacobian<2, 3> Hpoint = boost::none) const;
347
354 Unit3 bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself = boost::none,
355 OptionalJacobian<2, 6> Hpose = boost::none) const;
356
360
366 inline static std::pair<size_t, size_t> translationInterval() {
367 return std::make_pair(3, 5);
368 }
369
375 static std::pair<size_t, size_t> rotationInterval() {
376 return std::make_pair(0, 2);
377 }
378
384 Pose3 slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = boost::none,
385 OptionalJacobian<6, 6> Hy = boost::none) const;
386
388 GTSAM_EXPORT
389 friend std::ostream &operator<<(std::ostream &os, const Pose3& p);
390
391 private:
393 friend class boost::serialization::access;
394 template<class Archive>
395 void serialize(Archive & ar, const unsigned int /*version*/) {
396 ar & BOOST_SERIALIZATION_NVP(R_);
397 ar & BOOST_SERIALIZATION_NVP(t_);
398 }
400
401#ifdef GTSAM_USE_QUATERNIONS
402 // Align if we are using Quaternions
403 public:
405#endif
406};
407// Pose3 class
408
416template<>
417inline Matrix wedge<Pose3>(const Vector& xi) {
418 return Pose3::wedge(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5));
419}
420
421// Convenience typedef
422using Pose3Pair = std::pair<Pose3, Pose3>;
423using Pose3Pairs = std::vector<std::pair<Pose3, Pose3> >;
424
425// For MATLAB wrapper
426typedef std::vector<Pose3> Pose3Vector;
427
428template <>
429struct traits<Pose3> : public internal::LieGroup<Pose3> {};
430
431template <>
432struct traits<const Pose3> : public internal::LieGroup<Pose3> {};
433
434// bearing and range traits, used in RangeFactor
435template <>
436struct Bearing<Pose3, Point3> : HasBearing<Pose3, Point3, Unit3> {};
437
438template<>
439struct Bearing<Pose3, Pose3> : HasBearing<Pose3, Pose3, Unit3> {};
440
441template <typename T>
442struct Range<Pose3, T> : HasRange<Pose3, T, double> {};
443
444} // namespace gtsam
Base class and basic functions for Lie types.
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
Bearing-Range product.
3D Point
3D rotation represented as a rotation matrix or quaternion
Global functions in a separate testing namespace.
Definition chartTesting.h:28
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
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:36
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
Matrix wedge< Pose3 >(const Vector &xi)
wedge for Pose3:
Definition Pose3.h:417
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
Template to create a binary predicate.
Definition Testable.h:111
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
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
static Pose3 Identity()
identity for group operation
Definition Pose3.h:106
Pose3(const Pose3 &pose)
Copy constructor.
Definition Pose3.h:58
Pose3()
Default constructor is origin.
Definition Pose3.h:55
static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz)
wedge for Pose3:
Definition Pose3.h:233
Pose3(const Rot3 &R, const Point3 &t)
Construct from R,t.
Definition Pose3.h:63
Pose3(const Matrix &T)
Constructor from 4*4 matrix.
Definition Pose3.h:71
double z() const
get z
Definition Pose3.h:302
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:375
Pose3 operator*(const Pose3 &T) const
compose syntactic sugar
Definition Pose3.h:114
Rot3 Rotation
Pose Concept requirements.
Definition Pose3.h:41
double y() const
get y
Definition Pose3.h:297
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:366
Point3 operator*(const Point3 &point) const
syntactic sugar for transformFrom
Definition Pose3.h:260
double x() const
get x
Definition Pose3.h:292
Definition Pose3.h:207
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
Represents a 3D point on a unit sphere.
Definition Unit3.h:43