Loading...
Searching...
No Matches
Pose2.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/geometry/BearingRange.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/base/Lie.h>
#include <gtsam/dllexport.h>
namespace gtsam {
class Pose2: public LieGroup<Pose2, 3> {
public:
typedef Rot2 Rotation;
typedef Point2 Translation;
private:
Rot2 r_;
Point2 t_;
public:
Pose2() :
r_(traits<Rot2>::Identity()), t_(traits<Point2>::Identity()) {
}
Pose2(const Pose2& pose) : r_(pose.r_), t_(pose.t_) {}
Pose2(double x, double y, double theta) :
r_(Rot2::fromAngle(theta)), t_(x, y) {
}
Pose2(double theta, const Point2& t) :
r_(Rot2::fromAngle(theta)), t_(t) {
}
Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {}
Pose2(const Matrix &T) :
r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) {
assert(T.rows() == 3 && T.cols() == 3);
}
Pose2(const Vector& v) : Pose2() {
*this = Expmap(v);
}
static boost::optional<Pose2> Align(const Point2Pairs& abPointPairs);
// Version of Pose2::Align that takes 2 matrices.
static boost::optional<Pose2> Align(const Matrix& a, const Matrix& b);
GTSAM_EXPORT void print(const std::string& s = "") const;
GTSAM_EXPORT bool equals(const Pose2& pose, double tol = 1e-9) const;
inline static Pose2 Identity() { return Pose2(); }
GTSAM_EXPORT Pose2 inverse() const;
inline Pose2 operator*(const Pose2& p2) const {
return Pose2(r_*p2.r(), t_ + r_*p2.t());
}
GTSAM_EXPORT static Pose2 Expmap(const Vector3& xi, ChartJacobian H = boost::none);
GTSAM_EXPORT static Vector3 Logmap(const Pose2& p, ChartJacobian H = boost::none);
GTSAM_EXPORT Matrix3 AdjointMap() const;
inline Vector3 Adjoint(const Vector3& xi) const {
return AdjointMap()*xi;
}
GTSAM_EXPORT static Matrix3 adjointMap(const Vector3& v);
static Vector3 adjoint(const Vector3& xi, const Vector3& y) {
return adjointMap(xi) * y;
}
static Vector3 adjointTranspose(const Vector3& xi, const Vector3& y) {
return adjointMap(xi).transpose() * y;
}
// temporary fix for wrappers until case issue is resolved
static Matrix3 adjointMap_(const Vector3 &xi) { return adjointMap(xi);}
static Vector3 adjoint_(const Vector3 &xi, const Vector3 &y) { return adjoint(xi, y);}
static inline Matrix3 wedge(double vx, double vy, double w) {
Matrix3 m;
m << 0.,-w, vx,
w, 0., vy,
0., 0., 0.;
return m;
}
GTSAM_EXPORT static Matrix3 ExpmapDerivative(const Vector3& v);
GTSAM_EXPORT static Matrix3 LogmapDerivative(const Pose2& v);
// Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP
struct ChartAtOrigin {
GTSAM_EXPORT static Pose2 Retract(const Vector3& v, ChartJacobian H = boost::none);
GTSAM_EXPORT static Vector3 Local(const Pose2& r, ChartJacobian H = boost::none);
};
using LieGroup<Pose2, 3>::inverse; // version with derivative
GTSAM_EXPORT Point2 transformTo(const Point2& point,
OptionalJacobian<2, 3> Dpose = boost::none,
OptionalJacobian<2, 2> Dpoint = boost::none) const;
Matrix transformTo(const Matrix& points) const;
GTSAM_EXPORT Point2 transformFrom(const Point2& point,
OptionalJacobian<2, 3> Dpose = boost::none,
OptionalJacobian<2, 2> Dpoint = boost::none) const;
Matrix transformFrom(const Matrix& points) const;
inline Point2 operator*(const Point2& point) const {
return transformFrom(point);
}
inline double x() const { return t_.x(); }
inline double y() const { return t_.y(); }
inline double theta() const { return r_.theta(); }
inline const Point2& t() const { return t_; }
inline const Rot2& r() const { return r_; }
inline const Point2& translation() const { return t_; }
inline const Rot2& rotation() const { return r_; }
GTSAM_EXPORT Matrix3 matrix() const;
GTSAM_EXPORT Rot2 bearing(const Point2& point,
OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 2> H2=boost::none) const;
GTSAM_EXPORT Rot2 bearing(const Pose2& pose,
OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 3> H2=boost::none) const;
GTSAM_EXPORT double range(const Point2& point,
OptionalJacobian<1, 3> H1=boost::none,
OptionalJacobian<1, 2> H2=boost::none) const;
GTSAM_EXPORT double range(const Pose2& point,
OptionalJacobian<1, 3> H1=boost::none,
OptionalJacobian<1, 3> H2=boost::none) const;
inline static std::pair<size_t, size_t> translationInterval() { return std::make_pair(0, 1); }
static std::pair<size_t, size_t> rotationInterval() { return std::make_pair(2, 2); }
GTSAM_EXPORT
friend std::ostream &operator<<(std::ostream &os, const Pose2& p);
private:
// Serialization function
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(t_);
ar & BOOST_SERIALIZATION_NVP(r_);
}
public:
// Align for Point2, which is either derived from, or is typedef, of Vector2
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
}; // Pose2
template <>
inline Matrix wedge<Pose2>(const Vector& xi) {
// NOTE(chris): Need eval() as workaround for Apple clang + avx2.
return Matrix(Pose2::wedge(xi(0),xi(1),xi(2))).eval();
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
GTSAM_EXPORT boost::optional<Pose2>
GTSAM_DEPRECATED align(const Point2Pairs& pairs);
#endif
// Convenience typedef
using Pose2Pair = std::pair<Pose2, Pose2>;
using Pose2Pairs = std::vector<Pose2Pair>;
template <>
struct traits<Pose2> : public internal::LieGroup<Pose2> {};
template <>
struct traits<const Pose2> : public internal::LieGroup<Pose2> {};
// bearing and range traits, used in RangeFactor
template <typename T>
struct Bearing<Pose2, T> : HasBearing<Pose2, T, Rot2> {};
template <typename T>
struct Range<Pose2, T> : HasRange<Pose2, 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.
Point2.h
2D Point
BearingRange.h
Bearing-Range product.
Rot2.h
2D rotation
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition serialization.h:113
gtsam::wedge< Pose2 >
Matrix wedge< Pose2 >(const Vector &xi)
specialization for pose2 wedge function (generic template in Lie.h)
Definition Pose2.h:341
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
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::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::Pose2::bearing
GTSAM_EXPORT Rot2 bearing(const Point2 &point, OptionalJacobian< 1, 3 > H1=boost::none, OptionalJacobian< 1, 2 > H2=boost::none) const
Calculate bearing to a landmark.
Definition Pose2.cpp:245
gtsam::Pose2::operator*
Pose2 operator*(const Pose2 &p2) const
compose syntactic sugar
Definition Pose2.h:128
gtsam::Pose2::print
GTSAM_EXPORT void print(const std::string &s="") const
print with optional string
Definition Pose2.cpp:50
gtsam::Pose2::rotation
const Rot2 & rotation() const
rotation
Definition Pose2.h:261
gtsam::Pose2::AdjointMap
GTSAM_EXPORT Matrix3 AdjointMap() const
Calculate Adjoint map Ad_pose is 3*3 matrix that when applied to twist xi , returns Ad_pose(xi).
Definition Pose2.cpp:126
gtsam::Pose2::LogmapDerivative
static GTSAM_EXPORT Matrix3 LogmapDerivative(const Pose2 &v)
Derivative of Logmap.
Definition Pose2.cpp:179
gtsam::Pose2::y
double y() const
get y
Definition Pose2.h:246
gtsam::Pose2::Logmap
static GTSAM_EXPORT Vector3 Logmap(const Pose2 &p, ChartJacobian H=boost::none)
Log map at identity - return the canonical coordinates of this rotation.
Definition Pose2.cpp:82
gtsam::Pose2::inverse
GTSAM_EXPORT Pose2 inverse() const
inverse
Definition Pose2.cpp:201
gtsam::Pose2::operator<<
GTSAM_EXPORT friend std::ostream & operator<<(std::ostream &os, const Pose2 &p)
Output stream operator.
Definition Pose2.cpp:55
gtsam::Pose2::operator*
Point2 operator*(const Point2 &point) const
syntactic sugar for transformFrom
Definition Pose2.h:234
gtsam::Pose2::Pose2
Pose2(double x, double y, double theta)
construct from (x,y,theta)
Definition Pose2.h:68
gtsam::Pose2::t
const Point2 & t() const
translation
Definition Pose2.h:252
gtsam::Pose2::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 Pose2.h:309
gtsam::Pose2::Pose2
Pose2(double theta, const Point2 &t)
construct from rotation and translation
Definition Pose2.h:73
gtsam::Pose2::x
double x() const
get x
Definition Pose2.h:243
gtsam::Pose2::r
const Rot2 & r() const
rotation
Definition Pose2.h:255
gtsam::Pose2::Pose2
Pose2(const Vector &v)
Construct from canonical coordinates (Lie algebra).
Definition Pose2.h:91
gtsam::Pose2::Adjoint
Vector3 Adjoint(const Vector3 &xi) const
Apply AdjointMap to twist xi.
Definition Pose2.h:149
gtsam::Pose2::wedge
static Matrix3 wedge(double vx, double vy, double w)
wedge for SE(2):
Definition Pose2.h:183
gtsam::Pose2::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 Pose2.h:316
gtsam::Pose2::adjointMap
static GTSAM_EXPORT Matrix3 adjointMap(const Vector3 &v)
Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19.
Definition Pose2.cpp:137
gtsam::Pose2::transformTo
GTSAM_EXPORT Point2 transformTo(const Point2 &point, OptionalJacobian< 2, 3 > Dpose=boost::none, OptionalJacobian< 2, 2 > Dpoint=boost::none) const
Return point coordinates in pose coordinate frame.
Definition Pose2.cpp:207
gtsam::Pose2::theta
double theta() const
get theta
Definition Pose2.h:249
gtsam::Pose2::ExpmapDerivative
static GTSAM_EXPORT Matrix3 ExpmapDerivative(const Vector3 &v)
Derivative of Expmap.
Definition Pose2.cpp:148
gtsam::Pose2::equals
GTSAM_EXPORT bool equals(const Pose2 &pose, double tol=1e-9) const
assert equality up to a tolerance
Definition Pose2.cpp:61
gtsam::Pose2::Rotation
Rot2 Rotation
Pose Concept requirements.
Definition Pose2.h:41
gtsam::Pose2::Pose2
Pose2()
default constructor = origin
Definition Pose2.h:55
gtsam::Pose2::range
GTSAM_EXPORT double range(const Point2 &point, OptionalJacobian< 1, 3 > H1=boost::none, OptionalJacobian< 1, 2 > H2=boost::none) const
Calculate range to a landmark.
Definition Pose2.cpp:270
gtsam::Pose2::transformFrom
GTSAM_EXPORT Point2 transformFrom(const Point2 &point, OptionalJacobian< 2, 3 > Dpose=boost::none, OptionalJacobian< 2, 2 > Dpoint=boost::none) const
Return point coordinates in global frame.
Definition Pose2.cpp:226
gtsam::Pose2::adjointTranspose
static Vector3 adjointTranspose(const Vector3 &xi, const Vector3 &y)
The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.
Definition Pose2.h:168
gtsam::Pose2::Expmap
static GTSAM_EXPORT Pose2 Expmap(const Vector3 &xi, ChartJacobian H=boost::none)
Exponential map at identity - create a rotation from canonical coordinates .
Definition Pose2.cpp:66
gtsam::Pose2::Identity
static Pose2 Identity()
identity for group operation
Definition Pose2.h:122
gtsam::Pose2::Align
static boost::optional< Pose2 > Align(const Point2Pairs &abPointPairs)
Create Pose2 by aligning two point pairs A pose aTb is estimated between pairs (a_point,...
Definition Pose2.cpp:330
gtsam::Pose2::Pose2
Pose2(const Rot2 &r, const Point2 &t)
construct from r,t
Definition Pose2.h:78
gtsam::Pose2::adjoint
static Vector3 adjoint(const Vector3 &xi, const Vector3 &y)
Action of the adjointMap on a Lie-algebra vector y, with optional derivatives.
Definition Pose2.h:161
gtsam::Pose2::Pose2
Pose2(const Pose2 &pose)
copy constructor
Definition Pose2.h:60
gtsam::Pose2::translation
const Point2 & translation() const
translation
Definition Pose2.h:258
gtsam::Pose2::Pose2
Pose2(const Matrix &T)
Constructor from 3*3 matrix.
Definition Pose2.h:81
gtsam::Pose2::ChartAtOrigin
Definition Pose2.h:198
gtsam::Rot2
Rotation matrix NOTE: the angle theta is in radians unless explicitly stated.
Definition Rot2.h:36