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

Fix compilation

release/4.3a0
Frank Dellaert 2025-04-27 21:11:30 -04:00
parent ab605770fb
commit 51e89d298e
5 changed files with 37 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
examples/IEKF_NavstateExample.cpp
View File

@ -10,15 +10,15 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file LIEKF_NavstateExample.cpp * @file IEKF_NavstateExample.cpp
* @brief LIEKF on NavState (SE_2(3)) with IMU (predict) and GPS (update) * @brief InvariantEKF on NavState (SE_2(3)) with IMU (predict) and GPS (update)
* @date April 25, 2025 * @date April 25, 2025
* @authors Scott Baker, Matt Kielo, Frank Dellaert * @authors Scott Baker, Matt Kielo, Frank Dellaert
*/ */
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/OptionalJacobian.h> #include <gtsam/base/OptionalJacobian.h>
#include <gtsam/navigation/LIEKF.h> #include <gtsam/navigation/InvariantEKF.h>
#include <gtsam/navigation/NavState.h> #include <gtsam/navigation/NavState.h>
#include <iostream> #include <iostream>
@ -53,7 +53,7 @@ int main() {
// Initial state & covariances // Initial state & covariances
NavState X0; // R=I, v=0, t=0 NavState X0; // R=I, v=0, t=0
Matrix9 P0 = Matrix9::Identity() * 0.1; Matrix9 P0 = Matrix9::Identity() * 0.1;
LIEKF<NavState> ekf(X0, P0); InvariantEKF<NavState> ekf(X0, P0);
// Noise & timestep // Noise & timestep
double dt = 1.0; double dt = 1.0;

9
examples/IEKF_Rot3Example.cpp
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file LIEKF_Rot3Example.cpp * @file IEKF_Rot3Example.cpp
* @brief Left‐Invariant EKF on SO(3) with state‐dependent pitch/roll control * @brief Left‐Invariant EKF on SO(3) with state‐dependent pitch/roll control
* and a single magnetometer update. * and a single magnetometer update.
* @date April 25, 2025 * @date April 25, 2025
@ -20,11 +20,10 @@
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/OptionalJacobian.h> #include <gtsam/base/OptionalJacobian.h>
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
#include <gtsam/navigation/LIEKF.h> #include <gtsam/navigation/InvariantEKF.h>
#include <iostream> #include <iostream>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -39,7 +38,7 @@ Vector3 dynamicsSO3(const Rot3& X, OptionalJacobian<3, 3> H = {}) {
D_phi.row(2).setZero(); D_phi.row(2).setZero();
if (H) *H = -k * D_phi; // ∂(–kφ)/∂δR if (H) *H = -k * D_phi; // ∂(–kφ)/∂δR
return -k * phi; // xi ∈ 𝔰𝔬(3) return -k * phi; // xi ∈ 𝔰𝔬(3)
} }
// --- 2) Magnetometer model: z = R⁻¹ m, H = –[z]_× --- // --- 2) Magnetometer model: z = R⁻¹ m, H = –[z]_× ---
@ -54,7 +53,7 @@ int main() {
// Initial estimate (identity) and covariance // Initial estimate (identity) and covariance
const Rot3 R0 = Rot3::RzRyRx(0.1, -0.2, 0.3); const Rot3 R0 = Rot3::RzRyRx(0.1, -0.2, 0.3);
const Matrix3 P0 = Matrix3::Identity() * 0.1; const Matrix3 P0 = Matrix3::Identity() * 0.1;
LIEKF<Rot3> ekf(R0, P0); InvariantEKF<Rot3> ekf(R0, P0);
// Timestep, process noise, measurement noise // Timestep, process noise, measurement noise
double dt = 0.1; double dt = 0.1;

25
examples/IEKF_SE2Example.cpp
View File

@ -10,17 +10,17 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file LIEKF_SE2_SimpleGPSExample.cpp * @file IEKF_SE2Example.cpp
* @brief A left invariant Extended Kalman Filter example using a Lie group * @brief A left invariant Extended Kalman Filter example using a Lie group
* odometry as the prediction stage on SE(2) and * odometry as the prediction stage on SE(2) and
* *
* This example uses the templated LIEKF class to estimate the state of * This example uses the templated InvariantEKF class to estimate the state of
* an object using odometry / GPS measurements The prediction stage of the LIEKF * an object using odometry / GPS measurements The prediction stage of the
* uses a Lie Group element to propagate the stage in a discrete LIEKF. For most * InvariantEKF uses a Lie Group element to propagate the stage in a discrete
* cases, U = exp(u^ * dt) if u is a control vector that is constant over the * InvariantEKF. For most cases, U = exp(u^ * dt) if u is a control vector that
* interval dt. However, if u is not constant over dt, other approaches are * is constant over the interval dt. However, if u is not constant over dt,
* needed to find the value of U. This approach simply takes a Lie group element * other approaches are needed to find the value of U. This approach simply
* U, which can be found in various different ways. * takes a Lie group element U, which can be found in various different ways.
* *
* This data was compared to a left invariant EKF on SE(2) using identical * This data was compared to a left invariant EKF on SE(2) using identical
* measurements and noise from the source of the InEKF plugin * measurements and noise from the source of the InEKF plugin
@ -32,7 +32,7 @@
* @authors Scott Baker, Matt Kielo, Frank Dellaert * @authors Scott Baker, Matt Kielo, Frank Dellaert
*/ */
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/navigation/LIEKF.h> #include <gtsam/navigation/InvariantEKF.h>
#include <iostream> #include <iostream>
@ -45,8 +45,9 @@ using namespace gtsam;
Vector2 h_gps(const Pose2& X, OptionalJacobian<2, 3> H = {}) { Vector2 h_gps(const Pose2& X, OptionalJacobian<2, 3> H = {}) {
return X.translation(H); return X.translation(H);
} }
// Define a LIEKF class that uses the Pose2 Lie group as the state and the
// Vector2 measurement type. // Define a InvariantEKF class that uses the Pose2 Lie group as the state and
// the Vector2 measurement type.
int main() { int main() {
// // Initialize the filter's state, covariance, and time interval values. // // Initialize the filter's state, covariance, and time interval values.
Pose2 X0(0.0, 0.0, 0.0); Pose2 X0(0.0, 0.0, 0.0);
@ -54,7 +55,7 @@ int main() {
// Create the filter with the initial state, covariance, and measurement // Create the filter with the initial state, covariance, and measurement
// function. // function.
LIEKF<Pose2> ekf(X0, P0); InvariantEKF<Pose2> ekf(X0, P0);
// Define the process covariance and measurement covariance matrices Q and R. // Define the process covariance and measurement covariance matrices Q and R.
Matrix3 Q = (Vector3(0.05, 0.05, 0.001)).asDiagonal(); Matrix3 Q = (Vector3(0.05, 0.05, 0.001)).asDiagonal();

16
gtsam/navigation/InvariantEKF.h
View File

@ -10,10 +10,10 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file LIEKF.h * @file InvariantEKF.h
* @brief Left-Invariant Extended Kalman Filter (LIEKF) implementation * @brief Left-Invariant Extended Kalman Filter (InvariantEKF) implementation
* *
* This file defines the LIEKF class template for performing prediction and * This file defines the InvariantEKF class template for performing prediction and
* update steps of an Extended Kalman Filter on states residing in a Lie group. * update steps of an Extended Kalman Filter on states residing in a Lie group.
* The class supports state evolution via group composition and dynamics * The class supports state evolution via group composition and dynamics
* functions, along with measurement updates using tangent-space corrections. * functions, along with measurement updates using tangent-space corrections.
@ -34,8 +34,8 @@
namespace gtsam { namespace gtsam {
/** /**
* @class LIEKF * @class InvariantEKF
* @brief Left-Invariant Extended Kalman Filter (LIEKF) on a Lie group G * @brief Left-Invariant Extended Kalman Filter (InvariantEKF) on a Lie group G
* *
* @tparam G Lie group type providing: * @tparam G Lie group type providing:
* - static int dimension = tangent dimension * - static int dimension = tangent dimension
@ -50,7 +50,7 @@ namespace gtsam {
* using the left-invariant error in the tangent space. * using the left-invariant error in the tangent space.
*/ */
template <typename G> template <typename G>
class LIEKF { class InvariantEKF {
public: public:
/// Tangent-space dimension /// Tangent-space dimension
static constexpr int n = traits<G>::dimension; static constexpr int n = traits<G>::dimension;
@ -59,7 +59,7 @@ class LIEKF {
using MatrixN = Eigen::Matrix<double, n, n>; using MatrixN = Eigen::Matrix<double, n, n>;
/// Constructor: initialize with state and covariance /// Constructor: initialize with state and covariance
LIEKF(const G& X0, const Matrix& P0) : X_(X0), P_(P0) {} InvariantEKF(const G& X0, const Matrix& P0) : X_(X0), P_(P0) {}
/// @return current state estimate /// @return current state estimate
const G& state() const { return X_; } const G& state() const { return X_; }
@ -229,6 +229,6 @@ class LIEKF {
// Define static identity I_n // Define static identity I_n
template <typename G> template <typename G>
const typename LIEKF<G>::MatrixN LIEKF<G>::I_n = LIEKF<G>::MatrixN::Identity(); const typename InvariantEKF<G>::MatrixN InvariantEKF<G>::I_n = InvariantEKF<G>::MatrixN::Identity();
} // namespace gtsam } // namespace gtsam

16
gtsam/navigation/tests/testInvariantEKF.cpp
View File

@ -8,8 +8,8 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file testLIEKFNavState.cpp * @file testInvariantEKFNavState.cpp
* @brief Unit test for the NavState dynamics Jacobian in LIEKF example. * @brief Unit test for the NavState dynamics Jacobian in InvariantEKF example.
* @date April 26, 2025 * @date April 26, 2025
* @authors Scott Baker, Matt Kielo, Frank Dellaert * @authors Scott Baker, Matt Kielo, Frank Dellaert
*/ */
@ -18,12 +18,12 @@
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
#include <gtsam/navigation/LIEKF.h> #include <gtsam/navigation/InvariantEKF.h>
#include <gtsam/navigation/NavState.h> #include <gtsam/navigation/NavState.h>
using namespace gtsam; using namespace gtsam;
// Duplicate the dynamics function in LIEKF_Rot3Example // Duplicate the dynamics function in InvariantEKF_Rot3Example
namespace exampleSO3 { namespace exampleSO3 {
static constexpr double k = 0.5; static constexpr double k = 0.5;
Vector3 dynamics(const Rot3& X, OptionalJacobian<3, 3> H = {}) { Vector3 dynamics(const Rot3& X, OptionalJacobian<3, 3> H = {}) {
@ -64,12 +64,12 @@ TEST(IEKF, PredictNumericState) {
// Analytic Jacobian // Analytic Jacobian
Matrix3 actualH; Matrix3 actualH;
LIEKF<Rot3> iekf0(R0, P0); InvariantEKF<Rot3> iekf0(R0, P0);
iekf0.predictMean(exampleSO3::dynamics, dt, Q, actualH); iekf0.predictMean(exampleSO3::dynamics, dt, Q, actualH);
// wrap predict into a state->state functor (mapping on SO(3)) // wrap predict into a state->state functor (mapping on SO(3))
auto g = [&](const Rot3& R) -> Rot3 { auto g = [&](const Rot3& R) -> Rot3 {
LIEKF<Rot3> iekf(R, P0); InvariantEKF<Rot3> iekf(R, P0);
return iekf.predictMean(exampleSO3::dynamics, dt, Q); return iekf.predictMean(exampleSO3::dynamics, dt, Q);
}; };
@ -94,12 +94,12 @@ TEST(IEKF, StateAndControl) {
// Analytic Jacobian // Analytic Jacobian
Matrix3 actualH; Matrix3 actualH;
LIEKF<Rot3> iekf0(R0, P0); InvariantEKF<Rot3> iekf0(R0, P0);
iekf0.predictMean(f, dummy_u, dt, Q, actualH); iekf0.predictMean(f, dummy_u, dt, Q, actualH);
// wrap predict into a state->state functor (mapping on SO(3)) // wrap predict into a state->state functor (mapping on SO(3))
auto g = [&](const Rot3& R) -> Rot3 { auto g = [&](const Rot3& R) -> Rot3 {
LIEKF<Rot3> iekf(R, P0); InvariantEKF<Rot3> iekf(R, P0);
return iekf.predictMean(f, dummy_u, dt, Q); return iekf.predictMean(f, dummy_u, dt, Q);
}; };