gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
Similarity3.h
Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
19#pragma once
20
21#include <gtsam/base/Lie.h>
22#include <gtsam/base/Manifold.h>
23#include <gtsam/dllexport.h>
26#include <gtsam/geometry/Rot3.h>
27
28namespace gtsam {
29
30// Forward declarations
31class Pose3;
32
36class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
39 typedef Rot3 Rotation;
40 typedef Point3 Translation;
42
43 private:
44 Rot3 R_;
45 Point3 t_;
46 double s_;
47
48 public:
51
54
56 Similarity3(double s);
57
59 Similarity3(const Rot3& R, const Point3& t, double s);
60
62 Similarity3(const Matrix3& R, const Vector3& t, double s);
63
65 Similarity3(const Matrix4& T);
66
70
72 bool equals(const Similarity3& sim, double tol) const;
73
75 bool operator==(const Similarity3& other) const;
76
78 void print(const std::string& s) const;
79
80 friend std::ostream& operator<<(std::ostream& os, const Similarity3& p);
81
85
87 static Similarity3 Identity();
88
90 Similarity3 operator*(const Similarity3& S) const;
91
93 Similarity3 inverse() const;
94
98
100 Point3 transformFrom(const Point3& p, //
101 OptionalJacobian<3, 7> H1 = boost::none, //
102 OptionalJacobian<3, 3> H2 = boost::none) const;
103
115 Pose3 transformFrom(const Pose3& T) const;
116
118 Point3 operator*(const Point3& p) const;
119
123 static Similarity3 Align(const Point3Pairs& abPointPairs);
124
135 static Similarity3 Align(const std::vector<Pose3Pair>& abPosePairs);
136
140
144 static Vector7 Logmap(const Similarity3& s, //
145 OptionalJacobian<7, 7> Hm = boost::none);
146
149 static Similarity3 Expmap(const Vector7& v, //
150 OptionalJacobian<7, 7> Hm = boost::none);
151
154 static Similarity3 Retract(const Vector7& v,
155 ChartJacobian H = boost::none) {
156 return Similarity3::Expmap(v, H);
157 }
158 static Vector7 Local(const Similarity3& other,
159 ChartJacobian H = boost::none) {
160 return Similarity3::Logmap(other, H);
161 }
162 };
163
164 using LieGroup<Similarity3, 7>::inverse;
165
172 static Matrix4 wedge(const Vector7& xi);
173
175 Matrix7 AdjointMap() const;
176
180
182 Matrix4 matrix() const;
183
185 Rot3 rotation() const { return R_; }
186
188 Point3 translation() const { return t_; }
189
191 double scale() const { return s_; }
192
194 inline static size_t Dim() { return 7; }
195
197 inline size_t dim() const { return 7; }
198
202
203 private:
205 static Matrix3 GetV(Vector3 w, double lambda);
206
208};
209
210template <>
211inline Matrix wedge<Similarity3>(const Vector& xi) {
212 return Similarity3::wedge(xi);
213}
214
215template <>
216struct traits<Similarity3> : public internal::LieGroup<Similarity3> {};
217
218template <>
219struct traits<const Similarity3> : public internal::LieGroup<Similarity3> {};
220
221} // namespace gtsam
Base class and basic functions for Manifold types.
Base class and basic functions for Lie types.
3D Pose
3D rotation represented as a rotation matrix or quaternion
3D Point
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 size_t Dim()
Dimensionality of tangent space = 7 DOF - used to autodetect sizes.
Definition Similarity3.h:194
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