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

Added function predict in ImuFactor and CombinedImuFactor.

release/4.3a0
krunalchande 2015-03-07 17:47:59 -05:00
parent 4ebb1afa33
commit 9a469ad25f
5 changed files with 161 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/navigation/CombinedImuFactor.h
View File

@ -212,6 +212,16 @@ public:
boost::optional<Matrix&> H5 = boost::none,
boost::optional<Matrix&> H6 = boost::none) const;
/** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */
static PoseVelocityBias predict(const CombinedPreintegratedMeasurements& PIM, const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias_i, const Vector3& gravity,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false,
boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none,
boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) {
return PreintegrationBase::predict(PIM, pose_i, vel_i, bias_i, gravity, omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected_out, deltaVij_biascorrected_out);
}
private:
/** Serialization function */

9
gtsam/navigation/ImuFactor.h
View File

@ -194,6 +194,15 @@ public:
boost::optional<Matrix&> H4 = boost::none,
boost::optional<Matrix&> H5 = boost::none) const;
/** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */
static PoseVelocityBias predict(const PreintegratedMeasurements& PIM, const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias_i, const Vector3& gravity,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false,
boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none,
boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) {
return PreintegrationBase::predict(PIM, pose_i, vel_i, bias_i, gravity, omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected_out, deltaVij_biascorrected_out);
}
private:
/** Serialization function */

32
gtsam/navigation/PreintegrationBase.h
View File

@ -199,48 +199,48 @@ public:
/// Predict state at time j
//------------------------------------------------------------------------------
PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
static PoseVelocityBias predict(const PreintegrationBase& PIB, const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias_i, const Vector3& gravity,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false,
boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none,
boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) const {
boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) {
const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer();
const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope();
const Vector3 biasAccIncr = bias_i.accelerometer() - PIB.biasHat().accelerometer();
const Vector3 biasOmegaIncr = bias_i.gyroscope() - PIB.biasHat().gyroscope();
const Rot3& Rot_i = pose_i.rotation();
const Vector3& pos_i = pose_i.translation().vector();
// Predict state at time j
/* ---------------------------------------------------------------------------------------------------- */
Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr + delPdelBiasOmega() * biasOmegaIncr;
Vector3 deltaPij_biascorrected = PIB.deltaPij() + PIB.delPdelBiasAcc() * biasAccIncr + PIB.delPdelBiasOmega() * biasOmegaIncr;
if(deltaPij_biascorrected_out)// if desired, store this
*deltaPij_biascorrected_out = deltaPij_biascorrected;
Vector3 pos_j = pos_i + Rot_i.matrix() * deltaPij_biascorrected
+ vel_i * deltaTij()
- omegaCoriolis.cross(vel_i) * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * deltaTij()*deltaTij();
+ vel_i * PIB.deltaTij()
- omegaCoriolis.cross(vel_i) * PIB.deltaTij()*PIB.deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * PIB.deltaTij()*PIB.deltaTij();
Vector3 deltaVij_biascorrected = deltaVij() + delVdelBiasAcc() * biasAccIncr + delVdelBiasOmega() * biasOmegaIncr;
Vector3 deltaVij_biascorrected = PIB.deltaVij() + PIB.delVdelBiasAcc() * biasAccIncr + PIB.delVdelBiasOmega() * biasOmegaIncr;
if(deltaVij_biascorrected_out)// if desired, store this
*deltaVij_biascorrected_out = deltaVij_biascorrected;
Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * deltaVij_biascorrected
- 2 * omegaCoriolis.cross(vel_i) * deltaTij() // Coriolis term
+ gravity * deltaTij());
- 2 * omegaCoriolis.cross(vel_i) * PIB.deltaTij() // Coriolis term
+ gravity * PIB.deltaTij());
if(use2ndOrderCoriolis){
pos_j += - 0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij()*deltaTij(); // 2nd order coriolis term for position
vel_j += - omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij(); // 2nd order term for velocity
pos_j += - 0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * PIB.deltaTij()*PIB.deltaTij(); // 2nd order coriolis term for position
vel_j += - omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * PIB.deltaTij(); // 2nd order term for velocity
}
const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr);
const Rot3 deltaRij_biascorrected = PIB.biascorrectedDeltaRij(biasOmegaIncr);
// deltaRij_biascorrected = deltaRij * expmap(delRdelBiasOmega * biasOmegaIncr)
Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected);
Vector3 correctedOmega = biascorrectedOmega -
Rot_i.inverse().matrix() * omegaCoriolis * deltaTij(); // Coriolis term
Rot_i.inverse().matrix() * omegaCoriolis * PIB.deltaTij(); // Coriolis term
const Rot3 correctedDeltaRij =
Rot3::Expmap( correctedOmega );
const Rot3 Rot_j = Rot_i.compose( correctedDeltaRij );
@ -273,7 +273,7 @@ public:
// Evaluate residual error, according to [3]
/* ---------------------------------------------------------------------------------------------------- */
Vector3 deltaPij_biascorrected, deltaVij_biascorrected;
PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity,
PoseVelocityBias predictedState_j = PreintegrationBase::predict(*this, pose_i, vel_i, bias_i, gravity,
omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected, deltaVij_biascorrected);
// Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance

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

@ -294,7 +294,7 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity){
// Predict
Pose3 x1;
Vector3 v1(0, 0.0, 0.0);
PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis);
PoseVelocityBias poseVelocityBias = PreintegrationBase::predict(Combined_pre_int_data, x1, v1, bias, gravity, omegaCoriolis);
Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
Vector3 expectedVelocity; expectedVelocity<<0,1,0;
EXPECT(assert_equal(expectedPose, poseVelocityBias.pose));
@ -322,7 +322,7 @@ TEST(CombinedImuFactor, PredictRotation) {
// Predict
Pose3 x(Rot3().ypr(0,0, 0), Point3(0,0,0));
Vector3 v(0,0,0);
PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x,v,bias, gravity, omegaCoriolis);
PoseVelocityBias poseVelocityBias = CombinedImuFactor::predict(Combinedfactor.preintegratedMeasurements(), x,v,bias, gravity, omegaCoriolis);
Pose3 expectedPose(Rot3().ypr(M_PI/10, 0,0), Point3(0,0,0));
EXPECT(assert_equal(expectedPose, poseVelocityBias.pose, tol));
}

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

@ -131,8 +131,8 @@ Rot3 evaluatePreintegratedMeasurementsRotation(
measuredAccs, measuredOmegas, deltaTs).deltaRij());
}
Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT){
return Rot3::Expmap((measuredOmega - biasOmega) * deltaT);
Rot3 evaluateRotation(const Vector3 b_omegaMeas_nb, const Vector3 biasOmega, const double deltaT){
return Rot3::Expmap((b_omegaMeas_nb - biasOmega) * deltaT);
}
Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta){
@ -148,8 +148,8 @@ TEST( ImuFactor, PreintegratedMeasurements )
imuBias::ConstantBias bias(Vector3(0,0,0), Vector3(0,0,0)); ///< Current estimate of acceleration and angular rate biases
// Measurements
Vector3 measuredAcc(0.1, 0.0, 0.0);
Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0);
Vector3 b_accMeas(0.1, 0.0, 0.0);
Vector3 b_omegaMeas_nb(M_PI/100.0, 0.0, 0.0);
double deltaT = 0.5;
// Expected preintegrated values
@ -161,7 +161,7 @@ TEST( ImuFactor, PreintegratedMeasurements )
bool use2ndOrderIntegration = true;
// Actual preintegrated values
ImuFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration);
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
actual1.integrateMeasurement(b_accMeas, b_omegaMeas_nb, deltaT);
EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij()), 1e-6));
EXPECT(assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij()), 1e-6));
@ -170,13 +170,13 @@ TEST( ImuFactor, PreintegratedMeasurements )
// Integrate again
Vector3 expectedDeltaP2; expectedDeltaP2 << 0.025 + expectedDeltaP1(0) + 0.5*0.1*0.5*0.5, 0, 0;
Vector3 expectedDeltaV2 = Vector3(0.05, 0.0, 0.0) + expectedDeltaR1.matrix() * measuredAcc * 0.5;
Vector3 expectedDeltaV2 = Vector3(0.05, 0.0, 0.0) + expectedDeltaR1.matrix() * b_accMeas * 0.5;
Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI/100.0, 0.0, 0.0);
double expectedDeltaT2(1);
// Actual preintegrated values
ImuFactor::PreintegratedMeasurements actual2 = actual1;
actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
actual2.integrateMeasurement(b_accMeas, b_omegaMeas_nb, deltaT);
EXPECT(assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij()), 1e-6));
EXPECT(assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij()), 1e-6));
@ -195,17 +195,17 @@ TEST( ImuFactor, ErrorAndJacobians )
Vector3 v2(Vector3(0.5, 0.0, 0.0));
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
Vector3 n_gravity; n_gravity << 0, 0, 9.81;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0;
Vector3 measuredOmega; measuredOmega << M_PI/100, 0, 0;
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector();
Vector3 b_omegaMeas_nb; b_omegaMeas_nb << M_PI/100, 0, 0;
Vector3 b_accMeas = x1.rotation().unrotate(-Point3(n_gravity)).vector();
double deltaT = 1.0;
bool use2ndOrderIntegration = true;
ImuFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration);
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
pre_int_data.integrateMeasurement(b_accMeas, b_omegaMeas_nb, deltaT);
// Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis);
Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias);
@ -262,18 +262,18 @@ TEST( ImuFactor, ErrorAndJacobianWithBiases )
Vector3 v2(Vector3(0.5, 0.0, 0.0));
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
Vector3 n_gravity; n_gravity << 0, 0, 9.81;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1;
Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3;
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0);
Vector3 b_omegaMeas_nb; b_omegaMeas_nb << 0, 0, M_PI/10.0+0.3;
Vector3 b_accMeas = x1.rotation().unrotate(-Point3(n_gravity)).vector() + Vector3(0.2,0.0,0.0);
double deltaT = 1.0;
ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
Vector3(0.0, 0.0, 0.1)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
pre_int_data.integrateMeasurement(b_accMeas, b_omegaMeas_nb, deltaT);
// Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis);
SETDEBUG("ImuFactor evaluateError", false);
Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias);
@ -325,20 +325,20 @@ TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis )
Vector3 v2(Vector3(0.5, 0.0, 0.0));
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
Vector3 n_gravity; n_gravity << 0, 0, 9.81;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1;
Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3;
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0);
Vector3 b_omegaMeas_nb; b_omegaMeas_nb << 0, 0, M_PI/10.0+0.3;
Vector3 b_accMeas = x1.rotation().unrotate(-Point3(n_gravity)).vector() + Vector3(0.2,0.0,0.0);
double deltaT = 1.0;
ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
Vector3(0.0, 0.0, 0.1)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
pre_int_data.integrateMeasurement(b_accMeas, b_omegaMeas_nb, deltaT);
// Create factor
Pose3 bodyPsensor = Pose3();
bool use2ndOrderCoriolis = true;
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, bodyPsensor, use2ndOrderCoriolis);
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis, bodyPsensor, use2ndOrderCoriolis);
SETDEBUG("ImuFactor evaluateError", false);
Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias);
@ -387,14 +387,14 @@ TEST( ImuFactor, PartialDerivative_wrt_Bias )
Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias
// Measurements
Vector3 measuredOmega; measuredOmega << 0.1, 0, 0;
Vector3 b_omegaMeas_nb; b_omegaMeas_nb << 0.1, 0, 0;
double deltaT = 0.5;
// Compute numerical derivatives
Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, Vector3>(boost::bind(
&evaluateRotation, measuredOmega, _1, deltaT), Vector3(biasOmega));
&evaluateRotation, b_omegaMeas_nb, _1, deltaT), Vector3(biasOmega));
const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT);
const Matrix3 Jr = Rot3::ExpmapDerivative((b_omegaMeas_nb - biasOmega) * deltaT);
Matrix3 actualdelRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign
@ -428,20 +428,20 @@ TEST( ImuFactor, fistOrderExponential )
Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias
// Measurements
Vector3 measuredOmega; measuredOmega << 0.1, 0, 0;
Vector3 b_omegaMeas_nb; b_omegaMeas_nb << 0.1, 0, 0;
double deltaT = 1.0;
// change w.r.t. linearization point
double alpha = 0.0;
Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha;
const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT);
const Matrix3 Jr = Rot3::ExpmapDerivative((b_omegaMeas_nb - biasOmega) * deltaT);
Matrix3 delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign
const Matrix expectedRot = Rot3::Expmap((measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix();
const Matrix expectedRot = Rot3::Expmap((b_omegaMeas_nb - biasOmega - deltabiasOmega) * deltaT).matrix();
const Matrix3 hatRot = Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix();
const Matrix3 hatRot = Rot3::Expmap((b_omegaMeas_nb - biasOmega) * deltaT).matrix();
const Matrix3 actualRot =
hatRot * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix();
//hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega));
@ -758,21 +758,21 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt )
//
// // Pre-integrator
// imuBias::ConstantBias biasHat(Vector3(0, 0, 0.10), Vector3(0, 0, 0.10));
// Vector3 gravity; gravity << 0, 0, 9.81;
// Vector3 n_gravity; n_gravity << 0, 0, 9.81;
// Vector3 omegaCoriolis; omegaCoriolis << 0.0001, 0, 0.01;
// ImuFactor::PreintegratedMeasurements pre_int_data(biasHat, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity());
//
// // Pre-integrate Measurements
// Vector3 measuredAcc(0.1, 0.0, 0.0);
// Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0);
// Vector3 b_accMeas(0.1, 0.0, 0.0);
// Vector3 b_omegaMeas_nb(M_PI/100.0, 0.0, 0.0);
// double deltaT = 0.5;
// for(size_t i = 0; i < 50; ++i) {
// pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// pre_int_data.integrateMeasurement(b_accMeas, b_omegaMeas_nb, deltaT);
// }
//
// // Create factor
// noiseModel::Base::shared_ptr model = noiseModel::Gaussian::Covariance(pre_int_data.MeasurementCovariance());
// ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
// ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis);
//
// Values values;
// values.insert(X(1), x1);
@ -811,10 +811,10 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
Vector3 v2(Vector3(0.5, 0.0, 0.0));
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
Vector3 n_gravity; n_gravity << 0, 0, 9.81;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1;
Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3;
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0);
Vector3 b_omegaMeas_nb; b_omegaMeas_nb << 0, 0, M_PI/10.0+0.3;
Vector3 b_accMeas = x1.rotation().unrotate(-Point3(n_gravity)).vector() + Vector3(0.2,0.0,0.0);
double deltaT = 1.0;
const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.10,0.10)), Point3(1,0,0));
@ -822,10 +822,10 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
pre_int_data.integrateMeasurement(b_accMeas, b_omegaMeas_nb, deltaT);
// Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis);
// Expected Jacobians
Matrix H1e = numericalDerivative11<Vector,Pose3>(
@ -863,10 +863,10 @@ TEST(ImuFactor, PredictPositionAndVelocity){
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
Vector3 n_gravity; n_gravity << 0, 0, 9.81;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0;
Vector3 measuredOmega; measuredOmega << 0, 0, 0;//M_PI/10.0+0.3;
Vector3 measuredAcc; measuredAcc << 0,1,-9.81;
Vector3 b_omegaMeas_nb; b_omegaMeas_nb << 0, 0, 0;//M_PI/10.0+0.3;
Vector3 b_accMeas; b_accMeas << 0,1,-9.81;
double deltaT = 0.001;
Matrix I6x6(6,6);
@ -875,15 +875,15 @@ TEST(ImuFactor, PredictPositionAndVelocity){
ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), true);
for (int i = 0; i<1000; ++i) pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
for (int i = 0; i<1000; ++i) pre_int_data.integrateMeasurement(b_accMeas, b_omegaMeas_nb, deltaT);
// Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis);
// Predict
Pose3 x1;
Vector3 v1(0, 0.0, 0.0);
PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis);
PoseVelocityBias poseVelocity = PreintegrationBase::predict(pre_int_data, x1, v1, bias, n_gravity, omegaCoriolis);
Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
Vector3 expectedVelocity; expectedVelocity<<0,1,0;
EXPECT(assert_equal(expectedPose, poseVelocity.pose));
@ -895,10 +895,10 @@ TEST(ImuFactor, PredictRotation) {
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
Vector3 n_gravity; n_gravity << 0, 0, 9.81;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0;
Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10;//M_PI/10.0+0.3;
Vector3 measuredAcc; measuredAcc << 0,0,-9.81;
Vector3 b_omegaMeas_nb; b_omegaMeas_nb << 0, 0, M_PI/10;//M_PI/10.0+0.3;
Vector3 b_accMeas; b_accMeas << 0,0,-9.81;
double deltaT = 0.001;
Matrix I6x6(6,6);
@ -907,21 +907,94 @@ TEST(ImuFactor, PredictRotation) {
ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), true);
for (int i = 0; i<1000; ++i) pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
for (int i = 0; i<1000; ++i) pre_int_data.integrateMeasurement(b_accMeas, b_omegaMeas_nb, deltaT);
// Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, n_gravity, omegaCoriolis);
// Predict
Pose3 x1;
Vector3 v1(0, 0.0, 0.0);
PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis);
PoseVelocityBias poseVelocity = ImuFactor::predict(factor.preintegratedMeasurements(), x1, v1, bias, n_gravity, omegaCoriolis);
Pose3 expectedPose(Rot3().ypr(M_PI/10, 0, 0), Point3(0, 0, 0));
Vector3 expectedVelocity; expectedVelocity<<0,0,0;
EXPECT(assert_equal(expectedPose, poseVelocity.pose));
EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity)));
}
/* ************************************************************************* */
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
TEST(ImuFactor, CheckBiasCorrection) {
int numFactors = 2;
int numSamplesPreint = 1;
double g = 9.81;
// Measurements. Body frame and nav frame are both z-up
Vector3 b_omegaMeas_nb; b_omegaMeas_nb << 0, 0.3, 0.0;
Vector3 b_accMeas; b_accMeas << 0, 0, g;
Vector3 n_gravity; n_gravity << 0, 0, -g;
// Set up noise and other test params
imuBias::ConstantBias zeroBias(Vector3(0, 0, 0), Vector3(0.0, 0, 0)); // Biases (acc, rot)
Vector6 noiseBetweenBiasSigma; noiseBetweenBiasSigma << Vector3(2.0e-5, 2.0e-5, 2.0e-5), Vector3(3.0e-6, 3.0e-6, 3.0e-6);
SharedDiagonal biasNoiseModel = noiseModel::Diagonal::Sigmas(noiseBetweenBiasSigma);
Matrix3 accCov = 1e-4 * I_3x3;
Matrix3 gyroCov = 1e-6 * I_3x3;
Matrix3 integrationCov = 1e-8 * I_3x3;
double deltaT = 0.005;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0;
// Specify noise values on priors
Vector6 priorNoisePoseSigmas((Vector(6) << 0.001, 0.001, 0.001, 0.01, 0.01, 0.01).finished());
Vector3 priorNoiseVelSigmas((Vector(3) << 0.5, 0.5, 0.5).finished());
Vector6 priorNoiseBiasSigmas((Vector(6) << 1, 1, 1, 1, 1, 1).finished());
SharedDiagonal priorNoisePose = noiseModel::Diagonal::Sigmas(priorNoisePoseSigmas);
SharedDiagonal priorNoiseVel = noiseModel::Diagonal::Sigmas(priorNoiseVelSigmas);
SharedDiagonal priorNoiseBias = noiseModel::Diagonal::Sigmas(priorNoiseBiasSigmas);
Vector3 zeroVel(0, 0.0, 0.0);
// Instantiate graph and values
Values values;
NonlinearFactorGraph graph;
// Add prior factor and values
graph.add(PriorFactor<Pose3> (X(0), Pose3(), priorNoisePose));
graph.add(PriorFactor<Vector3>(V(0), zeroVel, priorNoiseVel));
graph.add(PriorFactor<imuBias::ConstantBias>(B(0), zeroBias, priorNoiseBias));
values.insert(X(0), Pose3());
values.insert(V(0), zeroVel);
values.insert(B(0), zeroBias);
for (int i = 1; i < numFactors; i++) {
// Preintegrate measurements
ImuFactor::PreintegratedMeasurements pre_int_data = ImuFactor::PreintegratedMeasurements(imuBias::ConstantBias(Vector3(0, 0.0, 0.0),
Vector3(0.0, 0.0, 0.0)), accCov, gyroCov, integrationCov, true);
for (int j = 0; j< numSamplesPreint; ++j) pre_int_data.integrateMeasurement(b_accMeas, b_omegaMeas_nb, deltaT);
// Create and add factor
ImuFactor factor(X(i-1), V(i-1), X(i), V(i), B(i-1), pre_int_data, n_gravity, omegaCoriolis);
graph.add(factor);
graph.add(BetweenFactor<imuBias::ConstantBias>(B(i-1), B(i), zeroBias, biasNoiseModel));
if (i == 30) graph.add(PriorFactor<Pose3>(X(i), Pose3(), priorNoisePose));
// Add values
values.insert(X(i), Pose3());
values.insert(V(i), zeroVel);
values.insert(B(i), zeroBias);
}
// Solve graph and find estimated bias
Values results = LevenbergMarquardtOptimizer(graph, values).optimize();
imuBias::ConstantBias biasActual = results.at<imuBias::ConstantBias>(B(numFactors-1));
//
// // set expected bias
// imuBias::ConstantBias biasExpected(Vector3(0,0,0), Vector3(0, 0.3, 0.0));
// EXPECT(assert_equal(biasExpected, biasActual, 1e-2));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */