mirror of
https://github.com/ceres-solver/ceres-solver.git
synced 2026-08-29 08:34:37 +08:00
Add RotationMatrixToQuaternion.
Use this function to implement RotationMatrixToAngleAxis. This simplifies the implementation of RotationMatrixToAngleAxis, just like Eigen does. It is also autodiff compatible, unlike the Eigen based version. Also significantly improve the test coverage of RotationMatrixToAngleAxis. Change-Id: Ic192a12fb5de952197ee24b0deedc45f195477f1
This commit is contained in:
Sameer Agarwal
committed by
Keir Mierle
Keir Mierle
parent
7c0ac8f995
commit
730aa537cd
+60
-73
@@ -1,5 +1,5 @@
|
||||
// Ceres Solver - A fast non-linear least squares minimizer
|
||||
// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
|
||||
// Copyright 2014 Google Inc. All rights reserved.
|
||||
// http://code.google.com/p/ceres-solver/
|
||||
//
|
||||
// Redistribution and use in source and binary forms, with or without
|
||||
@@ -47,7 +47,6 @@
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include "Eigen/Geometry"
|
||||
#include "glog/logging.h"
|
||||
|
||||
namespace ceres {
|
||||
@@ -95,6 +94,17 @@ void AngleAxisToQuaternion(const T* angle_axis, T* quaternion);
|
||||
template<typename T>
|
||||
void QuaternionToAngleAxis(const T* quaternion, T* angle_axis);
|
||||
|
||||
// Conversions between 3x3 rotation matrix (in column major order) and
|
||||
// quaternion rotation representations. Templated for use with
|
||||
// autodifferentiation.
|
||||
template <typename T>
|
||||
void RotationMatrixToQuaternion(const T* R, T* quaternion);
|
||||
|
||||
template <typename T, int row_stride, int col_stride>
|
||||
void RotationMatrixToQuaternion(
|
||||
const MatrixAdapter<const T, row_stride, col_stride>& R,
|
||||
T* quaternion);
|
||||
|
||||
// Conversions between 3x3 rotation matrix (in column major order) and
|
||||
// axis-angle rotation representations. Templated for use with
|
||||
// autodifferentiation.
|
||||
@@ -142,11 +152,11 @@ void EulerAnglesToRotationMatrix(
|
||||
// the cross-product matrix of [a b c]. Together with the property that
|
||||
// R(q1 * q2) = R(q1) * R(q2) this uniquely defines the mapping from q to R.
|
||||
//
|
||||
// The rotation matrix is row-major.
|
||||
//
|
||||
// No normalization of the quaternion is performed, i.e.
|
||||
// R = ||q||^2 * Q, where Q is an orthonormal matrix
|
||||
// such that det(Q) = 1 and Q*Q' = I
|
||||
//
|
||||
// WARNING: The rotation matrix is ROW MAJOR
|
||||
template <typename T> inline
|
||||
void QuaternionToScaledRotation(const T q[4], T R[3 * 3]);
|
||||
|
||||
@@ -157,6 +167,8 @@ void QuaternionToScaledRotation(
|
||||
|
||||
// Same as above except that the rotation matrix is normalized by the
|
||||
// Frobenius norm, so that R * R' = I (and det(R) = 1).
|
||||
//
|
||||
// WARNING: The rotation matrix is ROW MAJOR
|
||||
template <typename T> inline
|
||||
void QuaternionToRotation(const T q[4], T R[3 * 3]);
|
||||
|
||||
@@ -295,6 +307,46 @@ inline void QuaternionToAngleAxis(const T* quaternion, T* angle_axis) {
|
||||
}
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
void RotationMatrixToQuaternion(const T* R, T* angle_axis) {
|
||||
RotationMatrixToQuaternion(ColumnMajorAdapter3x3(R), angle_axis);
|
||||
}
|
||||
|
||||
// This algorithm comes from "Quaternion Calculus and Fast Animation",
|
||||
// Ken Shoemake, 1987 SIGGRAPH course notes
|
||||
template <typename T, int row_stride, int col_stride>
|
||||
void RotationMatrixToQuaternion(
|
||||
const MatrixAdapter<const T, row_stride, col_stride>& R,
|
||||
T* quaternion) {
|
||||
const T trace = R(0, 0) + R(1, 1) + R(2, 2);
|
||||
if (trace >= 0.0) {
|
||||
T t = sqrt(trace + T(1.0));
|
||||
quaternion[0] = T(0.5) * t;
|
||||
t = T(0.5) / t;
|
||||
quaternion[1] = (R(2, 1) - R(1, 2)) * t;
|
||||
quaternion[2] = (R(0, 2) - R(2, 0)) * t;
|
||||
quaternion[3] = (R(1, 0) - R(0, 1)) * t;
|
||||
} else {
|
||||
int i = 0;
|
||||
if (R(1, 1) > R(0, 0)) {
|
||||
i = 1;
|
||||
}
|
||||
|
||||
if (R(2, 2) > R(i, i)) {
|
||||
i = 2;
|
||||
}
|
||||
|
||||
const int j = (i + 1) % 3;
|
||||
const int k = (j + 1) % 3;
|
||||
T t = sqrt(R(i, i) - R(j, j) - R(k, k) + T(1.0));
|
||||
quaternion[i + 1] = T(0.5) * t;
|
||||
t = T(0.5) / t;
|
||||
quaternion[0] = (R(k, j) - R(j, k)) * t;
|
||||
quaternion[j + 1] = (R(j, i) + R(i, j)) * t;
|
||||
quaternion[k + 1] = (R(k, i) + R(i, k)) * t;
|
||||
}
|
||||
}
|
||||
|
||||
// The conversion of a rotation matrix to the angle-axis form is
|
||||
// numerically problematic when then rotation angle is close to zero
|
||||
// or to Pi. The following implementation detects when these two cases
|
||||
@@ -309,75 +361,10 @@ template <typename T, int row_stride, int col_stride>
|
||||
void RotationMatrixToAngleAxis(
|
||||
const MatrixAdapter<const T, row_stride, col_stride>& R,
|
||||
T* angle_axis) {
|
||||
// x = k * 2 * sin(theta), where k is the axis of rotation.
|
||||
angle_axis[0] = R(2, 1) - R(1, 2);
|
||||
angle_axis[1] = R(0, 2) - R(2, 0);
|
||||
angle_axis[2] = R(1, 0) - R(0, 1);
|
||||
|
||||
static const T kOne = T(1.0);
|
||||
static const T kTwo = T(2.0);
|
||||
|
||||
// Since the right hand side may give numbers just above 1.0 or
|
||||
// below -1.0 leading to atan misbehaving, we threshold.
|
||||
T costheta = std::min(std::max((R(0, 0) + R(1, 1) + R(2, 2) - kOne) / kTwo,
|
||||
T(-1.0)),
|
||||
kOne);
|
||||
|
||||
// sqrt is guaranteed to give non-negative results, so we only
|
||||
// threshold above.
|
||||
T sintheta = std::min(sqrt(angle_axis[0] * angle_axis[0] +
|
||||
angle_axis[1] * angle_axis[1] +
|
||||
angle_axis[2] * angle_axis[2]) / kTwo,
|
||||
kOne);
|
||||
|
||||
// Use the arctan2 to get the right sign on theta
|
||||
const T theta = atan2(sintheta, costheta);
|
||||
|
||||
// Case 1: sin(theta) is large enough, so dividing by it is not a
|
||||
// problem. We do not use abs here, because while jets.h imports
|
||||
// std::abs into the namespace, here in this file, abs resolves to
|
||||
// the int version of the function, which returns zero always.
|
||||
//
|
||||
// We use a threshold much larger then the machine epsilon, because
|
||||
// if sin(theta) is small, not only do we risk overflow but even if
|
||||
// that does not occur, just dividing by a small number will result
|
||||
// in numerical garbage. So we play it safe.
|
||||
static const double kThreshold = 1e-12;
|
||||
if ((sintheta > kThreshold) || (sintheta < -kThreshold)) {
|
||||
const T r = theta / (kTwo * sintheta);
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
angle_axis[i] *= r;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
// Case 2: theta ~ 0, means sin(theta) ~ theta to a good
|
||||
// approximation.
|
||||
if (costheta > 0.0) {
|
||||
const T kHalf = T(0.5);
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
angle_axis[i] *= kHalf;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
// Case 3: theta ~ pi, this is the hard case. Since theta is large,
|
||||
// and sin(theta) is small. Dividing theta by sin(theta) will either
|
||||
// give an overflow or worse still numerically meaningless
|
||||
// results. Thus we use an alternate more complicated and expensive
|
||||
// formula implemented by Eigen.
|
||||
Eigen::Matrix<T, 3, 3> rotation_matrix;
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
for (int j = 0; j < 3; ++j) {
|
||||
rotation_matrix(i,j) = R(i,j);
|
||||
}
|
||||
}
|
||||
|
||||
Eigen::AngleAxis<T> aa;
|
||||
aa.fromRotationMatrix(rotation_matrix);
|
||||
angle_axis[0] = aa.angle() * aa.axis()[0];
|
||||
angle_axis[1] = aa.angle() * aa.axis()[1];
|
||||
angle_axis[2] = aa.angle() * aa.axis()[2];
|
||||
T quaternion[4];
|
||||
RotationMatrixToQuaternion(R, quaternion);
|
||||
QuaternionToAngleAxis(quaternion, angle_axis);
|
||||
return;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
|
||||
Reference in New Issue
Block a user