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