Similarity2.cpp
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 
18 #include <gtsam/base/Manifold.h>
19 #include <gtsam/geometry/Pose2.h>
20 #include <gtsam/geometry/Rot3.h>
23 
24 namespace gtsam {
25 
26 using std::vector;
27 
28 namespace internal {
29 
31 static Point2Pairs SubtractCentroids(const Point2Pairs& abPointPairs,
32  const Point2Pair& centroids) {
33  Point2Pairs d_abPointPairs;
34  for (const auto& [a, b] : abPointPairs) {
35  Point2 da = a - centroids.first;
36  Point2 db = b - centroids.second;
37  d_abPointPairs.emplace_back(da, db);
38  }
39  return d_abPointPairs;
40 }
41 
43 static double CalculateScale(const Point2Pairs& d_abPointPairs,
44  const Rot2& aRb) {
45  double x = 0, y = 0;
46 
47  for (const auto& [da, db] : d_abPointPairs) {
48  const Vector2 da_prime = aRb * db;
49  y += da.transpose() * da_prime;
50  x += da_prime.transpose() * da_prime;
51  }
52  const double s = y / x;
53  return s;
54 }
55 
57 static Matrix2 CalculateH(const Point2Pairs& d_abPointPairs) {
58  Matrix2 H = Z_2x2;
59  for (const auto& [da, db] : d_abPointPairs) {
60  H += da * db.transpose();
61  }
62  return H;
63 }
64 
74 static Similarity2 Align(const Point2Pairs& d_abPointPairs, const Rot2& aRb,
75  const Point2Pair& centroids) {
76  const double s = CalculateScale(d_abPointPairs, aRb);
77  // dividing aTb by s is required because the registration cost function
78  // minimizes ||a - sRb - t||, whereas Sim(2) computes s(Rb + t)
79  const Point2 aTb = (centroids.first - s * (aRb * centroids.second)) / s;
80  return Similarity2(aRb, aTb, s);
81 }
82 
94 static Similarity2 AlignGivenR(const Point2Pairs& abPointPairs,
95  const Rot2& aRb) {
96  auto centroids = means(abPointPairs);
97  auto d_abPointPairs = internal::SubtractCentroids(abPointPairs, centroids);
98  return internal::Align(d_abPointPairs, aRb, centroids);
99 }
100 } // namespace internal
101 
102 Similarity2::Similarity2() : t_(0, 0), s_(1) {}
103 
104 Similarity2::Similarity2(double s) : t_(0, 0), s_(s) {}
105 
106 Similarity2::Similarity2(const Rot2& R, const Point2& t, double s)
107  : R_(R), t_(t), s_(s) {}
108 
109 Similarity2::Similarity2(const Matrix2& R, const Vector2& t, double s)
110  : R_(Rot2::ClosestTo(R)), t_(t), s_(s) {}
111 
113  : R_(Rot2::ClosestTo(T.topLeftCorner<2, 2>())),
114  t_(T.topRightCorner<2, 1>()),
115  s_(1.0 / T(2, 2)) {}
116 
117 bool Similarity2::equals(const Similarity2& other, double tol) const {
118  return R_.equals(other.R_, tol) &&
119  traits<Point2>::Equals(t_, other.t_, tol) && s_ < (other.s_ + tol) &&
120  s_ > (other.s_ - tol);
121 }
122 
124  return R_.matrix() == other.R_.matrix() && t_ == other.t_ && s_ == other.s_;
125 }
126 
127 void Similarity2::print(const std::string& s) const {
128  std::cout << std::endl;
129  std::cout << s;
130  rotation().print("\nR:\n");
131  std::cout << "t: " << translation().transpose() << " s: " << scale()
132  << std::endl;
133 }
134 
136 
138  return Similarity2(R_ * S.R_, ((1.0 / S.s_) * t_) + R_ * S.t_, s_ * S.s_);
139 }
140 
142  const Rot2 Rt = R_.inverse();
143  const Point2 sRt = Rt * (-s_ * t_);
144  return Similarity2(Rt, sRt, 1.0 / s_);
145 }
146 
148  const Point2 q = R_ * p + t_;
149  return s_ * q;
150 }
151 
153  Rot2 R = R_.compose(T.rotation());
154  Point2 t = Point2(s_ * (R_ * T.translation() + t_));
155  return Pose2(R, t);
156 }
157 
159  return transformFrom(p);
160 }
161 
163  // Refer to Chapter 3 of
164  // http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
165  if (abPointPairs.size() < 2)
166  throw std::runtime_error("input should have at least 2 pairs of points");
167  auto centroids = means(abPointPairs);
168  auto d_abPointPairs = internal::SubtractCentroids(abPointPairs, centroids);
169  Matrix2 H = internal::CalculateH(d_abPointPairs);
170  // ClosestTo finds rotation matrix closest to H in Frobenius sense
172  return internal::Align(d_abPointPairs, aRb, centroids);
173 }
174 
176  const size_t n = abPosePairs.size();
177  if (n < 2)
178  throw std::runtime_error("input should have at least 2 pairs of poses");
179 
180  // calculate rotation
181  vector<Rot2> rotations;
182  Point2Pairs abPointPairs;
183  rotations.reserve(n);
184  abPointPairs.reserve(n);
185  // Below denotes the pose of the i'th object/camera/etc
186  // in frame "a" or frame "b".
187  for (const auto& [aTi, bTi] : abPosePairs) {
188  const Rot2 aRb = aTi.rotation().compose(bTi.rotation().inverse());
189  rotations.emplace_back(aRb);
190  abPointPairs.emplace_back(aTi.translation(), bTi.translation());
191  }
192  const Rot2 aRb_estimate = FindKarcherMean<Rot2>(rotations);
193 
194  return internal::AlignGivenR(abPointPairs, aRb_estimate);
195 }
196 
199  const Vector2 u = S.t_;
200  const Vector1 w = Rot2::Logmap(S.R_);
201  const double s = log(S.s_);
202  Vector4 result;
203  result << u, w, s;
204  if (Hm) {
205  throw std::runtime_error("Similarity2::Logmap: derivative not implemented");
206  }
207  return result;
208 }
209 
212  const Vector2 t = v.head<2>();
213  const Rot2 R = Rot2::Expmap(v.segment<1>(2));
214  const double s = v[3];
215  if (Hm) {
216  throw std::runtime_error("Similarity2::Expmap: derivative not implemented");
217  }
218  return Similarity2(R, t, s);
219 }
220 
221 Matrix4 Similarity2::AdjointMap() const {
222  throw std::runtime_error("Similarity2::AdjointMap not implemented");
223 }
224 
225 std::ostream& operator<<(std::ostream& os, const Similarity2& p) {
226  os << "[" << p.rotation().theta() << " " << p.translation().transpose() << " "
227  << p.scale() << "]\';";
228  return os;
229 }
230 
231 Matrix3 Similarity2::matrix() const {
232  Matrix3 T;
233  T.topRows<2>() << R_.matrix(), t_;
234  T.bottomRows<1>() << 0, 0, 1.0 / s_;
235  return T;
236 }
237 
238 } // namespace gtsam
gtsam::Similarity2::Similarity2
Similarity2()
Default constructor.
Definition: Similarity2.cpp:102
w
RowVector3d w
Definition: Matrix_resize_int.cpp:3
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::Similarity2::Expmap
static Similarity2 Expmap(const Vector4 &v, OptionalJacobian< 4, 4 > Hm={})
Exponential map at the identity.
Definition: Similarity2.cpp:210
Pose2.h
2D Pose
aRb
Rot3 aRb
Definition: testFundamentalMatrix.cpp:146
s
RealScalar s
Definition: level1_cplx_impl.h:126
gtsam::operator<<
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Definition: testGroup.cpp:109
gtsam::Similarity2::operator==
bool operator==(const Similarity2 &other) const
Exact equality.
Definition: Similarity2.cpp:123
gtsam::LieGroup::compose
Class compose(const Class &g) const
Definition: Lie.h:48
x
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 x
Definition: gnuplot_common_settings.hh:12
Similarity2.h
Implementation of Similarity2 transform.
T
Eigen::Triplet< double > T
Definition: Tutorial_sparse_example.cpp:6
log
const EIGEN_DEVICE_FUNC LogReturnType log() const
Definition: ArrayCwiseUnaryOps.h:128
gtsam::Rot2::matrix
Matrix2 matrix() const
Definition: Rot2.cpp:85
os
ofstream os("timeSchurFactors.csv")
gtsam::means
Point2Pair means(const std::vector< Point2Pair > &abPointPairs)
Calculate the two means of a set of Point2 pairs.
Definition: Point2.cpp:116
result
Values result
Definition: OdometryOptimize.cpp:8
topRightCorner
mat1 topRightCorner(size/2, size/2)
gtsam::Similarity2::s_
double s_
Definition: Similarity2.h:45
Rot3.h
3D rotation represented as a rotation matrix or quaternion
gtsam::Similarity2::Identity
static Similarity2 Identity()
Return an identity transform.
Definition: Similarity2.cpp:135
gtsam::Point2Pair
std::pair< Point2, Point2 > Point2Pair
Definition: Point2.h:35
gtsam::Similarity2::t_
Point2 t_
Definition: Similarity2.h:44
n
int n
Definition: BiCGSTAB_simple.cpp:1
gtsam::internal::CalculateH
static Matrix2 CalculateH(const Point2Pairs &d_abPointPairs)
Form outer product H.
Definition: Similarity2.cpp:57
gtsam::internal::AlignGivenR
static Similarity2 AlignGivenR(const Point2Pairs &abPointPairs, const Rot2 &aRb)
This method estimates the similarity transform from point pairs, given a known or estimated rotation....
Definition: Similarity2.cpp:94
gtsam::Rot2::equals
bool equals(const Rot2 &R, double tol=1e-9) const
Definition: Rot2.cpp:51
Eigen::numext::q
EIGEN_DEVICE_FUNC const Scalar & q
Definition: SpecialFunctionsImpl.h:1984
gtsam::Similarity2::print
void print(const std::string &s="") const
Print with optional string.
Definition: Similarity2.cpp:127
gtsam::Rot2::Logmap
static Vector1 Logmap(const Rot2 &r, ChartJacobian H={})
Log map at identity - return the canonical coordinates of this rotation.
Definition: Rot2.cpp:77
example2::aTb
Point3 aTb
Definition: testEssentialMatrixFactor.cpp:541
topLeftCorner
Eigen::Block< Derived > topLeftCorner(MatrixBase< Derived > &m, int rows, int cols)
Definition: class_Block.cpp:8
Manifold.h
Base class and basic functions for Manifold types.
Vector2
Definition: test_operator_overloading.cpp:18
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
Eigen::Triplet< double >
gtsam::Rot2::print
void print(const std::string &s="theta") const
Definition: Rot2.cpp:46
gtsam::Similarity2
Definition: Similarity2.h:35
gtsam::internal::SubtractCentroids
static Point2Pairs SubtractCentroids(const Point2Pairs &abPointPairs, const Point2Pair &centroids)
Subtract centroids from point pairs.
Definition: Similarity2.cpp:31
y
Scalar * y
Definition: level1_cplx_impl.h:124
gtsam::Similarity2::rotation
Rot2 rotation() const
Return a GTSAM rotation.
Definition: Similarity2.h:178
gtsam::b
const G & b
Definition: Group.h:79
gtsam::Rot2
Definition: Rot2.h:35
gtsam::Rot2::ClosestTo
static Rot2 ClosestTo(const Matrix2 &M)
Definition: Rot2.cpp:133
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
KarcherMeanFactor-inl.h
gtsam::Similarity2::AdjointMap
Matrix4 AdjointMap() const
Project from one tangent space to another.
Definition: Similarity2.cpp:221
gtsam
traits
Definition: SFMdata.h:40
gtsam::traits
Definition: Group.h:36
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
gtsam::Similarity2::matrix
Matrix3 matrix() const
Calculate 4*4 matrix group equivalent.
Definition: Similarity2.cpp:231
gtsam::Similarity2::Logmap
static Vector4 Logmap(const Similarity2 &S, OptionalJacobian< 4, 4 > Hm={})
Definition: Similarity2.cpp:197
gtsam::Similarity2::scale
double scale() const
Return the scale.
Definition: Similarity2.h:184
gtsam::Similarity2::transformFrom
Point2 transformFrom(const Point2 &p) const
Action on a point p is s*(R*p+t)
Definition: Similarity2.cpp:147
gtsam::Similarity2::inverse
Similarity2 inverse() const
Return the inverse.
Definition: Similarity2.cpp:141
gtsam::Similarity2::equals
bool equals(const Similarity2 &sim, double tol) const
Compare with tolerance.
Definition: Similarity2.cpp:117
gtsam::Similarity2::Align
static Similarity2 Align(const Point2Pairs &abPointPairs)
Definition: Similarity2.cpp:162
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::internal::CalculateScale
static double CalculateScale(const Point2Pairs &d_abPointPairs, const Rot2 &aRb)
Form inner products x and y and calculate scale.
Definition: Similarity2.cpp:43
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::tol
const G double tol
Definition: Group.h:79
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
gtsam::Point2Pairs
std::vector< Point2Pair > Point2Pairs
Definition: Point2.h:38
gtsam::Similarity2::translation
Point2 translation() const
Return a GTSAM translation.
Definition: Similarity2.h:181
gtsam::Pose2Pairs
std::vector< Pose2Pair > Pose2Pairs
Definition: Pose2.h:362
internal
Definition: BandTriangularSolver.h:13
gtsam::Similarity2::R_
Rot2 R_
Definition: Similarity2.h:43
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)
gtsam::Rot2::inverse
Rot2 inverse() const
Definition: Rot2.h:113
pybind_wrapper_test_script.other
other
Definition: pybind_wrapper_test_script.py:42
R
Rot2 R(Rot2::fromAngle(0.1))
S
DiscreteKey S(1, 2)
gtsam::Similarity2::operator*
Similarity2 operator*(const Similarity2 &S) const
Composition.
Definition: Similarity2.cpp:137
gtsam::Pose2
Definition: Pose2.h:39
gtsam::Rot2::Expmap
static Rot2 Expmap(const Vector1 &v, ChartJacobian H={})
Exponential map at identity - create a rotation from canonical coordinates.
Definition: Rot2.cpp:67


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:35:16