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
171  Rot2 aRb = Rot2::ClosestTo(H);
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
bool operator==(const Similarity2 &other) const
Exact equality.
static double CalculateScale(const Point2Pairs &d_abPointPairs, const Rot2 &aRb)
Form inner products x and y and calculate scale.
Definition: Similarity2.cpp:43
Scalar * y
static Rot2 ClosestTo(const Matrix2 &M)
Definition: Rot2.cpp:133
Similarity2()
Default constructor.
static Rot2 Expmap(const Vector1 &v, ChartJacobian H={})
Exponential map at identity - create a rotation from canonical coordinates.
Definition: Rot2.cpp:67
Implementation of Similarity2 transform.
Vector2 Point2
Definition: Point2.h:32
int n
Rot2 R(Rot2::fromAngle(0.1))
static Vector1 Logmap(const Rot2 &r, ChartJacobian H={})
Log map at identity - return the canonical coordinates of this rotation.
Definition: Rot2.cpp:77
static Matrix2 CalculateH(const Point2Pairs &d_abPointPairs)
Form outer product H.
Definition: Similarity2.cpp:57
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)
EIGEN_DEVICE_FUNC const LogReturnType log() const
mat1 topRightCorner(size/2, size/2)
bool equals(const Rot2 &R, double tol=1e-9) const
Definition: Rot2.cpp:51
std::vector< Point2Pair > Point2Pairs
Definition: Point2.h:38
static Similarity2 Identity()
Return an identity transform.
std::vector< Pose2Pair > Pose2Pairs
Definition: Pose2.h:362
Values result
Point2 translation() const
Return a GTSAM translation.
Definition: Similarity2.h:180
static Point2Pairs SubtractCentroids(const Point2Pairs &abPointPairs, const Point2Pair &centroids)
Subtract centroids from point pairs.
Definition: Similarity2.cpp:31
Base class and basic functions for Manifold types.
Array< int, Dynamic, 1 > v
double scale() const
Return the scale.
Definition: Similarity2.h:183
Eigen::Triplet< double > T
bool equals(const Similarity2 &sim, double tol) const
Compare with tolerance.
RealScalar s
EIGEN_DEVICE_FUNC const Scalar & q
Class compose(const Class &g) const
Definition: Lie.h:48
const G & b
Definition: Group.h:86
Rot2 rotation() const
Return a GTSAM rotation.
Definition: Similarity2.h:177
RowVector3d w
traits
Definition: chartTesting.h:28
Matrix3 matrix() const
Calculate 4*4 matrix group equivalent.
Matrix4 AdjointMap() const
Project from one tangent space to another.
Point2Pair means(const std::vector< Point2Pair > &abPointPairs)
Calculate the two means of a set of Point2 pairs.
Definition: Point2.cpp:116
Matrix2 matrix() const
Definition: Rot2.cpp:85
static Vector4 Logmap(const Similarity2 &S, OptionalJacobian< 4, 4 > Hm={})
ofstream os("timeSchurFactors.csv")
Point2 transformFrom(const Point2 &p) const
Action on a point p is s*(R*p+t)
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
std::pair< Point2, Point2 > Point2Pair
Definition: Point2.h:35
static Similarity2 Expmap(const Vector4 &v, OptionalJacobian< 4, 4 > Hm={})
Exponential map at the identity.
Rot2 inverse() const
Definition: Rot2.h:113
float * p
void print(const std::string &s) const
Print with optional string.
void print(const std::string &s="theta") const
Definition: Rot2.cpp:46
const G double tol
Definition: Group.h:86
2D Pose
The matrix class, also used for vectors and row-vectors.
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
Similarity2 inverse() const
Return the inverse.
Similarity2 operator*(const Similarity2 &S) const
Composition.
static Similarity2 Align(const Point2Pairs &abPointPairs)
const Rot2 & rotation(OptionalJacobian< 1, 3 > Hself={}) const
rotation
Definition: Pose2.h:270
Point2 t(10, 10)
double theta() const
Definition: Rot2.h:186
3D rotation represented as a rotation matrix or quaternion
friend std::ostream & operator<<(std::ostream &os, const Similarity2 &p)
const Point2 & translation(OptionalJacobian< 2, 3 > Hself={}) const
translation
Definition: Pose2.h:261
Eigen::Block< Derived > topLeftCorner(MatrixBase< Derived > &m, int rows, int cols)
Definition: class_Block.cpp:8
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