Loading...
Searching...
No Matches
Pose3.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
// \callgraph
#pragma once
#include <gtsam/config.h>
#include <gtsam/geometry/BearingRange.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/base/Lie.h>
namespace gtsam {
class Pose2;
// forward declare
class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> {
public:
typedef Rot3 Rotation;
typedef Point3 Translation;
private:
Rot3 R_;
Point3 t_;
public:
Pose3() : R_(traits<Rot3>::Identity()), t_(traits<Point3>::Identity()) {}
Pose3(const Pose3& pose) :
R_(pose.R_), t_(pose.t_) {
}
Pose3(const Rot3& R, const Point3& t) :
R_(R), t_(t) {
}
explicit Pose3(const Pose2& pose2);
Pose3(const Matrix &T) :
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),
T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {
}
static Pose3 Create(const Rot3& R, const Point3& t,
OptionalJacobian<6, 3> HR = boost::none,
OptionalJacobian<6, 3> Ht = boost::none);
static boost::optional<Pose3> Align(const Point3Pairs& abPointPairs);
// Version of Pose3::Align that takes 2 matrices.
static boost::optional<Pose3> Align(const Matrix& a, const Matrix& b);
void print(const std::string& s = "") const;
bool equals(const Pose3& pose, double tol = 1e-9) const;
static Pose3 Identity() {
return Pose3();
}
Pose3 inverse() const;
Pose3 operator*(const Pose3& T) const {
return Pose3(R_ * T.R_, t_ + R_ * T.t_);
}
Pose3 interpolateRt(const Pose3& T, double t) const;
static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi = boost::none);
static Vector6 Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose = boost::none);
Matrix6 AdjointMap() const;
Vector6 Adjoint(const Vector6& xi_b,
OptionalJacobian<6, 6> H_this = boost::none,
OptionalJacobian<6, 6> H_xib = boost::none) const;
Vector6 AdjointTranspose(const Vector6& x,
OptionalJacobian<6, 6> H_this = boost::none,
OptionalJacobian<6, 6> H_x = boost::none) const;
static Matrix6 adjointMap(const Vector6& xi);
static Vector6 adjoint(const Vector6& xi, const Vector6& y,
OptionalJacobian<6, 6> Hxi = boost::none,
OptionalJacobian<6, 6> H_y = boost::none);
// temporary fix for wrappers until case issue is resolved
static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);}
static Vector6 adjoint_(const Vector6 &xi, const Vector6 &y) { return adjoint(xi, y);}
static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y,
OptionalJacobian<6, 6> Hxi = boost::none,
OptionalJacobian<6, 6> H_y = boost::none);
static Matrix6 ExpmapDerivative(const Vector6& xi);
static Matrix6 LogmapDerivative(const Pose3& xi);
// Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP
struct ChartAtOrigin {
static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = boost::none);
static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = boost::none);
};
static Matrix3 ComputeQforExpmapDerivative(
const Vector6& xi, double nearZeroThreshold = 1e-5);
using LieGroup<Pose3, 6>::inverse; // version with derivative
static Matrix wedge(double wx, double wy, double wz, double vx, double vy,
double vz) {
return (Matrix(4, 4) << 0., -wz, wy, vx, wz, 0., -wx, vy, -wy, wx, 0., vz, 0., 0., 0., 0.).finished();
}
Point3 transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself =
boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const;
Matrix transformFrom(const Matrix& points) const;
inline Point3 operator*(const Point3& point) const {
return transformFrom(point);
}
Point3 transformTo(const Point3& point, OptionalJacobian<3, 6> Hself =
boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const;
Matrix transformTo(const Matrix& points) const;
const Rot3& rotation(OptionalJacobian<3, 6> Hself = boost::none) const;
const Point3& translation(OptionalJacobian<3, 6> Hself = boost::none) const;
double x() const {
return t_.x();
}
double y() const {
return t_.y();
}
double z() const {
return t_.z();
}
Matrix4 matrix() const;
Pose3 transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself = boost::none,
OptionalJacobian<6, 6> HaTb = boost::none) const;
Pose3 transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself = boost::none,
OptionalJacobian<6, 6> HwTb = boost::none) const;
double range(const Point3& point, OptionalJacobian<1, 6> Hself = boost::none,
OptionalJacobian<1, 3> Hpoint = boost::none) const;
double range(const Pose3& pose, OptionalJacobian<1, 6> Hself = boost::none,
OptionalJacobian<1, 6> Hpose = boost::none) const;
Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> Hself = boost::none,
OptionalJacobian<2, 3> Hpoint = boost::none) const;
Unit3 bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself = boost::none,
OptionalJacobian<2, 6> Hpose = boost::none) const;
inline static std::pair<size_t, size_t> translationInterval() {
return std::make_pair(3, 5);
}
static std::pair<size_t, size_t> rotationInterval() {
return std::make_pair(0, 2);
}
Pose3 slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = boost::none,
OptionalJacobian<6, 6> Hy = boost::none) const;
GTSAM_EXPORT
friend std::ostream &operator<<(std::ostream &os, const Pose3& p);
private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(R_);
ar & BOOST_SERIALIZATION_NVP(t_);
}
#ifdef GTSAM_USE_QUATERNIONS
// Align if we are using Quaternions
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#endif
};
// Pose3 class
template<>
inline Matrix wedge<Pose3>(const Vector& xi) {
return Pose3::wedge(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5));
}
// Convenience typedef
using Pose3Pair = std::pair<Pose3, Pose3>;
using Pose3Pairs = std::vector<std::pair<Pose3, Pose3> >;
// For MATLAB wrapper
typedef std::vector<Pose3> Pose3Vector;
template <>
struct traits<Pose3> : public internal::LieGroup<Pose3> {};
template <>
struct traits<const Pose3> : public internal::LieGroup<Pose3> {};
// bearing and range traits, used in RangeFactor
template <>
struct Bearing<Pose3, Point3> : HasBearing<Pose3, Point3, Unit3> {};
template<>
struct Bearing<Pose3, Pose3> : HasBearing<Pose3, Pose3, Unit3> {};
template <typename T>
struct Range<Pose3, T> : HasRange<Pose3, T, double> {};
} // namespace gtsam
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
Lie.h
Base class and basic functions for Lie types.
Point3.h
3D Point
Rot3.h
3D rotation represented as a rotation matrix or quaternion
BearingRange.h
Bearing-Range product.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
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:36
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::wedge< Pose3 >
Matrix wedge< Pose3 >(const Vector &xi)
wedge for Pose3:
Definition Pose3.h:417
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::LieGroup
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition Lie.h:37
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition Lie.h:229
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
gtsam::Bearing
Definition BearingRange.h:34
gtsam::Range
Definition BearingRange.h:40
gtsam::HasBearing
Definition BearingRange.h:180
gtsam::HasRange
Definition BearingRange.h:194
gtsam::Pose2
A 2D pose (Point2,Rot2).
Definition Pose2.h:36
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3).
Definition Pose3.h:37
gtsam::Pose3::Identity
static Pose3 Identity()
identity for group operation
Definition Pose3.h:106
gtsam::Pose3::Pose3
Pose3(const Pose3 &pose)
Copy constructor.
Definition Pose3.h:58
gtsam::Pose3::Pose3
Pose3()
Default constructor is origin.
Definition Pose3.h:55
gtsam::Pose3::wedge
static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz)
wedge for Pose3:
Definition Pose3.h:233
gtsam::Pose3::Pose3
Pose3(const Rot3 &R, const Point3 &t)
Construct from R,t.
Definition Pose3.h:63
gtsam::Pose3::Pose3
Pose3(const Matrix &T)
Constructor from 4*4 matrix.
Definition Pose3.h:71
gtsam::Pose3::z
double z() const
get z
Definition Pose3.h:302
gtsam::Pose3::ComputeQforExpmapDerivative
static Matrix3 ComputeQforExpmapDerivative(const Vector6 &xi, double nearZeroThreshold=1e-5)
Compute the 3x3 bottom-left block Q of SE3 Expmap right derivative matrix J_r(xi) = [J_(w) Z_3x3; Q_r...
Definition Pose3.cpp:243
gtsam::Pose3::transformFrom
Point3 transformFrom(const Point3 &point, OptionalJacobian< 3, 6 > Hself=boost::none, OptionalJacobian< 3, 3 > Hpoint=boost::none) const
takes point in Pose coordinates and transforms it to world coordinates
Definition Pose3.cpp:347
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:375
gtsam::Pose3::operator*
Pose3 operator*(const Pose3 &T) const
compose syntactic sugar
Definition Pose3.h:114
gtsam::Pose3::Rotation
Rot3 Rotation
Pose Concept requirements.
Definition Pose3.h:41
gtsam::Pose3::y
double y() const
get y
Definition Pose3.h:297
gtsam::Pose3::inverse
Pose3 inverse() const
inverse transformation with derivatives
Definition Pose3.cpp:49
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:366
gtsam::Pose3::operator*
Point3 operator*(const Point3 &point) const
syntactic sugar for transformFrom
Definition Pose3.h:260
gtsam::Pose3::x
double x() const
get x
Definition Pose3.h:292
gtsam::Pose3::ChartAtOrigin
Definition Pose3.h:207
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43