SO3.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 
21 #include <gtsam/base/concepts.h>
22 #include <gtsam/geometry/SO3.h>
23 
24 #include <Eigen/SVD>
25 
26 #include <cmath>
27 #include <iostream>
28 #include <limits>
29 
30 namespace gtsam {
31 
32 //******************************************************************************
33 namespace so3 {
34 
35 GTSAM_EXPORT Matrix99 Dcompose(const SO3& Q) {
36  Matrix99 H;
37  auto R = Q.matrix();
38  H << I_3x3 * R(0, 0), I_3x3 * R(1, 0), I_3x3 * R(2, 0), //
39  I_3x3 * R(0, 1), I_3x3 * R(1, 1), I_3x3 * R(2, 1), //
40  I_3x3 * R(0, 2), I_3x3 * R(1, 2), I_3x3 * R(2, 2);
41  return H;
42 }
43 
44 GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, OptionalJacobian<9, 9> H) {
45  Matrix3 MR = M * R.matrix();
46  if (H) *H = Dcompose(R);
47  return MR;
48 }
49 
50 void ExpmapFunctor::init(bool nearZeroApprox) {
51  nearZero =
53  if (!nearZero) {
55  const double s2 = std::sin(theta / 2.0);
56  one_minus_cos = 2.0 * s2 * s2; // numerically better than [1 - cos(theta)]
57  }
58 }
59 
60 ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox)
61  : theta2(omega.dot(omega)), theta(std::sqrt(theta2)) {
62  const double wx = omega.x(), wy = omega.y(), wz = omega.z();
63  W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
64  init(nearZeroApprox);
65  if (!nearZero) {
66  K = W / theta;
67  KK = K * K;
68  }
69 }
70 
71 ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle,
72  bool nearZeroApprox)
73  : theta2(angle * angle), theta(angle) {
74  const double ax = axis.x(), ay = axis.y(), az = axis.z();
75  K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0;
76  W = K * angle;
77  init(nearZeroApprox);
78  if (!nearZero) {
79  KK = K * K;
80  }
81 }
82 
84  if (nearZero)
85  return SO3(I_3x3 + W);
86  else
87  return SO3(I_3x3 + sin_theta * K + one_minus_cos * KK);
88 }
89 
90 DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
91  : ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
92  if (nearZero) {
93  dexp_ = I_3x3 - 0.5 * W;
94  } else {
95  a = one_minus_cos / theta;
96  b = 1.0 - sin_theta / theta;
97  dexp_ = I_3x3 - a * K + b * KK;
98  }
99 }
100 
102  OptionalJacobian<3, 3> H2) const {
103  if (H1) {
104  if (nearZero) {
105  *H1 = 0.5 * skewSymmetric(v);
106  } else {
107  // TODO(frank): Iserles hints that there should be a form I + c*K + d*KK
108  const Vector3 Kv = K * v;
109  const double Da = (sin_theta - 2.0 * a) / theta2;
110  const double Db = (one_minus_cos - 3.0 * b) / theta2;
111  *H1 = (Db * K - Da * I_3x3) * Kv * omega.transpose() -
112  skewSymmetric(Kv * b / theta) +
113  (a * I_3x3 - b * K) * skewSymmetric(v / theta);
114  }
115  }
116  if (H2) *H2 = dexp_;
117  return dexp_ * v;
118 }
119 
121  OptionalJacobian<3, 3> H2) const {
122  const Matrix3 invDexp = dexp_.inverse();
123  const Vector3 c = invDexp * v;
124  if (H1) {
125  Matrix3 D_dexpv_omega;
126  applyDexp(c, D_dexpv_omega); // get derivative H of forward mapping
127  *H1 = -invDexp * D_dexpv_omega;
128  }
129  if (H2) *H2 = invDexp;
130  return c;
131 }
132 
133 } // namespace so3
134 
135 //******************************************************************************
136 template <>
137 GTSAM_EXPORT
138 SO3 SO3::AxisAngle(const Vector3& axis, double theta) {
139  return so3::ExpmapFunctor(axis, theta).expmap();
140 }
141 
142 //******************************************************************************
143 template <>
144 GTSAM_EXPORT
145 SO3 SO3::ClosestTo(const Matrix3& M) {
147  const auto& U = svd.matrixU();
148  const auto& V = svd.matrixV();
149  const double det = (U * V.transpose()).determinant();
150  return SO3(U * Vector3(1, 1, det).asDiagonal() * V.transpose());
151 }
152 
153 //******************************************************************************
154 template <>
155 GTSAM_EXPORT
156 SO3 SO3::ChordalMean(const std::vector<SO3>& rotations) {
157  // See Hartley13ijcv:
158  // Cost function C(R) = \sum sqr(|R-R_i|_F)
159  // Closed form solution = ClosestTo(C_e), where C_e = \sum R_i !!!!
160  Matrix3 C_e{Z_3x3};
161  for (const auto& R_i : rotations) {
162  C_e += R_i.matrix();
163  }
164  return ClosestTo(C_e);
165 }
166 
167 //******************************************************************************
168 template <>
169 GTSAM_EXPORT
170 Matrix3 SO3::Hat(const Vector3& xi) {
171  // skew symmetric matrix X = xi^
172  Matrix3 Y = Z_3x3;
173  Y(0, 1) = -xi(2);
174  Y(0, 2) = +xi(1);
175  Y(1, 2) = -xi(0);
176  return Y - Y.transpose();
177 }
178 
179 //******************************************************************************
180 template <>
181 GTSAM_EXPORT
182 Vector3 SO3::Vee(const Matrix3& X) {
183  Vector3 xi;
184  xi(0) = -X(1, 2);
185  xi(1) = +X(0, 2);
186  xi(2) = -X(0, 1);
187  return xi;
188 }
189 
190 //******************************************************************************
191 template <>
192 GTSAM_EXPORT
193 Matrix3 SO3::AdjointMap() const {
194  return matrix_;
195 }
196 
197 //******************************************************************************
198 template <>
199 GTSAM_EXPORT
200 SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
201  if (H) {
202  so3::DexpFunctor impl(omega);
203  *H = impl.dexp();
204  return impl.expmap();
205  } else {
206  return so3::ExpmapFunctor(omega).expmap();
207  }
208 }
209 
210 template <>
211 GTSAM_EXPORT
212 Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
213  return so3::DexpFunctor(omega).dexp();
214 }
215 
216 //******************************************************************************
217 /* Right Jacobian for Log map in SO(3) - equation (10.86) and following
218  equations in G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie
219  Groups", Volume 2, 2008.
220 
221  logmap( Rhat * expmap(omega) ) \approx logmap(Rhat) + Jrinv * omega
222 
223  where Jrinv = LogmapDerivative(omega). This maps a perturbation on the
224  manifold (expmap(omega)) to a perturbation in the tangent space (Jrinv *
225  omega)
226  */
227 template <>
228 GTSAM_EXPORT
229 Matrix3 SO3::LogmapDerivative(const Vector3& omega) {
230  using std::cos;
231  using std::sin;
232 
233  double theta2 = omega.dot(omega);
234  if (theta2 <= std::numeric_limits<double>::epsilon()) return I_3x3;
235  double theta = std::sqrt(theta2); // rotation angle
236 
237  // element of Lie algebra so(3): W = omega^
238  const Matrix3 W = Hat(omega);
239  return I_3x3 + 0.5 * W +
240  (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) *
241  W * W;
242 }
243 
244 //******************************************************************************
245 template <>
246 GTSAM_EXPORT
247 Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
248  using std::sin;
249  using std::sqrt;
250 
251  // note switch to base 1
252  const Matrix3& R = Q.matrix();
253  const double &R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2);
254  const double &R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2);
255  const double &R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2);
256 
257  // Get trace(R)
258  const double tr = R.trace();
259 
260  Vector3 omega;
261 
262  // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
263  // we do something special
264  if (tr + 1.0 < 1e-3) {
265  if (R33 > R22 && R33 > R11) {
266  // R33 is the largest diagonal, a=3, b=1, c=2
267  const double W = R21 - R12;
268  const double Q1 = 2.0 + 2.0 * R33;
269  const double Q2 = R31 + R13;
270  const double Q3 = R23 + R32;
271  const double r = sqrt(Q1);
272  const double one_over_r = 1 / r;
273  const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W);
274  const double sgn_w = W < 0 ? -1.0 : 1.0;
275  const double mag = M_PI - (2 * sgn_w * W) / norm;
276  const double scale = 0.5 * one_over_r * mag;
277  omega = sgn_w * scale * Vector3(Q2, Q3, Q1);
278  } else if (R22 > R11) {
279  // R22 is the largest diagonal, a=2, b=3, c=1
280  const double W = R13 - R31;
281  const double Q1 = 2.0 + 2.0 * R22;
282  const double Q2 = R23 + R32;
283  const double Q3 = R12 + R21;
284  const double r = sqrt(Q1);
285  const double one_over_r = 1 / r;
286  const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W);
287  const double sgn_w = W < 0 ? -1.0 : 1.0;
288  const double mag = M_PI - (2 * sgn_w * W) / norm;
289  const double scale = 0.5 * one_over_r * mag;
290  omega = sgn_w * scale * Vector3(Q3, Q1, Q2);
291  } else {
292  // R11 is the largest diagonal, a=1, b=2, c=3
293  const double W = R32 - R23;
294  const double Q1 = 2.0 + 2.0 * R11;
295  const double Q2 = R12 + R21;
296  const double Q3 = R31 + R13;
297  const double r = sqrt(Q1);
298  const double one_over_r = 1 / r;
299  const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W);
300  const double sgn_w = W < 0 ? -1.0 : 1.0;
301  const double mag = M_PI - (2 * sgn_w * W) / norm;
302  const double scale = 0.5 * one_over_r * mag;
303  omega = sgn_w * scale * Vector3(Q1, Q2, Q3);
304  }
305  } else {
306  double magnitude;
307  const double tr_3 = tr - 3.0; // could be non-negative if the matrix is off orthogonal
308  if (tr_3 < -1e-6) {
309  // this is the normal case -1 < trace < 3
310  double theta = acos((tr - 1.0) / 2.0);
311  magnitude = theta / (2.0 * sin(theta));
312  } else {
313  // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
314  // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2)
315  // see https://github.com/borglab/gtsam/issues/746 for details
316  magnitude = 0.5 - tr_3 / 12.0 + tr_3*tr_3/60.0;
317  }
318  omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
319  }
320 
321  if (H) *H = LogmapDerivative(omega);
322  return omega;
323 }
324 
325 //******************************************************************************
326 // Chart at origin for SO3 is *not* Cayley but actual Expmap/Logmap
327 
328 template <>
329 GTSAM_EXPORT
330 SO3 SO3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H) {
331  return Expmap(omega, H);
332 }
333 
334 template <>
335 GTSAM_EXPORT
336 Vector3 SO3::ChartAtOrigin::Local(const SO3& R, ChartJacobian H) {
337  return Logmap(R, H);
338 }
339 
340 //******************************************************************************
341 // local vectorize
342 static Vector9 vec3(const Matrix3& R) {
343  return Eigen::Map<const Vector9>(R.data());
344 }
345 
346 // so<3> generators
347 static std::vector<Matrix3> G3({SO3::Hat(Vector3::Unit(0)),
348  SO3::Hat(Vector3::Unit(1)),
349  SO3::Hat(Vector3::Unit(2))});
350 
351 // vectorized generators
352 static const Matrix93 P3 =
353  (Matrix93() << vec3(G3[0]), vec3(G3[1]), vec3(G3[2])).finished();
354 
355 //******************************************************************************
356 template <>
357 GTSAM_EXPORT
358 Vector9 SO3::vec(OptionalJacobian<9, 3> H) const {
359  const Matrix3& R = matrix_;
360  if (H) {
361  // As Luca calculated (for SO4), this is (I3 \oplus R) * P3
362  *H << R * P3.block<3, 3>(0, 0), R * P3.block<3, 3>(3, 0),
363  R * P3.block<3, 3>(6, 0);
364  }
365  return gtsam::vec3(R);
366 }
367 //******************************************************************************
368 
369 } // end namespace gtsam
static MatrixNN Hat(const TangentVector &xi)
Definition: SOn-inl.h:29
const char Y
static SO ChordalMean(const std::vector< SO > &rotations)
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
GTSAM_EXPORT Vector3 applyDexp(const Vector3 &v, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Multiplies with dexp(), with optional derivatives.
Definition: SO3.cpp:101
static SO AxisAngle(const Vector3 &axis, double theta)
Constructor from axis and angle. Only defined for SO3.
Eigen::Vector3d Vector3
Definition: Vector.h:43
double dot(const V1 &a, const V2 &b)
Definition: Vector.h:195
static MatrixDD ExpmapDerivative(const TangentVector &omega)
Derivative of Expmap, currently only defined for SO3.
Definition: SOn-inl.h:72
A matrix or vector expression mapping an existing array of data.
Definition: Map.h:94
void determinant(const MatrixType &m)
Definition: determinant.cpp:14
ExpmapFunctor(const Vector3 &omega, bool nearZeroApprox=false)
Constructor with element of Lie algebra so(3)
Definition: SO3.cpp:60
static SO Expmap(const TangentVector &omega, ChartJacobian H={})
Definition: SOn-inl.h:67
static MatrixDD LogmapDerivative(const TangentVector &omega)
Derivative of Logmap, currently only defined for SO3.
Definition: SOn-inl.h:82
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
#define M_PI
Definition: main.h:106
Rot2 R(Rot2::fromAngle(0.1))
Jet< T, N > acos(const Jet< T, N > &f)
Definition: jet.h:432
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
Pose2_ Expmap(const Vector3_ &xi)
const Vector3 omega
Definition: SO3.h:158
Definition: BFloat16.h:88
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
GTSAM_EXPORT DexpFunctor(const Vector3 &omega, bool nearZeroApprox=false)
Constructor with element of Lie algebra so(3)
Definition: SO3.cpp:90
3*3 matrix representation of SO(3)
static const Matrix93 P3
Definition: SO3.cpp:352
Functor implementing Exponential map.
Definition: SO3.h:136
static double epsilon
Definition: testRot3.cpp:37
SO< 3 > SO3
Definition: SO3.h:34
void init(bool nearZeroApprox=false)
Definition: SO3.cpp:50
const Matrix3 & dexp() const
Definition: SO3.h:172
GTSAM_EXPORT Matrix99 Dcompose(const SO3 &Q)
(constant) Jacobian of compose wrpt M
Definition: SO3.cpp:35
Eigen::Matrix< double, 9, 3 > Matrix93
const MatrixNN & matrix() const
Return matrix.
Definition: SOn.h:157
Array< int, Dynamic, 1 > v
Vector::Scalar omega(const Vector &t, const Vector &s, RealScalar angle)
Definition: IDRS.h:36
Array< double, 1, 3 > e(1./3., 0.5, 2.)
const double theta2
Definition: SO3.h:138
void svd(const Matrix &A, Matrix &U, Vector &S, Matrix &V)
Definition: Matrix.cpp:558
Vector xi
Definition: testPose2.cpp:148
traits
Definition: chartTesting.h:28
Matrix3 skewSymmetric(double wx, double wy, double wz)
Definition: base/Matrix.h:400
GTSAM_EXPORT Vector3 applyInvDexp(const Vector3 &v, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Multiplies with dexp().inverse(), with optional derivatives.
Definition: SO3.cpp:120
static TangentVector Logmap(const SO &R, ChartJacobian H={})
Definition: SOn-inl.h:77
MatrixDD AdjointMap() const
Adjoint map.
Definition: SO4.cpp:159
Functor that implements Exponential map and its derivatives.
Definition: SO3.h:157
static std::vector< Matrix3 > G3({SO3::Hat(Vector3::Unit(0)), SO3::Hat(Vector3::Unit(1)), SO3::Hat(Vector3::Unit(2))})
The quaternion class used to represent 3D orientations and rotations.
static double scale(double x, double a, double b, double t1, double t2)
Scale x from [a, b] to [t1, t2].
Definition: Chebyshev.cpp:35
Two-sided Jacobi SVD decomposition of a rectangular matrix.
static Vector9 vec3(const Matrix3 &R)
Definition: SO3.cpp:342
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
#define X
Definition: icosphere.cpp:20
SO3 expmap() const
Rodrigues formula.
Definition: SO3.cpp:83
static SO ClosestTo(const MatrixNN &M)
static TangentVector Vee(const MatrixNN &X)
Inverse of Hat. See note about xi element order in Hat.
Definition: SOn-inl.h:35
GTSAM_EXPORT Matrix3 compose(const Matrix3 &M, const SO3 &R, OptionalJacobian< 9, 9 > H)
Definition: SO3.cpp:44
VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H={}) const
Definition: SOn-inl.h:88


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