rotation.h
Go to the documentation of this file.
1 // Ceres Solver - A fast non-linear least squares minimizer
2 // Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
3 // http://code.google.com/p/ceres-solver/
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 //
8 // * Redistributions of source code must retain the above copyright notice,
9 // this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright notice,
11 // this list of conditions and the following disclaimer in the documentation
12 // and/or other materials provided with the distribution.
13 // * Neither the name of Google Inc. nor the names of its contributors may be
14 // used to endorse or promote products derived from this software without
15 // specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 // Author: keir@google.com (Keir Mierle)
30 // sameeragarwal@google.com (Sameer Agarwal)
31 //
32 // Templated functions for manipulating rotations. The templated
33 // functions are useful when implementing functors for automatic
34 // differentiation.
35 //
36 // In the following, the Quaternions are laid out as 4-vectors, thus:
37 //
38 // q[0] scalar part.
39 // q[1] coefficient of i.
40 // q[2] coefficient of j.
41 // q[3] coefficient of k.
42 //
43 // where: i*i = j*j = k*k = -1 and i*j = k, j*k = i, k*i = j.
44 
45 #ifndef CERES_PUBLIC_ROTATION_H_
46 #define CERES_PUBLIC_ROTATION_H_
47 
48 #include <algorithm>
49 #include <cmath>
50 #include <limits>
51 #include <assert.h>
52 #define DCHECK assert
53 
54 namespace ceres {
55 
56 // Trivial wrapper to index linear arrays as matrices, given a fixed
57 // column and row stride. When an array "T* array" is wrapped by a
58 //
59 // (const) MatrixAdapter<T, row_stride, col_stride> M"
60 //
61 // the expression M(i, j) is equivalent to
62 //
63 // arrary[i * row_stride + j * col_stride]
64 //
65 // Conversion functions to and from rotation matrices accept
66 // MatrixAdapters to permit using row-major and column-major layouts,
67 // and rotation matrices embedded in larger matrices (such as a 3x4
68 // projection matrix).
69 template <typename T, int row_stride, int col_stride>
71 
72 // Convenience functions to create a MatrixAdapter that treats the
73 // array pointed to by "pointer" as a 3x3 (contiguous) column-major or
74 // row-major matrix.
75 template <typename T>
77 
78 template <typename T>
80 
81 // Convert a value in combined axis-angle representation to a quaternion.
82 // The value angle_axis is a triple whose norm is an angle in radians,
83 // and whose direction is aligned with the axis of rotation,
84 // and quaternion is a 4-tuple that will contain the resulting quaternion.
85 // The implementation may be used with auto-differentiation up to the first
86 // derivative, higher derivatives may have unexpected results near the origin.
87 template<typename T>
88 void AngleAxisToQuaternion(const T* angle_axis, T* quaternion);
89 
90 // Convert a quaternion to the equivalent combined axis-angle representation.
91 // The value quaternion must be a unit quaternion - it is not normalized first,
92 // and angle_axis will be filled with a value whose norm is the angle of
93 // rotation in radians, and whose direction is the axis of rotation.
94 // The implemention may be used with auto-differentiation up to the first
95 // derivative, higher derivatives may have unexpected results near the origin.
96 template<typename T>
97 void QuaternionToAngleAxis(const T* quaternion, T* angle_axis);
98 
99 // Conversions between 3x3 rotation matrix (in column major order) and
100 // axis-angle rotation representations. Templated for use with
101 // autodifferentiation.
102 template <typename T>
103 void RotationMatrixToAngleAxis(const T* R, T* angle_axis);
104 
105 template <typename T, int row_stride, int col_stride>
108  T* angle_axis);
109 
110 template <typename T>
111 void AngleAxisToRotationMatrix(const T* angle_axis, T* R);
112 
113 template <typename T, int row_stride, int col_stride>
115  const T* angle_axis,
117 
118 // Conversions between 3x3 rotation matrix (in row major order) and
119 // Euler angle (in degrees) rotation representations.
120 //
121 // The {pitch,roll,yaw} Euler angles are rotations around the {x,y,z}
122 // axes, respectively. They are applied in that same order, so the
123 // total rotation R is Rz * Ry * Rx.
124 template <typename T>
125 void EulerAnglesToRotationMatrix(const T* euler, int row_stride, T* R);
126 
127 template <typename T, int row_stride, int col_stride>
129  const T* euler,
131 
132 // Convert a 4-vector to a 3x3 scaled rotation matrix.
133 //
134 // The choice of rotation is such that the quaternion [1 0 0 0] goes to an
135 // identity matrix and for small a, b, c the quaternion [1 a b c] goes to
136 // the matrix
137 //
138 // [ 0 -c b ]
139 // I + 2 [ c 0 -a ] + higher order terms
140 // [ -b a 0 ]
141 //
142 // which corresponds to a Rodrigues approximation, the last matrix being
143 // the cross-product matrix of [a b c]. Together with the property that
144 // R(q1 * q2) = R(q1) * R(q2) this uniquely defines the mapping from q to R.
145 //
146 // The rotation matrix is row-major.
147 //
148 // No normalization of the quaternion is performed, i.e.
149 // R = ||q||^2 * Q, where Q is an orthonormal matrix
150 // such that det(Q) = 1 and Q*Q' = I
151 template <typename T> inline
152 void QuaternionToScaledRotation(const T q[4], T R[3 * 3]);
153 
154 template <typename T, int row_stride, int col_stride> inline
156  const T q[4],
158 
159 // Same as above except that the rotation matrix is normalized by the
160 // Frobenius norm, so that R * R' = I (and det(R) = 1).
161 template <typename T> inline
162 void QuaternionToRotation(const T q[4], T R[3 * 3]);
163 
164 template <typename T, int row_stride, int col_stride> inline
166  const T q[4],
168 
169 // Rotates a point pt by a quaternion q:
170 //
171 // result = R(q) * pt
172 //
173 // Assumes the quaternion is unit norm. This assumption allows us to
174 // write the transform as (something)*pt + pt, as is clear from the
175 // formula below. If you pass in a quaternion with |q|^2 = 2 then you
176 // WILL NOT get back 2 times the result you get for a unit quaternion.
177 template <typename T> inline
178 void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]);
179 
180 // With this function you do not need to assume that q has unit norm.
181 // It does assume that the norm is non-zero.
182 template <typename T> inline
183 void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]);
184 
185 // zw = z * w, where * is the Quaternion product between 4 vectors.
186 template<typename T> inline
187 void QuaternionProduct(const T z[4], const T w[4], T zw[4]);
188 
189 // xy = x cross y;
190 template<typename T> inline
191 void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]);
192 
193 template<typename T> inline
194 T DotProduct(const T x[3], const T y[3]);
195 
196 // y = R(angle_axis) * x;
197 template<typename T> inline
198 void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]);
199 
200 // --- IMPLEMENTATION
201 
202 template<typename T, int row_stride, int col_stride>
203 struct MatrixAdapter {
205  explicit MatrixAdapter(T* pointer)
206  : pointer_(pointer)
207  {}
208 
209  T& operator()(int r, int c) const {
210  return pointer_[r * row_stride + c * col_stride];
211  }
212 };
213 
214 template <typename T>
216  return MatrixAdapter<T, 1, 3>(pointer);
217 }
218 
219 template <typename T>
221  return MatrixAdapter<T, 3, 1>(pointer);
222 }
223 
224 template<typename T>
225 inline void AngleAxisToQuaternion(const T* angle_axis, T* quaternion) {
226  const T& a0 = angle_axis[0];
227  const T& a1 = angle_axis[1];
228  const T& a2 = angle_axis[2];
229  const T theta_squared = a0 * a0 + a1 * a1 + a2 * a2;
230 
231  // For points not at the origin, the full conversion is numerically stable.
232  if (theta_squared > T(0.0)) {
233  const T theta = sqrt(theta_squared);
234  const T half_theta = theta * T(0.5);
235  const T k = sin(half_theta) / theta;
236  quaternion[0] = cos(half_theta);
237  quaternion[1] = a0 * k;
238  quaternion[2] = a1 * k;
239  quaternion[3] = a2 * k;
240  } else {
241  // At the origin, sqrt() will produce NaN in the derivative since
242  // the argument is zero. By approximating with a Taylor series,
243  // and truncating at one term, the value and first derivatives will be
244  // computed correctly when Jets are used.
245  const T k(0.5);
246  quaternion[0] = T(1.0);
247  quaternion[1] = a0 * k;
248  quaternion[2] = a1 * k;
249  quaternion[3] = a2 * k;
250  }
251 }
252 
253 template<typename T>
254 inline void QuaternionToAngleAxis(const T* quaternion, T* angle_axis) {
255  const T& q1 = quaternion[1];
256  const T& q2 = quaternion[2];
257  const T& q3 = quaternion[3];
258  const T sin_squared_theta = q1 * q1 + q2 * q2 + q3 * q3;
259 
260  // For quaternions representing non-zero rotation, the conversion
261  // is numerically stable.
262  if (sin_squared_theta > T(0.0)) {
263  const T sin_theta = sqrt(sin_squared_theta);
264  const T& cos_theta = quaternion[0];
265 
266  // If cos_theta is negative, theta is greater than pi/2, which
267  // means that angle for the angle_axis vector which is 2 * theta
268  // would be greater than pi.
269  //
270  // While this will result in the correct rotation, it does not
271  // result in a normalized angle-axis vector.
272  //
273  // In that case we observe that 2 * theta ~ 2 * theta - 2 * pi,
274  // which is equivalent saying
275  //
276  // theta - pi = atan(sin(theta - pi), cos(theta - pi))
277  // = atan(-sin(theta), -cos(theta))
278  //
279  const T two_theta =
280  T(2.0) * ((cos_theta < 0.0)
281  ? atan2(-sin_theta, -cos_theta)
282  : atan2(sin_theta, cos_theta));
283  const T k = two_theta / sin_theta;
284  angle_axis[0] = q1 * k;
285  angle_axis[1] = q2 * k;
286  angle_axis[2] = q3 * k;
287  } else {
288  // For zero rotation, sqrt() will produce NaN in the derivative since
289  // the argument is zero. By approximating with a Taylor series,
290  // and truncating at one term, the value and first derivatives will be
291  // computed correctly when Jets are used.
292  const T k(2.0);
293  angle_axis[0] = q1 * k;
294  angle_axis[1] = q2 * k;
295  angle_axis[2] = q3 * k;
296  }
297 }
298 
299 // The conversion of a rotation matrix to the angle-axis form is
300 // numerically problematic when then rotation angle is close to zero
301 // or to Pi. The following implementation detects when these two cases
302 // occurs and deals with them by taking code paths that are guaranteed
303 // to not perform division by a small number.
304 template <typename T>
305 inline void RotationMatrixToAngleAxis(const T* R, T* angle_axis) {
307 }
308 
309 template <typename T, int row_stride, int col_stride>
312  T* angle_axis) {
313  // x = k * 2 * sin(theta), where k is the axis of rotation.
314  angle_axis[0] = R(2, 1) - R(1, 2);
315  angle_axis[1] = R(0, 2) - R(2, 0);
316  angle_axis[2] = R(1, 0) - R(0, 1);
317 
318  static const T kOne = T(1.0);
319  static const T kTwo = T(2.0);
320 
321  // Since the right hand side may give numbers just above 1.0 or
322  // below -1.0 leading to atan misbehaving, we threshold.
323  T costheta = std::min(std::max((R(0, 0) + R(1, 1) + R(2, 2) - kOne) / kTwo,
324  T(-1.0)),
325  kOne);
326 
327  // sqrt is guaranteed to give non-negative results, so we only
328  // threshold above.
329  T sintheta = std::min(sqrt(angle_axis[0] * angle_axis[0] +
330  angle_axis[1] * angle_axis[1] +
331  angle_axis[2] * angle_axis[2]) / kTwo,
332  kOne);
333 
334  // Use the arctan2 to get the right sign on theta
335  const T theta = atan2(sintheta, costheta);
336 
337  // Case 1: sin(theta) is large enough, so dividing by it is not a
338  // problem. We do not use abs here, because while jets.h imports
339  // std::abs into the namespace, here in this file, abs resolves to
340  // the int version of the function, which returns zero always.
341  //
342  // We use a threshold much larger then the machine epsilon, because
343  // if sin(theta) is small, not only do we risk overflow but even if
344  // that does not occur, just dividing by a small number will result
345  // in numerical garbage. So we play it safe.
346  static const double kThreshold = 1e-12;
347  if ((sintheta > kThreshold) || (sintheta < -kThreshold)) {
348  const T r = theta / (kTwo * sintheta);
349  for (int i = 0; i < 3; ++i) {
350  angle_axis[i] *= r;
351  }
352  return;
353  }
354 
355  // Case 2: theta ~ 0, means sin(theta) ~ theta to a good
356  // approximation.
357  if (costheta > 0.0) {
358  const T kHalf = T(0.5);
359  for (int i = 0; i < 3; ++i) {
360  angle_axis[i] *= kHalf;
361  }
362  return;
363  }
364 
365  // Case 3: theta ~ pi, this is the hard case. Since theta is large,
366  // and sin(theta) is small. Dividing by theta by sin(theta) will
367  // either give an overflow or worse still numerically meaningless
368  // results. Thus we use an alternate more complicated formula
369  // here.
370 
371  // Since cos(theta) is negative, division by (1-cos(theta)) cannot
372  // overflow.
373  const T inv_one_minus_costheta = kOne / (kOne - costheta);
374 
375  // We now compute the absolute value of coordinates of the axis
376  // vector using the diagonal entries of R. To resolve the sign of
377  // these entries, we compare the sign of angle_axis[i]*sin(theta)
378  // with the sign of sin(theta). If they are the same, then
379  // angle_axis[i] should be positive, otherwise negative.
380  for (int i = 0; i < 3; ++i) {
381  angle_axis[i] = theta * sqrt((R(i, i) - costheta) * inv_one_minus_costheta);
382  if (((sintheta < 0.0) && (angle_axis[i] > 0.0)) ||
383  ((sintheta > 0.0) && (angle_axis[i] < 0.0))) {
384  angle_axis[i] = -angle_axis[i];
385  }
386  }
387 }
388 
389 template <typename T>
390 inline void AngleAxisToRotationMatrix(const T* angle_axis, T* R) {
392 }
393 
394 template <typename T, int row_stride, int col_stride>
396  const T* angle_axis,
398  static const T kOne = T(1.0);
399  const T theta2 = DotProduct(angle_axis, angle_axis);
400  if (theta2 > T(std::numeric_limits<double>::epsilon())) {
401  // We want to be careful to only evaluate the square root if the
402  // norm of the angle_axis vector is greater than zero. Otherwise
403  // we get a division by zero.
404  const T theta = sqrt(theta2);
405  const T wx = angle_axis[0] / theta;
406  const T wy = angle_axis[1] / theta;
407  const T wz = angle_axis[2] / theta;
408 
409  const T costheta = cos(theta);
410  const T sintheta = sin(theta);
411 
412  R(0, 0) = costheta + wx*wx*(kOne - costheta);
413  R(1, 0) = wz*sintheta + wx*wy*(kOne - costheta);
414  R(2, 0) = -wy*sintheta + wx*wz*(kOne - costheta);
415  R(0, 1) = wx*wy*(kOne - costheta) - wz*sintheta;
416  R(1, 1) = costheta + wy*wy*(kOne - costheta);
417  R(2, 1) = wx*sintheta + wy*wz*(kOne - costheta);
418  R(0, 2) = wy*sintheta + wx*wz*(kOne - costheta);
419  R(1, 2) = -wx*sintheta + wy*wz*(kOne - costheta);
420  R(2, 2) = costheta + wz*wz*(kOne - costheta);
421  } else {
422  // Near zero, we switch to using the first order Taylor expansion.
423  R(0, 0) = kOne;
424  R(1, 0) = angle_axis[2];
425  R(2, 0) = -angle_axis[1];
426  R(0, 1) = -angle_axis[2];
427  R(1, 1) = kOne;
428  R(2, 1) = angle_axis[0];
429  R(0, 2) = angle_axis[1];
430  R(1, 2) = -angle_axis[0];
431  R(2, 2) = kOne;
432  }
433 }
434 
435 template <typename T>
436 inline void EulerAnglesToRotationMatrix(const T* euler,
437  const int row_stride_parameter,
438  T* R) {
439  DCHECK(row_stride_parameter==3);
441 }
442 
443 template <typename T, int row_stride, int col_stride>
445  const T* euler,
447  const double kPi = 3.14159265358979323846;
448  const T degrees_to_radians(kPi / 180.0);
449 
450  const T pitch(euler[0] * degrees_to_radians);
451  const T roll(euler[1] * degrees_to_radians);
452  const T yaw(euler[2] * degrees_to_radians);
453 
454  const T c1 = cos(yaw);
455  const T s1 = sin(yaw);
456  const T c2 = cos(roll);
457  const T s2 = sin(roll);
458  const T c3 = cos(pitch);
459  const T s3 = sin(pitch);
460 
461  R(0, 0) = c1*c2;
462  R(0, 1) = -s1*c3 + c1*s2*s3;
463  R(0, 2) = s1*s3 + c1*s2*c3;
464 
465  R(1, 0) = s1*c2;
466  R(1, 1) = c1*c3 + s1*s2*s3;
467  R(1, 2) = -c1*s3 + s1*s2*c3;
468 
469  R(2, 0) = -s2;
470  R(2, 1) = c2*s3;
471  R(2, 2) = c2*c3;
472 }
473 
474 template <typename T> inline
475 void QuaternionToScaledRotation(const T q[4], T R[3 * 3]) {
477 }
478 
479 template <typename T, int row_stride, int col_stride> inline
481  const T q[4],
483  // Make convenient names for elements of q.
484  T a = q[0];
485  T b = q[1];
486  T c = q[2];
487  T d = q[3];
488  // This is not to eliminate common sub-expression, but to
489  // make the lines shorter so that they fit in 80 columns!
490  T aa = a * a;
491  T ab = a * b;
492  T ac = a * c;
493  T ad = a * d;
494  T bb = b * b;
495  T bc = b * c;
496  T bd = b * d;
497  T cc = c * c;
498  T cd = c * d;
499  T dd = d * d;
500 
501  R(0, 0) = aa + bb - cc - dd; R(0, 1) = T(2) * (bc - ad); R(0, 2) = T(2) * (ac + bd); // NOLINT
502  R(1, 0) = T(2) * (ad + bc); R(1, 1) = aa - bb + cc - dd; R(1, 2) = T(2) * (cd - ab); // NOLINT
503  R(2, 0) = T(2) * (bd - ac); R(2, 1) = T(2) * (ab + cd); R(2, 2) = aa - bb - cc + dd; // NOLINT
504 }
505 
506 template <typename T> inline
507 void QuaternionToRotation(const T q[4], T R[3 * 3]) {
509 }
510 
511 template <typename T, int row_stride, int col_stride> inline
512 void QuaternionToRotation(const T q[4],
515 
516  T normalizer = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3];
517  CHECK_NE(normalizer, T(0));
518  normalizer = T(1) / normalizer;
519 
520  for (int i = 0; i < 3; ++i) {
521  for (int j = 0; j < 3; ++j) {
522  R(i, j) *= normalizer;
523  }
524  }
525 }
526 
527 template <typename T> inline
528 void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) {
529  const T t2 = q[0] * q[1];
530  const T t3 = q[0] * q[2];
531  const T t4 = q[0] * q[3];
532  const T t5 = -q[1] * q[1];
533  const T t6 = q[1] * q[2];
534  const T t7 = q[1] * q[3];
535  const T t8 = -q[2] * q[2];
536  const T t9 = q[2] * q[3];
537  const T t1 = -q[3] * q[3];
538  result[0] = T(2) * ((t8 + t1) * pt[0] + (t6 - t4) * pt[1] + (t3 + t7) * pt[2]) + pt[0]; // NOLINT
539  result[1] = T(2) * ((t4 + t6) * pt[0] + (t5 + t1) * pt[1] + (t9 - t2) * pt[2]) + pt[1]; // NOLINT
540  result[2] = T(2) * ((t7 - t3) * pt[0] + (t2 + t9) * pt[1] + (t5 + t8) * pt[2]) + pt[2]; // NOLINT
541 }
542 
543 template <typename T> inline
544 void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) {
545  // 'scale' is 1 / norm(q).
546  const T scale = T(1) / sqrt(q[0] * q[0] +
547  q[1] * q[1] +
548  q[2] * q[2] +
549  q[3] * q[3]);
550 
551  // Make unit-norm version of q.
552  const T unit[4] = {
553  scale * q[0],
554  scale * q[1],
555  scale * q[2],
556  scale * q[3],
557  };
558 
560 }
561 
562 template<typename T> inline
563 void QuaternionProduct(const T z[4], const T w[4], T zw[4]) {
564  zw[0] = z[0] * w[0] - z[1] * w[1] - z[2] * w[2] - z[3] * w[3];
565  zw[1] = z[0] * w[1] + z[1] * w[0] + z[2] * w[3] - z[3] * w[2];
566  zw[2] = z[0] * w[2] - z[1] * w[3] + z[2] * w[0] + z[3] * w[1];
567  zw[3] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0];
568 }
569 
570 // xy = x cross y;
571 template<typename T> inline
572 void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]) {
573  x_cross_y[0] = x[1] * y[2] - x[2] * y[1];
574  x_cross_y[1] = x[2] * y[0] - x[0] * y[2];
575  x_cross_y[2] = x[0] * y[1] - x[1] * y[0];
576 }
577 
578 template<typename T> inline
579 T DotProduct(const T x[3], const T y[3]) {
580  return (x[0] * y[0] + x[1] * y[1] + x[2] * y[2]);
581 }
582 
583 template<typename T> inline
584 void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) {
585  const T theta2 = DotProduct(angle_axis, angle_axis);
586  if (theta2 > T(std::numeric_limits<double>::epsilon())) {
587  // Away from zero, use the rodriguez formula
588  //
589  // result = pt costheta +
590  // (w x pt) * sintheta +
591  // w (w . pt) (1 - costheta)
592  //
593  // We want to be careful to only evaluate the square root if the
594  // norm of the angle_axis vector is greater than zero. Otherwise
595  // we get a division by zero.
596  //
597  const T theta = sqrt(theta2);
598  const T costheta = cos(theta);
599  const T sintheta = sin(theta);
600  const T theta_inverse = 1.0 / theta;
601 
602  const T w[3] = { angle_axis[0] * theta_inverse,
603  angle_axis[1] * theta_inverse,
604  angle_axis[2] * theta_inverse };
605 
606  // Explicitly inlined evaluation of the cross product for
607  // performance reasons.
608  const T w_cross_pt[3] = { w[1] * pt[2] - w[2] * pt[1],
609  w[2] * pt[0] - w[0] * pt[2],
610  w[0] * pt[1] - w[1] * pt[0] };
611  const T tmp =
612  (w[0] * pt[0] + w[1] * pt[1] + w[2] * pt[2]) * (T(1.0) - costheta);
613 
614  result[0] = pt[0] * costheta + w_cross_pt[0] * sintheta + w[0] * tmp;
615  result[1] = pt[1] * costheta + w_cross_pt[1] * sintheta + w[1] * tmp;
616  result[2] = pt[2] * costheta + w_cross_pt[2] * sintheta + w[2] * tmp;
617  } else {
618  // Near zero, the first order Taylor approximation of the rotation
619  // matrix R corresponding to a vector w and angle w is
620  //
621  // R = I + hat(w) * sin(theta)
622  //
623  // But sintheta ~ theta and theta * w = angle_axis, which gives us
624  //
625  // R = I + hat(w)
626  //
627  // and actually performing multiplication with the point pt, gives us
628  // R * pt = pt + w x pt.
629  //
630  // Switching to the Taylor expansion near zero provides meaningful
631  // derivatives when evaluated using Jets.
632  //
633  // Explicitly inlined evaluation of the cross product for
634  // performance reasons.
635  const T w_cross_pt[3] = { angle_axis[1] * pt[2] - angle_axis[2] * pt[1],
636  angle_axis[2] * pt[0] - angle_axis[0] * pt[2],
637  angle_axis[0] * pt[1] - angle_axis[1] * pt[0] };
638 
639  result[0] = pt[0] + w_cross_pt[0];
640  result[1] = pt[1] + w_cross_pt[1];
641  result[2] = pt[2] + w_cross_pt[2];
642  }
643 }
644 
645 } // namespace ceres
646 
647 #endif // CERES_PUBLIC_ROTATION_H_
w
RowVector3d w
Definition: Matrix_resize_int.cpp:3
ceres::QuaternionToRotation
void QuaternionToRotation(const T q[4], T R[3 *3])
Definition: rotation.h:507
ceres::DotProduct
T DotProduct(const T x[3], const T y[3])
Definition: rotation.h:579
ceres::atan2
Jet< T, N > atan2(const Jet< T, N > &g, const Jet< T, N > &f)
Definition: jet.h:556
ceres::AngleAxisToQuaternion
void AngleAxisToQuaternion(const T *angle_axis, T *quaternion)
Definition: rotation.h:225
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
ceres::QuaternionToAngleAxis
void QuaternionToAngleAxis(const T *quaternion, T *angle_axis)
Definition: rotation.h:254
ceres::EulerAnglesToRotationMatrix
void EulerAnglesToRotationMatrix(const T *euler, int row_stride, T *R)
Definition: rotation.h:436
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
b
Scalar * b
Definition: benchVecAdd.cpp:17
ceres::MatrixAdapter::operator()
T & operator()(int r, int c) const
Definition: rotation.h:209
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
pt
static const Point3 pt(1.0, 2.0, 3.0)
cd
static double cd[7]
Definition: fresnl.c:92
T
Eigen::Triplet< double > T
Definition: Tutorial_sparse_example.cpp:6
result
Values result
Definition: OdometryOptimize.cpp:8
kOne
static const Vector kOne
Definition: testLPSolver.cpp:37
ceres::cos
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
ceres::QuaternionRotatePoint
void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3])
Definition: rotation.h:544
ceres::QuaternionProduct
void QuaternionProduct(const T z[4], const T w[4], T zw[4])
Definition: rotation.h:563
c1
static double c1
Definition: airy.c:54
quaternion
void quaternion(void)
Definition: geo_quaternion.cpp:46
ceres::CrossProduct
void CrossProduct(const T x[3], const T y[3], T x_cross_y[3])
Definition: rotation.h:572
epsilon
static double epsilon
Definition: testRot3.cpp:37
align_3::a1
Point2 a1
Definition: testPose2.cpp:769
ceres::ColumnMajorAdapter3x3
MatrixAdapter< T, 1, 3 > ColumnMajorAdapter3x3(T *pointer)
Definition: rotation.h:215
scale
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 scale
Definition: gnuplot_common_settings.hh:54
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
ceres::AngleAxisRotatePoint
void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3])
Definition: rotation.h:584
Eigen::numext::q
EIGEN_DEVICE_FUNC const Scalar & q
Definition: SpecialFunctionsImpl.h:1984
ceres::UnitQuaternionRotatePoint
void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3])
Definition: rotation.h:528
ceres::RotationMatrixToAngleAxis
void RotationMatrixToAngleAxis(const T *R, T *angle_axis)
Definition: rotation.h:305
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
Eigen::Triplet< double >
ceres
Definition: autodiff.h:152
y
Scalar * y
Definition: level1_cplx_impl.h:124
ceres::MatrixAdapter::pointer_
T * pointer_
Definition: rotation.h:204
ceres::RowMajorAdapter3x3
MatrixAdapter< T, 3, 1 > RowMajorAdapter3x3(T *pointer)
Definition: rotation.h:220
a
ArrayXXi a
Definition: Array_initializer_list_23_cxx11.cpp:1
align_3::a2
Point2 a2
Definition: testPose2.cpp:770
ceres::MatrixAdapter::MatrixAdapter
MatrixAdapter(T *pointer)
Definition: rotation.h:205
ceres::MatrixAdapter
Definition: rotation.h:70
c2
static double c2
Definition: airy.c:55
min
#define min(a, b)
Definition: datatypes.h:19
ceres::QuaternionToScaledRotation
void QuaternionToScaledRotation(const T q[4], T R[3 *3])
Definition: rotation.h:475
ceres::AngleAxisToRotationMatrix
void AngleAxisToRotationMatrix(const T *angle_axis, T *R)
Definition: rotation.h:390
max
#define max(a, b)
Definition: datatypes.h:20
ceres::sqrt
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
DCHECK
#define DCHECK
Definition: rotation.h:52
R
Rot2 R(Rot2::fromAngle(0.1))


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