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

Use inverse() beyond pi

release/4.3a0
Frank Dellaert 2025-04-18 09:13:05 -04:00
parent 52efba91d8
commit e9e839f6a5
3 changed files with 19 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam/geometry/SO3.cpp
View File

@ -144,6 +144,16 @@ DexpFunctor::DexpFunctor(const Vector3& omega, double nearZeroThresholdSq, doubl
DexpFunctor::DexpFunctor(const Vector3& omega) DexpFunctor::DexpFunctor(const Vector3& omega)
: DexpFunctor(omega, kNearZeroThresholdSq, kNearPiThresholdSq) {} : DexpFunctor(omega, kNearZeroThresholdSq, kNearPiThresholdSq) {}
Matrix3 DexpFunctor::rightJacobianInverse() const {
if (theta > M_PI) return rightJacobian().inverse();
return I_3x3 + 0.5 * W + D * WW;
}
Matrix3 DexpFunctor::leftJacobianInverse() const {
if (theta > M_PI) return leftJacobian().inverse();
return I_3x3 - 0.5 * W + D * WW;
}
// Multiplies v with left Jacobian through vector operations only. // Multiplies v with left Jacobian through vector operations only.
Vector3 DexpFunctor::applyRightJacobian(const Vector3& v, OptionalJacobian<3, 3> H1, Vector3 DexpFunctor::applyRightJacobian(const Vector3& v, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const { OptionalJacobian<3, 3> H2) const {
@ -177,7 +187,7 @@ Vector3 DexpFunctor::applyRightJacobianInverse(const Vector3& v, OptionalJacobia
return c; return c;
} }
Vector3 so3::DexpFunctor::applyLeftJacobian(const Vector3& v, Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v,
OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const {
const Vector3 Wv = gtsam::cross(omega, v); const Vector3 Wv = gtsam::cross(omega, v);
@ -196,7 +206,7 @@ Vector3 so3::DexpFunctor::applyLeftJacobian(const Vector3& v,
return v + B * Wv + C * WWv; return v + B * Wv + C * WWv;
} }
Vector3 so3::DexpFunctor::applyLeftJacobianInverse(const Vector3& v, Vector3 DexpFunctor::applyLeftJacobianInverse(const Vector3& v,
OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const {
const Matrix3 invJl = leftJacobianInverse(); const Matrix3 invJl = leftJacobianInverse();
const Vector3 c = invJl * v; const Vector3 c = invJl * v;

6
gtsam/geometry/SO3.h
View File

@ -193,10 +193,12 @@ struct GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
Matrix3 leftJacobian() const { return I_3x3 + B * W + C * WW; } Matrix3 leftJacobian() const { return I_3x3 + B * W + C * WW; }
/// Inverse of right Jacobian /// Inverse of right Jacobian
Matrix3 rightJacobianInverse() const { return I_3x3 + 0.5 * W + D * WW; } /// For |omega|>pi uses rightJacobian().inverse(), as unstable beyond pi!
Matrix3 rightJacobianInverse() const;
// Inverse of left Jacobian // Inverse of left Jacobian
Matrix3 leftJacobianInverse() const { return I_3x3 - 0.5 * W + D * WW; } /// For |omega|>pi uses leftJacobian().inverse(), as unstable beyond pi!
Matrix3 leftJacobianInverse() const;
/// Multiplies with rightJacobian(), with optional derivatives /// Multiplies with rightJacobian(), with optional derivatives
Vector3 applyRightJacobian(const Vector3& v, Vector3 applyRightJacobian(const Vector3& v,

4
gtsam/geometry/tests/testSO3.cpp
View File

@ -335,7 +335,9 @@ namespace test_cases {
{1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}, //, {1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}, //,
{1e-4, 0, 0}, {0, 1e-4, 0}, {0, 0, 1e-4} }; {1e-4, 0, 0}, {0, 1e-4, 0}, {0, 0, 1e-4} };
std::vector<Vector3> large{ std::vector<Vector3> large{
{0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.1, 0.2, 0.3} }; {0, 0, 0}, {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.1, 0.2, 0.3},
{0.4, 0.5, 0.6}, {0.7, 0.8, 0.9}, {1.1, 1.2, 1.3}, {1.4, 1.5, 1.6},
{1.7, 1.8, 1.9}, {2, 2, 2}, {3, 3, 3}, {4, 4, 4}, {5, 5, 5} };
auto omegas = [](bool nearZero) { return nearZero ? small : large; }; auto omegas = [](bool nearZero) { return nearZero ? small : large; };
std::vector<Vector3> vs{ {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.4, 0.3, 0.2} }; std::vector<Vector3> vs{ {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, {0.4, 0.3, 0.2} };
} // namespace test_cases } // namespace test_cases