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

added truth revealing unit test for Combined Imu factor (and fixed latest changes, that, moving updatePreintegratedMeasurements before, were creating a bug)

release/4.3a0
Luca 2014-12-09 15:17:26 -05:00
parent 64dfde3ae6
commit 53b59bf488
2 changed files with 38 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -95,6 +95,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
const Vector3 theta_i = thetaRij(); // super-expensive parametrization of so(3) const Vector3 theta_i = thetaRij(); // super-expensive parametrization of so(3)
const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i);
const Matrix3 R_i = deltaRij(); // store this
// Update preintegrated measurements. TODO Frank moved from end of this function !!! // Update preintegrated measurements. TODO Frank moved from end of this function !!!
updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT); updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT);
@ -108,10 +109,10 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
Matrix3 H_vel_pos = Z_3x3; Matrix3 H_vel_pos = Z_3x3;
Matrix3 H_vel_vel = I_3x3; Matrix3 H_vel_vel = I_3x3;
Matrix3 H_vel_angles = - deltaRij() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; Matrix3 H_vel_angles = - R_i * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
// analytic expression corresponding to the following numerical derivative // analytic expression corresponding to the following numerical derivative
// Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); // Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
Matrix3 H_vel_biasacc = - deltaRij() * deltaT; Matrix3 H_vel_biasacc = - R_i * deltaT;
Matrix3 H_angles_pos = Z_3x3; Matrix3 H_angles_pos = Z_3x3;
Matrix3 H_angles_vel = Z_3x3; Matrix3 H_angles_vel = Z_3x3;

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

@ -52,7 +52,7 @@ Vector updatePreintegratedMeasurementsTest(
Rot3 deltaRij_old = Rot3::Expmap(logDeltaRij_old); Rot3 deltaRij_old = Rot3::Expmap(logDeltaRij_old);
Matrix3 dRij = deltaRij_old.matrix(); Matrix3 dRij = deltaRij_old.matrix();
Vector3 temp = dRij * correctedAcc * deltaT; Vector3 temp = dRij * (correctedAcc - bias_old.accelerometer()) * deltaT;
Vector3 deltaPij_new; Vector3 deltaPij_new;
if(!use2ndOrderIntegration_){ if(!use2ndOrderIntegration_){
deltaPij_new = deltaPij_old + deltaVij_old * deltaT; deltaPij_new = deltaPij_old + deltaVij_old * deltaT;
@ -60,7 +60,7 @@ Vector updatePreintegratedMeasurementsTest(
deltaPij_new += deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT; deltaPij_new += deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT;
} }
Vector3 deltaVij_new = deltaVij_old + temp; Vector3 deltaVij_new = deltaVij_old + temp;
Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap(correctedOmega * deltaT); Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap((correctedOmega-bias_old.gyroscope()) * deltaT);
Vector3 logDeltaRij_new = Rot3::Logmap(deltaRij_new); Vector3 logDeltaRij_new = Rot3::Logmap(deltaRij_new);
imuBias::ConstantBias bias_new(bias_old); imuBias::ConstantBias bias_new(bias_old);
Vector result(15); result << deltaPij_new, deltaVij_new, logDeltaRij_new, bias_new.vector(); Vector result(15); result << deltaPij_new, deltaVij_new, logDeltaRij_new, bias_new.vector();
@ -320,7 +320,7 @@ TEST(CombinedImuFactor, PredictRotation) {
TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation )
{ {
// Linearization point // Linearization point
imuBias::ConstantBias bias = imuBias::ConstantBias(); ///< Current estimate of acceleration and rotation rate biases imuBias::ConstantBias bias_old = imuBias::ConstantBias(); ///< Current estimate of acceleration and rotation rate biases
Pose3 body_P_sensor = Pose3(); Pose3 body_P_sensor = Pose3();
// Measurements // Measurements
@ -340,7 +340,7 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation )
} }
// Actual preintegrated values // Actual preintegrated values
CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated = CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs); evaluatePreintegratedMeasurements(bias_old, measuredAccs, measuredOmegas, deltaTs);
// so far we only created a nontrivial linearization point for the preintegrated measurements // so far we only created a nontrivial linearization point for the preintegrated measurements
// Now we add a new measurement and ask for Jacobians // Now we add a new measurement and ask for Jacobians
@ -351,32 +351,37 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation )
const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement
const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement
const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement
//
// Matrix Factual, Gactual; Matrix Factual, Gactual;
// preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT,
// body_P_sensor, Factual, Gactual); body_P_sensor, Factual, Gactual);
//
// bool use2ndOrderIntegration = false; bool use2ndOrderIntegration = false;
//
// // Compute expected F wrt positions // Compute expected F wrt positions (15,3)
// Matrix df_dpos = Matrix df_dpos =
// numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest, numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest,
// _1, deltaVij_old, logDeltaRij_old, _1, deltaVij_old, logDeltaRij_old, bias_old,
// newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old);
// // Compute expected F wrt velocities // Compute expected F wrt velocities (15,3)
// Matrix df_dvel = Matrix df_dvel =
// numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest, numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest,
// deltaPij_old, _1, logDeltaRij_old, deltaPij_old, _1, logDeltaRij_old, bias_old,
// newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old);
// // Compute expected F wrt angles // Compute expected F wrt angles (15,3)
// Matrix df_dangle = Matrix df_dangle =
// numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest, numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest,
// deltaPij_old, deltaVij_old, _1, deltaPij_old, deltaVij_old, _1, bias_old,
// newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), logDeltaRij_old); newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), logDeltaRij_old);
// Matrix Fexpected(9,9); // Compute expected F wrt angles (15,6)
// Matrix df_dbias =
// Fexpected << df_dpos, df_dvel, df_dangle; numericalDerivative11<Vector, imuBias::ConstantBias>(boost::bind(&updatePreintegratedMeasurementsTest,
// EXPECT(assert_equal(Fexpected, Factual)); deltaPij_old, deltaVij_old, logDeltaRij_old, _1,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old);
Matrix Fexpected(15,15);
Fexpected << df_dpos, df_dvel, df_dangle, df_dbias;
EXPECT(assert_equal(Fexpected, Factual));
// //
// // Compute expected G wrt integration noise // // Compute expected G wrt integration noise
// Matrix df_dintNoise(9,3); // Matrix df_dintNoise(9,3);