#include <Pose2.h>
Classes | |
struct | ChartAtOrigin |
Public Types | |
typedef Rot2 | Rotation |
typedef Point2 | Translation |
Public Types inherited from gtsam::LieGroup< Pose2, 3 > | |
enum | |
typedef OptionalJacobian< N, N > | ChartJacobian |
typedef Eigen::Matrix< double, N, N > | Jacobian |
typedef Eigen::Matrix< double, N, 1 > | TangentVector |
Private Attributes | |
Rot2 | r_ |
Point2 | t_ |
Standard Constructors | |
Pose2 () | |
Pose2 (const Pose2 &pose) | |
Pose2 (double x, double y, double theta) | |
Pose2 (double theta, const Point2 &t) | |
Pose2 (const Rot2 &r, const Point2 &t) | |
Pose2 (const Matrix &T) | |
Advanced Constructors | |
Pose2 (const Vector &v) | |
static std::optional< Pose2 > | Align (const Point2Pairs &abPointPairs) |
static std::optional< Pose2 > | Align (const Matrix &a, const Matrix &b) |
Testable | |
void | print (const std::string &s="") const |
bool | equals (const Pose2 &pose, double tol=1e-9) const |
Group | |
Pose2 | inverse () const |
inverse More... | |
Pose2 | operator* (const Pose2 &p2) const |
compose syntactic sugar More... | |
static Pose2 | Identity () |
identity for group operation More... | |
Lie Group | |
Matrix3 | AdjointMap () const |
Vector3 | Adjoint (const Vector3 &xi) const |
Apply AdjointMap to twist xi. More... | |
static Pose2 | Expmap (const Vector3 &xi, ChartJacobian H={}) |
Exponential map at identity - create a rotation from canonical coordinates . More... | |
static Vector3 | Logmap (const Pose2 &p, ChartJacobian H={}) |
Log map at identity - return the canonical coordinates of this rotation. More... | |
static Matrix3 | adjointMap (const Vector3 &v) |
static Vector3 | adjoint (const Vector3 &xi, const Vector3 &y) |
static Vector3 | adjointTranspose (const Vector3 &xi, const Vector3 &y) |
static Matrix3 | adjointMap_ (const Vector3 &xi) |
static Vector3 | adjoint_ (const Vector3 &xi, const Vector3 &y) |
static Matrix3 | wedge (double vx, double vy, double w) |
static Matrix3 | ExpmapDerivative (const Vector3 &v) |
Derivative of Expmap. More... | |
static Matrix3 | LogmapDerivative (const Pose2 &v) |
Derivative of Logmap. More... | |
Group Action on Point2 | |
Point2 | transformTo (const Point2 &point, OptionalJacobian< 2, 3 > Dpose={}, OptionalJacobian< 2, 2 > Dpoint={}) const |
Matrix | transformTo (const Matrix &points) const |
transform many points in world coordinates and transform to Pose. More... | |
Point2 | transformFrom (const Point2 &point, OptionalJacobian< 2, 3 > Dpose={}, OptionalJacobian< 2, 2 > Dpoint={}) const |
Matrix | transformFrom (const Matrix &points) const |
transform many points in Pose coordinates and transform to world. More... | |
Point2 | operator* (const Point2 &point) const |
Standard Interface | |
double | x () const |
get x More... | |
double | y () const |
get y More... | |
double | theta () const |
get theta More... | |
const Point2 & | t () const |
translation More... | |
const Rot2 & | r () const |
rotation More... | |
const Point2 & | translation (OptionalJacobian< 2, 3 > Hself={}) const |
translation More... | |
const Rot2 & | rotation (OptionalJacobian< 1, 3 > Hself={}) const |
rotation More... | |
Matrix3 | matrix () const |
Rot2 | bearing (const Point2 &point, OptionalJacobian< 1, 3 > H1={}, OptionalJacobian< 1, 2 > H2={}) const |
Rot2 | bearing (const Pose2 &pose, OptionalJacobian< 1, 3 > H1={}, OptionalJacobian< 1, 3 > H2={}) const |
double | range (const Point2 &point, OptionalJacobian< 1, 3 > H1={}, OptionalJacobian< 1, 2 > H2={}) const |
double | range (const Pose2 &point, OptionalJacobian< 1, 3 > H1={}, OptionalJacobian< 1, 3 > H2={}) const |
Advanced Interface | |
static std::pair< size_t, size_t > | translationInterval () |
static std::pair< size_t, size_t > | rotationInterval () |
GTSAM_EXPORT friend std::ostream & | operator<< (std::ostream &os, const Pose2 &p) |
Output stream operator. More... | |
Additional Inherited Members | |
Public Member Functions inherited from gtsam::LieGroup< Pose2, 3 > | |
Pose2 | between (const Pose2 &g) const |
Pose2 | between (const Pose2 &g, ChartJacobian H1, ChartJacobian H2={}) const |
SOn | between (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const |
GTSAM_EXPORT SOn | between (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const |
Pose2 | compose (const Pose2 &g) const |
Pose2 | compose (const Pose2 &g, ChartJacobian H1, ChartJacobian H2={}) const |
SOn | compose (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const |
GTSAM_EXPORT SOn | compose (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const |
const Pose2 & | derived () const |
Pose2 | expmap (const TangentVector &v) const |
Pose2 | expmap (const TangentVector &v, ChartJacobian H1, ChartJacobian H2={}) const |
expmap with optional derivatives More... | |
Pose2 | inverse (ChartJacobian H) const |
TangentVector | localCoordinates (const Pose2 &g) const |
localCoordinates as required by manifold concept: finds tangent vector between *this and g More... | |
TangentVector | localCoordinates (const Pose2 &g, ChartJacobian H1, ChartJacobian H2={}) const |
localCoordinates with optional derivatives More... | |
TangentVector | logmap (const Pose2 &g) const |
TangentVector | logmap (const Pose2 &g, ChartJacobian H1, ChartJacobian H2={}) const |
logmap with optional derivatives More... | |
Pose2 | retract (const TangentVector &v) const |
retract as required by manifold concept: applies v at *this More... | |
Pose2 | retract (const TangentVector &v, ChartJacobian H1, ChartJacobian H2={}) const |
retract with optional derivatives More... | |
Static Public Member Functions inherited from gtsam::LieGroup< Pose2, 3 > | |
static TangentVector | LocalCoordinates (const Pose2 &g) |
LocalCoordinates at origin: possible in Lie group because it has an identity. More... | |
static TangentVector | LocalCoordinates (const Pose2 &g, ChartJacobian H) |
LocalCoordinates at origin with optional derivative. More... | |
static Pose2 | Retract (const TangentVector &v) |
Retract at origin: possible in Lie group because it has an identity. More... | |
static Pose2 | Retract (const TangentVector &v, ChartJacobian H) |
Retract at origin with optional derivative. More... | |
typedef Rot2 gtsam::Pose2::Rotation |
typedef Point2 gtsam::Pose2::Translation |
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
Matrix3 gtsam::Pose2::AdjointMap | ( | ) | const |
|
static |
|
inlinestatic |
|
static |
Rot2 gtsam::Pose2::bearing | ( | const Point2 & | point, |
OptionalJacobian< 1, 3 > | H1 = {} , |
||
OptionalJacobian< 1, 2 > | H2 = {} |
||
) | const |
Rot2 gtsam::Pose2::bearing | ( | const Pose2 & | pose, |
OptionalJacobian< 1, 3 > | H1 = {} , |
||
OptionalJacobian< 1, 3 > | H2 = {} |
||
) | const |
|
static |
|
static |
|
inlinestatic |
|
static |
|
static |
void gtsam::Pose2::print | ( | const std::string & | s = "" | ) | const |
double gtsam::Pose2::range | ( | const Point2 & | point, |
OptionalJacobian< 1, 3 > | H1 = {} , |
||
OptionalJacobian< 1, 2 > | H2 = {} |
||
) | const |
double gtsam::Pose2::range | ( | const Pose2 & | point, |
OptionalJacobian< 1, 3 > | H1 = {} , |
||
OptionalJacobian< 1, 3 > | H2 = {} |
||
) | const |
|
inline |
Point2 gtsam::Pose2::transformFrom | ( | const Point2 & | point, |
OptionalJacobian< 2, 3 > | Dpose = {} , |
||
OptionalJacobian< 2, 2 > | Dpoint = {} |
||
) | const |
Point2 gtsam::Pose2::transformTo | ( | const Point2 & | point, |
OptionalJacobian< 2, 3 > | Dpose = {} , |
||
OptionalJacobian< 2, 2 > | Dpoint = {} |
||
) | const |
|
inline |
|
inlinestatic |
|
friend |