12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Pass Eigen arguments by const reference (#38) 

Fixes #35.
master
catskul 2016-10-11 03:57:41 -04:00 committed by Wolfgang Hess
parent 49ec6a9e37
commit 045f6a7522
1 changed files with 15 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
cartographer/transform/transform.h
View File

@ -51,31 +51,33 @@ T GetYaw(const Rigid3<T>& transform) {
// rotation as the given 'quaternion'. // rotation as the given 'quaternion'.
template <typename T> template <typename T>
Eigen::Matrix<T, 3, 1> RotationQuaternionToAngleAxisVector( Eigen::Matrix<T, 3, 1> RotationQuaternionToAngleAxisVector(
Eigen::Quaternion<T> quaternion) { const Eigen::Quaternion<T>& quaternion) {
quaternion.normalize(); Eigen::Quaternion<T> normalized_quaternion = quaternion.normalized();
// We choose the quaternion with positive 'w', i.e., the one with a smaller // We choose the quaternion with positive 'w', i.e., the one with a smaller
// angle that represents this orientation. // angle that represents this orientation.
if (quaternion.w() < 0.) { if (normalized_quaternion.w() < 0.) {
// Multiply by -1. http://eigen.tuxfamily.org/bz/show_bug.cgi?id=560 // Multiply by -1. http://eigen.tuxfamily.org/bz/show_bug.cgi?id=560
quaternion.w() *= T(-1.); normalized_quaternion.w() *= T(-1.);
quaternion.x() *= T(-1.); normalized_quaternion.x() *= T(-1.);
quaternion.y() *= T(-1.); normalized_quaternion.y() *= T(-1.);
quaternion.z() *= T(-1.); normalized_quaternion.z() *= T(-1.);
} }
// We convert the quaternion into a vector along the rotation axis with // We convert the normalized_quaternion into a vector along the rotation axis
// length of the rotation angle. // with length of the rotation angle.
const T angle = T(2.) * atan2(quaternion.vec().norm(), quaternion.w()); const T angle = T(2.) * atan2(normalized_quaternion.vec().norm(),
normalized_quaternion.w());
constexpr double kCutoffAngle = 1e-7; // We linearize below this angle. constexpr double kCutoffAngle = 1e-7; // We linearize below this angle.
const T scale = angle < kCutoffAngle ? T(2.) : angle / sin(angle / T(2.)); const T scale = angle < kCutoffAngle ? T(2.) : angle / sin(angle / T(2.));
return Eigen::Matrix<T, 3, 1>(scale * quaternion.x(), scale * quaternion.y(), return Eigen::Matrix<T, 3, 1>(scale * normalized_quaternion.x(),
scale * quaternion.z()); scale * normalized_quaternion.y(),
scale * normalized_quaternion.z());
} }
// Returns a quaternion representing the same rotation as the given 'angle_axis' // Returns a quaternion representing the same rotation as the given 'angle_axis'
// vector. // vector.
template <typename T> template <typename T>
Eigen::Quaternion<T> AngleAxisVectorToRotationQuaternion( Eigen::Quaternion<T> AngleAxisVectorToRotationQuaternion(
Eigen::Matrix<T, 3, 1> angle_axis) { const Eigen::Matrix<T, 3, 1>& angle_axis) {
T scale = T(0.5); T scale = T(0.5);
T w = T(1.); T w = T(1.);
constexpr double kCutoffAngle = 1e-8; // We linearize below this angle. constexpr double kCutoffAngle = 1e-8; // We linearize below this angle.