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

fixed IncrementalFixedLagSmooth

release/4.3a0
mkielo3 2025-05-16 00:12:09 -04:00
parent 5702ec791b
commit 32b8effde2
4 changed files with 23 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/geometry/Gal3.cpp
View File

@ -40,6 +40,21 @@ namespace gtsam {
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
namespace { // Anonymous namespace for internal linkage namespace { // Anonymous namespace for internal linkage
constexpr double kSmallAngleThreshold = 1e-10; constexpr double kSmallAngleThreshold = 1e-10;
// The type of the Lie algebra (matrix representation)
using LieAlgebra = Matrix5;
// Helper functions for accessing tangent vector components
Eigen::Block<Vector10, 3, 1> rho(Vector10& v) { return v.block<3, 1>(0, 0); }
Eigen::Block<Vector10, 3, 1> nu(Vector10& v) { return v.block<3, 1>(3, 0); }
Eigen::Block<Vector10, 3, 1> theta(Vector10& v) { return v.block<3, 1>(6, 0); }
Eigen::Block<Vector10, 1, 1> t_tan(Vector10& v) { return v.block<1, 1>(9, 0); }
// Const versions
Eigen::Block<const Vector10, 3, 1> rho(const Vector10& v) { return v.block<3, 1>(0, 0); }
Eigen::Block<const Vector10, 3, 1> nu(const Vector10& v) { return v.block<3, 1>(3, 0); }
Eigen::Block<const Vector10, 3, 1> theta(const Vector10& v) { return v.block<3, 1>(6, 0); }
Eigen::Block<const Vector10, 1, 1> t_tan(const Vector10& v) { return v.block<1, 1>(9, 0); }
} // end anonymous namespace } // end anonymous namespace
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------

14
gtsam/geometry/Gal3.h
View File

@ -167,20 +167,6 @@ class GTSAM_EXPORT Gal3 : public LieGroup<Gal3, 10> {
/// @name Lie Group Static Functions /// @name Lie Group Static Functions
/// @{ /// @{
/// The type of the Lie algebra (matrix representation)
using LieAlgebra = Matrix5;
// Helper functions for accessing tangent vector components
static Eigen::Block<Vector10, 3, 1> rho(Vector10& v) { return v.block<3, 1>(0, 0); }
static Eigen::Block<Vector10, 3, 1> nu(Vector10& v) { return v.block<3, 1>(3, 0); }
static Eigen::Block<Vector10, 3, 1> theta(Vector10& v) { return v.block<3, 1>(6, 0); }
static Eigen::Block<Vector10, 1, 1> t_tan(Vector10& v) { return v.block<1, 1>(9, 0); }
// Const versions
static Eigen::Block<const Vector10, 3, 1> rho(const Vector10& v) { return v.block<3, 1>(0, 0); }
static Eigen::Block<const Vector10, 3, 1> nu(const Vector10& v) { return v.block<3, 1>(3, 0); }
static Eigen::Block<const Vector10, 3, 1> theta(const Vector10& v) { return v.block<3, 1>(6, 0); }
static Eigen::Block<const Vector10, 1, 1> t_tan(const Vector10& v) { return v.block<1, 1>(9, 0); }
/// Exponential map at identity: tangent vector xi -> manifold element g /// Exponential map at identity: tangent vector xi -> manifold element g
static Gal3 Expmap(const Vector10& xi, OptionalJacobian<10, 10> Hxi = {}); static Gal3 Expmap(const Vector10& xi, OptionalJacobian<10, 10> Hxi = {});

3
gtsam/navigation/LieGroupEKF.h
View File

@ -26,6 +26,9 @@
#include <gtsam/navigation/ManifoldEKF.h> // Include the base class #include <gtsam/navigation/ManifoldEKF.h> // Include the base class
#include <gtsam/base/Lie.h> // Include for Lie group traits and operations #include <gtsam/base/Lie.h> // Include for Lie group traits and operations
// #include "gtsam/base/Matrix.h"
#include "gtsam/base/lieProxies.h"
#include <Eigen/Dense> #include <Eigen/Dense>
#include <type_traits> #include <type_traits>

10
gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h
View File

@ -11,11 +11,11 @@
#pragma once #pragma once
// #ifdef _MSC_VER #ifdef _MSC_VER
// #pragma message("IncrementalFixedLagSmoother was moved to the gtsam/nonlinear directory") #pragma message("IncrementalFixedLagSmoother was moved to the gtsam/nonlinear directory")
// #else #else
// #warning "IncrementalFixedLagSmoother was moved to the gtsam/nonlinear directory" #warning "IncrementalFixedLagSmoother was moved to the gtsam/nonlinear directory"
// #endif #endif
#include <gtsam/nonlinear/IncrementalFixedLagSmoother.h> #include <gtsam/nonlinear/IncrementalFixedLagSmoother.h>