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

No more functor for Q

release/4.3a0
Frank Dellaert 2024-12-16 13:28:19 -05:00
parent 98697251bd
commit b11387167d
2 changed files with 26 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
gtsam/geometry/Pose3.cpp
View File

@ -22,6 +22,7 @@
#include <cmath>
#include <iostream>
#include <string>
#include "gtsam/geometry/Rot3.h"
namespace gtsam {
@ -239,30 +240,14 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& pose, ChartJacobian Hpose) {
#endif
}
/* ************************************************************************* */
namespace pose3 {
struct GTSAM_EXPORT ExpmapFunctor : public so3::DexpFunctor {
ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false)
: so3::DexpFunctor(omega, nearZeroApprox) {
}
// Compute the bottom-left 3x3 block of the SE(3) Expmap derivative.
Matrix3 computeQ(const Vector3& v) const {
// X translate from left to right for our right expmap convention:
Matrix X = rightJacobian() * leftJacobianInverse();
Matrix3 H;
applyLeftJacobian(v, H);
return X * H;
}
};
} // namespace pose3
/* ************************************************************************* */
Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi,
double nearZeroThreshold) {
const auto w = xi.head<3>();
const auto v = xi.tail<3>();
return pose3::ExpmapFunctor(w).computeQ(v);
Matrix3 Q;
ExpmapTranslation(w, v, Q, {}, nearZeroThreshold);
return Q;
}
/* ************************************************************************* */
@ -273,13 +258,22 @@ Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v,
const double theta2 = w.dot(w);
bool nearZero = (theta2 <= nearZeroThreshold);
if (Q) *Q = pose3::ExpmapFunctor(w, nearZero).computeQ(v);
if (nearZero) {
return v + 0.5 * w.cross(v);
if (Q) {
// Instantiate functor for Dexp-related operations:
so3::DexpFunctor local(w, nearZero);
// X translate from left to right for our right expmap convention:
Matrix3 X = local.rightJacobian() * local.leftJacobianInverse();
Matrix3 H;
Vector t = local.applyLeftJacobian(v, H);
*Q = X * H;
return t;
} else if (nearZero) {
// (I_3x3 + B * W + C * WW) * v with B->0.5, C->1/6
Vector3 Wv = w.cross(v);
return v + 0.5 * Wv + w.cross(Wv) * so3::DexpFunctor::one_sixth;
} else {
// NOTE(Frank): t can also be computed by calling applyLeftJacobian(v), but
// formulas below convey geometric insight and creating functor is not free.
// NOTE(Frank): if Q is not asked we use formulas below instead of calling
// applyLeftJacobian(v), as they convey geometric insight and are faster.
Vector3 t_parallel = w * w.dot(v); // translation parallel to axis
Vector3 w_cross_v = w.cross(v); // translation orthogonal to axis
Rot3 rotation = R.value_or(Rot3::Expmap(w));
@ -314,7 +308,6 @@ const Point3& Pose3::translation(OptionalJacobian<3, 6> Hself) const {
}
/* ************************************************************************* */
const Rot3& Pose3::rotation(OptionalJacobian<3, 6> Hself) const {
if (Hself) {
*Hself << I_3x3, Z_3x3;

11
gtsam/geometry/tests/testPose3.cpp
View File

@ -900,17 +900,6 @@ TEST(Pose3, ExpmapDerivative4) {
}
}
TEST( Pose3, ExpmapDerivativeQr) {
Vector6 w = Vector6::Random();
w.head<3>().normalize();
w.head<3>() = w.head<3>() * 0.9e-2;
Matrix3 actualQr = Pose3::ComputeQforExpmapDerivative(w, 0.01);
Matrix expectedH = numericalDerivative21<Pose3, Vector6,
OptionalJacobian<6, 6> >(&Pose3::Expmap, w, {});
Matrix3 expectedQr = expectedH.bottomLeftCorner<3, 3>();
EXPECT(assert_equal(expectedQr, actualQr, 1e-6));
}
/* ************************************************************************* */
TEST( Pose3, LogmapDerivative) {
Matrix6 actualH;