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

included Jacobian of logmap in quaternion mode

release/4.3a0
Luca 2014-12-08 18:39:47 -05:00
parent 54ee973309
commit 02f92e4e04
1 changed files with 12 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
gtsam/geometry/Rot3Q.cpp
View File

@ -146,21 +146,22 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector3 Rot3::Logmap(const Rot3& R) { Vector3 Rot3::Logmap(const Rot3& R, boost::optional<Matrix3&> H) {
using std::acos; using std::acos;
using std::sqrt; using std::sqrt;
static const double twoPi = 2.0 * M_PI, static const double twoPi = 2.0 * M_PI,
// define these compile time constants to avoid std::abs: // define these compile time constants to avoid std::abs:
NearlyOne = 1.0 - 1e-10, NearlyNegativeOne = -1.0 + 1e-10; NearlyOne = 1.0 - 1e-10, NearlyNegativeOne = -1.0 + 1e-10;
Vector3 thetaR;
const Quaternion& q = R.quaternion_; const Quaternion& q = R.quaternion_;
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
return (2 - 2 * (qw - 1) / 3) * q.vec(); thetaR = (2 - 2 * (qw - 1) / 3) * q.vec();
} else if (qw < NearlyNegativeOne) { } else if (qw < NearlyNegativeOne) {
// Angle is zero, return zero vector // Angle is zero, return zero vector
return Vector3::Zero(); thetaR = Vector3::Zero();
} 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);
@ -169,8 +170,14 @@ namespace gtsam {
angle -= twoPi; angle -= twoPi;
else if (angle < -M_PI) else if (angle < -M_PI)
angle += twoPi; angle += twoPi;
return (angle / s) * q.vec(); thetaR = (angle / s) * q.vec();
} }
if(H){
H->resize(3,3);
*H = Rot3::rightJacobianExpMapSO3inverse(thetaR);
}
return thetaR;
} }
/* ************************************************************************* */ /* ************************************************************************* */