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

Hat and Vee in navigation

release/4.3a0
Frank Dellaert 2025-03-08 11:39:25 -05:00
parent e0c3be9ab7
commit 8cf3d04336
4 changed files with 116 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
gtsam/navigation/NavState.cpp
View File

@ -270,6 +270,29 @@ Matrix9 NavState::LogmapDerivative(const NavState& state) {
return J;
}
//------------------------------------------------------------------------------
Matrix5 NavState::Hat(const Vector9& xi) {
Matrix5 X;
const double wx = xi(0), wy = xi(1), wz = xi(2);
const double px = xi(3), py = xi(4), pz = xi(5);
const double vx = xi(6), vy = xi(7), vz = xi(8);
X << 0., -wz, wy, px, vx,
wz, 0., -wx, py, vy,
-wy, wx, 0., pz, vz,
0., 0., 0., 0., 0.,
0., 0., 0., 0., 0.;
return X;
}
//------------------------------------------------------------------------------
Vector9 NavState::Vee(const Matrix5& Xi) {
Vector9 xi;
xi << Xi(2, 1), Xi(0, 2), Xi(1, 0),
Xi(0, 3), Xi(1, 3), Xi(2, 3),
Xi(0, 4), Xi(1, 4), Xi(2, 4);
return xi;
}
//------------------------------------------------------------------------------
NavState NavState::ChartAtOrigin::Retract(const Vector9& xi,
ChartJacobian Hxi) {

25
gtsam/navigation/NavState.h
View File

@ -56,18 +56,27 @@ public:
NavState() :
t_(0, 0, 0), v_(Vector3::Zero()) {
}
/// Construct from attitude, position, velocity
NavState(const Rot3& R, const Point3& t, const Velocity3& v) :
R_(R), t_(t), v_(v) {
}
/// Construct from pose and velocity
NavState(const Pose3& pose, const Velocity3& v) :
R_(pose.rotation()), t_(pose.translation()), v_(v) {
}
/// Construct from SO(3) and R^6
NavState(const Matrix3& R, const Vector6& tv) :
R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) {
}
/// Construct from Matrix5
NavState(const Matrix5& T) :
R_(T.block<3, 3>(0, 0)), t_(T.block<3, 1>(0, 3)), v_(T.block<3, 1>(0, 4)) {
}
/// Named constructor with derivatives
static NavState Create(const Rot3& R, const Point3& t, const Velocity3& v,
OptionalJacobian<9, 3> H1 = {},
@ -154,10 +163,6 @@ public:
/// Syntactic sugar
const Rot3& rotation() const { return attitude(); };
/// @}
/// @name Lie Group
/// @{
// Tangent space sugar.
// TODO(frank): move to private navstate namespace in cpp
static Eigen::Block<Vector9, 3, 1> dR(Vector9& v) {
@ -189,6 +194,12 @@ public:
OptionalJacobian<9, 9> H1 = {}, OptionalJacobian<9, 9> H2 =
{}) const;
/// @}
/// @name Lie Group
/// @{
using LieAlgebra = Matrix5;
/**
* Exponential map at identity - create a NavState from canonical coordinates
* \f$ [R_x,R_y,R_z,T_x,T_y,T_z,V_x,V_y,V_z] \f$
@ -242,6 +253,12 @@ public:
static Vector9 Local(const NavState& state, ChartJacobian Hstate = {});
};
/// Hat maps from tangent vector to Lie algebra
static LieAlgebra Hat(const TangentVector& xi);
/// Vee maps from Lie algebra to tangent vector
static TangentVector Vee(const LieAlgebra& X);
/// @}
/// @name Dynamics
/// @{

15
gtsam/navigation/navigation.i
View File

@ -54,9 +54,24 @@ class NavState {
gtsam::Vector velocity() const;
gtsam::Pose3 pose() const;
// Manifold
gtsam::NavState retract(const gtsam::Vector& x) const;
gtsam::Vector localCoordinates(const gtsam::NavState& g) const;
// Lie Group
static gtsam::NavState Expmap(gtsam::Vector v);
static gtsam::Vector Logmap(const gtsam::NavState& p);
static gtsam::NavState Expmap(gtsam::Vector v, Eigen::Ref<Eigen::MatrixXd> H);
static gtsam::Vector Logmap(const gtsam::NavState& p, Eigen::Ref<Eigen::MatrixXd> H);
gtsam::NavState expmap(gtsam::Vector v);
gtsam::NavState expmap(gtsam::Vector v, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2);
gtsam::Vector logmap(const gtsam::NavState& p);
gtsam::Vector logmap(const gtsam::NavState& p, Eigen::Ref<Eigen::MatrixXd> H1, Eigen::Ref<Eigen::MatrixXd> H2);
gtsam::Matrix AdjointMap() const;
gtsam::Vector Adjoint(gtsam::Vector xi) const;
static gtsam::Matrix Hat(const gtsam::Vector& xi);
static gtsam::Vector Vee(const gtsam::Matrix& xi);
// enabling serialization functionality
void serialize() const;
};

57
gtsam/navigation/tests/testNavState.cpp
View File

@ -533,6 +533,63 @@ TEST(NavState, Adjoint_compose_full) {
EXPECT(assert_equal(expected, actual, 1e-6));
}
/* ************************************************************************* */
TEST(NavState, HatAndVee) {
// Create a few test vectors
Vector9 v1(1, 2, 3, 4, 5, 6, 7, 8, 9);
Vector9 v2(0.1, -0.5, 1.0, -1.0, 0.5, 2.0, 0.3, -0.2, 0.8);
Vector9 v3 = Vector9::Zero();
// Test that Vee(Hat(v)) == v for various inputs
EXPECT(assert_equal(v1, NavState::Vee(NavState::Hat(v1))));
EXPECT(assert_equal(v2, NavState::Vee(NavState::Hat(v2))));
EXPECT(assert_equal(v3, NavState::Vee(NavState::Hat(v3))));
// Check the structure of the Lie Algebra element
Matrix5 expected;
expected << 0, -3, 2, 4, 7,
3, 0, -1, 5, 8,
-2, 1, 0, 6, 9,
0, 0, 0, 0, 0,
0, 0, 0, 0, 0;
EXPECT(assert_equal(expected, NavState::Hat(v1)));
}
/* ************************************************************************* */
// Checks correct exponential map (Expmap) with brute force matrix exponential
TEST(NavState, BruteForceExpmap1) {
const Vector9 xi(0, 0, 0, 14, 24, 34, 15, 25, 35);
EXPECT(assert_equal(NavState::Expmap(xi), expm<NavState>(xi), 1e-6));
}
TEST(NavState, BruteForceExpmap2) {
const Vector9 xi(0.1, 0.2, 0.3, 0, 0, 0, 0, 0, 0);
EXPECT(assert_equal(NavState::Expmap(xi), expm<NavState>(xi), 1e-6));
}
TEST(NavState, BruteForceExpmap3) {
const Vector9 xi(0.1, 0.2, 0.3, 4, 5, 6, 7, 8, 9);
EXPECT(assert_equal(NavState::Expmap(xi), expm<NavState>(xi), 1e-6));
}
/* ************************************************************************* */
// assert that T*Hat(xi)*T^-1 is equal to Hat(Ad_T(xi))
TEST(NavState, Adjoint_hat)
{
Matrix5 expected = T.matrix() * NavState::Hat(screwNavState::xi) * T.matrix().inverse();
Matrix5 xiprime = NavState::Hat(T.Adjoint(screwNavState::xi));
EXPECT(assert_equal(expected, xiprime, 1e-6));
Matrix5 expected2 = T2.matrix() * NavState::Hat(screwNavState::xi) * T2.matrix().inverse();
Matrix5 xiprime2 = NavState::Hat(T2.Adjoint(screwNavState::xi));
EXPECT(assert_equal(expected2, xiprime2, 1e-6));
Matrix5 expected3 = T3.matrix() * NavState::Hat(screwNavState::xi) * T3.matrix().inverse();
Matrix5 xiprime3 = NavState::Hat(T3.Adjoint(screwNavState::xi));
EXPECT(assert_equal(expected3, xiprime3, 1e-6));
}
/* ************************************************************************* */
TEST(NavState, Retract_LocalCoordinates) {
Vector9 d;