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

Merge remote-tracking branch 'origin/feature/SO3_refactor' into feature/ImuFactorPush2

release/4.3a0
Frank Dellaert 2016-02-01 13:08:15 -08:00
parent 4f214ad7d1 e3954d541a
commit d2a2fbbc82
1 changed files with 5 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
gtsam/geometry/SO3.cpp
View File

@ -63,7 +63,7 @@ SO3 ExpmapFunctor::expmap() const {
if (nearZero) if (nearZero)
return I_3x3 + W; return I_3x3 + W;
else else
return I_3x3 + sin_theta * K + one_minus_cos * K * K; return I_3x3 + sin_theta * K + one_minus_cos * KK;
} }
DexpFunctor::DexpFunctor(const Vector3& omega) DexpFunctor::DexpFunctor(const Vector3& omega)
@ -86,13 +86,12 @@ Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
} }
if (H1) { if (H1) {
// TODO(frank): Iserles hints that there should be a form I + c*K + d*KK // TODO(frank): Iserles hints that there should be a form I + c*K + d*KK
const Vector3 Kv = omega.cross(v / theta); const Vector3 Kv = K * v;
const Vector3 KKv = omega.cross(Kv / theta);
const Matrix3 T = skewSymmetric(v / theta);
const double Da = (sin_theta - 2.0 * a) / theta2; const double Da = (sin_theta - 2.0 * a) / theta2;
const double Db = (one_minus_cos - 3.0 * b) / theta2; const double Db = (one_minus_cos - 3.0 * b) / theta2;
*H1 = (-Da * Kv + Db * KKv) * omega.transpose() + a * T - *H1 = (Db * K - Da * I_3x3) * Kv * omega.transpose() -
skewSymmetric(Kv * b / theta) - b * K * T; skewSymmetric(Kv * b / theta) +
(a * I_3x3 - b * K) * skewSymmetric(v / theta);
} }
if (H2) *H2 = dexp_; if (H2) *H2 = dexp_;
return dexp_ * v; return dexp_ * v;