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

refactoring F and G

release/4.3a0
dellaert 2015-07-31 15:32:16 -07:00
parent 8aca431913
commit 7901077a7a
3 changed files with 76 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -50,18 +50,33 @@ void PreintegratedCombinedMeasurements::resetIntegration() {
preintMeasCov_.setZero();
}
//------------------------------------------------------------------------------
// sugar for derivative blocks
#define D_R_R(H) (H)->block<3,3>(0,0)
#define D_R_t(H) (H)->block<3,3>(0,3)
#define D_R_v(H) (H)->block<3,3>(0,6)
#define D_t_R(H) (H)->block<3,3>(3,0)
#define D_t_t(H) (H)->block<3,3>(3,3)
#define D_t_v(H) (H)->block<3,3>(3,6)
#define D_v_R(H) (H)->block<3,3>(6,0)
#define D_v_t(H) (H)->block<3,3>(6,3)
#define D_v_v(H) (H)->block<3,3>(6,6)
#define D_a_a(H) (H)->block<3,3>(9,9)
#define D_g_g(H) (H)->block<3,3>(12,12)
//------------------------------------------------------------------------------
void PreintegratedCombinedMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
boost::optional<Matrix&> F_test, boost::optional<Matrix&> G_test) {
OptionalJacobian<15, 15> F_test, OptionalJacobian<15, 21> G_test) {
const Matrix3 dRij = deltaRij_.matrix(); // expensive when quaternion
const Matrix3 dRij = deltaXij_.R(); // expensive when quaternion
// Update preintegrated measurements.
Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr
Matrix9 F_9x9; // overall Jacobian wrt preintegrated measurements (df/dx)
Matrix93 G1,G2;
updatePreintegratedMeasurements(measuredAcc, measuredOmega, deltaT,
&D_incrR_integratedOmega, &F_9x9);
&D_incrR_integratedOmega, &F_9x9, &G1, &G2);
// Update preintegrated measurements covariance: as in [2] we consider a first order propagation that
// can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we
@ -74,17 +89,11 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
// overall Jacobian wrt preintegrated measurements (df/dx)
Eigen::Matrix<double,15,15> F;
// for documentation:
// F << I_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3,
// Z_3x3, I_3x3, H_vel_angles, H_vel_biasacc, Z_3x3,
// Z_3x3, Z_3x3, H_angles_angles, Z_3x3, H_angles_biasomega,
// Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3,
// Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3;
F.setZero();
F.block<9, 9>(0, 0) = F_9x9;
F.block<3, 3>(0, 12) = H_angles_biasomega;
F.block<3, 3>(6, 9) = H_vel_biasacc;
F.block<6, 6>(9, 9) = I_6x6;
F.block<3, 3>(3, 9) = H_vel_biasacc;
F.block<3, 3>(6, 12) = H_angles_biasomega;
// first order uncertainty propagation
// Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose()
@ -92,38 +101,36 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
G_measCov_Gt.setZero(15, 15);
// BLOCK DIAGONAL TERMS
G_measCov_Gt.block<3, 3>(0, 0) = deltaT * p().integrationCovariance;
G_measCov_Gt.block<3, 3>(3, 3) = (1 / deltaT) * (H_vel_biasacc)
D_t_t(&G_measCov_Gt) = deltaT * p().integrationCovariance;
D_v_v(&G_measCov_Gt) = (1 / deltaT) * (H_vel_biasacc)
* (p().accelerometerCovariance + p().biasAccOmegaInit.block<3, 3>(0, 0))
* (H_vel_biasacc.transpose());
G_measCov_Gt.block<3, 3>(6, 6) = (1 / deltaT) * (H_angles_biasomega)
D_R_R(&G_measCov_Gt) = (1 / deltaT) * (H_angles_biasomega)
* (p().gyroscopeCovariance + p().biasAccOmegaInit.block<3, 3>(3, 3))
* (H_angles_biasomega.transpose());
G_measCov_Gt.block<3, 3>(9, 9) = (1 / deltaT) * p().biasAccCovariance;
G_measCov_Gt.block<3, 3>(12, 12) = (1 / deltaT) * p().biasOmegaCovariance;
D_a_a(&G_measCov_Gt) = (1 / deltaT) * p().biasAccCovariance;
D_g_g(&G_measCov_Gt) = (1 / deltaT) * p().biasOmegaCovariance;
// OFF BLOCK DIAGONAL TERMS
Matrix3 block23 = H_vel_biasacc * p().biasAccOmegaInit.block<3, 3>(3, 0)
Matrix3 temp = H_vel_biasacc * p().biasAccOmegaInit.block<3, 3>(3, 0)
* H_angles_biasomega.transpose();
G_measCov_Gt.block<3, 3>(3, 6) = block23;
G_measCov_Gt.block<3, 3>(6, 3) = block23.transpose();
D_v_R(&G_measCov_Gt) = temp;
D_R_v(&G_measCov_Gt) = temp.transpose();
preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt;
// F_test and G_test are used for testing purposes and are not needed by the factor
if (F_test) {
F_test->resize(15, 15);
(*F_test) << F;
}
if (G_test) {
G_test->resize(15, 21);
// This is for testing & documentation
///< measurementCovariance_ : cov[integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21)
(*G_test) << //
I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, //
Z_3x3, -H_vel_biasacc, Z_3x3, Z_3x3, Z_3x3, H_vel_biasacc, Z_3x3, //
Z_3x3, Z_3x3, -H_angles_biasomega, Z_3x3, Z_3x3, Z_3x3, H_angles_biasomega, //
Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3, Z_3x3, //
Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3;
-H_angles_biasomega, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, H_angles_biasomega, //
Z_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, //
Z_3x3, Z_3x3, -H_vel_biasacc, Z_3x3, Z_3x3, H_vel_biasacc, Z_3x3, //
Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3, Z_3x3, //
Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3;
}
}
@ -198,7 +205,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
Matrix93 D_r_vel_i, D_r_vel_j;
// error wrt preintegrated measurements
Vector9 r_pvR = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i,
Vector9 r_Rpv = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i,
H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0,
H4 ? &D_r_vel_j : 0, H5 ? &D_r_bias_i : 0);
@ -242,7 +249,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
// overall error
Vector r(15);
r << r_pvR, fbias; // vector of size 15
r << r_Rpv, fbias; // vector of size 15
return r;
}

8
gtsam/navigation/CombinedImuFactor.h
View File

@ -142,10 +142,10 @@ class PreintegratedCombinedMeasurements : public PreintegrationBase {
* @param body_P_sensor Optional sensor frame (pose of the IMU in the body
* frame)
*/
void integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
boost::optional<Matrix&> F_test = boost::none,
boost::optional<Matrix&> G_test = boost::none);
void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, double deltaT,
OptionalJacobian<15, 15> F_test = boost::none,
OptionalJacobian<15, 21> G_test = boost::none);
/// methods to access class variables
Matrix preintMeasCov() const { return preintMeasCov_; }

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

@ -47,36 +47,30 @@ namespace {
Vector updatePreintegratedMeasurementsTest(const Vector3 deltaPij_old,
const Vector3& deltaVij_old, const Rot3& deltaRij_old,
const imuBias::ConstantBias& bias_old, const Vector3& correctedAcc,
const Vector3& correctedOmega, const double deltaT,
const bool use2ndOrderIntegration) {
const Vector3& correctedOmega, const double deltaT) {
Matrix3 dRij = deltaRij_old.matrix();
Vector3 temp = dRij * (correctedAcc - bias_old.accelerometer()) * deltaT;
Vector3 deltaPij_new;
if (!use2ndOrderIntegration) {
deltaPij_new = deltaPij_old + deltaVij_old * deltaT;
} else {
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;
Rot3 deltaRij_new = deltaRij_old
* Rot3::Expmap((correctedOmega - bias_old.gyroscope()) * deltaT);
Vector3 logDeltaRij_new = Rot3::Logmap(deltaRij_new); // not important any more
imuBias::ConstantBias bias_new(bias_old);
Vector result(15);
result << deltaPij_new, deltaVij_new, logDeltaRij_new, bias_new.vector();
result << logDeltaRij_new, deltaPij_new, deltaVij_new, bias_new.vector();
return result;
}
Rot3 updatePreintegratedMeasurementsRot(const Vector3 deltaPij_old,
const Vector3& deltaVij_old, const Rot3& deltaRij_old,
const imuBias::ConstantBias& bias_old, const Vector3& correctedAcc,
const Vector3& correctedOmega, const double deltaT,
const bool use2ndOrderIntegration) {
const Vector3& correctedOmega, const double deltaT) {
Vector result = updatePreintegratedMeasurementsTest(deltaPij_old,
deltaVij_old, deltaRij_old, bias_old, correctedAcc, correctedOmega,
deltaT, use2ndOrderIntegration);
deltaT);
return Rot3::Expmap(result.segment < 3 > (6));
}
@ -377,10 +371,8 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) {
Matrix oldPreintCovariance = pim.preintMeasCov();
Matrix Factual, Gactual;
pim.integrateMeasurement(newMeasuredAcc, newMeasuredOmega,
newDeltaT, Factual, Gactual);
bool use2ndOrderIntegration = false;
pim.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, Factual,
Gactual);
//////////////////////////////////////////////////////////////////////////////////////////////
// COMPUTE NUMERICAL DERIVATIVES FOR F
@ -388,51 +380,51 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) {
// Compute expected F wrt positions (15,3)
Matrix df_dpos = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedMeasurementsTest, _1, deltaVij_old,
deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT,
use2ndOrderIntegration), deltaPij_old);
deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT
), deltaPij_old);
// rotation part has to be done properly, on manifold
df_dpos.block<3, 3>(6, 0) = numericalDerivative11<Rot3, Vector3>(
df_dpos.block<3, 3>(0, 0) = numericalDerivative11<Rot3, Vector3>(
boost::bind(&updatePreintegratedMeasurementsRot, _1, deltaVij_old,
deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT,
use2ndOrderIntegration), deltaPij_old);
deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT
), deltaPij_old);
EXPECT(assert_equal(df_dpos, Factual.block<15, 3>(0, 3), 1e-5));
// Compute expected F wrt velocities (15,3)
Matrix df_dvel = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, _1,
deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT,
use2ndOrderIntegration), deltaVij_old);
deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT
), deltaVij_old);
// rotation part has to be done properly, on manifold
df_dvel.block<3, 3>(6, 0) = numericalDerivative11<Rot3, Vector3>(
df_dvel.block<3, 3>(0, 0) = numericalDerivative11<Rot3, Vector3>(
boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, _1,
deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT,
use2ndOrderIntegration), deltaVij_old);
deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT
), deltaVij_old);
EXPECT(assert_equal(df_dvel, Factual.block<15, 3>(0, 6), 1e-5));
// Compute expected F wrt angles (15,3)
Matrix df_dangle = numericalDerivative11<Vector, Rot3>(
boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
deltaVij_old, _1, bias_old, newMeasuredAcc, newMeasuredOmega,
newDeltaT, use2ndOrderIntegration), deltaRij_old);
newDeltaT), deltaRij_old);
// rotation part has to be done properly, on manifold
df_dangle.block<3, 3>(6, 0) = numericalDerivative11<Rot3, Rot3>(
df_dangle.block<3, 3>(0, 0) = numericalDerivative11<Rot3, Rot3>(
boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
deltaVij_old, _1, bias_old, newMeasuredAcc, newMeasuredOmega,
newDeltaT, use2ndOrderIntegration), deltaRij_old);
newDeltaT), deltaRij_old);
EXPECT(assert_equal(df_dangle, Factual.block<15, 3>(0, 0), 1e-5));
// Compute expected F wrt biases (15,6)
Matrix df_dbias = numericalDerivative11<Vector, imuBias::ConstantBias>(
boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega,
newDeltaT, use2ndOrderIntegration), bias_old);
newDeltaT), bias_old);
// rotation part has to be done properly, on manifold
df_dbias.block<3, 6>(6, 0) =
df_dbias.block<3, 6>(0, 0) =
numericalDerivative11<Rot3, imuBias::ConstantBias>(
boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
deltaVij_old, deltaRij_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,1e-5));
newDeltaT), bias_old);
EXPECT(assert_equal(df_dbias, Factual.block<15, 6>(0, 9), 1e-5));
//////////////////////////////////////////////////////////////////////////////////////////////
// COMPUTE NUMERICAL DERIVATIVES FOR G
@ -444,24 +436,24 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) {
// Compute expected G wrt acc noise (15,3)
Matrix df_daccNoise = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT,
use2ndOrderIntegration), newMeasuredAcc);
deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT
), newMeasuredAcc);
// rotation part has to be done properly, on manifold
df_daccNoise.block<3, 3>(6, 0) = numericalDerivative11<Rot3, Vector3>(
boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT,
use2ndOrderIntegration), newMeasuredAcc);
deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT
), newMeasuredAcc);
// Compute expected G wrt gyro noise (15,3)
Matrix df_domegaNoise = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT,
use2ndOrderIntegration), newMeasuredOmega);
deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT
), newMeasuredOmega);
// rotation part has to be done properly, on manifold
df_domegaNoise.block<3, 3>(6, 0) = numericalDerivative11<Rot3, Vector3>(
boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT,
use2ndOrderIntegration), newMeasuredOmega);
deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT
), newMeasuredOmega);
// Compute expected G wrt bias random walk noise (15,6)
Matrix df_rwBias(15, 6); // random walk on the bias does not appear in the first 9 entries
@ -472,13 +464,13 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) {
Matrix df_dinitBias = numericalDerivative11<Vector, imuBias::ConstantBias>(
boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega,
newDeltaT, use2ndOrderIntegration), bias_old);
newDeltaT), bias_old);
// rotation part has to be done properly, on manifold
df_dinitBias.block<3, 6>(6, 0) = numericalDerivative11<Rot3,
imuBias::ConstantBias>(
boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega,
newDeltaT, use2ndOrderIntegration), bias_old);
newDeltaT), bias_old);
df_dinitBias.block<6, 6>(9, 0) = Z_6x6; // only has to influence first 9 rows
Matrix Gexpected(15, 21);