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

11
gtsam/geometry/Pose3.cpp
View File

@ -240,17 +240,15 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) {
/* ************************************************************************* */ /* ************************************************************************* */
namespace pose3 { namespace pose3 {
class ExpmapFunctor : public so3::DexpFunctor { class GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor {
private: protected:
static constexpr double one_twenty_fourth = 1.0 / 24.0;
// Constant used in computeQ // Constant used in computeQ
double F; // (B - 0.5) / theta2 or -1/24 for theta->0 double F; // (B - 0.5) / theta2 or -1/24 for theta->0
public: public:
ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false) ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false)
: so3::DexpFunctor(omega, nearZeroApprox) { : 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 // Compute the bottom-left 3x3 block of the SE(3) Expmap derivative
@ -262,6 +260,9 @@ class ExpmapFunctor : public so3::DexpFunctor {
return -0.5 * V + C * (W * V + V * 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); 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 } // namespace pose3