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


gtsam
Author(s):
autogenerated on Sun Feb 16 2025 04:03:11