Rot3M.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/config.h> // Get GTSAM_USE_QUATERNIONS macro
22 
23 #ifndef GTSAM_USE_QUATERNIONS
24 
25 #include <gtsam/geometry/Rot3.h>
26 #include <gtsam/geometry/SO3.h>
27 #include <cmath>
28 
29 using namespace std;
30 
31 namespace gtsam {
32 
33 /* ************************************************************************* */
34 Rot3::Rot3() : rot_(I_3x3) {}
35 
36 /* ************************************************************************* */
37 Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) {
38  Matrix3 R;
39  R << col1, col2, col3;
40  rot_ = SO3(R);
41 }
42 
43 /* ************************************************************************* */
44 Rot3::Rot3(double R11, double R12, double R13, double R21, double R22,
45  double R23, double R31, double R32, double R33) {
46  Matrix3 R;
47  R << R11, R12, R13, R21, R22, R23, R31, R32, R33;
48  rot_ = SO3(R);
49 }
50 
51 /* ************************************************************************* */
53 }
54 
55 /* ************************************************************************* */
56 Rot3 Rot3::Rx(double t) {
57  double st = sin(t), ct = cos(t);
58  return Rot3(
59  1, 0, 0,
60  0, ct,-st,
61  0, st, ct);
62 }
63 
64 /* ************************************************************************* */
65 Rot3 Rot3::Ry(double t) {
66  double st = sin(t), ct = cos(t);
67  return Rot3(
68  ct, 0, st,
69  0, 1, 0,
70  -st, 0, ct);
71 }
72 
73 /* ************************************************************************* */
74 Rot3 Rot3::Rz(double t) {
75  double st = sin(t), ct = cos(t);
76  return Rot3(
77  ct,-st, 0,
78  st, ct, 0,
79  0, 0, 1);
80 }
81 
82 /* ************************************************************************* */
83 // Considerably faster than composing matrices above !
84 Rot3 Rot3::RzRyRx(double x, double y, double z, OptionalJacobian<3, 1> Hx,
86  double cx=cos(x),sx=sin(x);
87  double cy=cos(y),sy=sin(y);
88  double cz=cos(z),sz=sin(z);
89  double ss_ = sx * sy;
90  double cs_ = cx * sy;
91  double sc_ = sx * cy;
92  double cc_ = cx * cy;
93  double c_s = cx * sz;
94  double s_s = sx * sz;
95  double _cs = cy * sz;
96  double _cc = cy * cz;
97  double s_c = sx * cz;
98  double c_c = cx * cz;
99  double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz;
100  if (Hx) (*Hx) << 1, 0, 0;
101  if (Hy) (*Hy) << 0, cx, -sx;
102  if (Hz) (*Hz) << -sy, sc_, cc_;
103  return Rot3(
104  _cc,- c_s + ssc, s_s + csc,
105  _cs, c_c + sss, -s_c + css,
106  -sy, sc_, cc_
107  );
108 }
109 
110 /* ************************************************************************* */
113 
117 
118  Matrix3 rot = rot_.matrix(), rot_orth;
119 
120  // Check if determinant is already 1.
121  // If yes, then return the current Rot3.
122  if (std::fabs(rot.determinant()-1) < 1e-12) return Rot3(rot_);
123 
124  Vector3 x = rot.block<1, 3>(0, 0), y = rot.block<1, 3>(1, 0);
125  double error = x.dot(y);
126 
127  Vector3 x_ort = x - (error / 2) * y, y_ort = y - (error / 2) * x;
128  Vector3 z_ort = x_ort.cross(y_ort);
129 
130  rot_orth.block<1, 3>(0, 0) = 0.5 * (3 - x_ort.dot(x_ort)) * x_ort;
131  rot_orth.block<1, 3>(1, 0) = 0.5 * (3 - y_ort.dot(y_ort)) * y_ort;
132  rot_orth.block<1, 3>(2, 0) = 0.5 * (3 - z_ort.dot(z_ort)) * z_ort;
133 
134  return Rot3(rot_orth);
135 }
136 
137 /* ************************************************************************* */
138 Rot3 Rot3::operator*(const Rot3& R2) const {
139  return Rot3(rot_*R2.rot_);
140 }
141 
142 /* ************************************************************************* */
143 Matrix3 Rot3::transpose() const {
144  return rot_.matrix().transpose();
145 }
146 
147 /* ************************************************************************* */
150  if (H1) *H1 = rot_.matrix() * skewSymmetric(-p.x(), -p.y(), -p.z());
151  if (H2) *H2 = rot_.matrix();
152  return rot_.matrix() * p;
153 }
154 
155 /* ************************************************************************* */
156 // Log map at identity - return the canonical coordinates of this rotation
158  return SO3::Logmap(R.rot_,H);
159 }
160 
161 /* ************************************************************************* */
163  if (H) throw std::runtime_error("Rot3::CayleyChart::Retract Derivative");
164  const double x = omega(0), y = omega(1), z = omega(2);
165  const double x2 = x * x, y2 = y * y, z2 = z * z;
166  const double xy = x * y, xz = x * z, yz = y * z;
167  const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0 * f;
168  return Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f,
169  (xy + 2 * z) * _2f, (4 - x2 + y2 - z2) * f, (yz - 2 * x) * _2f,
170  (xz - 2 * y) * _2f, (yz + 2 * x) * _2f, (4 - x2 - y2 + z2) * f);
171 }
172 
173 /* ************************************************************************* */
175  if (H) throw std::runtime_error("Rot3::CayleyChart::Local Derivative");
176  // Create a fixed-size matrix
177  Matrix3 A = R.matrix();
178 
179  // Check if (A+I) is invertible. Same as checking for -1 eigenvalue.
180  if ((A + I_3x3).determinant() == 0.0) {
181  throw std::runtime_error("Rot3::CayleyChart::Local Invalid Rotation");
182  }
183 
184  // Mathematica closed form optimization.
185  // The following are the essential computations for the following algorithm
186  // 1. Compute the inverse of P = (A+I), using a closed-form formula since P is 3x3
187  // 2. Compute the Cayley transform C = 2 * P^{-1} * (A-I)
188  // 3. C is skew-symmetric, so we pick out the computations corresponding only to x, y, and z.
189  const double a = A(0, 0), b = A(0, 1), c = A(0, 2);
190  const double d = A(1, 0), e = A(1, 1), f = A(1, 2);
191  const double g = A(2, 0), h = A(2, 1), i = A(2, 2);
192  const double di = d * i, ce = c * e, cd = c * d, fg = f * g;
193  const double M = 1 + e - f * h + i + e * i;
194  const double K = -4.0 / (cd * h + M + a * M - g * (c + ce) - b * (d + di - fg));
195  const double x = a * f - cd + f;
196  const double y = b * f - ce - c;
197  const double z = fg - di - d;
198  return K * Vector3(x, y, z);
199 }
200 
201 /* ************************************************************************* */
204  if (mode == Rot3::EXPMAP) return Expmap(omega, H);
205  if (mode == Rot3::CAYLEY) return CayleyChart::Retract(omega, H);
206  else throw std::runtime_error("Rot3::Retract: unknown mode");
207 }
208 
209 /* ************************************************************************* */
212  if (mode == Rot3::EXPMAP) return Logmap(R, H);
213  if (mode == Rot3::CAYLEY) return CayleyChart::Local(R, H);
214  else throw std::runtime_error("Rot3::Local: unknown mode");
215 }
216 
217 /* ************************************************************************* */
218 Matrix3 Rot3::matrix() const {
219  return rot_.matrix();
220 }
221 
222 /* ************************************************************************* */
223 Point3 Rot3::r1() const { return Point3(rot_.matrix().col(0)); }
224 
225 /* ************************************************************************* */
226 Point3 Rot3::r2() const { return Point3(rot_.matrix().col(1)); }
227 
228 /* ************************************************************************* */
229 Point3 Rot3::r3() const { return Point3(rot_.matrix().col(2)); }
230 
231 /* ************************************************************************* */
233  return gtsam::Quaternion(rot_.matrix());
234 }
235 
236 /* ************************************************************************* */
237 
238 } // namespace gtsam
239 
240 #endif
int EIGEN_BLAS_FUNC() rot(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy, RealScalar *pc, RealScalar *ps)
SO3 rot_
Definition: Rot3.h:65
static Rot3 RzRyRx(double x, double y, double z, OptionalJacobian< 3, 1 > Hx={}, OptionalJacobian< 3, 1 > Hy={}, OptionalJacobian< 3, 1 > Hz={})
Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis.
Definition: Rot3M.cpp:84
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
Point3 r1() const
first column
Definition: Rot3M.cpp:223
Scalar * y
static const Unit3 z2
Eigen::Vector3d Vector3
Definition: Vector.h:43
void determinant(const MatrixType &m)
Definition: determinant.cpp:14
Use the Lie group exponential map to retract.
Definition: Rot3.h:340
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
Rot2 R(Rot2::fromAngle(0.1))
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
Rot3 operator*(const Rot3 &R2) const
Syntatic sugar for composing two rotations.
Definition: Rot3M.cpp:138
static Rot3 Retract(const Vector3 &v, ChartJacobian H={})
Definition: Rot3M.cpp:202
const double cx
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
Definition: BFloat16.h:88
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
Matrix< float, 2, 1 > xy
Definition: LLT_solve.cpp:7
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
3*3 matrix representation of SO(3)
Real fabs(const Real &a)
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
void g(const string &key, int i)
Definition: testBTree.cpp:41
static Rot3 Retract(const Vector3 &v, OptionalJacobian< 3, 3 > H={})
Definition: Rot3M.cpp:162
Point3 r2() const
second column
Definition: Rot3M.cpp:226
static Vector3 Local(const Rot3 &r, OptionalJacobian< 3, 3 > H={})
Definition: Rot3M.cpp:174
Retract and localCoordinates using the Cayley transform.
Definition: Rot3.h:342
static Rot3 Rx(double t)
Rotation around X axis as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when look...
Definition: Rot3M.cpp:56
SO< 3 > SO3
Definition: SO3.h:34
static EIGEN_DEVICE_FUNC Matrix< Scalar, 2, 2 > toRotationMatrix(const Scalar &s)
Definition: RotationBase.h:182
static Vector3 Logmap(const Rot3 &R, OptionalJacobian< 3, 3 > H={})
Definition: Rot3M.cpp:157
const MatrixNN & matrix() const
Return matrix.
Definition: SOn.h:157
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
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.)
EIGEN_DEVICE_FUNC const Scalar & q
Rot3 normalized() const
Definition: Rot3M.cpp:111
#define ROT3_DEFAULT_COORDINATES_MODE
Definition: Rot3.h:43
const G & b
Definition: Group.h:86
traits
Definition: chartTesting.h:28
gtsam::Quaternion toQuaternion() const
Definition: Rot3M.cpp:232
const double h
Matrix3 skewSymmetric(double wx, double wy, double wz)
Definition: base/Matrix.h:400
static TangentVector Logmap(const SO &R, ChartJacobian H={})
Definition: SOn-inl.h:77
static Rot3 Rz(double t)
Rotation around Z axis as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when look...
Definition: Rot3M.cpp:74
The quaternion class used to represent 3D orientations and rotations.
const double cy
Matrix3 matrix() const
Definition: Rot3M.cpp:218
float * p
static double error
Definition: testRot3.cpp:37
static const DiscreteKey mode(modeKey, 2)
Vector3 Point3
Definition: Point3.h:38
static Vector3 Local(const Rot3 &r, ChartJacobian H={})
Definition: Rot3M.cpp:210
Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Definition: Rot3M.cpp:148
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
Matrix3 transpose() const
Definition: Rot3M.cpp:143
Point2 t(10, 10)
static Rot3 Ry(double t)
Rotation around Y axis as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when look...
Definition: Rot3M.cpp:65
Point3 r3() const
third column
Definition: Rot3M.cpp:229
CoordinatesMode
Definition: Rot3.h:339
3D rotation represented as a rotation matrix or quaternion
Eigen::Quaternion< double, Eigen::DontAlign > Quaternion


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