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

Use consistent check on angle norm

release/4.3a0
Frank Dellaert 2015-07-05 15:39:15 -07:00
parent 4342aa5901
commit c978935e8e
2 changed files with 26 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
gtsam/geometry/Quaternion.h
View File

@ -20,6 +20,7 @@
#include <gtsam/base/Lie.h> #include <gtsam/base/Lie.h>
#include <gtsam/base/concepts.h> #include <gtsam/base/concepts.h>
#include <gtsam/geometry/SO3.h> // Logmap/Expmap derivatives #include <gtsam/geometry/SO3.h> // Logmap/Expmap derivatives
#include <limits>
#define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options> #define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options>
@ -73,14 +74,22 @@ struct traits<QUATERNION_TYPE> {
return g.inverse(); return g.inverse();
} }
/// Exponential map, simply be converting omega to axis/angle representation /// Exponential map, using the inlined code from Eigen's converseion from axis/angle
static Q Expmap(const Eigen::Ref<const TangentVector>& omega, static Q Expmap(const Eigen::Ref<const TangentVector>& omega,
ChartJacobian H = boost::none) { ChartJacobian H = boost::none) {
if(H) *H = SO3::ExpmapDerivative(omega); using std::cos;
if (omega.isZero()) return Q::Identity(); using std::sin;
else { if (H) *H = SO3::ExpmapDerivative(omega.template cast<double>());
_Scalar angle = omega.norm(); _Scalar theta2 = omega.dot(omega);
return Q(Eigen::AngleAxis<_Scalar>(angle, omega / angle)); if (theta2 > std::numeric_limits<_Scalar>::epsilon()) {
_Scalar theta = std::sqrt(theta2);
_Scalar ha = _Scalar(0.5) * theta;
Vector3 vec = (sin(ha) / theta) * omega;
return Q(cos(ha), vec.x(), vec.y(), vec.z());
} else {
// first order approximation sin(theta/2)/theta = 0.5
Vector3 vec = _Scalar(0.5) * omega;
return Q(1.0, vec.x(), vec.y(), vec.z());
} }
} }
@ -93,9 +102,9 @@ struct traits<QUATERNION_TYPE> {
static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-10, static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-10,
NearlyNegativeOne = -1.0 + 1e-10; NearlyNegativeOne = -1.0 + 1e-10;
Vector3 omega; TangentVector omega;
const double qw = q.w(); const _Scalar qw = q.w();
// See Quaternion-Logmap.nb in doc for Taylor expansions // See Quaternion-Logmap.nb in doc for Taylor expansions
if (qw > NearlyOne) { if (qw > NearlyOne) {
// Taylor expansion of (angle / s) at 1 // Taylor expansion of (angle / s) at 1
@ -107,7 +116,7 @@ struct traits<QUATERNION_TYPE> {
omega = (-8. / 3. - 2. / 3. * qw) * q.vec(); omega = (-8. / 3. - 2. / 3. * qw) * q.vec();
} else { } else {
// Normal, away from zero case // Normal, away from zero case
double angle = 2 * acos(qw), s = sqrt(1 - qw * qw); _Scalar angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
// Important: convert to [-pi,pi] to keep error continuous // Important: convert to [-pi,pi] to keep error continuous
if (angle > M_PI) if (angle > M_PI)
angle -= twoPi; angle -= twoPi;
@ -116,7 +125,7 @@ struct traits<QUATERNION_TYPE> {
omega = (angle / s) * q.vec(); omega = (angle / s) * q.vec();
} }
if(H) *H = SO3::LogmapDerivative(omega); if(H) *H = SO3::LogmapDerivative(omega.template cast<double>());
return omega; return omega;
} }

10
gtsam/geometry/SO3.cpp
View File

@ -133,8 +133,9 @@ Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
using std::cos; using std::cos;
using std::sin; using std::sin;
if(zero(omega)) return I_3x3; double theta2 = omega.dot(omega);
double theta = omega.norm(); // rotation angle if (theta2 <= std::numeric_limits<double>::epsilon()) return I_3x3;
double theta = std::sqrt(theta2); // rotation angle
#ifdef DUY_VERSION #ifdef DUY_VERSION
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version) /// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
Matrix3 X = skewSymmetric(omega); Matrix3 X = skewSymmetric(omega);
@ -164,8 +165,9 @@ Matrix3 SO3::LogmapDerivative(const Vector3& omega) {
using std::cos; using std::cos;
using std::sin; using std::sin;
if(zero(omega)) return I_3x3; double theta2 = omega.dot(omega);
double theta = omega.norm(); if (theta2 <= std::numeric_limits<double>::epsilon()) return I_3x3;
double theta = std::sqrt(theta2); // rotation angle
#ifdef DUY_VERSION #ifdef DUY_VERSION
/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version) /// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
Matrix3 X = skewSymmetric(omega); Matrix3 X = skewSymmetric(omega);