23#include <gtsam/dllexport.h>
40 typedef Point3 Translation;
62 Similarity3(
const Matrix3& R,
const Vector3& t,
double s);
78 void print(
const std::string& s)
const;
80 friend std::ostream& operator<<(std::ostream& os,
const Similarity3& p);
123 static Similarity3 Align(
const Point3Pairs& abPointPairs);
135 static Similarity3 Align(
const std::vector<Pose3Pair>& abPosePairs);
172 static Matrix4
wedge(
const Vector7& xi);
175 Matrix7 AdjointMap()
const;
182 Matrix4 matrix()
const;
194 inline static size_t Dim() {
return 7; }
197 inline size_t dim()
const {
return 7; }
205 static Matrix3 GetV(Vector3 w,
double lambda);
211inline Matrix wedge<Similarity3>(
const Vector& xi) {
Base class and basic functions for Lie types.
Base class and basic functions for Manifold types.
3D rotation represented as a rotation matrix or quaternion
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
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition: Point3.h:36
bool operator==(const Matrix &A, const Matrix &B)
equality is just equal_with_abs_tol 1e-9
Definition: Matrix.h:100
Matrix wedge(const Vector &x)
Declaration of wedge (see Murray94book) used to convert from n exponential coordinates to n*n element...
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 3D pose (R,t) : (Rot3,Point3)
Definition: Pose3.h:37
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
3D similarity transform
Definition: Similarity3.h:36
static Vector7 Logmap(const Similarity3 &s, OptionalJacobian< 7, 7 > Hm=boost::none)
Log map at the identity .
Definition: Similarity3.cpp:256
static size_t Dim()
Dimensionality of tangent space = 7 DOF - used to autodetect sizes.
Definition: Similarity3.h:194
static Similarity3 Expmap(const Vector7 &v, OptionalJacobian< 7, 7 > Hm=boost::none)
Exponential map at the identity.
Definition: Similarity3.cpp:269
Point3 translation() const
Return a GTSAM translation.
Definition: Similarity3.h:188
Rot3 rotation() const
Return a GTSAM rotation.
Definition: Similarity3.h:185
size_t dim() const
Dimensionality of tangent space = 7 DOF.
Definition: Similarity3.h:197
static Matrix4 wedge(const Vector7 &xi)
wedge for Similarity3:
Definition: Similarity3.cpp:198
double scale() const
Return the scale.
Definition: Similarity3.h:191
Chart at the origin.
Definition: Similarity3.h:153