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

Add PreintegratedRotation to wrapper

release/4.3a0
Frank Dellaert 2025-04-05 19:00:25 -04:00
parent f4a79517c1
commit 2ae55d7482
3 changed files with 31 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/navigation/PreintegratedRotation.cpp
View File

@ -27,7 +27,7 @@ namespace gtsam {
void PreintegratedRotationParams::print(const string& s) const { void PreintegratedRotationParams::print(const string& s) const {
cout << (s.empty() ? s : s + "\n") << endl; cout << (s.empty() ? s : s + "\n") << endl;
cout << "gyroscopeCovariance:\n[\n" << gyroscopeCovariance << "\n]" << endl; cout << "gyroscopeCovariance:\n" << gyroscopeCovariance << endl;
if (omegaCoriolis) if (omegaCoriolis)
cout << "omegaCoriolis = (" << omegaCoriolis->transpose() << ")" << endl; cout << "omegaCoriolis = (" << omegaCoriolis->transpose() << ")" << endl;
if (body_P_sensor) body_P_sensor->print("body_P_sensor"); if (body_P_sensor) body_P_sensor->print("body_P_sensor");

12
gtsam/navigation/PreintegratedRotation.h
View File

@ -126,13 +126,13 @@ class GTSAM_EXPORT PreintegratedRotation {
Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
/// Default constructor for serialization
PreintegratedRotation() {}
public: public:
/// @name Constructors /// @name Constructors
/// @{ /// @{
/// Default constructor for serialization
PreintegratedRotation() {}
/// Default constructor, resets integration to zero /// Default constructor, resets integration to zero
explicit PreintegratedRotation(const std::shared_ptr<Params>& p) : p_(p) { explicit PreintegratedRotation(const std::shared_ptr<Params>& p) : p_(p) {
resetIntegration(); resetIntegration();
@ -149,9 +149,6 @@ class GTSAM_EXPORT PreintegratedRotation {
/// @name Basic utilities /// @name Basic utilities
/// @{ /// @{
/// Re-initialize PreintegratedMeasurements
void resetIntegration();
/// check parameters equality: checks whether shared pointer points to same Params object. /// check parameters equality: checks whether shared pointer points to same Params object.
bool matchesParamsWith(const PreintegratedRotation& other) const { bool matchesParamsWith(const PreintegratedRotation& other) const {
return p_ == other.p_; return p_ == other.p_;
@ -175,6 +172,9 @@ class GTSAM_EXPORT PreintegratedRotation {
/// @name Main functionality /// @name Main functionality
/// @{ /// @{
/// Re-initialize PreintegratedMeasurements
void resetIntegration();
/** /**
* @brief Calculate an incremental rotation given the gyro measurement and a * @brief Calculate an incremental rotation given the gyro measurement and a
* time interval, and update both deltaTij_ and deltaRij_. * time interval, and update both deltaTij_ and deltaRij_.

23
gtsam/navigation/navigation.i
View File

@ -97,6 +97,29 @@ virtual class PreintegratedRotationParams {
void serialize() const; void serialize() const;
}; };
class PreintegratedRotation {
// Constructors
PreintegratedRotation(const gtsam::PreintegratedRotationParams* params);
// Standard Interface
void resetIntegration();
void integrateGyroMeasurement(const gtsam::Vector& measuredOmega, const gtsam::Vector& biasHat, double deltaT);
gtsam::Rot3 biascorrectedDeltaRij(const gtsam::Vector& biasOmegaIncr) const;
gtsam::Vector integrateCoriolis(const gtsam::Rot3& rot_i) const;
// Access instance variables
double deltaTij() const;
gtsam::Rot3 deltaRij() const;
gtsam::Matrix delRdelBiasOmega() const;
// Testable
void print(string s = "") const;
bool equals(const gtsam::PreintegratedRotation& expected, double tol) const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/navigation/PreintegrationParams.h> #include <gtsam/navigation/PreintegrationParams.h>
virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
PreintegrationParams(gtsam::Vector n_gravity); PreintegrationParams(gtsam::Vector n_gravity);