20#include <gtsam/config.h>
42 typedef Point3 Translation;
59 R_(pose.R_), t_(pose.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)) {
86 static boost::optional<Pose3> Align(
const Point3Pairs& abPointPairs);
89 static boost::optional<Pose3> Align(
const Matrix& a,
const Matrix& b);
96 void print(
const std::string& s =
"")
const;
99 bool equals(
const Pose3& pose,
double tol = 1e-9)
const;
111 Pose3 inverse()
const;
115 return Pose3(R_ * T.R_, t_ + R_ * T.t_);
132 Pose3 interpolateRt(
const Pose3& T,
double t)
const;
148 Matrix6 AdjointMap()
const;
156 Vector6 Adjoint(
const Vector6& xi_b,
161 Vector6 AdjointTranspose(
const Vector6& x,
180 static Matrix6 adjointMap(
const Vector6& xi);
185 static Vector6 adjoint(
const Vector6& xi,
const Vector6& y,
190 static Matrix6 adjointMap_(
const Vector6 &xi) {
return adjointMap(xi);}
191 static Vector6 adjoint_(
const Vector6 &xi,
const Vector6 &y) {
return adjoint(xi, y);}
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);
201 static Matrix6 ExpmapDerivative(
const Vector6& xi);
204 static Matrix6 LogmapDerivative(
const Pose3& xi);
208 static Pose3 Retract(
const Vector6& xi, ChartJacobian Hxi = boost::none);
209 static Vector6 Local(
const Pose3& pose, ChartJacobian Hpose = boost::none);
221 static Matrix3 ComputeQforExpmapDerivative(
222 const Vector6& xi,
double nearZeroThreshold = 1e-5);
233 static Matrix
wedge(
double wx,
double wy,
double wz,
double vx,
double vy,
235 return (Matrix(4, 4) << 0., -wz, wy, vx, wz, 0., -wx, vy, -wy, wx, 0., vz, 0., 0., 0., 0.).finished();
257 Matrix transformFrom(
const Matrix& points)
const;
261 return transformFrom(point);
307 Matrix4 matrix()
const;
367 return std::make_pair(3, 5);
376 return std::make_pair(0, 2);
389 friend std::ostream &operator<<(std::ostream &os,
const Pose3& p);
393 friend class boost::serialization::access;
394 template<
class Archive>
395 void serialize(Archive & ar,
const unsigned int ) {
396 ar & BOOST_SERIALIZATION_NVP(R_);
397 ar & BOOST_SERIALIZATION_NVP(t_);
401#ifdef GTSAM_USE_QUATERNIONS
418 return Pose3::wedge(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5));
422using Pose3Pair = std::pair<Pose3, Pose3>;
423using Pose3Pairs = std::vector<std::pair<Pose3, Pose3> >;
426typedef std::vector<Pose3> Pose3Vector;
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
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
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