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

update gtsam:: namespace in navigation.i

release/4.3a0
Varun Agrawal 2024-06-28 16:16:43 -04:00
parent b6d697291e
commit 14ea7085e4
1 changed files with 70 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

140
gtsam/navigation/navigation.i
View File

@ -10,7 +10,7 @@ namespace imuBias {
class ConstantBias { class ConstantBias {
// Constructors // Constructors
ConstantBias(); ConstantBias();
ConstantBias(Vector biasAcc, Vector biasGyro); ConstantBias(gtsam::Vector biasAcc, gtsam::Vector biasGyro);
// Testable // Testable
void print(string s = "") const; void print(string s = "") const;
@ -25,11 +25,11 @@ class ConstantBias {
gtsam::imuBias::ConstantBias operator-(const gtsam::imuBias::ConstantBias& b) const; gtsam::imuBias::ConstantBias operator-(const gtsam::imuBias::ConstantBias& b) const;
// Standard Interface // Standard Interface
Vector vector() const; gtsam::Vector vector() const;
Vector accelerometer() const; gtsam::Vector accelerometer() const;
Vector gyroscope() const; gtsam::Vector gyroscope() const;
Vector correctAccelerometer(Vector measurement) const; gtsam::Vector correctAccelerometer(gtsam::Vector measurement) const;
Vector correctGyroscope(Vector measurement) const; gtsam::Vector correctGyroscope(gtsam::Vector measurement) const;
// enabling serialization functionality // enabling serialization functionality
void serialize() const; void serialize() const;
@ -41,8 +41,8 @@ class ConstantBias {
class NavState { class NavState {
// Constructors // Constructors
NavState(); NavState();
NavState(const gtsam::Rot3& R, const gtsam::Point3& t, Vector v); NavState(const gtsam::Rot3& R, const gtsam::Point3& t, gtsam::Vector v);
NavState(const gtsam::Pose3& pose, Vector v); NavState(const gtsam::Pose3& pose, gtsam::Vector v);
// Testable // Testable
void print(string s = "") const; void print(string s = "") const;
@ -51,11 +51,11 @@ class NavState {
// Access // Access
gtsam::Rot3 attitude() const; gtsam::Rot3 attitude() const;
gtsam::Point3 position() const; gtsam::Point3 position() const;
Vector velocity() const; gtsam::Vector velocity() const;
gtsam::Pose3 pose() const; gtsam::Pose3 pose() const;
gtsam::NavState retract(const Vector& x) const; gtsam::NavState retract(const gtsam::Vector& x) const;
Vector localCoordinates(const gtsam::NavState& g) const; gtsam::Vector localCoordinates(const gtsam::NavState& g) const;
// enabling serialization functionality // enabling serialization functionality
void serialize() const; void serialize() const;
@ -69,19 +69,19 @@ virtual class PreintegratedRotationParams {
void print(string s = "") const; void print(string s = "") const;
bool equals(const gtsam::PreintegratedRotationParams& expected, double tol); bool equals(const gtsam::PreintegratedRotationParams& expected, double tol);
void setGyroscopeCovariance(Matrix cov); void setGyroscopeCovariance(gtsam::Matrix cov);
void setOmegaCoriolis(Vector omega); void setOmegaCoriolis(gtsam::Vector omega);
void setBodyPSensor(const gtsam::Pose3& pose); void setBodyPSensor(const gtsam::Pose3& pose);
Matrix getGyroscopeCovariance() const; gtsam::Matrix getGyroscopeCovariance() const;
std::optional<Vector> getOmegaCoriolis() const; std::optional<gtsam::Vector> getOmegaCoriolis() const;
std::optional<gtsam::Pose3> getBodyPSensor() const; std::optional<gtsam::Pose3> getBodyPSensor() const;
}; };
#include <gtsam/navigation/PreintegrationParams.h> #include <gtsam/navigation/PreintegrationParams.h>
virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
PreintegrationParams(Vector n_gravity); PreintegrationParams(gtsam::Vector n_gravity);
gtsam::Vector n_gravity; gtsam::Vector n_gravity;
@ -94,12 +94,12 @@ virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
void print(string s = "") const; void print(string s = "") const;
bool equals(const gtsam::PreintegrationParams& expected, double tol); bool equals(const gtsam::PreintegrationParams& expected, double tol);
void setAccelerometerCovariance(Matrix cov); void setAccelerometerCovariance(gtsam::Matrix cov);
void setIntegrationCovariance(Matrix cov); void setIntegrationCovariance(gtsam::Matrix cov);
void setUse2ndOrderCoriolis(bool flag); void setUse2ndOrderCoriolis(bool flag);
Matrix getAccelerometerCovariance() const; gtsam::Matrix getAccelerometerCovariance() const;
Matrix getIntegrationCovariance() const; gtsam::Matrix getIntegrationCovariance() const;
bool getUse2ndOrderCoriolis() const; bool getUse2ndOrderCoriolis() const;
// enabling serialization functionality // enabling serialization functionality
@ -118,19 +118,19 @@ class PreintegratedImuMeasurements {
bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol); bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol);
// Standard Interface // Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, void integrateMeasurement(gtsam::Vector measuredAcc, gtsam::Vector measuredOmega,
double deltaT); double deltaT);
void resetIntegration(); void resetIntegration();
void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat);
Matrix preintMeasCov() const; gtsam::Matrix preintMeasCov() const;
Vector preintegrated() const; gtsam::Vector preintegrated() const;
double deltaTij() const; double deltaTij() const;
gtsam::Rot3 deltaRij() const; gtsam::Rot3 deltaRij() const;
Vector deltaPij() const; gtsam::Vector deltaPij() const;
Vector deltaVij() const; gtsam::Vector deltaVij() const;
gtsam::imuBias::ConstantBias biasHat() const; gtsam::imuBias::ConstantBias biasHat() const;
Vector biasHatVector() const; gtsam::Vector biasHatVector() const;
gtsam::NavState predict(const gtsam::NavState& state_i, gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias) const; const gtsam::imuBias::ConstantBias& bias) const;
@ -145,14 +145,14 @@ virtual class ImuFactor: gtsam::NonlinearFactor {
// Standard Interface // Standard Interface
gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const; gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const;
Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i, gtsam::Vector evaluateError(const gtsam::Pose3& pose_i, gtsam::Vector vel_i,
const gtsam::Pose3& pose_j, Vector vel_j, const gtsam::Pose3& pose_j, gtsam::Vector vel_j,
const gtsam::imuBias::ConstantBias& bias); const gtsam::imuBias::ConstantBias& bias);
}; };
#include <gtsam/navigation/CombinedImuFactor.h> #include <gtsam/navigation/CombinedImuFactor.h>
virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams { virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams {
PreintegrationCombinedParams(Vector n_gravity); PreintegrationCombinedParams(gtsam::Vector n_gravity);
static gtsam::PreintegrationCombinedParams* MakeSharedD(double g); static gtsam::PreintegrationCombinedParams* MakeSharedD(double g);
static gtsam::PreintegrationCombinedParams* MakeSharedU(double g); static gtsam::PreintegrationCombinedParams* MakeSharedU(double g);
@ -163,13 +163,13 @@ virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams {
void print(string s = "") const; void print(string s = "") const;
bool equals(const gtsam::PreintegrationCombinedParams& expected, double tol); bool equals(const gtsam::PreintegrationCombinedParams& expected, double tol);
void setBiasAccCovariance(Matrix cov); void setBiasAccCovariance(gtsam::Matrix cov);
void setBiasOmegaCovariance(Matrix cov); void setBiasOmegaCovariance(gtsam::Matrix cov);
void setBiasAccOmegaInit(Matrix cov); void setBiasAccOmegaInit(gtsam::Matrix cov);
Matrix getBiasAccCovariance() const ; gtsam::Matrix getBiasAccCovariance() const ;
Matrix getBiasOmegaCovariance() const ; gtsam::Matrix getBiasOmegaCovariance() const ;
Matrix getBiasAccOmegaInit() const; gtsam::Matrix getBiasAccOmegaInit() const;
}; };
@ -184,18 +184,18 @@ class PreintegratedCombinedMeasurements {
double tol); double tol);
// Standard Interface // Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, void integrateMeasurement(gtsam::Vector measuredAcc, gtsam::Vector measuredOmega,
double deltaT); double deltaT);
void resetIntegration(); void resetIntegration();
void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat); void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat);
Matrix preintMeasCov() const; gtsam::Matrix preintMeasCov() const;
double deltaTij() const; double deltaTij() const;
gtsam::Rot3 deltaRij() const; gtsam::Rot3 deltaRij() const;
Vector deltaPij() const; gtsam::Vector deltaPij() const;
Vector deltaVij() const; gtsam::Vector deltaVij() const;
gtsam::imuBias::ConstantBias biasHat() const; gtsam::imuBias::ConstantBias biasHat() const;
Vector biasHatVector() const; gtsam::Vector biasHatVector() const;
gtsam::NavState predict(const gtsam::NavState& state_i, gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias) const; const gtsam::imuBias::ConstantBias& bias) const;
}; };
@ -207,8 +207,8 @@ virtual class CombinedImuFactor: gtsam::NoiseModelFactor {
// Standard Interface // Standard Interface
gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const; gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const;
Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i, gtsam::Vector evaluateError(const gtsam::Pose3& pose_i, gtsam::Vector vel_i,
const gtsam::Pose3& pose_j, Vector vel_j, const gtsam::Pose3& pose_j, gtsam::Vector vel_j,
const gtsam::imuBias::ConstantBias& bias_i, const gtsam::imuBias::ConstantBias& bias_i,
const gtsam::imuBias::ConstantBias& bias_j); const gtsam::imuBias::ConstantBias& bias_j);
}; };
@ -217,12 +217,12 @@ virtual class CombinedImuFactor: gtsam::NoiseModelFactor {
class PreintegratedAhrsMeasurements { class PreintegratedAhrsMeasurements {
// Standard Constructor // Standard Constructor
PreintegratedAhrsMeasurements(const gtsam::PreintegrationParams* params, PreintegratedAhrsMeasurements(const gtsam::PreintegrationParams* params,
const Vector& biasHat); const gtsam::Vector& biasHat);
PreintegratedAhrsMeasurements(const gtsam::PreintegrationParams* p, PreintegratedAhrsMeasurements(const gtsam::PreintegrationParams* p,
const Vector& bias_hat, double deltaTij, const gtsam::Vector& bias_hat, double deltaTij,
const gtsam::Rot3& deltaRij, const gtsam::Rot3& deltaRij,
const Matrix& delRdelBiasOmega, const gtsam::Matrix& delRdelBiasOmega,
const Matrix& preint_meas_cov); const gtsam::Matrix& preint_meas_cov);
PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs); PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs);
// Testable // Testable
@ -232,27 +232,27 @@ class PreintegratedAhrsMeasurements {
// get Data // get Data
gtsam::Rot3 deltaRij() const; gtsam::Rot3 deltaRij() const;
double deltaTij() const; double deltaTij() const;
Vector biasHat() const; gtsam::Vector biasHat() const;
// Standard Interface // Standard Interface
void integrateMeasurement(Vector measuredOmega, double deltaT); void integrateMeasurement(gtsam::Vector measuredOmega, double deltaT);
void resetIntegration() ; void resetIntegration() ;
}; };
virtual class AHRSFactor : gtsam::NonlinearFactor { virtual class AHRSFactor : gtsam::NonlinearFactor {
AHRSFactor(size_t rot_i, size_t rot_j,size_t bias, AHRSFactor(size_t rot_i, size_t rot_j,size_t bias,
const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis); const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, gtsam::Vector omegaCoriolis);
AHRSFactor(size_t rot_i, size_t rot_j, size_t bias, AHRSFactor(size_t rot_i, size_t rot_j, size_t bias,
const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis, const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, gtsam::Vector omegaCoriolis,
const gtsam::Pose3& body_P_sensor); const gtsam::Pose3& body_P_sensor);
// Standard Interface // Standard Interface
gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const; gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const;
Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j, gtsam::Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j,
Vector bias) const; gtsam::Vector bias) const;
gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias, gtsam::Rot3 predict(const gtsam::Rot3& rot_i, gtsam::Vector bias,
const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements,
Vector omegaCoriolis) const; gtsam::Vector omegaCoriolis) const;
}; };
#include <gtsam/navigation/AttitudeFactor.h> #include <gtsam/navigation/AttitudeFactor.h>
@ -333,25 +333,25 @@ virtual class BarometricFactor : gtsam::NonlinearFactor {
#include <gtsam/navigation/Scenario.h> #include <gtsam/navigation/Scenario.h>
virtual class Scenario { virtual class Scenario {
gtsam::Pose3 pose(double t) const; gtsam::Pose3 pose(double t) const;
Vector omega_b(double t) const; gtsam::Vector omega_b(double t) const;
Vector velocity_n(double t) const; gtsam::Vector velocity_n(double t) const;
Vector acceleration_n(double t) const; gtsam::Vector acceleration_n(double t) const;
gtsam::Rot3 rotation(double t) const; gtsam::Rot3 rotation(double t) const;
gtsam::NavState navState(double t) const; gtsam::NavState navState(double t) const;
Vector velocity_b(double t) const; gtsam::Vector velocity_b(double t) const;
Vector acceleration_b(double t) const; gtsam::Vector acceleration_b(double t) const;
}; };
virtual class ConstantTwistScenario : gtsam::Scenario { virtual class ConstantTwistScenario : gtsam::Scenario {
ConstantTwistScenario(Vector w, Vector v); ConstantTwistScenario(gtsam::Vector w, gtsam::Vector v);
ConstantTwistScenario(Vector w, Vector v, ConstantTwistScenario(gtsam::Vector w, gtsam::Vector v,
const gtsam::Pose3& nTb0); const gtsam::Pose3& nTb0);
}; };
virtual class AcceleratingScenario : gtsam::Scenario { virtual class AcceleratingScenario : gtsam::Scenario {
AcceleratingScenario(const gtsam::Rot3& nRb, const gtsam::Point3& p0, AcceleratingScenario(const gtsam::Rot3& nRb, const gtsam::Point3& p0,
Vector v0, Vector a_n, gtsam::Vector v0, gtsam::Vector a_n,
Vector omega_b); gtsam::Vector omega_b);
}; };
#include <gtsam/navigation/ScenarioRunner.h> #include <gtsam/navigation/ScenarioRunner.h>
@ -360,11 +360,11 @@ class ScenarioRunner {
const gtsam::PreintegrationParams* p, const gtsam::PreintegrationParams* p,
double imuSampleTime, double imuSampleTime,
const gtsam::imuBias::ConstantBias& bias); const gtsam::imuBias::ConstantBias& bias);
Vector gravity_n() const; gtsam::Vector gravity_n() const;
Vector actualAngularVelocity(double t) const; gtsam::Vector actualAngularVelocity(double t) const;
Vector actualSpecificForce(double t) const; gtsam::Vector actualSpecificForce(double t) const;
Vector measuredAngularVelocity(double t) const; gtsam::Vector measuredAngularVelocity(double t) const;
Vector measuredSpecificForce(double t) const; gtsam::Vector measuredSpecificForce(double t) const;
double imuSampleTime() const; double imuSampleTime() const;
gtsam::PreintegratedImuMeasurements integrate( gtsam::PreintegratedImuMeasurements integrate(
double T, const gtsam::imuBias::ConstantBias& estimatedBias, double T, const gtsam::imuBias::ConstantBias& estimatedBias,
@ -372,10 +372,10 @@ class ScenarioRunner {
gtsam::NavState predict( gtsam::NavState predict(
const gtsam::PreintegratedImuMeasurements& pim, const gtsam::PreintegratedImuMeasurements& pim,
const gtsam::imuBias::ConstantBias& estimatedBias) const; const gtsam::imuBias::ConstantBias& estimatedBias) const;
Matrix estimateCovariance( gtsam::Matrix estimateCovariance(
double T, size_t N, double T, size_t N,
const gtsam::imuBias::ConstantBias& estimatedBias) const; const gtsam::imuBias::ConstantBias& estimatedBias) const;
Matrix estimateNoiseCovariance(size_t N) const; gtsam::Matrix estimateNoiseCovariance(size_t N) const;
}; };
} }