SO4.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010-2019, 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 
19 #include <gtsam/base/concepts.h>
20 #include <gtsam/base/timing.h>
21 #include <gtsam/geometry/SO4.h>
22 #include <gtsam/geometry/Unit3.h>
23 
24 #include <Eigen/Eigenvalues>
25 
26 #include <algorithm>
27 #include <cmath>
28 #include <iostream>
29 #include <random>
30 #include <vector>
31 
32 using namespace std;
33 
34 namespace gtsam {
35 
36 // TODO(frank): is this better than SOn::Random?
37 // /* *************************************************************************
38 // */ static Vector3 randomOmega(std::mt19937 &rng) {
39 // static std::uniform_real_distribution<double> randomAngle(-M_PI, M_PI);
40 // return Unit3::Random(rng).unitVector() * randomAngle(rng);
41 // }
42 
43 // /* *************************************************************************
44 // */
45 // // Create random SO(4) element using direct product of lie algebras.
46 // SO4 SO4::Random(std::mt19937 &rng) {
47 // Vector6 delta;
48 // delta << randomOmega(rng), randomOmega(rng);
49 // return SO4::Expmap(delta);
50 // }
51 
52 //******************************************************************************
53 template <>
54 GTSAM_EXPORT
55 Matrix4 SO4::Hat(const Vector6& xi) {
56  // skew symmetric matrix X = xi^
57  // Unlike Luca, makes upper-left the SO(3) subgroup.
58  Matrix4 Y = Z_4x4;
59  Y(0, 1) = -xi(5);
60  Y(0, 2) = +xi(4);
61  Y(1, 2) = -xi(3);
62  Y(0, 3) = +xi(2);
63  Y(1, 3) = -xi(1);
64  Y(2, 3) = +xi(0);
65  return Y - Y.transpose();
66 }
67 
68 //******************************************************************************
69 template <>
70 GTSAM_EXPORT
71 Vector6 SO4::Vee(const Matrix4& X) {
72  Vector6 xi;
73  xi(5) = -X(0, 1);
74  xi(4) = +X(0, 2);
75  xi(3) = -X(1, 2);
76  xi(2) = +X(0, 3);
77  xi(1) = -X(1, 3);
78  xi(0) = +X(2, 3);
79  return xi;
80 }
81 
82 //******************************************************************************
83 /* Exponential map, porting MATLAB implementation by Luca, which follows
84  * "SOME REMARKS ON THE EXPONENTIAL MAP ON THE GROUPS SO(n) AND SE(n)" by
85  * Ramona-Andreaa Rohan */
86 template <>
87 GTSAM_EXPORT
88 SO4 SO4::Expmap(const Vector6& xi, ChartJacobian H) {
89  using namespace std;
90  if (H) throw std::runtime_error("SO4::Expmap Jacobian");
91 
92  // skew symmetric matrix X = xi^
93  const Matrix4 X = Hat(xi);
94 
95  // do eigen-decomposition
97  Eigen::Vector4cd e = eig.eigenvalues();
98  using std::abs;
99  sort(e.data(), e.data() + 4, [](complex<double> a, complex<double> b) {
100  return abs(a.imag()) > abs(b.imag());
101  });
102 
103  // Get a and b from eigenvalues +/i ai and +/- bi
104  double a = e[0].imag(), b = e[2].imag();
105  if (!e.real().isZero() || e[1].imag() != -a || e[3].imag() != -b) {
106  throw runtime_error("SO4::Expmap: wrong eigenvalues.");
107  }
108 
109  // Build expX = exp(xi^)
110  Matrix4 expX;
111  using std::cos;
112  using std::sin;
113  const auto X2 = X * X;
114  const auto X3 = X2 * X;
115  double a2 = a * a, a3 = a2 * a, b2 = b * b, b3 = b2 * b;
116  if (a != 0 && b == 0) {
117  double c2 = (1 - cos(a)) / a2, c3 = (a - sin(a)) / a3;
118  return SO4(I_4x4 + X + c2 * X2 + c3 * X3);
119  } else if (a == b && b != 0) {
120  double sin_a = sin(a), cos_a = cos(a);
121  double c0 = (a * sin_a + 2 * cos_a) / 2,
122  c1 = (3 * sin_a - a * cos_a) / (2 * a), c2 = sin_a / (2 * a),
123  c3 = (sin_a - a * cos_a) / (2 * a3);
124  return SO4(c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3);
125  } else if (a != b) {
126  double sin_a = sin(a), cos_a = cos(a);
127  double sin_b = sin(b), cos_b = cos(b);
128  double c0 = (b2 * cos_a - a2 * cos_b) / (b2 - a2),
129  c1 = (b3 * sin_a - a3 * sin_b) / (a * b * (b2 - a2)),
130  c2 = (cos_a - cos_b) / (b2 - a2),
131  c3 = (b * sin_a - a * sin_b) / (a * b * (b2 - a2));
132  return SO4(c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3);
133  } else {
134  return SO4();
135  }
136 }
137 
138 //******************************************************************************
139 // local vectorize
140 static SO4::VectorN2 vec4(const Matrix4& Q) {
141  return Eigen::Map<const SO4::VectorN2>(Q.data());
142 }
143 
144 // so<4> generators
145 static std::vector<Matrix4, Eigen::aligned_allocator<Matrix4> > G4(
146  {SO4::Hat(Vector6::Unit(0)), SO4::Hat(Vector6::Unit(1)),
147  SO4::Hat(Vector6::Unit(2)), SO4::Hat(Vector6::Unit(3)),
148  SO4::Hat(Vector6::Unit(4)), SO4::Hat(Vector6::Unit(5))});
149 
150 // vectorized generators
152  (Eigen::Matrix<double, 16, 6>() << vec4(G4[0]), vec4(G4[1]), vec4(G4[2]),
153  vec4(G4[3]), vec4(G4[4]), vec4(G4[5]))
154  .finished();
155 
156 //******************************************************************************
157 template <>
158 GTSAM_EXPORT
159 Matrix6 SO4::AdjointMap() const {
160  // Elaborate way of calculating the AdjointMap
161  // TODO(frank): find a closed form solution. In SO(3) is just R :-/
162  const Matrix4& Q = matrix_;
163  const Matrix4 Qt = Q.transpose();
164  Matrix6 A;
165  for (size_t i = 0; i < 6; i++) {
166  // Calculate column i of linear map for coeffcient of Gi
167  A.col(i) = SO4::Vee(Q * G4[i] * Qt);
168  }
169  return A;
170 }
171 
172 //******************************************************************************
173 template <>
174 GTSAM_EXPORT
175 SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) const {
176  const Matrix& Q = matrix_;
177  if (H) {
178  // As Luca calculated, this is (I4 \oplus Q) * P4
179  *H << Q * P4.block<4, 6>(0, 0), Q * P4.block<4, 6>(4, 0),
180  Q * P4.block<4, 6>(8, 0), Q * P4.block<4, 6>(12, 0);
181  }
182  return gtsam::vec4(Q);
183 }
184 
186 template <>
187 GTSAM_EXPORT
188 SO4 SO4::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian H) {
189  if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian");
190  gttic(SO4_Retract);
191  const Matrix4 X = Hat(xi / 2);
192  return SO4((I_4x4 + X) * (I_4x4 - X).inverse());
193 }
194 
195 //******************************************************************************
196 template <>
197 GTSAM_EXPORT
198 Vector6 SO4::ChartAtOrigin::Local(const SO4& Q, ChartJacobian H) {
199  if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian");
200  const Matrix4& R = Q.matrix();
201  const Matrix4 X = (I_4x4 - R) * (I_4x4 + R).inverse();
202  return -2 * Vee(X);
203 }
204 
205 //******************************************************************************
206 GTSAM_EXPORT Matrix3 topLeft(const SO4& Q, OptionalJacobian<9, 6> H) {
207  const Matrix4& R = Q.matrix();
208  const Matrix3 M = R.topLeftCorner<3, 3>();
209  if (H) {
210  const Vector3 m1 = M.col(0), m2 = M.col(1), m3 = M.col(2),
211  q = R.topRightCorner<3, 1>();
212  *H << Z_3x1, Z_3x1, -q, Z_3x1, -m3, m2, //
213  Z_3x1, q, Z_3x1, m3, Z_3x1, -m1, //
214  -q, Z_3x1, Z_3x1, -m2, m1, Z_3x1;
215  }
216  return M;
217 }
218 
219 //******************************************************************************
220 GTSAM_EXPORT Matrix43 stiefel(const SO4& Q, OptionalJacobian<12, 6> H) {
221  const Matrix4& R = Q.matrix();
222  const Matrix43 M = R.leftCols<3>();
223  if (H) {
224  const auto &m1 = R.col(0), m2 = R.col(1), m3 = R.col(2), q = R.col(3);
225  *H << Z_4x1, Z_4x1, -q, Z_4x1, -m3, m2, //
226  Z_4x1, q, Z_4x1, m3, Z_4x1, -m1, //
227  -q, Z_4x1, Z_4x1, -m2, m1, Z_4x1;
228  }
229  return M;
230 }
231 
232 //******************************************************************************
233 
234 } // end namespace gtsam
gtsam::topLeft
GTSAM_EXPORT Matrix3 topLeft(const SO4 &Q, OptionalJacobian< 9, 6 > H)
Definition: SO4.cpp:206
timing.h
Timing utilities.
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::utils.numerical_derivative.X2
X2
Definition: numerical_derivative.py:26
P4
static double P4[]
Definition: jv.c:570
gtsam::SO4
SO< 4 > SO4
Definition: SO4.h:34
inverse
const EIGEN_DEVICE_FUNC InverseReturnType inverse() const
Definition: ArrayCwiseUnaryOps.h:411
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::stiefel
GTSAM_EXPORT Matrix43 stiefel(const SO4 &Q, OptionalJacobian< 12, 6 > H)
Definition: SO4.cpp:220
screwPose2::xi
Vector xi
Definition: testPose2.cpp:148
ceres::sin
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
gtsam::Z_3x1
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Definition: Vector.h:46
b
Scalar * b
Definition: benchVecAdd.cpp:17
gtsam::Y
GaussianFactorGraphValuePair Y
Definition: HybridGaussianProductFactor.cpp:29
simple_graph::b2
Vector2 b2(4, -5)
m1
Matrix3d m1
Definition: IOFormat.cpp:2
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
X
#define X
Definition: icosphere.cpp:20
concepts.h
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
SO4.h
4*4 matrix representation of SO(4)
gtsam::utils.numerical_derivative.X3
X3
Definition: numerical_derivative.py:27
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
Unit3.h
c1
static double c1
Definition: airy.c:54
m2
MatrixType m2(n_dims)
Expmap
Pose2_ Expmap(const Vector3_ &xi)
Definition: InverseKinematicsExampleExpressions.cpp:47
align_3::a3
Point2 a3
Definition: testPose2.cpp:771
Eigen::numext::q
EIGEN_DEVICE_FUNC const Scalar & q
Definition: SpecialFunctionsImpl.h:1984
gtsam::vec4
static SO4::VectorN2 vec4(const Matrix4 &Q)
Definition: SO4.cpp:140
eig
SelfAdjointEigenSolver< PlainMatrixType > eig(mat, computeVectors?ComputeEigenvectors:EigenvaluesOnly)
simple_graph::b3
Vector2 b3(3, -6)
Eigen::Map
A matrix or vector expression mapping an existing array of data.
Definition: Map.h:94
m3
static const DiscreteKey m3(M(3), 2)
Eigen::Quaternion
The quaternion class used to represent 3D orientations and rotations.
Definition: ForwardDeclarations.h:293
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
gtsam
traits
Definition: SFMdata.h:40
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
align_3::a2
Point2 a2
Definition: testPose2.cpp:770
std
Definition: BFloat16.h:88
c2
static double c2
Definition: airy.c:55
Eigen::EigenSolver
Computes eigenvalues and eigenvectors of general matrices.
Definition: EigenSolver.h:64
gtsam::SO
Definition: SOn.h:54
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
abs
#define abs(x)
Definition: datatypes.h:17
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::G4
static std::vector< Matrix4, Eigen::aligned_allocator< Matrix4 > > G4({SO4::Hat(Vector6::Unit(0)), SO4::Hat(Vector6::Unit(1)), SO4::Hat(Vector6::Unit(2)), SO4::Hat(Vector6::Unit(3)), SO4::Hat(Vector6::Unit(4)), SO4::Hat(Vector6::Unit(5))})
R
Rot2 R(Rot2::fromAngle(0.1))
gttic
#define gttic(label)
Definition: timing.h:295
complex
Definition: datatypes.h:12
M
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51


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