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

Fixed derivatives

release/4.3a0
dellaert 2015-02-10 22:34:56 +01:00
parent c1d5b652cc
commit 101129a9c7
1 changed files with 8 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/geometry/Quaternion.h
View File

@ -19,6 +19,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
#define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options> #define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options>
@ -98,7 +99,7 @@ struct traits<QUATERNION_TYPE> {
_Scalar angle = omega.norm(); _Scalar angle = omega.norm();
return Q(Eigen::AngleAxis<_Scalar>(angle, omega / angle)); return Q(Eigen::AngleAxis<_Scalar>(angle, omega / angle));
} }
if (H) CONCEPT_NOT_IMPLEMENTED; if(H) *H = SO3::ExpmapDerivative(omega);
} }
/// We use our own Logmap, as there is a slight bug in Eigen /// We use our own Logmap, as there is a slight bug in Eigen
@ -110,6 +111,8 @@ 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;
const double qw = q.w(); const double qw = q.w();
if (qw > NearlyOne) { if (qw > NearlyOne) {
// Taylor expansion of (angle / s) at 1 // Taylor expansion of (angle / s) at 1
@ -118,7 +121,7 @@ struct traits<QUATERNION_TYPE> {
} else if (qw < NearlyNegativeOne) { } else if (qw < NearlyNegativeOne) {
// Taylor expansion of (angle / s) at -1 // Taylor expansion of (angle / s) at -1
//return (-2 - 2 * (1 + qw) / 3) * q.vec(); //return (-2 - 2 * (1 + qw) / 3) * q.vec();
return (-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); double angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
@ -127,10 +130,11 @@ struct traits<QUATERNION_TYPE> {
angle -= twoPi; angle -= twoPi;
else if (angle < -M_PI) else if (angle < -M_PI)
angle += twoPi; angle += twoPi;
return (angle / s) * q.vec(); omega = (angle / s) * q.vec();
} }
if (H) CONCEPT_NOT_IMPLEMENTED; if(H) *H = SO3::LogmapDerivative(omega);
return omega;
} }
/// @} /// @}