22#include <gtsam/dllexport.h>
39 typedef Point2 Translation;
61 Similarity2(
const Matrix2& R,
const Vector2& t,
double s);
77 void print(
const std::string& s)
const;
79 friend std::ostream& operator<<(std::ostream& os,
const Similarity2& p);
120 static Similarity2 Align(
const Point2Pairs& abPointPairs);
135 static Similarity2 Align(
const Pose2Pairs& abPosePairs);
155 ChartJacobian H = boost::none) {
156 return Similarity2::Expmap(v, H);
159 ChartJacobian H = boost::none) {
160 return Similarity2::Logmap(other, H);
165 Matrix4 AdjointMap()
const;
174 Matrix3 matrix()
const;
186 inline static size_t Dim() {
return 4; }
189 inline size_t dim()
const {
return 4; }
Base class and basic functions for Lie types.
Base class and basic functions for Manifold types.
Global functions in a separate testing namespace.
Definition chartTesting.h:28
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition Point2.h:47
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
bool operator==(const Matrix &A, const Matrix &B)
equality is just equal_with_abs_tol 1e-9
Definition Matrix.h:100
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
A 2D pose (Point2,Rot2)
Definition Pose2.h:36
Rotation matrix NOTE: the angle theta is in radians unless explicitly stated.
Definition Rot2.h:36
2D similarity transform
Definition Similarity2.h:35
Point2 translation() const
Return a GTSAM translation.
Definition Similarity2.h:180
double scale() const
Return the scale.
Definition Similarity2.h:183
static size_t Dim()
Dimensionality of tangent space = 4 DOF - used to autodetect sizes.
Definition Similarity2.h:186
Rot2 rotation() const
Return a GTSAM rotation.
Definition Similarity2.h:177
size_t dim() const
Dimensionality of tangent space = 4 DOF.
Definition Similarity2.h:189
Chart at the origin.
Definition Similarity2.h:153