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

Fixed MATLAB wrapper (old-style interface still)

release/4.3a0
dellaert 2015-07-18 23:11:27 -07:00
parent 0df1e345a3
commit a5d49a261e
1 changed files with 27 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
gtsam.h
View File

@ -2481,18 +2481,18 @@ class ConstantBias {
class PoseVelocityBias{ class PoseVelocityBias{
PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias); PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias);
}; };
class ImuFactorPreintegratedMeasurements { class PreintegratedImuMeasurements {
// Standard Constructor // Standard Constructor
ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration); PreintegratedImuMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration);
ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance); PreintegratedImuMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance);
// ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs); // PreintegratedImuMeasurements(const gtsam::PreintegratedImuMeasurements& rhs);
// Testable // Testable
void print(string s) const; void print(string s) const;
bool equals(const gtsam::ImuFactorPreintegratedMeasurements& expected, double tol); bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol);
double deltaTij() const; double deltaTij() const;
Matrix deltaRij() const; gtsam::Rot3 deltaRij() const;
Vector deltaPij() const; Vector deltaPij() const;
Vector deltaVij() const; Vector deltaVij() const;
Vector biasHatVector() const; Vector biasHatVector() const;
@ -2505,25 +2505,24 @@ class ImuFactorPreintegratedMeasurements {
// Standard Interface // Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias, gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
Vector gravity, Vector omegaCoriolis) const; Vector gravity, Vector omegaCoriolis) const;
}; };
virtual class ImuFactor : gtsam::NonlinearFactor { virtual class ImuFactor : gtsam::NonlinearFactor {
ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias, ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias,
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis); const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias, ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias,
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis, const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis,
const gtsam::Pose3& body_P_sensor); const gtsam::Pose3& body_P_sensor);
// Standard Interface // Standard Interface
gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const; gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const;
}; };
#include <gtsam/navigation/CombinedImuFactor.h> #include <gtsam/navigation/CombinedImuFactor.h>
class CombinedImuFactorPreintegratedMeasurements { class PreintegratedCombinedMeasurements {
// Standard Constructor // Standard Constructor
CombinedImuFactorPreintegratedMeasurements( PreintegratedCombinedMeasurements(
const gtsam::imuBias::ConstantBias& bias, const gtsam::imuBias::ConstantBias& bias,
Matrix measuredAccCovariance, Matrix measuredAccCovariance,
Matrix measuredOmegaCovariance, Matrix measuredOmegaCovariance,
@ -2531,7 +2530,7 @@ class CombinedImuFactorPreintegratedMeasurements {
Matrix biasAccCovariance, Matrix biasAccCovariance,
Matrix biasOmegaCovariance, Matrix biasOmegaCovariance,
Matrix biasAccOmegaInit); Matrix biasAccOmegaInit);
CombinedImuFactorPreintegratedMeasurements( PreintegratedCombinedMeasurements(
const gtsam::imuBias::ConstantBias& bias, const gtsam::imuBias::ConstantBias& bias,
Matrix measuredAccCovariance, Matrix measuredAccCovariance,
Matrix measuredOmegaCovariance, Matrix measuredOmegaCovariance,
@ -2540,14 +2539,14 @@ class CombinedImuFactorPreintegratedMeasurements {
Matrix biasOmegaCovariance, Matrix biasOmegaCovariance,
Matrix biasAccOmegaInit, Matrix biasAccOmegaInit,
bool use2ndOrderIntegration); bool use2ndOrderIntegration);
// CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs); // PreintegratedCombinedMeasurements(const gtsam::PreintegratedCombinedMeasurements& rhs);
// Testable // Testable
void print(string s) const; void print(string s) const;
bool equals(const gtsam::CombinedImuFactorPreintegratedMeasurements& expected, double tol); bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, double tol);
double deltaTij() const; double deltaTij() const;
Matrix deltaRij() const; gtsam::Rot3 deltaRij() const;
Vector deltaPij() const; Vector deltaPij() const;
Vector deltaVij() const; Vector deltaVij() const;
Vector biasHatVector() const; Vector biasHatVector() const;
@ -2560,53 +2559,51 @@ class CombinedImuFactorPreintegratedMeasurements {
// Standard Interface // Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias, gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
Vector gravity, Vector omegaCoriolis) const; Vector gravity, Vector omegaCoriolis) const;
}; };
virtual class CombinedImuFactor : gtsam::NonlinearFactor { virtual class CombinedImuFactor : gtsam::NonlinearFactor {
CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j, CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j,
const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis); const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
// Standard Interface // Standard Interface
gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const; gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const;
}; };
#include <gtsam/navigation/AHRSFactor.h> #include <gtsam/navigation/AHRSFactor.h>
class AHRSFactorPreintegratedMeasurements { class PreintegratedAhrsMeasurements {
// Standard Constructor // Standard Constructor
AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance); PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance);
AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance); PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance);
AHRSFactorPreintegratedMeasurements(const gtsam::AHRSFactorPreintegratedMeasurements& rhs); PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs);
// Testable // Testable
void print(string s) const; void print(string s) const;
bool equals(const gtsam::AHRSFactorPreintegratedMeasurements& expected, double tol); bool equals(const gtsam::PreintegratedAhrsMeasurements& expected, double tol);
// get Data // get Data
Matrix deltaRij() const; gtsam::Rot3 deltaRij() const;
double deltaTij() const; double deltaTij() const;
Vector biasHat() const; Vector biasHat() const;
// Standard Interface // Standard Interface
void integrateMeasurement(Vector measuredOmega, double deltaT); void integrateMeasurement(Vector measuredOmega, double deltaT);
void integrateMeasurement(Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
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::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, Vector omegaCoriolis); const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, 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::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, Vector omegaCoriolis, const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis,
const gtsam::Pose3& body_P_sensor); const gtsam::Pose3& body_P_sensor);
// Standard Interface // Standard Interface
gtsam::AHRSFactorPreintegratedMeasurements preintegratedMeasurements() const; gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const;
Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j, Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j,
Vector bias) const; Vector bias) const;
gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias, gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias,
const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements,
Vector omegaCoriolis) const; Vector omegaCoriolis) const;
}; };