gtsam 4.2
gtsam
Loading...
Searching...
No Matches
gtsam::Similarity2 Class Reference

Detailed Description

2D similarity transform

Inheritance diagram for gtsam::Similarity2:

Testable

bool equals (const Similarity2 &sim, double tol) const
 Compare with tolerance.
bool operator== (const Similarity2 &other) const
 Exact equality.
void print (const std::string &s) const
 Print with optional string.
std::ostream & operator<< (std::ostream &os, const Similarity2 &p)

Group

Similarity2 operator* (const Similarity2 &S) const
 Composition.
Similarity2 inverse () const
 Return the inverse.
static Similarity2 Identity ()
 Return an identity transform.

Group action on Point2

Point2 transformFrom (const Point2 &p) const
 Action on a point p is s*(R*p+t).
Pose2 transformFrom (const Pose2 &T) const
 Action on a pose T.
Point2 operator* (const Point2 &p) const
static Similarity2 Align (const Point2Pairs &abPointPairs)
 Create Similarity2 by aligning at least two point pairs.
static Similarity2 Align (const Pose2Pairs &abPosePairs)
 Create the Similarity2 object that aligns at least two pose pairs.

Lie Group

Matrix4 AdjointMap () const
 Project from one tangent space to another.
static Vector4 Logmap (const Similarity2 &S, OptionalJacobian< 4, 4 > Hm=boost::none)
 Log map at the identity \( [t_x, t_y, \delta, \lambda] \).
static Similarity2 Expmap (const Vector4 &v, OptionalJacobian< 4, 4 > Hm=boost::none)
 Exponential map at the identity.

Standard interface

Matrix3 matrix () const
 Calculate 4*4 matrix group equivalent.
Rot2 rotation () const
 Return a GTSAM rotation.
Point2 translation () const
 Return a GTSAM translation.
double scale () const
 Return the scale.
size_t dim () const
 Dimensionality of tangent space = 4 DOF.
static size_t Dim ()
 Dimensionality of tangent space = 4 DOF - used to autodetect sizes.

Public Member Functions

Constructors
 Similarity2 ()
 Default constructor.
 Similarity2 (double s)
 Construct pure scaling.
 Similarity2 (const Rot2 &R, const Point2 &t, double s)
 Construct from GTSAM types.
 Similarity2 (const Matrix2 &R, const Vector2 &t, double s)
 Construct from Eigen types.
 Similarity2 (const Matrix3 &T)
 Construct from matrix [R t; 0 s^-1].
Public Member Functions inherited from gtsam::LieGroup< Similarity2, 4 >
SOn compose (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const
SOn between (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const
GTSAM_EXPORT SOn compose (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const
GTSAM_EXPORT SOn between (const SOn &g, DynamicJacobian H1, DynamicJacobian H2) const
const Similarity2derived () const
Similarity2 inverse (ChartJacobian H) const
Similarity2 expmap (const TangentVector &v) const
 expmap as required by manifold concept Applies exponential map to v and composes with *this
TangentVector logmap (const Similarity2 &g) const
 logmap as required by manifold concept Applies logarithmic map to group element that takes *this to g
Similarity2 retract (const TangentVector &v) const
 retract as required by manifold concept: applies v at *this
TangentVector localCoordinates (const Similarity2 &g) const
 localCoordinates as required by manifold concept: finds tangent vector between *this and g

Classes

struct  ChartAtOrigin
 Chart at the origin. More...

Additional Inherited Members

Static Public Member Functions inherited from gtsam::LieGroup< Similarity2, 4 >
static Similarity2 Retract (const TangentVector &v)
 Retract at origin: possible in Lie group because it has an identity.
static TangentVector LocalCoordinates (const Similarity2 &g)
 LocalCoordinates at origin: possible in Lie group because it has an identity.
Public Types inherited from gtsam::LieGroup< Similarity2, 4 >
enum  
typedef OptionalJacobian< N, N > ChartJacobian
typedef Eigen::Matrix< double, N, N > Jacobian
typedef Eigen::Matrix< double, N, 1 > TangentVector

Member Function Documentation

◆ Align()

Similarity2 gtsam::Similarity2::Align ( const Pose2Pairs & abPosePairs)
static

Create the Similarity2 object that aligns at least two pose pairs.

Each pair is of the form (aTi, bTi). Given a list of pairs in frame a, and a list of pairs in frame b, Align() will compute the best-fit Similarity2 aSb transformation to align them. First, the rotation aRb will be computed as the average (Karcher mean) of many estimates aRb (from each pair). Afterwards, the scale factor will be computed using the algorithm described here: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf

◆ transformFrom()

Pose2 gtsam::Similarity2::transformFrom ( const Pose2 & T) const

Action on a pose T.

|Rs ts| |R t| |Rs*R Rs*t+ts| |0 1/s| * |0 1| = | 0 1/s |, the result is still a Sim2 object. To retrieve a Pose2, we normalized the scale value into 1. |Rs*R Rs*t+ts| |Rs*R s(Rs*t+ts)| | 0 1/s | = | 0 1 |

This group action satisfies the compatibility condition. For more details, refer to: https://en.wikipedia.org/wiki/Group_action


The documentation for this class was generated from the following files: