#include <Similarity2.h>
Classes | |
struct | ChartAtOrigin |
Chart at the origin. More... | |
Public Member Functions | |
Constructors | |
Similarity2 () | |
Default constructor. More... | |
Similarity2 (double s) | |
Construct pure scaling. More... | |
Similarity2 (const Rot2 &R, const Point2 &t, double s) | |
Construct from GTSAM types. More... | |
Similarity2 (const Matrix2 &R, const Vector2 &t, double s) | |
Construct from Eigen types. More... | |
Similarity2 (const Matrix3 &T) | |
Construct from matrix [R t; 0 s^-1]. More... | |
Public Member Functions inherited from gtsam::LieGroup< Similarity2, 4 > | |
Similarity2 | between (const Similarity2 &g) const |
Similarity2 | between (const Similarity2 &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 |
Similarity2 | compose (const Similarity2 &g) const |
Similarity2 | compose (const Similarity2 &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 Similarity2 & | derived () const |
Similarity2 | expmap (const TangentVector &v) const |
Similarity2 | expmap (const TangentVector &v, ChartJacobian H1, ChartJacobian H2={}) const |
expmap with optional derivatives More... | |
Similarity2 | inverse (ChartJacobian H) const |
TangentVector | localCoordinates (const Similarity2 &g) const |
localCoordinates as required by manifold concept: finds tangent vector between *this and g More... | |
TangentVector | localCoordinates (const Similarity2 &g, ChartJacobian H1, ChartJacobian H2={}) const |
localCoordinates with optional derivatives More... | |
TangentVector | logmap (const Similarity2 &g) const |
TangentVector | logmap (const Similarity2 &g, ChartJacobian H1, ChartJacobian H2={}) const |
logmap with optional derivatives More... | |
Similarity2 | retract (const TangentVector &v) const |
retract as required by manifold concept: applies v at *this More... | |
Similarity2 | retract (const TangentVector &v, ChartJacobian H1, ChartJacobian H2={}) const |
retract with optional derivatives More... | |
Private Types | |
Pose Concept | |
typedef Rot2 | Rotation |
typedef Point2 | Translation |
Private Attributes | |
Rot2 | R_ |
double | s_ |
Point2 | t_ |
Testable | |
bool | equals (const Similarity2 &sim, double tol) const |
Compare with tolerance. More... | |
bool | operator== (const Similarity2 &other) const |
Exact equality. More... | |
void | print (const std::string &s="") const |
Print with optional string. More... | |
GTSAM_EXPORT friend std::ostream & | operator<< (std::ostream &os, const Similarity2 &p) |
Group | |
Similarity2 | operator* (const Similarity2 &S) const |
Composition. More... | |
Similarity2 | inverse () const |
Return the inverse. More... | |
static Similarity2 | Identity () |
Return an identity transform. More... | |
Group action on Point2 | |
Point2 | transformFrom (const Point2 &p) const |
Action on a point p is s*(R*p+t) More... | |
Pose2 | transformFrom (const Pose2 &T) const |
Point2 | operator* (const Point2 &p) const |
static Similarity2 | Align (const Point2Pairs &abPointPairs) |
static Similarity2 | Align (const Pose2Pairs &abPosePairs) |
Lie Group | |
Matrix4 | AdjointMap () const |
Project from one tangent space to another. More... | |
static Vector4 | Logmap (const Similarity2 &S, OptionalJacobian< 4, 4 > Hm={}) |
static Similarity2 | Expmap (const Vector4 &v, OptionalJacobian< 4, 4 > Hm={}) |
Exponential map at the identity. More... | |
Standard interface | |
Matrix3 | matrix () const |
Calculate 4*4 matrix group equivalent. More... | |
Rot2 | rotation () const |
Return a GTSAM rotation. More... | |
Point2 | translation () const |
Return a GTSAM translation. More... | |
double | scale () const |
Return the scale. More... | |
size_t | dim () const |
Dimensionality of tangent space = 4 DOF. More... | |
static size_t | Dim () |
Dimensionality of tangent space = 4 DOF - used to autodetect sizes. More... | |
Additional Inherited Members | |
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 |
Static Public Member Functions inherited from gtsam::LieGroup< Similarity2, 4 > | |
static TangentVector | LocalCoordinates (const Similarity2 &g) |
LocalCoordinates at origin: possible in Lie group because it has an identity. More... | |
static TangentVector | LocalCoordinates (const Similarity2 &g, ChartJacobian H) |
LocalCoordinates at origin with optional derivative. More... | |
static Similarity2 | Retract (const TangentVector &v) |
Retract at origin: possible in Lie group because it has an identity. More... | |
static Similarity2 | Retract (const TangentVector &v, ChartJacobian H) |
Retract at origin with optional derivative. More... | |
2D similarity transform
Definition at line 35 of file Similarity2.h.
|
private |
Definition at line 38 of file Similarity2.h.
|
private |
Definition at line 39 of file Similarity2.h.
gtsam::Similarity2::Similarity2 | ( | ) |
Default constructor.
Definition at line 102 of file Similarity2.cpp.
gtsam::Similarity2::Similarity2 | ( | double | s | ) |
Construct pure scaling.
Definition at line 104 of file Similarity2.cpp.
Construct from GTSAM types.
Definition at line 106 of file Similarity2.cpp.
gtsam::Similarity2::Similarity2 | ( | const Matrix2 & | R, |
const Vector2 & | t, | ||
double | s | ||
) |
Construct from Eigen types.
Definition at line 109 of file Similarity2.cpp.
gtsam::Similarity2::Similarity2 | ( | const Matrix3 & | T | ) |
Construct from matrix [R t; 0 s^-1].
Definition at line 112 of file Similarity2.cpp.
Matrix4 gtsam::Similarity2::AdjointMap | ( | ) | const |
Project from one tangent space to another.
Definition at line 221 of file Similarity2.cpp.
|
static |
Create Similarity2 by aligning at least two point pairs
Definition at line 162 of file Similarity2.cpp.
|
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
Definition at line 175 of file Similarity2.cpp.
|
inlinestatic |
Dimensionality of tangent space = 4 DOF - used to autodetect sizes.
Definition at line 187 of file Similarity2.h.
|
inline |
Dimensionality of tangent space = 4 DOF.
Definition at line 190 of file Similarity2.h.
bool gtsam::Similarity2::equals | ( | const Similarity2 & | sim, |
double | tol | ||
) | const |
Compare with tolerance.
Definition at line 117 of file Similarity2.cpp.
|
static |
Exponential map at the identity.
Definition at line 210 of file Similarity2.cpp.
|
static |
Return an identity transform.
Definition at line 135 of file Similarity2.cpp.
Similarity2 gtsam::Similarity2::inverse | ( | ) | const |
Return the inverse.
Definition at line 141 of file Similarity2.cpp.
|
static |
Log map at the identity
Definition at line 197 of file Similarity2.cpp.
Matrix3 gtsam::Similarity2::matrix | ( | ) | const |
Calculate 4*4 matrix group equivalent.
Definition at line 231 of file Similarity2.cpp.
Definition at line 158 of file Similarity2.cpp.
Similarity2 gtsam::Similarity2::operator* | ( | const Similarity2 & | S | ) | const |
Composition.
Definition at line 137 of file Similarity2.cpp.
bool gtsam::Similarity2::operator== | ( | const Similarity2 & | other | ) | const |
Exact equality.
Definition at line 123 of file Similarity2.cpp.
void gtsam::Similarity2::print | ( | const std::string & | s = "" | ) | const |
Print with optional string.
Definition at line 127 of file Similarity2.cpp.
|
inline |
Return a GTSAM rotation.
Definition at line 178 of file Similarity2.h.
|
inline |
Return the scale.
Definition at line 184 of file Similarity2.h.
Action on a point p is s*(R*p+t)
Definition at line 147 of file Similarity2.cpp.
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
Definition at line 152 of file Similarity2.cpp.
|
inline |
Return a GTSAM translation.
Definition at line 181 of file Similarity2.h.
|
friend |
Definition at line 225 of file Similarity2.cpp.
|
private |
Definition at line 43 of file Similarity2.h.
|
private |
Definition at line 45 of file Similarity2.h.
|
private |
Definition at line 44 of file Similarity2.h.