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

Changed return from Rot3 back to Matrix. Added imuBias in gtsam.h

release/4.3a0
krunalchande 2014-11-21 21:57:18 -05:00
parent cef280d7c4
commit 9230f4269b
7 changed files with 51 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
gtsam.h
View File

@ -1737,6 +1737,7 @@ class Values {
void insert(size_t j, const gtsam::Cal3DS2& t); void insert(size_t j, const gtsam::Cal3DS2& t);
void insert(size_t j, const gtsam::Cal3Bundler& t); void insert(size_t j, const gtsam::Cal3Bundler& t);
void insert(size_t j, const gtsam::EssentialMatrix& t); void insert(size_t j, const gtsam::EssentialMatrix& t);
void insert(size_t j, const gtsam::imuBias::ConstantBias& t);
void update(size_t j, const gtsam::Point2& t); void update(size_t j, const gtsam::Point2& t);
void update(size_t j, const gtsam::Point3& t); void update(size_t j, const gtsam::Point3& t);
@ -1748,9 +1749,10 @@ class Values {
void update(size_t j, const gtsam::Cal3DS2& t); void update(size_t j, const gtsam::Cal3DS2& t);
void update(size_t j, const gtsam::Cal3Bundler& t); void update(size_t j, const gtsam::Cal3Bundler& t);
void update(size_t j, const gtsam::EssentialMatrix& t); void update(size_t j, const gtsam::EssentialMatrix& t);
void update(size_t j, const gtsam::imuBias::ConstantBias& t);
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2,
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2}> gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::imuBias::ConstantBias}>
T at(size_t j); T at(size_t j);
}; };
@ -2390,6 +2392,9 @@ class ConstantBias {
}///\namespace imuBias }///\namespace imuBias
#include <gtsam/navigation/ImuFactor.h> #include <gtsam/navigation/ImuFactor.h>
class PoseVelocity{
PoseVelocity(const gtsam::Pose3& pose, const gtsam::Vector3 velocity);
};
class ImuFactorPreintegratedMeasurements { class ImuFactorPreintegratedMeasurements {
// Standard Constructor // Standard Constructor
ImuFactorPreintegratedMeasurements(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, bool use2ndOrderIntegration);
@ -2399,12 +2404,19 @@ class ImuFactorPreintegratedMeasurements {
// Testable // Testable
void print(string s) const; void print(string s) const;
bool equals(const gtsam::ImuFactorPreintegratedMeasurements& expected, double tol); bool equals(const gtsam::ImuFactorPreintegratedMeasurements& expected, double tol);
Matrix measurementCovariance() const; Matrix measurementCovariance() const;
Matrix deltaRij() const; Matrix deltaRij() const;
double deltaTij() const; double deltaTij() const;
Vector deltaPij() const; Vector deltaPij() const;
Vector deltaVij() const; Vector deltaVij() const;
Vector biasHat() const; Vector biasHat() const;
Matrix delPdelBiasAcc() const;
Matrix delPdelBiasOmega() const;
Matrix delVdelBiasAcc() const;
Matrix delVdelBiasOmega() const;
Matrix delRdelBiasOmega() const;
Matrix PreintMeasCov() const;
// Standard Interface // Standard Interface
@ -2418,11 +2430,9 @@ 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::ImuFactorPreintegratedMeasurements& 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::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
void Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, gtsam::Pose3& pose_j, gtsam::Vector3& vel_j, gtsam::PoseVelocity Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias,
const gtsam::imuBias::ConstantBias& bias,
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis) const; Vector gravity, Vector omegaCoriolis) const;
}; };
@ -2461,13 +2471,15 @@ virtual class AHRSFactor : gtsam::NonlinearFactor {
gtsam::AHRSFactorPreintegratedMeasurements preintegratedMeasurements() const; gtsam::AHRSFactorPreintegratedMeasurements 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,
const gtsam::imuBias::ConstantBias& bias) const; const gtsam::imuBias::ConstantBias& bias) const;
void predict(const gtsam::Rot3& rot_i, gtsam::Rot3& rot_j, gtsam::Rot3 predict(const gtsam::Rot3& rot_i, const gtsam::imuBias::ConstantBias& bias,
const gtsam::imuBias::ConstantBias& bias,
const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector omegaCoriolis) const; Vector omegaCoriolis) const;
}; };
#include <gtsam/navigation/CombinedImuFactor.h> #include <gtsam/navigation/CombinedImuFactor.h>
class PoseVelocityBias{
PoseVelocityBias(const gtsam::Pose3& pose, const gtsam::Vector3 velocity, const gtsam::imuBias::ConstantBias& bias);
};
class CombinedImuFactorPreintegratedMeasurements { class CombinedImuFactorPreintegratedMeasurements {
// Standard Constructor // Standard Constructor
CombinedImuFactorPreintegratedMeasurements( CombinedImuFactorPreintegratedMeasurements(
@ -2497,11 +2509,18 @@ class CombinedImuFactorPreintegratedMeasurements {
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); void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
Matrix measurementCovariance() const;
Matrix deltaRij() const; Matrix deltaRij() const;
double deltaTij() const; double deltaTij() const;
Vector deltaPij() const; Vector deltaPij() const;
Vector deltaVij() const; Vector deltaVij() const;
Vector biasHat() const; Vector biasHat() const;
Matrix delPdelBiasAcc() const;
Matrix delPdelBiasOmega() const;
Matrix delVdelBiasAcc() const;
Matrix delVdelBiasOmega() const;
Matrix delRdelBiasOmega() const;
Matrix PreintMeasCov() const;
}; };
virtual class CombinedImuFactor : gtsam::NonlinearFactor { virtual class CombinedImuFactor : gtsam::NonlinearFactor {
@ -2510,23 +2529,7 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor {
// Standard Interface // Standard Interface
gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const; gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
void Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, gtsam::Pose3& pose_j, gtsam::Vector3& vel_j, gtsam::PoseVelocityBias Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias_i,
const gtsam::imuBias::ConstantBias& bias_i, const gtsam::imuBias::ConstantBias& bias_j,
const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis);
static gtsam::imuBias::ConstantBias predictImuBias(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i,
const gtsam::imuBias::ConstantBias& bias_i,
const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis);
static gtsam::LieVector predictVelocity(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i,
const gtsam::imuBias::ConstantBias& bias_i,
const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis);
static gtsam::Pose3 predictPose(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i,
const gtsam::imuBias::ConstantBias& bias_i,
const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements, const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis); Vector gravity, Vector omegaCoriolis);
}; };

2
gtsam/navigation/AHRSFactor.h
View File

@ -83,7 +83,7 @@ public:
tol); tol);
} }
Matrix measurementCovariance() const {return measurementCovariance_;} Matrix measurementCovariance() const {return measurementCovariance_;}
Rot3 deltaRij() const {return deltaRij_;} Matrix deltaRij() const {return deltaRij_.matrix();}
double deltaTij() const {return deltaTij_;} double deltaTij() const {return deltaTij_;}
Vector biasHat() const {return biasHat_.vector();} Vector biasHat() const {return biasHat_.vector();}
Matrix3 delRdelBiasOmega() {return delRdelBiasOmega_;} Matrix3 delRdelBiasOmega() {return delRdelBiasOmega_;}

15
gtsam/navigation/CombinedImuFactor.h
View File

@ -317,18 +317,17 @@ namespace gtsam {
} }
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
Matrix measurementCovariance() const {return measurementCovariance_;} Matrix measurementCovariance() const {return measurementCovariance_;}
Rot3 deltaRij() const {return deltaRij_;} Matrix deltaRij() const {return deltaRij_.matrix();}
double deltaTij() const{return deltaTij_;} double deltaTij() const{return deltaTij_;}
Vector deltaPij() const {return deltaPij_;} Vector deltaPij() const {return deltaPij_;}
Vector deltaVij() const {return deltaVij_;} Vector deltaVij() const {return deltaVij_;}
Vector biasHat() const { return biasHat_.vector();} Vector biasHat() const { return biasHat_.vector();}
Matrix delPdelBiasAcc() { return delPdelBiasAcc_;} Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;}
Matrix delPdelBiasOmega() { return delPdelBiasOmega_;} Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;}
Matrix delVdelBiasAcc() { return delVdelBiasAcc_;} Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;}
Matrix delVdelBiasOmega() { return delVdelBiasOmega_;} Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;}
Matrix delRdelBiasOmega() { return delRdelBiasOmega_;} Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;}
Matrix PreintMeasCov() { return PreintMeasCov_;} Matrix PreintMeasCov() const { return PreintMeasCov_;}
private: private:
/** Serialization function */ /** Serialization function */

14
gtsam/navigation/ImuFactor.h
View File

@ -137,17 +137,17 @@ struct PoseVelocity {
&& equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol); && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol);
} }
Matrix measurementCovariance() const {return measurementCovariance_;} Matrix measurementCovariance() const {return measurementCovariance_;}
Rot3 deltaRij() const {return deltaRij_;} Matrix deltaRij() const {return deltaRij_.matrix();}
double deltaTij() const{return deltaTij_;} double deltaTij() const{return deltaTij_;}
Vector deltaPij() const {return deltaPij_;} Vector deltaPij() const {return deltaPij_;}
Vector deltaVij() const {return deltaVij_;} Vector deltaVij() const {return deltaVij_;}
Vector biasHat() const { return biasHat_.vector();} Vector biasHat() const { return biasHat_.vector();}
Matrix delPdelBiasAcc() { return delPdelBiasAcc_;} Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;}
Matrix delPdelBiasOmega() { return delPdelBiasOmega_;} Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;}
Matrix delVdelBiasAcc() { return delVdelBiasAcc_;} Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;}
Matrix delVdelBiasOmega() { return delVdelBiasOmega_;} Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;}
Matrix delRdelBiasOmega() { return delRdelBiasOmega_;} Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;}
Matrix PreintMeasCov() { return PreintMeasCov_;} Matrix PreintMeasCov() const { return PreintMeasCov_;}
void resetIntegration(){ void resetIntegration(){

8
gtsam/navigation/tests/testAHRSFactor.cpp
View File

@ -68,8 +68,8 @@ Rot3 evaluatePreintegratedMeasurementsRotation(
const list<Vector3>& measuredOmegas, const list<Vector3>& measuredOmegas,
const list<double>& deltaTs, const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0, 0.0, 0.0)) { const Vector3& initialRotationRate = Vector3(0.0, 0.0, 0.0)) {
return evaluatePreintegratedMeasurements(bias, measuredOmegas, return Rot3(evaluatePreintegratedMeasurements(bias, measuredOmegas,
deltaTs, initialRotationRate).deltaRij(); deltaTs, initialRotationRate).deltaRij());
} }
Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega,
@ -99,7 +99,7 @@ TEST( AHRSFactor, PreintegratedMeasurements ) {
AHRSFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero()); AHRSFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero());
actual1.integrateMeasurement(measuredOmega, deltaT); actual1.integrateMeasurement(measuredOmega, deltaT);
EXPECT(assert_equal(expectedDeltaR1, actual1.deltaRij(), 1e-6)); EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6));
DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-6); DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-6);
// Integrate again // Integrate again
@ -110,7 +110,7 @@ TEST( AHRSFactor, PreintegratedMeasurements ) {
AHRSFactor::PreintegratedMeasurements actual2 = actual1; AHRSFactor::PreintegratedMeasurements actual2 = actual1;
actual2.integrateMeasurement(measuredOmega, deltaT); actual2.integrateMeasurement(measuredOmega, deltaT);
EXPECT(assert_equal(expectedDeltaR2, actual2.deltaRij(), 1e-6)); EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()), 1e-6));
DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6); DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6);
} }

4
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -89,8 +89,8 @@ Rot3 evaluatePreintegratedMeasurementsRotation(
const list<double>& deltaTs, const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
{ {
return evaluatePreintegratedMeasurements(bias, return Rot3(evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs).deltaRij(); measuredAccs, measuredOmegas, deltaTs).deltaRij());
} }
} }

8
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -101,8 +101,8 @@ Rot3 evaluatePreintegratedMeasurementsRotation(
const list<double>& deltaTs, const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
{ {
return evaluatePreintegratedMeasurements(bias, return Rot3(evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaRij(); measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaRij());
} }
Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT) Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT)
@ -142,7 +142,7 @@ TEST( ImuFactor, PreintegratedMeasurements )
EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij()), 1e-6)); EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij()), 1e-6));
EXPECT(assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij()), 1e-6)); EXPECT(assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij()), 1e-6));
EXPECT(assert_equal(expectedDeltaR1, actual1.deltaRij(), 1e-6)); EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6));
DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-6); DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-6);
// Integrate again // Integrate again
@ -157,7 +157,7 @@ TEST( ImuFactor, PreintegratedMeasurements )
EXPECT(assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij()), 1e-6)); EXPECT(assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij()), 1e-6));
EXPECT(assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij()), 1e-6)); EXPECT(assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij()), 1e-6));
EXPECT(assert_equal(expectedDeltaR2, actual2.deltaRij(), 1e-6)); EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()), 1e-6));
DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6); DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6);
} }