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

Export

release/4.3a0
Frank Dellaert 2024-12-15 23:39:45 -05:00
parent 9b0f3c3b38
commit a32bb7bf09
1 changed files with 7 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/geometry/Pose3.cpp
View File

@ -240,18 +240,16 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) {
/* ************************************************************************* */
namespace pose3 {
class ExpmapFunctor : public so3::DexpFunctor {
private:
static constexpr double one_twenty_fourth = 1.0 / 24.0;
class GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor {
protected:
// 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) {
F = nearZero ? - one_twenty_fourth : (B - 0.5) / theta2;
}
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
@ -262,6 +260,9 @@ class ExpmapFunctor : public so3::DexpFunctor {
return -0.5 * V + C * (W * V + V * W - WVW) +
F * (WW * V + V * WW - 3 * WVW) - 0.5 * E * (WVW * W + W * WVW);
}
protected:
static constexpr double _one_twenty_fourth = - 1.0 / 24.0;
};
} // namespace pose3