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
gtsam::SO3
SO< 3 > SO3
Definition: SO3.h:34
gtsam::Rot3::normalized
Rot3 normalized() const
Definition: Rot3M.cpp:111
gtsam::Rot3::r2
Point3 r2() const
second column
Definition: Rot3M.cpp:226
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::Quaternion
Eigen::Quaternion< double, Eigen::DontAlign > Quaternion
Definition: geometry/Quaternion.h:182
gtsam::Rot3::ChartAtOrigin::Retract
static Rot3 Retract(const Vector3 &v, ChartJacobian H={})
Definition: Rot3M.cpp:202
Eigen::internal::toRotationMatrix
static EIGEN_DEVICE_FUNC Matrix< Scalar, 2, 2 > toRotationMatrix(const Scalar &s)
Definition: RotationBase.h:182
cy
const double cy
Definition: testSmartStereoFactor_iSAM2.cpp:50
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
d
static const double d[K][N]
Definition: igam.h:11
ceres::sin
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
xy
Matrix< float, 2, 1 > xy
Definition: LLT_solve.cpp:7
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
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::Rot3::operator*
Rot3 operator*(const Rot3 &R2) const
Syntatic sugar for composing two rotations.
Definition: Rot3M.cpp:138
cd
static double cd[7]
Definition: fresnl.c:92
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
h
const double h
Definition: testSimpleHelicopter.cpp:19
gtsam::skewSymmetric
Matrix3 skewSymmetric(double wx, double wy, double wz)
Definition: base/Matrix.h:400
gtsam::Rot3::r3
Point3 r3() const
third column
Definition: Rot3M.cpp:229
rot
int EIGEN_BLAS_FUNC() rot(int *n, RealScalar *px, int *incx, RealScalar *py, int *incy, RealScalar *pc, RealScalar *ps)
Definition: level1_real_impl.h:79
determinant
void determinant(const MatrixType &m)
Definition: determinant.cpp:14
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
Rot3.h
3D rotation represented as a rotation matrix or quaternion
boost::multiprecision::fabs
Real fabs(const Real &a)
Definition: boostmultiprec.cpp:119
SO3.h
3*3 matrix representation of SO(3)
gtsam::Rot3::ChartAtOrigin::Local
static Vector3 Local(const Rot3 &r, ChartJacobian H={})
Definition: Rot3M.cpp:210
gtsam::Rot3::rotate
Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Definition: Rot3M.cpp:148
gtsam::Rot3::Rot3
Rot3()
Definition: Rot3M.cpp:34
Eigen::numext::q
EIGEN_DEVICE_FUNC const Scalar & q
Definition: SpecialFunctionsImpl.h:1984
gtsam::SO::matrix
const MatrixNN & matrix() const
Return matrix.
Definition: SOn.h:157
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::Rot3::CayleyChart::Local
static Vector3 Local(const Rot3 &r, OptionalJacobian< 3, 3 > H={})
Definition: Rot3M.cpp:174
gtsam::Rot3::toQuaternion
gtsam::Quaternion toQuaternion() const
Definition: Rot3M.cpp:232
gtsam::Rot3::CayleyChart::Retract
static Rot3 Retract(const Vector3 &v, OptionalJacobian< 3, 3 > H={})
Definition: Rot3M.cpp:162
gtsam::Rot3::r1
Point3 r1() const
first column
Definition: Rot3M.cpp:223
gtsam::Rot3::Ry
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
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
Eigen::internal::omega
Vector::Scalar omega(const Vector &t, const Vector &s, RealScalar angle)
Definition: IDRS.h:36
gtsam::SO< 3 >::Logmap
static TangentVector Logmap(const SO &R, ChartJacobian H={})
Definition: SOn-inl.h:77
g
void g(const string &key, int i)
Definition: testBTree.cpp:41
gtsam::Rot3::EXPMAP
@ EXPMAP
Use the Lie group exponential map to retract.
Definition: Rot3.h:340
y
Scalar * y
Definition: level1_cplx_impl.h:124
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
gtsam::b
const G & b
Definition: Group.h:79
gtsam::Rot3::Rx
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
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
so3::R2
SO3 R2
Definition: testShonanFactor.cpp:43
gtsam
traits
Definition: chartTesting.h:28
gtsam::Rot3::rot_
SO3 rot_
Definition: Rot3.h:65
error
static double error
Definition: testRot3.cpp:37
K
#define K
Definition: igam.h:8
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
std
Definition: BFloat16.h:88
gtsam::Rot3::Expmap
static Rot3 Expmap(const Vector3 &v, OptionalJacobian< 3, 3 > H={})
Definition: Rot3.h:374
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::Rot3::Rz
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
gtsam::Rot3::transpose
Matrix3 transpose() const
Definition: Rot3M.cpp:143
gtsam::Rot3::matrix
Matrix3 matrix() const
Definition: Rot3M.cpp:218
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::Rot3::CoordinatesMode
CoordinatesMode
Definition: Rot3.h:339
gtsam::Rot3::Logmap
static Vector3 Logmap(const Rot3 &R, OptionalJacobian< 3, 3 > H={})
Definition: Rot3M.cpp:157
mode
static const DiscreteKey mode(modeKey, 2)
cx
const double cx
Definition: testSmartStereoFactor_iSAM2.cpp:49
align_3::t
Point2 t(10, 10)
gtsam::Rot3::RzRyRx
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,...
Definition: Rot3M.cpp:84
so3::R12
SO3 R12
Definition: testShonanFactor.cpp:44
gtsam::Rot3::CAYLEY
@ CAYLEY
Retract and localCoordinates using the Cayley transform.
Definition: Rot3.h:342
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
R
Rot2 R(Rot2::fromAngle(0.1))
z2
static const Unit3 z2
Definition: testRotateFactor.cpp:43
ROT3_DEFAULT_COORDINATES_MODE
#define ROT3_DEFAULT_COORDINATES_MODE
Definition: Rot3.h:43
M
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51


gtsam
Author(s):
autogenerated on Thu Jun 13 2024 03:04:59