Skip to content

NEES Comparison #2374

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
dellaert wants to merge 3 commits into
base: develop
Choose a base branch
from
Draft

NEES Comparison #2374

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
6852dd6
Expose Manifold PIM
dellaert Jan 26, 2026
ea25f54
Better wrapping
dellaert Jan 26, 2026
a2fad09
NEES comparison
dellaert Jan 26, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion gtsam/navigation/PreintegrationBase.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,10 +55,10 @@ class GTSAM_EXPORT PreintegrationBase {
/// Default constructor for serialization
PreintegrationBase() {}

public:
/// Virtual destructor for serialization
virtual ~PreintegrationBase() {}

public:
/// @name Constructors
/// @{

Expand Down
93 changes: 61 additions & 32 deletions gtsam/navigation/navigation.i
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -159,38 +159,80 @@ virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
void serialize() const;
};

#include <gtsam/navigation/PreintegrationBase.h>
virtual class PreintegrationBase {
// Basic utilities
void resetIntegration();
void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat);
bool matchesParamsWith(const gtsam::PreintegrationBase& other) const;

// Access
gtsam::imuBias::ConstantBias biasHat() const;
double deltaTij() const;
gtsam::Vector deltaPij() const;
gtsam::Vector deltaVij() const;
gtsam::Rot3 deltaRij() const;
gtsam::NavState deltaXij() const;
gtsam::Vector biasHatVector() const;

// Standard Interface
void integrateMeasurement(gtsam::Vector measuredAcc, gtsam::Vector measuredOmega,
double deltaT);
gtsam::Vector biasCorrectedDelta(const gtsam::imuBias::ConstantBias& bias_i) const;
gtsam::Vector biasCorrectedDelta(const gtsam::imuBias::ConstantBias& bias_i,
Eigen::Ref<Eigen::MatrixXd> H) const;
gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias_i) const;
gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias_i,
Eigen::Ref<Eigen::MatrixXd> H1,
Eigen::Ref<Eigen::MatrixXd> H2) const;
gtsam::Vector computeError(const gtsam::NavState& state_i,
const gtsam::NavState& state_j,
const gtsam::imuBias::ConstantBias& bias_i,
Eigen::Ref<Eigen::MatrixXd> H1,
Eigen::Ref<Eigen::MatrixXd> H2,
Eigen::Ref<Eigen::MatrixXd> H3) const;
gtsam::Vector computeErrorAndJacobians(const gtsam::Pose3& pose_i,
gtsam::Vector vel_i,
const gtsam::Pose3& pose_j,
gtsam::Vector vel_j,
const gtsam::imuBias::ConstantBias& bias_i,
Eigen::Ref<Eigen::MatrixXd> H1,
Eigen::Ref<Eigen::MatrixXd> H2,
Eigen::Ref<Eigen::MatrixXd> H3,
Eigen::Ref<Eigen::MatrixXd> H4,
Eigen::Ref<Eigen::MatrixXd> H5) const;

// Testable
void print(string s = "") const;
};

#include <gtsam/navigation/ImuFactor.h>
class PreintegratedImuMeasurements {
template <PreintegrationType>
class PreintegratedImuMeasurementsT : gtsam::PreintegrationBase {
// Constructors
PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params);
PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params,
PreintegratedImuMeasurementsT(const gtsam::PreintegrationParams* params);
PreintegratedImuMeasurementsT(const gtsam::PreintegrationParams* params,
const gtsam::imuBias::ConstantBias& bias);

// Testable
void print(string s = "") const;
bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol);
bool equals(const gtsam::PreintegratedImuMeasurementsT<PreintegrationType>& expected, double tol);

// Standard Interface
void integrateMeasurement(gtsam::Vector measuredAcc, gtsam::Vector measuredOmega,
double deltaT);
void resetIntegration();
void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat);

gtsam::Matrix preintMeasCov() const;
gtsam::Vector preintegrated() const;
double deltaTij() const;
gtsam::Rot3 deltaRij() const;
gtsam::Vector deltaPij() const;
gtsam::Vector deltaVij() const;
gtsam::imuBias::ConstantBias biasHat() const;
gtsam::Vector biasHatVector() const;
gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias) const;
// gtsam::Vector preintegrated() const; only define for TangentPreintegration

// enabling serialization functionality
void serialize() const;
};

typedef gtsam::PreintegratedImuMeasurementsT<gtsam::DefaultPreintegrationType> PreintegratedImuMeasurements;
typedef gtsam::PreintegratedImuMeasurementsT<gtsam::ManifoldPreintegration> PreintegratedImuMeasurementsManifold;
// Defining below leads to multiply defined errors: as Tangent is default, use PreintegratedImuMeasurements
// typedef gtsam::PreintegratedImuMeasurementsT<gtsam::TangentPreintegration> PreintegratedImuMeasurementsTangent;

virtual class ImuFactor: gtsam::NonlinearFactor {
ImuFactor(gtsam::Key pose_i, gtsam::Key vel_i, gtsam::Key pose_j, gtsam::Key vel_j,
gtsam::Key bias,
Expand Down Expand Up @@ -245,7 +287,7 @@ virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams {
void serialize() const;
};

class PreintegratedCombinedMeasurements {
class PreintegratedCombinedMeasurements : gtsam::PreintegrationBase {
// Constructors
PreintegratedCombinedMeasurements(
const gtsam::PreintegrationCombinedParams* params);
Expand All @@ -258,20 +300,7 @@ class PreintegratedCombinedMeasurements {
double tol);

// Standard Interface
void integrateMeasurement(gtsam::Vector measuredAcc,
gtsam::Vector measuredOmega, double deltaT);
void resetIntegration();
void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat);

gtsam::Matrix preintMeasCov() const;
double deltaTij() const;
gtsam::Rot3 deltaRij() const;
gtsam::Vector deltaPij() const;
gtsam::Vector deltaVij() const;
gtsam::imuBias::ConstantBias biasHat() const;
gtsam::Vector biasHatVector() const;
gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias) const;

// enable serialization functionality
void serialize() const;
Expand Down
Loading
Loading