27#include <gtsam/dllexport.h>
42 typedef Point2 Translation;
82 r_(
Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) {
83 assert(T.rows() == 3 && T.cols() == 3);
102 static boost::optional<Pose2>
Align(
const Point2Pairs& abPointPairs);
105 static boost::optional<Pose2>
Align(
const Matrix& a,
const Matrix& b);
112 GTSAM_EXPORT
void print(
const std::string& s =
"")
const;
115 GTSAM_EXPORT
bool equals(
const Pose2& pose,
double tol = 1e-9)
const;
129 return Pose2(r_*p2.
r(), t_ + r_*p2.
t());
137 GTSAM_EXPORT
static Pose2 Expmap(
const Vector3& xi, ChartJacobian H = boost::none);
140 GTSAM_EXPORT
static Vector3
Logmap(
const Pose2& p, ChartJacobian H = boost::none);
149 inline Vector3
Adjoint(
const Vector3& xi)
const {
156 GTSAM_EXPORT
static Matrix3
adjointMap(
const Vector3& v);
161 static Vector3
adjoint(
const Vector3& xi,
const Vector3&
y) {
173 static Matrix3 adjointMap_(
const Vector3 &xi) {
return adjointMap(xi);}
174 static Vector3 adjoint_(
const Vector3 &xi,
const Vector3 &
y) {
return adjoint(xi,
y);}
183 static inline Matrix3
wedge(
double vx,
double vy,
double w) {
199 GTSAM_EXPORT
static Pose2 Retract(
const Vector3& v, ChartJacobian H = boost::none);
200 GTSAM_EXPORT
static Vector3 Local(
const Pose2&
r, ChartJacobian H = boost::none);
243 inline double x()
const {
return t_.x(); }
246 inline double y()
const {
return t_.y(); }
255 inline const Rot2&
r()
const {
return r_; }
264 GTSAM_EXPORT Matrix3 matrix()
const;
287 GTSAM_EXPORT
double range(
const Point2& point,
296 GTSAM_EXPORT
double range(
const Pose2& point,
327 friend class boost::serialization::access;
328 template<
class Archive>
329 void serialize(Archive & ar,
const unsigned int ) {
330 ar & BOOST_SERIALIZATION_NVP(t_);
331 ar & BOOST_SERIALIZATION_NVP(r_);
346#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
352GTSAM_EXPORT boost::optional<Pose2>
353GTSAM_DEPRECATED align(
const Point2Pairs& pairs);
357using Pose2Pair = std::pair<Pose2, Pose2>;
358using Pose2Pairs = std::vector<Pose2Pair>;
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
Base class and basic functions for Lie types.
Global functions in a separate testing namespace.
Definition chartTesting.h:28
Matrix wedge< Pose2 >(const Vector &xi)
specialization for pose2 wedge function (generic template in Lie.h)
Definition Pose2.h:341
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
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
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
Pose2 operator*(const Pose2 &p2) const
compose syntactic sugar
Definition Pose2.h:128
GTSAM_EXPORT void print(const std::string &s="") const
print with optional string
Definition Pose2.cpp:50
const Rot2 & rotation() const
rotation
Definition Pose2.h:261
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
static GTSAM_EXPORT Matrix3 LogmapDerivative(const Pose2 &v)
Derivative of Logmap.
Definition Pose2.cpp:179
double y() const
get y
Definition Pose2.h:246
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_EXPORT Pose2 inverse() const
inverse
Definition Pose2.cpp:201
GTSAM_EXPORT friend std::ostream & operator<<(std::ostream &os, const Pose2 &p)
Output stream operator.
Definition Pose2.cpp:55
Point2 operator*(const Point2 &point) const
syntactic sugar for transformFrom
Definition Pose2.h:234
Pose2(double x, double y, double theta)
construct from (x,y,theta)
Definition Pose2.h:68
const Point2 & t() const
translation
Definition Pose2.h:252
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
Pose2(double theta, const Point2 &t)
construct from rotation and translation
Definition Pose2.h:73
double x() const
get x
Definition Pose2.h:243
const Rot2 & r() const
rotation
Definition Pose2.h:255
Pose2(const Vector &v)
Construct from canonical coordinates (Lie algebra)
Definition Pose2.h:91
Vector3 Adjoint(const Vector3 &xi) const
Apply AdjointMap to twist xi.
Definition Pose2.h:149
static Matrix3 wedge(double vx, double vy, double w)
wedge for SE(2):
Definition Pose2.h:183
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
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_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
double theta() const
get theta
Definition Pose2.h:249
static GTSAM_EXPORT Matrix3 ExpmapDerivative(const Vector3 &v)
Derivative of Expmap.
Definition Pose2.cpp:148
Rot2 Rotation
Pose Concept requirements.
Definition Pose2.h:41
Pose2()
default constructor = origin
Definition Pose2.h:55
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
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
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
static Pose2 Identity()
identity for group operation
Definition Pose2.h:122
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
Pose2(const Rot2 &r, const Point2 &t)
construct from r,t
Definition Pose2.h:78
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
Pose2(const Pose2 &pose)
copy constructor
Definition Pose2.h:60
const Point2 & translation() const
translation
Definition Pose2.h:258
Pose2(const Matrix &T)
Constructor from 3*3 matrix.
Definition Pose2.h:81
Rotation matrix NOTE: the angle theta is in radians unless explicitly stated.
Definition Rot2.h:36
double theta() const
return angle (RADIANS)
Definition Rot2.h:187