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

Use X to map left to right

release/4.3a0
Frank Dellaert 2024-12-16 12:48:52 -05:00
parent 76c9537847
commit 6c84a2b539
1 changed files with 7 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam/geometry/Pose3.cpp
View File

@ -22,6 +22,7 @@
#include <cmath>
#include <iostream>
#include <string>
#include "gtsam/geometry/Rot3.h"
namespace gtsam {
@ -241,25 +242,18 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) {
/* ************************************************************************* */
namespace pose3 {
struct GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor {
// Constant used in computeQ
double F; // (B - 0.5) / theta2 or -1/24 for theta->0
ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false)
: 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
// how to compute mess below from applyLeftJacobian derivatives in w and v.
// Compute the bottom-left 3x3 block of the SE(3) Expmap derivative.
Matrix3 computeQ(const Vector3& v) const {
const Matrix3 V = skewSymmetric(v);
const Matrix3 WVW = W * V * W;
return -0.5 * V + C * (W * V + V * W - WVW) +
F * (WW * V + V * WW - 3 * WVW) - 0.5 * E * (WVW * W + W * WVW);
// X translate from left to right for our right expmap convention:
Matrix X = rightJacobian() * leftJacobianInverse();
Matrix3 H;
applyLeftJacobian(v, H);
return X * H;
}
static constexpr double _one_twenty_fourth = - 1.0 / 24.0;
};
} // namespace pose3