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

Incredible simplification using E and F

release/4.3a0
Frank Dellaert 2024-12-15 17:55:22 -05:00
parent e96d8487e4
commit 9b0f3c3b38
1 changed files with 8 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
gtsam/geometry/Pose3.cpp
View File

@ -243,11 +243,15 @@ namespace pose3 {
class ExpmapFunctor : public so3::DexpFunctor {
private:
static constexpr double one_twenty_fourth = 1.0 / 24.0;
static constexpr double one_one_hundred_twentieth = 1.0 / 120.0;
// Constant used in computeQ
double F; // (B - 0.5) / theta2 or -1/24 for theta->0
public:
ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false)
: so3::DexpFunctor(omega, nearZeroApprox) {}
: so3::DexpFunctor(omega, nearZeroApprox) {
F = nearZero ? - one_twenty_fourth : (B - 0.5) / theta2;
}
// Compute the bottom-left 3x3 block of the SE(3) Expmap derivative
// TODO(Frank): t = applyLeftJacobian(v), it would be nice to understand
@ -255,17 +259,8 @@ class ExpmapFunctor : public so3::DexpFunctor {
Matrix3 computeQ(const Vector3& v) const {
const Matrix3 V = skewSymmetric(v);
const Matrix3 WVW = W * V * W;
if (!nearZero) {
// Simplified from closed-form formula in Barfoot14tro eq. (102)
return -0.5 * V + C * (W * V + V * W - WVW) +
(B - 0.5) / theta2 * (WW * V + V * WW - 3 * WVW) -
0.5 * (B - 3 * C) / theta2 * (WVW * W + W * WVW);
} else {
return -0.5 * V + one_sixth * (W * V + V * W - WVW) -
one_twenty_fourth * (WW * V + V * WW - 3 * WVW) +
one_one_hundred_twentieth * (WVW * W + W * WVW);
}
return -0.5 * V + C * (W * V + V * W - WVW) +
F * (WW * V + V * WW - 3 * WVW) - 0.5 * E * (WVW * W + W * WVW);
}
};
} // namespace pose3