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>
24 #include <gtsam/geometry/Point3.h>
25 #include <gtsam/geometry/Pose3.h>
26 #include <gtsam/geometry/Rot3.h>
27 
28 namespace gtsam {
29 
30 // Forward declarations
31 class Pose3;
32 
36 class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
39  typedef Rot3 Rotation;
42 
43  private:
46  double s_;
47 
48  public:
51 
53  Similarity3();
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 = {}, //
102  OptionalJacobian<3, 3> H2 = {}) 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 = {});
146 
149  static Similarity3 Expmap(const Vector7& v, //
150  OptionalJacobian<7, 7> Hm = {});
151 
153  struct ChartAtOrigin {
154  static Similarity3 Retract(const Vector7& v,
155  ChartJacobian H = {}) {
156  return Similarity3::Expmap(v, H);
157  }
158  static Vector7 Local(const Similarity3& other,
159  ChartJacobian H = {}) {
160  return Similarity3::Logmap(other, H);
161  }
162  };
163 
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 
210 template <>
212  return Similarity3::wedge(xi);
213 }
214 
215 template <>
216 struct traits<Similarity3> : public internal::LieGroup<Similarity3> {};
217 
218 template <>
219 struct traits<const Similarity3> : public internal::LieGroup<Similarity3> {};
220 
221 } // namespace gtsam
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
static Vector7 Logmap(const Similarity3 &s, OptionalJacobian< 7, 7 > Hm={})
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Definition: testGroup.cpp:109
static size_t Dim()
Dimensionality of tangent space = 7 DOF - used to autodetect sizes.
Definition: Similarity3.h:194
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
Eigen::Vector3d Vector3
Definition: Vector.h:43
static Similarity3 Retract(const Vector7 &v, ChartJacobian H={})
Definition: Similarity3.h:154
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Rot2 R(Rot2::fromAngle(0.1))
bool operator==(const Matrix &A, const Matrix &B)
Definition: base/Matrix.h:99
Pose2_ Expmap(const Vector3_ &xi)
static Matrix4 wedge(const Vector7 &xi)
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange [*:*] noreverse nowriteback set trange [*:*] noreverse nowriteback set urange [*:*] noreverse nowriteback set vrange [*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
DiscreteKey S(1, 2)
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition: Point2.h:52
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
EIGEN_DEVICE_FUNC const InverseReturnType inverse() const
static Vector7 Local(const Similarity3 &other, ChartJacobian H={})
Definition: Similarity3.h:158
Matrix wedge< Similarity3 >(const Vector &xi)
Definition: Similarity3.h:211
Matrix wedge(const Vector &x)
Point3_ transformFrom(const Pose3_ &x, const Point3_ &p)
Eigen::VectorXd Vector
Definition: Vector.h:38
Point3 translation() const
Return a GTSAM translation.
Definition: Similarity3.h:188
Base class and basic functions for Manifold types.
Array< int, Dynamic, 1 > v
RealScalar s
RowVector3d w
Base class and basic functions for Lie types.
cout<< "The eigenvalues of A are:"<< endl<< ces.eigenvalues()<< endl;cout<< "The matrix of eigenvectors, V, is:"<< endl<< ces.eigenvectors()<< endl<< endl;complex< float > lambda
static Similarity3 Expmap(const Vector7 &v, OptionalJacobian< 7, 7 > Hm={})
Vector xi
Definition: testPose2.cpp:148
traits
Definition: chartTesting.h:28
ofstream os("timeSchurFactors.csv")
Both LieGroupTraits and Testable.
Definition: Lie.h:229
3D Point
float * p
const G double tol
Definition: Group.h:86
Vector3 Point3
Definition: Point3.h:38
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
3D Pose
std::vector< Point3Pair > Point3Pairs
Definition: Point3.h:45
double scale() const
Return the scale.
Definition: Similarity3.h:191
Point2 t(10, 10)
3D rotation represented as a rotation matrix or quaternion
static Similarity2 Align(const Point2Pairs &d_abPointPairs, const Rot2 &aRb, const Point2Pair &centroids)
This method estimates the similarity transform from differences point pairs, given a known or estimat...
Definition: Similarity2.cpp:74


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:43