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

Merge pull request #1979 from borglab/fix/faster_expmaps 

Faster SE(3) and SE_2(3) exponential maps
release/4.3a0
Frank Dellaert 2025-01-16 14:43:57 -05:00 committed by GitHub
parent 3c382dd47f 2042977d28
commit 61f2fdd042
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
6 changed files with 84 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

76
gtsam/geometry/Pose3.cpp
View File

@ -183,18 +183,37 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi) {
// Get angular velocity omega and translational velocity v from twist xi
const Vector3 w = xi.head<3>(), v = xi.tail<3>();
// Compute rotation using Expmap
Matrix3 Jr;
Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr);
// Instantiate functor for Dexp-related operations:
const bool nearZero = (w.dot(w) <= 1e-5);
const so3::DexpFunctor local(w, nearZero);
// Compute translation and optionally its Jacobian Q in w
// The Jacobian in v is the right Jacobian Jr of SO(3), which we already have.
Matrix3 Q;
const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr);
// Compute rotation using Expmap
#ifdef GTSAM_USE_QUATERNIONS
const Rot3 R = traits<gtsam::Quaternion>::Expmap(w);
#else
const Rot3 R(local.expmap());
#endif
// The translation t = local.leftJacobian() * v.
// Here we call applyLeftJacobian, which is faster if you don't need
// Jacobians, and returns Jacobian of t with respect to w if asked.
// NOTE(Frank): t = applyLeftJacobian(v) does the same as the intuitive formulas
// t_parallel = w * w.dot(v); // translation parallel to axis
// w_cross_v = w.cross(v); // translation orthogonal to axis
// t = (w_cross_v - Rot3::Expmap(w) * w_cross_v + t_parallel) / theta2;
// but functor does not need R, deals automatically with the case where theta2
// is near zero, and also gives us the machinery for the Jacobians.
Matrix3 H;
const Vector3 t = local.applyLeftJacobian(v, Hxi ? &H : nullptr);
if (Hxi) {
*Hxi << Jr, Z_3x3, //
Q, Jr;
// The Jacobian of expmap is given by the right Jacobian of SO(3):
const Matrix3 Jr = local.rightJacobian();
// We are creating a Pose3, so we still need to chain H with R^T, the
// Jacobian of Pose3::Create with respect to t.
const Matrix3 Q = R.matrix().transpose() * H;
*Hxi << Jr, Z_3x3, // Jr here *is* the Jacobian of expmap
Q, Jr; // Here Jr = R^T * J_l, with J_l the Jacobian of t in v.
}
return Pose3(R, t);
@ -260,45 +279,18 @@ Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi,
double nearZeroThreshold) {
const auto w = xi.head<3>();
const auto v = xi.tail<3>();
Matrix3 Q;
ExpmapTranslation(w, v, Q, {}, nearZeroThreshold);
return Q;
}
/* ************************************************************************* */
// NOTE(Frank): t = applyLeftJacobian(v) does the same as the intuitive formulas
// t_parallel = w * w.dot(v); // translation parallel to axis
// w_cross_v = w.cross(v); // translation orthogonal to axis
// t = (w_cross_v - Rot3::Expmap(w) * w_cross_v + t_parallel) / theta2;
// but functor does not need R, deals automatically with the case where theta2
// is near zero, and also gives us the machinery for the Jacobians.
Vector3 Pose3::ExpmapTranslation(const Vector3& w, const Vector3& v,
OptionalJacobian<3, 3> Q,
OptionalJacobian<3, 3> J,
double nearZeroThreshold) {
const double theta2 = w.dot(w);
bool nearZero = (theta2 <= nearZeroThreshold);
// Instantiate functor for Dexp-related operations:
bool nearZero = (w.dot(w) <= nearZeroThreshold);
so3::DexpFunctor local(w, nearZero);
// Call applyLeftJacobian which is faster than local.leftJacobian() * v if you
// don't need Jacobians, and returns Jacobian of t with respect to w if asked.
// Call applyLeftJacobian to get its Jacobian
Matrix3 H;
Vector t = local.applyLeftJacobian(v, Q ? &H : nullptr);
local.applyLeftJacobian(v, H);
// We return Jacobians for use in Expmap, so we multiply with X, that
// translates from left to right for our right expmap convention:
if (Q) {
Matrix3 X = local.rightJacobian() * local.leftJacobianInverse();
*Q = X * H;
}
if (J) {
*J = local.rightJacobian(); // = X * local.leftJacobian();
}
return t;
// Multiply with R^T to account for the Pose3::Create Jacobian.
const Matrix3 R = local.expmap();
return R.transpose() * H;
}
/* ************************************************************************* */

17
gtsam/geometry/Pose3.h
View File

@ -220,27 +220,10 @@ public:
* (see Chirikjian11book2, pg 44, eq 10.95.
* The closed-form formula is identical to formula 102 in Barfoot14tro where
* Q_l of the SE3 Expmap left derivative matrix is given.
* This is the Jacobian of ExpmapTranslation and computed there.
*/
static Matrix3 ComputeQforExpmapDerivative(
const Vector6& xi, double nearZeroThreshold = 1e-5);
/**
* Compute the translation part of the exponential map, with Jacobians.
* @param w 3D angular velocity
* @param v 3D velocity
* @param Q Optionally, compute 3x3 Jacobian wrpt w
* @param J Optionally, compute 3x3 Jacobian wrpt v = right Jacobian of SO(3)
* @param nearZeroThreshold threshold for small values
* @note This function returns Jacobians Q and J corresponding to the bottom
* blocks of the SE(3) exponential, and translated from left to right from the
* applyLeftJacobian Jacobians.
*/
static Vector3 ExpmapTranslation(const Vector3& w, const Vector3& v,
OptionalJacobian<3, 3> Q = {},
OptionalJacobian<3, 3> J = {},
double nearZeroThreshold = 1e-5);
using LieGroup<Pose3, 6>::inverse; // version with derivative
/**

12
gtsam/geometry/SO3.cpp
View File

@ -92,7 +92,7 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle,
init(nearZeroApprox);
}
SO3 ExpmapFunctor::expmap() const { return SO3(I_3x3 + A * W + B * WW); }
Matrix3 ExpmapFunctor::expmap() const { return I_3x3 + A * W + B * WW; }
DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
: ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
@ -193,7 +193,7 @@ Vector3 DexpFunctor::applyLeftJacobianInverse(const Vector3& v,
template <>
GTSAM_EXPORT
SO3 SO3::AxisAngle(const Vector3& axis, double theta) {
return so3::ExpmapFunctor(axis, theta).expmap();
return SO3(so3::ExpmapFunctor(axis, theta).expmap());
}
//******************************************************************************
@ -251,11 +251,11 @@ template <>
GTSAM_EXPORT
SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
if (H) {
so3::DexpFunctor impl(omega);
*H = impl.dexp();
return impl.expmap();
so3::DexpFunctor local(omega);
*H = local.dexp();
return SO3(local.expmap());
} else {
return so3::ExpmapFunctor(omega).expmap();
return SO3(so3::ExpmapFunctor(omega).expmap());
}
}

2
gtsam/geometry/SO3.h
View File

@ -150,7 +150,7 @@ struct GTSAM_EXPORT ExpmapFunctor {
ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false);
/// Rodrigues formula
SO3 expmap() const;
Matrix3 expmap() const;
protected:
void init(bool nearZeroApprox = false);

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

@ -163,22 +163,22 @@ TEST(SO3, ExpmapFunctor) {
// axis angle version
so3::ExpmapFunctor f1(axis, angle);
SO3 actual1 = f1.expmap();
SO3 actual1(f1.expmap());
CHECK(assert_equal(expected, actual1.matrix(), 1e-5));
// axis angle version, negative angle
so3::ExpmapFunctor f2(axis, angle - 2*M_PI);
SO3 actual2 = f2.expmap();
SO3 actual2(f2.expmap());
CHECK(assert_equal(expected, actual2.matrix(), 1e-5));
// omega version
so3::ExpmapFunctor f3(axis * angle);
SO3 actual3 = f3.expmap();
SO3 actual3(f3.expmap());
CHECK(assert_equal(expected, actual3.matrix(), 1e-5));
// omega version, negative angle
so3::ExpmapFunctor f4(axis * (angle - 2*M_PI));
SO3 actual4 = f4.expmap();
SO3 actual4(f4.expmap());
CHECK(assert_equal(expected, actual4.matrix(), 1e-5));
}

56
gtsam/navigation/NavState.cpp
View File

@ -117,20 +117,32 @@ NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) {
// Get angular velocity w and components rho (for t) and nu (for v) from xi
Vector3 w = xi.head<3>(), rho = xi.segment<3>(3), nu = xi.tail<3>();
// Compute rotation using Expmap
Matrix3 Jr;
Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr);
// Instantiate functor for Dexp-related operations:
const bool nearZero = (w.dot(w) <= 1e-5);
const so3::DexpFunctor local(w, nearZero);
// Compute translations and optionally their Jacobians Q in w
// The Jacobians with respect to rho and nu are equal to Jr
Matrix3 Qt, Qv;
Vector3 t = Pose3::ExpmapTranslation(w, rho, Hxi ? &Qt : nullptr);
Vector3 v = Pose3::ExpmapTranslation(w, nu, Hxi ? &Qv : nullptr);
// Compute rotation using Expmap
#ifdef GTSAM_USE_QUATERNIONS
const Rot3 R = traits<gtsam::Quaternion>::Expmap(w);
#else
const Rot3 R(local.expmap());
#endif
// Compute translation and velocity. See Pose3::Expmap
Matrix3 H_t_w, H_v_w;
const Vector3 t = local.applyLeftJacobian(rho, Hxi ? &H_t_w : nullptr);
const Vector3 v = local.applyLeftJacobian(nu, Hxi ? &H_v_w : nullptr);
if (Hxi) {
*Hxi << Jr, Z_3x3, Z_3x3, //
Qt, Jr, Z_3x3, //
Qv, Z_3x3, Jr;
const Matrix3 Jr = local.rightJacobian();
// We are creating a NavState, so we still need to chain H_t_w and H_v_w
// with R^T, the Jacobian of Navstate::Create with respect to both t and v.
const Matrix3 M = R.matrix();
*Hxi << Jr, Z_3x3, Z_3x3, // Jr here *is* the Jacobian of expmap
M.transpose() * H_t_w, Jr, Z_3x3, //
M.transpose() * H_v_w, Z_3x3, Jr;
// In the last two rows, Jr = R^T * J_l, see Barfoot eq. (8.83).
// J_l is the Jacobian of applyLeftJacobian in the second argument.
}
return NavState(R, t, v);
@ -231,11 +243,22 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) {
const Vector3 w = xi.head<3>();
Vector3 rho = xi.segment<3>(3);
Vector3 nu = xi.tail<3>();
Matrix3 Qt, Qv;
const Rot3 R = Rot3::Expmap(w);
Pose3::ExpmapTranslation(w, rho, Qt);
Pose3::ExpmapTranslation(w, nu, Qv);
// Instantiate functor for Dexp-related operations:
const bool nearZero = (w.dot(w) <= 1e-5);
const so3::DexpFunctor local(w, nearZero);
// Call applyLeftJacobian to get its Jacobians
Matrix3 H_t_w, H_v_w;
local.applyLeftJacobian(rho, H_t_w);
local.applyLeftJacobian(nu, H_v_w);
// Multiply with R^T to account for NavState::Create Jacobian.
const Matrix3 R = local.expmap();
const Matrix3 Qt = R.transpose() * H_t_w;
const Matrix3 Qv = R.transpose() * H_v_w;
// Now compute the blocks of the LogmapDerivative Jacobian
const Matrix3 Jw = Rot3::LogmapDerivative(w);
const Matrix3 Qt2 = -Jw * Qt * Jw;
const Matrix3 Qv2 = -Jw * Qv * Jw;
@ -247,7 +270,6 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) {
return J;
}
//------------------------------------------------------------------------------
NavState NavState::ChartAtOrigin::Retract(const Vector9& xi,
ChartJacobian Hxi) {