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  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
81  const Similarity3& p);
82 
86 
88  static Similarity3 Identity();
89 
91  Similarity3 operator*(const Similarity3& S) const;
92 
94  Similarity3 inverse() const;
95 
99 
101  Point3 transformFrom(const Point3& p, //
102  OptionalJacobian<3, 7> H1 = {}, //
103  OptionalJacobian<3, 3> H2 = {}) const;
104 
116  Pose3 transformFrom(const Pose3& T) const;
117 
119  Point3 operator*(const Point3& p) const;
120 
124  static Similarity3 Align(const Point3Pairs& abPointPairs);
125 
136  static Similarity3 Align(const Pose3Pairs& abPosePairs);
137 
141 
145  static Vector7 Logmap(const Similarity3& s, //
146  OptionalJacobian<7, 7> Hm = {});
147 
150  static Similarity3 Expmap(const Vector7& v, //
151  OptionalJacobian<7, 7> Hm = {});
152 
154  struct ChartAtOrigin {
155  static Similarity3 Retract(const Vector7& v,
156  ChartJacobian H = {}) {
157  return Similarity3::Expmap(v, H);
158  }
159  static Vector7 Local(const Similarity3& other,
160  ChartJacobian H = {}) {
161  return Similarity3::Logmap(other, H);
162  }
163  };
164 
166 
173  static Matrix4 wedge(const Vector7& xi);
174 
176  Matrix7 AdjointMap() const;
177 
181 
183  Matrix4 matrix() const;
184 
186  Rot3 rotation() const { return R_; }
187 
189  Point3 translation() const { return t_; }
190 
192  double scale() const { return s_; }
193 
195  inline static size_t Dim() { return 7; }
196 
198  inline size_t dim() const { return 7; }
199 
203 
204  private:
205 
206  #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
207 
208  friend class boost::serialization::access;
209  template<class Archive>
210  void serialize(Archive & ar, const unsigned int /*version*/) {
211  ar & BOOST_SERIALIZATION_NVP(R_);
212  ar & BOOST_SERIALIZATION_NVP(t_);
213  ar & BOOST_SERIALIZATION_NVP(s_);
214  }
215  #endif
216 
218  static Matrix3 GetV(Vector3 w, double lambda);
219 
221 };
222 
223 template <>
225  return Similarity3::wedge(xi);
226 }
227 
228 template <>
229 struct traits<Similarity3> : public internal::LieGroup<Similarity3> {};
230 
231 template <>
232 struct traits<const Similarity3> : public internal::LieGroup<Similarity3> {};
233 
234 } // namespace gtsam
gtsam::Similarity3::scale
double scale() const
Return the scale.
Definition: Similarity3.h:192
gtsam::Similarity3::Logmap
static Vector7 Logmap(const Similarity3 &s, OptionalJacobian< 7, 7 > Hm={})
Definition: Similarity3.cpp:254
w
RowVector3d w
Definition: Matrix_resize_int.cpp:3
gtsam::wedge
Matrix wedge(const Vector &x)
H
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
Definition: gnuplot_common_settings.hh:74
gtsam::Similarity3::wedge
static Matrix4 wedge(const Vector7 &xi)
Definition: Similarity3.cpp:196
gtsam::Point3Pairs
std::vector< Point3Pair > Point3Pairs
Definition: Point3.h:45
inverse
const EIGEN_DEVICE_FUNC InverseReturnType inverse() const
Definition: ArrayCwiseUnaryOps.h:411
s
RealScalar s
Definition: level1_cplx_impl.h:126
gtsam::operator<<
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Definition: testGroup.cpp:109
screwPose2::xi
Vector xi
Definition: testPose2.cpp:148
gtsam::Similarity3::Translation
Point3 Translation
Definition: Similarity3.h:40
gtsam::Similarity3::dim
size_t dim() const
Dimensionality of tangent space = 7 DOF.
Definition: Similarity3.h:198
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Point3.h
3D Point
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::Similarity3::ChartAtOrigin::Retract
static Similarity3 Retract(const Vector7 &v, ChartJacobian H={})
Definition: Similarity3.h:155
gtsam::operator==
bool operator==(const Matrix &A, const Matrix &B)
Definition: base/Matrix.h:99
os
ofstream os("timeSchurFactors.csv")
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::operator*
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition: Point2.h:52
Rot3.h
3D rotation represented as a rotation matrix or quaternion
Expmap
Pose2_ Expmap(const Vector3_ &xi)
Definition: InverseKinematicsExampleExpressions.cpp:47
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
gtsam::transformFrom
Point3_ transformFrom(const Pose3_ &x, const Point3_ &p)
Definition: slam/expressions.h:47
gtsam::Similarity3::Dim
static size_t Dim()
Dimensionality of tangent space = 7 DOF - used to autodetect sizes.
Definition: Similarity3.h:195
gtsam::Similarity3::translation
Point3 translation() const
Return a GTSAM translation.
Definition: Similarity3.h:189
gtsam::Similarity3::s_
double s_
Definition: Similarity3.h:46
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition: Lie.h:229
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::Similarity3::R_
Rot3 R_
Definition: Similarity3.h:44
gtsam::Similarity3::Rotation
Rot3 Rotation
Definition: Similarity3.h:39
Manifold.h
Base class and basic functions for Manifold types.
gtsam::Similarity3::ChartAtOrigin::Local
static Vector7 Local(const Similarity3 &other, ChartJacobian H={})
Definition: Similarity3.h:159
gtsam::Pose3Pairs
std::vector< std::pair< Pose3, Pose3 > > Pose3Pairs
Definition: Pose3.h:425
Eigen::Triplet< double >
lambda
static double lambda[]
Definition: jv.c:524
gtsam::equals
Definition: Testable.h:112
matrix
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
Definition: gtsam/3rdparty/Eigen/blas/common.h:110
Lie.h
Base class and basic functions for Lie types.
gtsam
traits
Definition: chartTesting.h:28
gtsam::traits
Definition: Group.h:36
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
gtsam::Similarity3::rotation
Rot3 rotation() const
Return a GTSAM rotation.
Definition: Similarity3.h:186
p
float * p
Definition: Tutorial_Map_using.cpp:9
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::Similarity3::t_
Point3 t_
Definition: Similarity3.h:45
gtsam::LieGroup
Definition: Lie.h:37
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::Similarity3::ChartAtOrigin
Chart at the origin.
Definition: Similarity3.h:154
gtsam::internal::Align
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
align_3::t
Point2 t(10, 10)
Pose3
Definition: testDependencies.h:3
pybind_wrapper_test_script.other
other
Definition: pybind_wrapper_test_script.py:42
gtsam::Similarity3
Definition: Similarity3.h:36
gtsam::LieGroup< Similarity3, 7 >::inverse
Similarity3 inverse(ChartJacobian H) const
Definition: Lie.h:71
gtsam::wedge< Similarity3 >
Matrix wedge< Similarity3 >(const Vector &xi)
Definition: Similarity3.h:224
R
Rot2 R(Rot2::fromAngle(0.1))
Pose3.h
3D Pose
S
DiscreteKey S(1, 2)
gtsam::Similarity3::Expmap
static Similarity3 Expmap(const Vector7 &v, OptionalJacobian< 7, 7 > Hm={})
Definition: Similarity3.cpp:267


gtsam
Author(s):
autogenerated on Thu Jun 13 2024 03:05:19