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

Modernize ExmapTranslation

release/4.3a0
Frank Dellaert 2024-12-16 14:08:02 -05:00
parent b11387167d
commit 2aa36d4f7a
4 changed files with 50 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
gtsam/geometry/Pose3.cpp
View File

@ -164,22 +164,22 @@ Pose3 Pose3::interpolateRt(const Pose3& T, double t) const {
}
/* ************************************************************************* */
/** Modified from Murray94book version (which assumes w and v normalized?) */
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 Jw;
Rot3 R = Rot3::Expmap(w, Hxi ? &Jw : nullptr);
Matrix3 Jr;
Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr);
// Compute translation and optionally its Jacobian in w
// 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, R);
const Vector3 t = ExpmapTranslation(w, v, Hxi ? &Q : nullptr);
if (Hxi) {
*Hxi << Jw, Z_3x3,
Q, Jw;
*Hxi << Jr, Z_3x3, //
Q, Jr;
}
return Pose3(R, t);
@ -251,35 +251,39 @@ Matrix3 Pose3::ComputeQforExpmapDerivative(const Vector6& xi,
}
/* ************************************************************************* */
// 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,
const std::optional<Rot3>& R,
OptionalJacobian<3, 3> J,
double nearZeroThreshold) {
const double theta2 = w.dot(w);
bool nearZero = (theta2 <= nearZeroThreshold);
// Instantiate functor for Dexp-related operations:
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.
Matrix3 H;
Vector t = local.applyLeftJacobian(v, Q ? &H : nullptr);
// 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) {
// 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): 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));
Vector3 t = (w_cross_v - rotation * w_cross_v + t_parallel) / theta2;
return t;
}
if (J) {
*J = local.rightJacobian(); // = X * local.leftJacobian();
}
return t;
}
/* ************************************************************************* */

10
gtsam/geometry/Pose3.h
View File

@ -223,17 +223,19 @@ public:
const Vector6& xi, double nearZeroThreshold = 1e-5);
/**
* Compute the translation part of the exponential map, with derivative.
* 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 R Optionally, precomputed as Rot3::Expmap(w)
* @param J Optionally, compute 3x3 Jacobian wrpt v = right Jacobian of SO(3)
* @param nearZeroThreshold threshold for small values
* Note Q is 3x3 bottom-left block of SE3 Expmap right derivative matrix
* @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 = {},
const std::optional<Rot3>& R = {},
OptionalJacobian<3, 3> J = {},
double nearZeroThreshold = 1e-5);
using LieGroup<Pose3, 6>::inverse; // version with derivative

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

@ -304,6 +304,7 @@ TEST(SO3, JacobianLogmap) {
EXPECT(assert_equal(Jexpected, Jactual));
}
//******************************************************************************
namespace test_cases {
std::vector<Vector3> small{{0, 0, 0}, //
{1e-5, 0, 0}, {0, 1e-5, 0}, {0, 0, 1e-5}, //,

27
gtsam/navigation/NavState.cpp
View File

@ -114,24 +114,23 @@ NavState NavState::inverse() const {
//------------------------------------------------------------------------------
NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi) {
// Get angular velocity w, translational velocity v, and acceleration a
Vector3 w = xi.head<3>();
Vector3 rho = xi.segment<3>(3);
Vector3 nu = xi.tail<3>();
// Get angular velocity w and components rho and nu from xi
Vector3 w = xi.head<3>(), rho = xi.segment<3>(3), nu = xi.tail<3>();
// Compute rotation using Expmap
Rot3 R = Rot3::Expmap(w);
Matrix3 Jr;
Rot3 R = Rot3::Expmap(w, Hxi ? &Jr : nullptr);
// Compute translations and optionally their Jacobians
// 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, R);
Vector3 v = Pose3::ExpmapTranslation(w, nu, Hxi ? &Qv : nullptr, R);
Vector3 t = Pose3::ExpmapTranslation(w, rho, Hxi ? &Qt : nullptr);
Vector3 v = Pose3::ExpmapTranslation(w, nu, Hxi ? &Qv : nullptr);
if (Hxi) {
const Matrix3 Jw = Rot3::ExpmapDerivative(w);
*Hxi << Jw, Z_3x3, Z_3x3,
Qt, Jw, Z_3x3,
Qv, Z_3x3, Jw;
*Hxi << Jr, Z_3x3, Z_3x3, //
Qt, Jr, Z_3x3, //
Qv, Z_3x3, Jr;
}
return NavState(R, t, v);
@ -235,8 +234,8 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) {
Matrix3 Qt, Qv;
const Rot3 R = Rot3::Expmap(w);
Pose3::ExpmapTranslation(w, rho, Qt, R);
Pose3::ExpmapTranslation(w, nu, Qv, R);
Pose3::ExpmapTranslation(w, rho, Qt);
Pose3::ExpmapTranslation(w, nu, Qv);
const Matrix3 Jw = Rot3::LogmapDerivative(w);
const Matrix3 Qt2 = -Jw * Qt * Jw;
const Matrix3 Qv2 = -Jw * Qv * Jw;