Rot3.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 
22 #include <gtsam/geometry/Rot3.h>
23 #include <gtsam/geometry/SO3.h>
24 
25 #include <cmath>
26 #include <random>
27 
28 using namespace std;
29 
30 namespace gtsam {
31 
32 /* ************************************************************************* */
33 void Rot3::print(const std::string& s) const {
34  cout << (s.empty() ? "R: " : s + " ");
35  gtsam::print(static_cast<Matrix>(matrix()));
36 }
37 
38 /* ************************************************************************* */
39 Rot3 Rot3::Random(std::mt19937& rng) {
40  Unit3 axis = Unit3::Random(rng);
41  uniform_real_distribution<double> randomAngle(-M_PI, M_PI);
42  double angle = randomAngle(rng);
43  return AxisAngle(axis, angle);
44 }
45 
46 
47 
48 /* ************************************************************************* */
49 Rot3 Rot3::AlignPair(const Unit3& axis, const Unit3& a_p, const Unit3& b_p) {
50  // if a_p is already aligned with b_p, return the identity rotation
51  if (std::abs(a_p.dot(b_p)) > 0.999999999) {
52  return Rot3();
53  }
54 
55  // Check axis was not degenerate cross product
56  const Vector3 z = axis.unitVector();
57  if (z.hasNaN())
58  throw std::runtime_error("AlignSinglePair: axis has Nans");
59 
60  // Now, calculate rotation that takes b_p to a_p
61  const Matrix3 P = I_3x3 - z * z.transpose(); // orthogonal projector
62  const Vector3 a_po = P * a_p.unitVector(); // point in a orthogonal to axis
63  const Vector3 b_po = P * b_p.unitVector(); // point in b orthogonal to axis
64  const Vector3 x = a_po.normalized(); // x-axis in axis-orthogonal plane, along a_p vector
65  const Vector3 y = z.cross(x); // y-axis in axis-orthogonal plane
66  const double u = x.dot(b_po); // x-coordinate for b_po
67  const double v = y.dot(b_po); // y-coordinate for b_po
68  double angle = std::atan2(v, u);
69  return Rot3::AxisAngle(z, -angle);
70 }
71 
72 /* ************************************************************************* */
73 Rot3 Rot3::AlignTwoPairs(const Unit3& a_p, const Unit3& b_p, //
74  const Unit3& a_q, const Unit3& b_q) {
75  // there are three frames in play:
76  // a: the first frame in which p and q are measured
77  // b: the second frame in which p and q are measured
78  // i: intermediate, after aligning first pair
79 
80  // First, find rotation around that aligns a_p and b_p
81  Rot3 i_R_b = AlignPair(a_p.cross(b_p), a_p, b_p);
82 
83  // Rotate points in frame b to the intermediate frame,
84  // in which we expect the point p to be aligned now
85  Unit3 i_q = i_R_b * b_q;
86  assert(assert_equal(a_p, i_R_b * b_p, 1e-6));
87 
88  // Now align second pair: we need to align i_q to a_q
89  Rot3 a_R_i = AlignPair(a_p, a_q, i_q);
90  assert(assert_equal(a_p, a_R_i * a_p, 1e-6));
91  assert(assert_equal(a_q, a_R_i * i_q, 1e-6));
92 
93  // The desired rotation is the product of both
94  Rot3 a_R_b = a_R_i * i_R_b;
95  return a_R_b;
96 }
97 
98 /* ************************************************************************* */
99 bool Rot3::equals(const Rot3 & R, double tol) const {
100  return equal_with_abs_tol(matrix(), R.matrix(), tol);
101 }
102 
103 /* ************************************************************************* */
105  return rotate(p);
106 }
107 
108 /* ************************************************************************* */
111  Matrix32 Dp;
112  Unit3 q = Unit3(rotate(p.point3(Hp ? &Dp : 0)));
113  if (Hp) *Hp = q.basis().transpose() * matrix() * Dp;
114  if (HR) *HR = -q.basis().transpose() * matrix() * p.skew();
115  return q;
116 }
117 
118 /* ************************************************************************* */
121  Matrix32 Dp;
122  Unit3 q = Unit3(unrotate(p.point3(Dp)));
123  if (Hp) *Hp = q.basis().transpose() * matrix().transpose () * Dp;
124  if (HR) *HR = q.basis().transpose() * q.skew();
125  return q;
126 }
127 
128 /* ************************************************************************* */
129 Unit3 Rot3::operator*(const Unit3& p) const {
130  return rotate(p);
131 }
132 
133 /* ************************************************************************* */
134 // see doc/math.lyx, SO(3) section
136  OptionalJacobian<3,3> H2) const {
137  const Matrix3& Rt = transpose();
138  Point3 q(Rt * p); // q = Rt*p
139  const double wx = q.x(), wy = q.y(), wz = q.z();
140  if (H1)
141  *H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
142  if (H2)
143  *H2 = Rt;
144  return q;
145 }
146 
147 /* ************************************************************************* */
148 Point3 Rot3::column(int index) const{
149  if(index == 3)
150  return r3();
151  else if(index == 2)
152  return r2();
153  else if(index == 1)
154  return r1(); // default returns r1
155  else
156  throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3");
157 }
158 
159 /* ************************************************************************* */
160 Vector3 Rot3::xyz(OptionalJacobian<3, 3> H) const {
161  Matrix3 I;Vector3 q;
162  if (H) {
163  Matrix93 mH;
164  const auto m = matrix();
165 #ifdef GTSAM_USE_QUATERNIONS
166  SO3{m}.vec(mH);
167 #else
168  rot_.vec(mH);
169 #endif
170 
171  Matrix39 qHm;
172  std::tie(I, q) = RQ(m, qHm);
173 
174  // TODO : Explore whether this expression can be optimized as both
175  // qHm and mH are super-sparse
176  *H = qHm * mH;
177  } else
178  std::tie(I, q) = RQ(matrix());
179  return q;
180 }
181 
182 /* ************************************************************************* */
183 Vector3 Rot3::ypr(OptionalJacobian<3, 3> H) const {
184  Vector3 q = xyz(H);
185  if (H) H->row(0).swap(H->row(2));
186 
187  return Vector3(q(2),q(1),q(0));
188 }
189 
190 /* ************************************************************************* */
191 Vector3 Rot3::rpy(OptionalJacobian<3, 3> H) const { return xyz(H); }
192 
193 /* ************************************************************************* */
194 double Rot3::roll(OptionalJacobian<1, 3> H) const {
195  double r;
196  if (H) {
197  Matrix3 xyzH;
198  r = xyz(xyzH)(0);
199  *H = xyzH.row(0);
200  } else
201  r = xyz()(0);
202  return r;
203 }
204 
205 /* ************************************************************************* */
206 double Rot3::pitch(OptionalJacobian<1, 3> H) const {
207  double p;
208  if (H) {
209  Matrix3 xyzH;
210  p = xyz(xyzH)(1);
211  *H = xyzH.row(1);
212  } else
213  p = xyz()(1);
214  return p;
215 }
216 
217 /* ************************************************************************* */
218 double Rot3::yaw(OptionalJacobian<1, 3> H) const {
219  double y;
220  if (H) {
221  Matrix3 xyzH;
222  y = xyz(xyzH)(2);
223  *H = xyzH.row(2);
224  } else
225  y = xyz()(2);
226  return y;
227 }
228 
229 /* ************************************************************************* */
230 pair<Unit3, double> Rot3::axisAngle() const {
231  const Vector3 omega = Rot3::Logmap(*this);
232  return std::pair<Unit3, double>(Unit3(omega), omega.norm());
233 }
234 
235 /* ************************************************************************* */
236 Matrix3 Rot3::ExpmapDerivative(const Vector3& x) {
237  return SO3::ExpmapDerivative(x);
238 }
239 
240 /* ************************************************************************* */
241 Matrix3 Rot3::LogmapDerivative(const Vector3& x) {
242  return SO3::LogmapDerivative(x);
243 }
244 
245 /* ************************************************************************* */
246 pair<Matrix3, Vector3> RQ(const Matrix3& A, OptionalJacobian<3, 9> H) {
247  const double x = -atan2(-A(2, 1), A(2, 2));
248  const auto Qx = Rot3::Rx(-x).matrix();
249  const Matrix3 B = A * Qx;
250 
251  const double y = -atan2(B(2, 0), B(2, 2));
252  const auto Qy = Rot3::Ry(-y).matrix();
253  const Matrix3 C = B * Qy;
254 
255  const double z = -atan2(-C(1, 0), C(1, 1));
256  const auto Qz = Rot3::Rz(-z).matrix();
257  const Matrix3 R = C * Qz;
258 
259  if (H) {
260  if (std::abs(y - M_PI / 2) < 1e-2)
261  throw std::runtime_error(
262  "Rot3::RQ : Derivative undefined at singularity (gimbal lock)");
263 
264  auto atan_d1 = [](double y, double x) { return x / (x * x + y * y); };
265  auto atan_d2 = [](double y, double x) { return -y / (x * x + y * y); };
266 
267  const auto sx = -Qx(2, 1), cx = Qx(1, 1);
268  const auto sy = -Qy(0, 2), cy = Qy(0, 0);
269 
270  *H = Matrix39::Zero();
271  // First, calculate the derivate of x
272  (*H)(0, 5) = atan_d1(A(2, 1), A(2, 2));
273  (*H)(0, 8) = atan_d2(A(2, 1), A(2, 2));
274 
275  // Next, calculate the derivate of y. We have
276  // b20 = a20 and b22 = a21 * sx + a22 * cx
277  (*H)(1, 2) = -atan_d1(B(2, 0), B(2, 2));
278  const auto yHb22 = -atan_d2(B(2, 0), B(2, 2));
279  (*H)(1, 5) = yHb22 * sx;
280  (*H)(1, 8) = yHb22 * cx;
281 
282  // Next, calculate the derivate of z. We have
283  // c10 = a10 * cy + a11 * sx * sy + a12 * cx * sy
284  // c11 = a11 * cx - a12 * sx
285  const auto c10Hx = (A(1, 1) * cx - A(1, 2) * sx) * sy;
286  const auto c10Hy = A(1, 2) * cx * cy + A(1, 1) * cy * sx - A(1, 0) * sy;
287  Vector9 c10HA = c10Hx * H->row(0) + c10Hy * H->row(1);
288  c10HA[1] = cy;
289  c10HA[4] = sx * sy;
290  c10HA[7] = cx * sy;
291 
292  const auto c11Hx = -A(1, 2) * cx - A(1, 1) * sx;
293  Vector9 c11HA = c11Hx * H->row(0);
294  c11HA[4] = cx;
295  c11HA[7] = -sx;
296 
297  H->block<1, 9>(2, 0) =
298  atan_d1(C(1, 0), C(1, 1)) * c10HA + atan_d2(C(1, 0), C(1, 1)) * c11HA;
299  }
300 
301  const auto xyz = Vector3(x, y, z);
302  return make_pair(R, xyz);
303 }
304 
305 /* ************************************************************************* */
306 ostream &operator<<(ostream &os, const Rot3& R) {
307  os << R.matrix().format(matlabFormat());
308  return os;
309 }
310 
311 /* ************************************************************************* */
312 Rot3 Rot3::slerp(double t, const Rot3& other) const {
313  return interpolate(*this, other, t);
314 }
315 
316 /* ************************************************************************* */
317 
318 } // namespace gtsam
319 
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
Eigen::internal::print
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
Definition: NEON/PacketMath.h:3115
rng
static std::mt19937 rng
Definition: timeFactorOverhead.cpp:31
B
Matrix< SCALARB, Dynamic, Dynamic, opt_B > B
Definition: bench_gemm.cpp:49
gtsam::column
const MATRIX::ConstColXpr column(const MATRIX &A, size_t j)
Definition: base/Matrix.h:210
cy
const double cy
Definition: testSmartStereoFactor_iSAM2.cpp:50
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
r2
static const double r2
Definition: testSmartRangeFactor.cpp:32
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::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Definition: base/Matrix.h:80
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::matlabFormat
const Eigen::IOFormat & matlabFormat()
Definition: Matrix.cpp:139
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
os
ofstream os("timeSchurFactors.csv")
r1
static const double r1
Definition: testSmartRangeFactor.cpp:32
gtsam::operator<<
ostream & operator<<(ostream &os, const Rot3 &R)
Definition: Rot3.cpp:306
A
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
Definition: bench_gemm.cpp:48
Rot3.h
3D rotation represented as a rotation matrix or quaternion
biased_x_rotation::omega
const double omega
Definition: testPreintegratedRotation.cpp:32
SO3.h
3*3 matrix representation of SO(3)
gtsam::Unit3::dot
double dot(const Unit3 &q, OptionalJacobian< 1, 2 > H1={}, OptionalJacobian< 1, 2 > H2={}) const
Return dot product with q.
Definition: Unit3.cpp:174
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
I
#define I
Definition: main.h:112
Eigen::numext::q
EIGEN_DEVICE_FUNC const Scalar & q
Definition: SpecialFunctionsImpl.h:1984
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::interpolate
T interpolate(const T &X, const T &Y, double t, typename MakeOptionalJacobian< T, T >::type Hx={}, typename MakeOptionalJacobian< T, T >::type Hy={})
Definition: Lie.h:327
r3
static const double r3
Definition: testSmartRangeFactor.cpp:32
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
y
Scalar * y
Definition: level1_cplx_impl.h:124
matrix
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
Definition: gtsam/3rdparty/Eigen/blas/common.h:110
atan2
AnnoyingScalar atan2(const AnnoyingScalar &y, const AnnoyingScalar &x)
Definition: AnnoyingScalar.h:110
C
Matrix< Scalar, Dynamic, Dynamic > C
Definition: bench_gemm.cpp:50
gtsam
traits
Definition: SFMdata.h:40
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
std
Definition: BFloat16.h:88
p
float * p
Definition: Tutorial_Map_using.cpp:9
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
P
static double P[]
Definition: ellpe.c:68
gtsam::SO< 3 >
gtsam::testing::unrotate
P unrotate(const T &r, const P &pt)
Definition: lieProxies.h:50
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
Eigen::Matrix< double, 9, 3 >
abs
#define abs(x)
Definition: datatypes.h:17
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
cx
const double cx
Definition: testSmartStereoFactor_iSAM2.cpp:49
M_PI
#define M_PI
Definition: mconf.h:117
gtsam::Unit3::cross
Unit3 cross(const Unit3 &q) const
Cross-product between two Unit3s.
Definition: Unit3.h:162
gtsam::Unit3::unitVector
Vector3 unitVector(OptionalJacobian< 3, 2 > H={}) const
Return unit-norm Vector.
Definition: Unit3.cpp:151
align_3::t
Point2 t(10, 10)
pybind_wrapper_test_script.other
other
Definition: pybind_wrapper_test_script.py:42
gtsam::testing::rotate
P rotate(const T &r, const P &pt)
Definition: lieProxies.h:47
R
Rot2 R(Rot2::fromAngle(0.1))
gtsam::RQ
pair< Matrix3, Vector3 > RQ(const Matrix3 &A, OptionalJacobian< 3, 9 > H)
Definition: Rot3.cpp:246
operator*
Vector3_ operator*(const Double_ &s, const Vector3_ &v)
Definition: InverseKinematicsExampleExpressions.cpp:42
HR
#define HR
Definition: mincover.c:26


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:04:01