Similarity3.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 
20 
21 #include <gtsam/geometry/Pose3.h>
22 #include <gtsam/base/Manifold.h>
24 
25 namespace gtsam {
26 
27 using std::vector;
28 
29 namespace internal {
31 static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs,
32  const Point3Pair &centroids) {
33  Point3Pairs d_abPointPairs;
34  for (const auto& [a, b] : abPointPairs) {
35  Point3 da = a - centroids.first;
36  Point3 db = b - centroids.second;
37  d_abPointPairs.emplace_back(da, db);
38  }
39  return d_abPointPairs;
40 }
41 
43 // We force the scale to be a non-negative quantity
44 // (see Section 10.1 of https://ethaneade.com/lie_groups.pdf)
45 static double calculateScale(const Point3Pairs &d_abPointPairs,
46  const Rot3 &aRb) {
47  double x = 0, y = 0;
48  Point3 da, db;
49  for (const auto& [da, db] : d_abPointPairs) {
50  const Vector3 da_prime = aRb * db;
51  y += da.transpose() * da_prime;
52  x += da_prime.transpose() * da_prime;
53  }
54  const double s = std::fabs(y / x);
55  return s;
56 }
57 
59 static Matrix3 calculateH(const Point3Pairs &d_abPointPairs) {
60  Matrix3 H = Z_3x3;
61  for (const auto& [da, db] : d_abPointPairs) {
62  H += da * db.transpose();
63  }
64  return H;
65 }
66 
68 // given a known or estimated rotation and point centroids.
69 static Similarity3 align(const Point3Pairs &d_abPointPairs, const Rot3 &aRb,
70  const Point3Pair &centroids) {
71  const double s = calculateScale(d_abPointPairs, aRb);
72  // dividing aTb by s is required because the registration cost function
73  // minimizes ||a - sRb - t||, whereas Sim(3) computes s(Rb + t)
74  const Point3 aTb = (centroids.first - s * (aRb * centroids.second)) / s;
75  return Similarity3(aRb, aTb, s);
76 }
77 
79 // Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3
80 static Similarity3 alignGivenR(const Point3Pairs &abPointPairs,
81  const Rot3 &aRb) {
82  auto centroids = means(abPointPairs);
83  auto d_abPointPairs = internal::subtractCentroids(abPointPairs, centroids);
84  return align(d_abPointPairs, aRb, centroids);
85 }
86 } // namespace internal
87 
89  t_(0,0,0), s_(1) {
90 }
91 
93  t_(0,0,0), s_(s) {
94 }
95 
96 Similarity3::Similarity3(const Rot3& R, const Point3& t, double s) :
97  R_(R), t_(t), s_(s) {
98 }
99 
100 Similarity3::Similarity3(const Matrix3& R, const Vector3& t, double s) :
101  R_(R), t_(t), s_(s) {
102 }
103 
104 Similarity3::Similarity3(const Matrix4& T) :
105  R_(T.topLeftCorner<3, 3>()), t_(T.topRightCorner<3, 1>()), s_(1.0 / T(3, 3)) {
106 }
107 
108 bool Similarity3::equals(const Similarity3& other, double tol) const {
109  return R_.equals(other.R_, tol) && traits<Point3>::Equals(t_, other.t_, tol)
110  && s_ < (other.s_ + tol) && s_ > (other.s_ - tol);
111 }
112 
114  return R_.matrix() == other.R_.matrix() && t_ == other.t_ && s_ == other.s_;
115 }
116 
117 void Similarity3::print(const std::string& s) const {
118  std::cout << std::endl;
119  std::cout << s;
120  rotation().print("\nR:\n");
121  std::cout << "t: " << translation().transpose() << " s: " << scale() << std::endl;
122 }
123 
125  return Similarity3();
126 }
128  return Similarity3(R_ * S.R_, ((1.0 / S.s_) * t_) + R_ * S.t_, s_ * S.s_);
129 }
130 
132  const Rot3 Rt = R_.inverse();
133  const Point3 sRt = Rt * (-s_ * t_);
134  return Similarity3(Rt, sRt, 1.0 / s_);
135 }
136 
139  const Point3 q = R_ * p + t_;
140  if (H1) {
141  // For this derivative, see LieGroups.pdf
142  const Matrix3 sR = s_ * R_.matrix();
143  const Matrix3 DR = sR * skewSymmetric(-p.x(), -p.y(), -p.z());
144  *H1 << DR, sR, sR * p;
145  }
146  if (H2)
147  *H2 = s_ * R_.matrix(); // just 3*3 sub-block of matrix()
148  return s_ * q;
149 }
150 
152  Rot3 R = R_.compose(T.rotation());
153  Point3 t = Point3(s_ * (R_ * T.translation() + t_));
154  return Pose3(R, t);
155 }
156 
158  return transformFrom(p);
159 }
160 
162  // Refer to Chapter 3 of
163  // http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
164  if (abPointPairs.size() < 3)
165  throw std::runtime_error("input should have at least 3 pairs of points");
166  auto centroids = means(abPointPairs);
167  auto d_abPointPairs = internal::subtractCentroids(abPointPairs, centroids);
168  Matrix3 H = internal::calculateH(d_abPointPairs);
169  // ClosestTo finds rotation matrix closest to H in Frobenius sense
170  Rot3 aRb = Rot3::ClosestTo(H);
171  return internal::align(d_abPointPairs, aRb, centroids);
172 }
173 
174 Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
175  const size_t n = abPosePairs.size();
176  if (n < 2)
177  throw std::runtime_error("input should have at least 2 pairs of poses");
178 
179  // calculate rotation
180  vector<Rot3> rotations;
181  Point3Pairs abPointPairs;
182  rotations.reserve(n);
183  abPointPairs.reserve(n);
184  // Below denotes the pose of the i'th object/camera/etc in frame "a" or frame "b"
185  Pose3 aTi, bTi;
186  for (const auto &[aTi, bTi] : abPosePairs) {
187  const Rot3 aRb = aTi.rotation().compose(bTi.rotation().inverse());
188  rotations.emplace_back(aRb);
189  abPointPairs.emplace_back(aTi.translation(), bTi.translation());
190  }
191  const Rot3 aRb_estimate = FindKarcherMean<Rot3>(rotations);
192 
193  return internal::alignGivenR(abPointPairs, aRb_estimate);
194 }
195 
196 Matrix4 Similarity3::wedge(const Vector7 &xi) {
197  // http://www.ethaneade.org/latex2html/lie/node29.html
198  const auto w = xi.head<3>();
199  const auto u = xi.segment<3>(3);
200  const double lambda = xi[6];
201  Matrix4 W;
202  W << skewSymmetric(w), u, 0, 0, 0, -lambda;
203  return W;
204 }
205 
206 Matrix7 Similarity3::AdjointMap() const {
207  // http://www.ethaneade.org/latex2html/lie/node30.html
208  const Matrix3 R = R_.matrix();
209  const Vector3 t = t_;
210  const Matrix3 A = s_ * skewSymmetric(t) * R;
211  Matrix7 adj;
212  adj << R, Z_3x3, Matrix31::Zero(), // 3*7
213  A, s_ * R, -s_ * t, // 3*7
214  Matrix16::Zero(), 1; // 1*7
215  return adj;
216 }
217 
218 Matrix3 Similarity3::GetV(Vector3 w, double lambda) {
219  // http://www.ethaneade.org/latex2html/lie/node29.html
220  const double theta2 = w.transpose() * w;
221  double Y, Z, W;
222  if (theta2 > 1e-9) {
223  const double theta = sqrt(theta2);
224  const double X = sin(theta) / theta;
225  Y = (1 - cos(theta)) / theta2;
226  Z = (1 - X) / theta2;
227  W = (0.5 - Y) / theta2;
228  } else {
229  // Taylor series expansion for theta=0, X not needed (as is 1)
230  Y = 0.5 - theta2 / 24.0;
231  Z = 1.0 / 6.0 - theta2 / 120.0;
232  W = 1.0 / 24.0 - theta2 / 720.0;
233  }
234  const double lambda2 = lambda * lambda, lambda3 = lambda2 * lambda;
235  const double expMinLambda = exp(-lambda);
236  double A, alpha = 0.0, beta, mu;
237  if (lambda2 > 1e-9) {
238  A = (1.0 - expMinLambda) / lambda;
239  alpha = 1.0 / (1.0 + theta2 / lambda2);
240  beta = (expMinLambda - 1 + lambda) / lambda2;
241  mu = (1 - lambda + (0.5 * lambda2) - expMinLambda) / lambda3;
242  } else {
243  A = 1.0 - lambda / 2.0 + lambda2 / 6.0;
244  beta = 0.5 - lambda / 6.0 + lambda2 / 24.0 - lambda3 / 120.0;
245  mu = 1.0 / 6.0 - lambda / 24.0 + lambda2 / 120.0 - lambda3 / 720.0;
246  }
247  const double gamma = Y - (lambda * Z), upsilon = Z - (lambda * W);
248  const double B = alpha * (beta - gamma) + gamma;
249  const double C = alpha * (mu - upsilon) + upsilon;
250  const Matrix3 Wx = skewSymmetric(w[0], w[1], w[2]);
251  return A * I_3x3 + B * Wx + C * Wx * Wx;
252 }
253 
255  // To get the logmap, calculate w and lambda, then solve for u as shown by Ethan at
256  // www.ethaneade.org/latex2html/lie/node29.html
257  const Vector3 w = Rot3::Logmap(T.R_);
258  const double lambda = log(T.s_);
259  Vector7 result;
260  result << w, GetV(w, lambda).inverse() * T.t_, lambda;
261  if (Hm) {
262  throw std::runtime_error("Similarity3::Logmap: derivative not implemented");
263  }
264  return result;
265 }
266 
268  const auto w = v.head<3>();
269  const auto u = v.segment<3>(3);
270  const double lambda = v[6];
271  if (Hm) {
272  throw std::runtime_error("Similarity3::Expmap: derivative not implemented");
273  }
274  const Matrix3 V = GetV(w, lambda);
275  return Similarity3(Rot3::Expmap(w), Point3(V * u), exp(lambda));
276 }
277 
278 std::ostream &operator<<(std::ostream &os, const Similarity3& p) {
279  os << "[" << p.rotation().xyz().transpose() << " "
280  << p.translation().transpose() << " " << p.scale() << "]\';";
281  return os;
282 }
283 
284 Matrix4 Similarity3::matrix() const {
285  Matrix4 T;
286  T.topRows<3>() << R_.matrix(), t_;
287  T.bottomRows<1>() << 0, 0, 0, 1.0 / s_;
288  return T;
289 }
290 
291 } // namespace gtsam
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
Definition: bench_gemm.cpp:49
bool operator==(const Similarity3 &other) const
Exact equality.
static Similarity3 Align(const Point3Pairs &abPointPairs)
const char Y
static Vector7 Logmap(const Similarity3 &s, OptionalJacobian< 7, 7 > Hm={})
void print(const std::string &s) const
Print with optional string.
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Definition: Pose3.cpp:308
Scalar * y
Rot3 rotation() const
Return a GTSAM rotation.
Definition: Similarity3.h:185
Eigen::Vector3d Vector3
Definition: Vector.h:43
bool equals(const Rot3 &p, double tol=1e-9) const
Definition: Rot3.cpp:99
double mu
int n
Rot2 R(Rot2::fromAngle(0.1))
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
Similarity3 inverse() const
Return the inverse.
Key W(std::uint64_t j)
static Matrix4 wedge(const Vector7 &xi)
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
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)
Similarity3 operator*(const Similarity3 &S) const
Composition.
Real fabs(const Real &a)
EIGEN_DEVICE_FUNC const LogReturnType log() const
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
static Rot3 Expmap(const Vector3 &v, OptionalJacobian< 3, 3 > H={})
Definition: Rot3.h:374
Matrix7 AdjointMap() const
Project from one tangent space to another.
static Similarity3 Identity()
Return an identity transform.
mat1 topRightCorner(size/2, size/2)
static Similarity3 align(const Point3Pairs &d_abPointPairs, const Rot3 &aRb, const Point3Pair &centroids)
This method estimates the similarity transform from differences point pairs,.
Definition: Similarity3.cpp:69
Rot3 inverse() const
inverse of a rotation
Definition: Rot3.h:308
EIGEN_DEVICE_FUNC const ExpReturnType exp() const
static double calculateScale(const Point3Pairs &d_abPointPairs, const Rot3 &aRb)
Form inner products x and y and calculate scale.
Definition: Similarity3.cpp:45
#define Z
Definition: icosphere.cpp:21
Values result
Vector3 xyz(OptionalJacobian< 3, 3 > H={}) const
Definition: Rot3.cpp:160
static Vector3 Logmap(const Rot3 &R, OptionalJacobian< 3, 3 > H={})
Definition: Rot3M.cpp:157
Point3 translation() const
Return a GTSAM translation.
Definition: Similarity3.h:188
Base class and basic functions for Manifold types.
Similarity3()
Default constructor.
Definition: Similarity3.cpp:88
Array< int, Dynamic, 1 > v
static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs, const Point3Pair &centroids)
Subtract centroids from point pairs.
Definition: Similarity3.cpp:31
Eigen::Triplet< double > T
RealScalar alpha
Array< double, 1, 3 > e(1./3., 0.5, 2.)
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
RowVector3d w
Matrix4 matrix() const
Calculate 4*4 matrix group equivalent.
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={})
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:50
Vector xi
Definition: testPose2.cpp:148
Point3 transformFrom(const Point3 &p, OptionalJacobian< 3, 7 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Action on a point p is s*(R*p+t)
traits
Definition: chartTesting.h:28
friend std::ostream & operator<<(std::ostream &os, const Similarity3 &p)
Point2Pair means(const std::vector< Point2Pair > &abPointPairs)
Calculate the two means of a set of Point2 pairs.
Definition: Point2.cpp:116
Matrix3 skewSymmetric(double wx, double wy, double wz)
Definition: base/Matrix.h:400
ofstream os("timeSchurFactors.csv")
Matrix3 matrix() const
Definition: Rot3M.cpp:218
float * p
static Matrix3 calculateH(const Point3Pairs &d_abPointPairs)
Form outer product H.
Definition: Similarity3.cpp:59
static Matrix3 GetV(Vector3 w, double lambda)
Calculate expmap and logmap coefficients.
bool equals(const Similarity3 &sim, double tol) const
Compare with tolerance.
static Rot3 ClosestTo(const Matrix3 &M)
Definition: Rot3.h:271
void print(const std::string &s="") const
Definition: Rot3.cpp:33
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:315
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
const G double tol
Definition: Group.h:86
Vector3 Point3
Definition: Point3.h:38
#define X
Definition: icosphere.cpp:20
Implementation of Similarity3 transform.
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
3D Pose
std::vector< Point3Pair > Point3Pairs
Definition: Point3.h:45
static Similarity3 alignGivenR(const Point3Pairs &abPointPairs, const Rot3 &aRb)
This method estimates the similarity transform from point pairs, given a known or estimated rotation...
Definition: Similarity3.cpp:80
double scale() const
Return the scale.
Definition: Similarity3.h:191
Point2 t(10, 10)
Eigen::Block< Derived > topLeftCorner(MatrixBase< Derived > &m, int rows, int cols)
Definition: class_Block.cpp:8
std::pair< Point3, Point3 > Point3Pair
Definition: Point3.h:42


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