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
* @brief LIEKF on NavState (SE_2(3)) with IMU (predict) and GPS (update)
* @file IEKF_NavstateExample.cpp
* @brief InvariantEKF on NavState (SE_2(3)) with IMU (predict) and GPS (update)
* @date April 25, 2025
* @authors Scott Baker, Matt Kielo, Frank Dellaert
*/
#include <gtsam/base/Matrix.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/navigation/LIEKF.h>
#include <gtsam/navigation/InvariantEKF.h>
#include <gtsam/navigation/NavState.h>
#include <iostream>
@ -53,7 +53,7 @@ int main() {
// Initial state & covariances
NavState X0; // R=I, v=0, t=0
Matrix9 P0 = Matrix9::Identity() * 0.1;
LIEKF<NavState> ekf(X0, P0);
InvariantEKF<NavState> ekf(X0, P0);
// Noise & timestep
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
* and a single magnetometer update.
* @date April 25, 2025
@ -20,11 +20,10 @@
#include <gtsam/base/Matrix.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/navigation/LIEKF.h>
#include <gtsam/navigation/InvariantEKF.h>
#include <iostream>
using namespace std;
using namespace gtsam;
@ -39,7 +38,7 @@ Vector3 dynamicsSO3(const Rot3& X, OptionalJacobian<3, 3> H = {}) {
D_phi.row(2).setZero();
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]_× ---
@ -54,7 +53,7 @@ int main() {
// Initial estimate (identity) and covariance
const Rot3 R0 = Rot3::RzRyRx(0.1, -0.2, 0.3);
const Matrix3 P0 = Matrix3::Identity() * 0.1;
LIEKF<Rot3> ekf(R0, P0);
InvariantEKF<Rot3> ekf(R0, P0);
// Timestep, process noise, measurement noise
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
* odometry as the prediction stage on SE(2) and
*
* This example uses the templated LIEKF class to estimate the state of
* an object using odometry / GPS measurements The prediction stage of the LIEKF
* uses a Lie Group element to propagate the stage in a discrete LIEKF. For most
* cases, U = exp(u^ * dt) if u is a control vector that is constant over the
* interval dt. However, if u is not constant over dt, other approaches are
* needed to find the value of U. This approach simply takes a Lie group element
* U, which can be found in various different ways.
* This example uses the templated InvariantEKF class to estimate the state of
* an object using odometry / GPS measurements The prediction stage of the
* InvariantEKF uses a Lie Group element to propagate the stage in a discrete
* InvariantEKF. For most cases, U = exp(u^ * dt) if u is a control vector that
* is constant over the interval dt. However, if u is not constant over dt,
* other approaches are needed to find the value of U. This approach simply
* 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
* measurements and noise from the source of the InEKF plugin
@ -32,7 +32,7 @@
* @authors Scott Baker, Matt Kielo, Frank Dellaert
*/
#include <gtsam/geometry/Pose2.h>
#include <gtsam/navigation/LIEKF.h>
#include <gtsam/navigation/InvariantEKF.h>
#include <iostream>
@ -45,8 +45,9 @@ using namespace gtsam;
Vector2 h_gps(const Pose2& X, OptionalJacobian<2, 3> 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() {
// // Initialize the filter's state, covariance, and time interval values.
Pose2 X0(0.0, 0.0, 0.0);
@ -54,7 +55,7 @@ int main() {
// Create the filter with the initial state, covariance, and measurement
// function.
LIEKF<Pose2> ekf(X0, P0);
InvariantEKF<Pose2> ekf(X0, P0);
// Define the process covariance and measurement covariance matrices Q and R.
Matrix3 Q = (Vector3(0.05, 0.05, 0.001)).asDiagonal();

16
gtsam/navigation/InvariantEKF.h
View File

@ -10,10 +10,10 @@
* -------------------------------------------------------------------------- */
/**
* @file LIEKF.h
* @brief Left-Invariant Extended Kalman Filter (LIEKF) implementation
* @file InvariantEKF.h
* @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.
* The class supports state evolution via group composition and dynamics
* functions, along with measurement updates using tangent-space corrections.
@ -34,8 +34,8 @@
namespace gtsam {
/**
* @class LIEKF
* @brief Left-Invariant Extended Kalman Filter (LIEKF) on a Lie group G
* @class InvariantEKF
* @brief Left-Invariant Extended Kalman Filter (InvariantEKF) on a Lie group G
*
* @tparam G Lie group type providing:
* - static int dimension = tangent dimension
@ -50,7 +50,7 @@ namespace gtsam {
* using the left-invariant error in the tangent space.
*/
template <typename G>
class LIEKF {
class InvariantEKF {
public:
/// Tangent-space dimension
static constexpr int n = traits<G>::dimension;
@ -59,7 +59,7 @@ class LIEKF {
using MatrixN = Eigen::Matrix<double, n, n>;
/// 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
const G& state() const { return X_; }
@ -229,6 +229,6 @@ class LIEKF {
// Define static identity I_n
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

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

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