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

minor refactor to follow the math better

release/4.3a0
Varun Agrawal 2022-05-08 17:34:14 -04:00
parent ce7c71b7d7
commit a17134dd6a
1 changed files with 21 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -116,7 +116,6 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
Matrix3 theta_H_biasOmegaInit = -theta_H_omega; Matrix3 theta_H_biasOmegaInit = -theta_H_omega;
Matrix3 pos_H_biasAccInit = -pos_H_acc; Matrix3 pos_H_biasAccInit = -pos_H_acc;
Matrix3 vel_H_biasAcc = -vel_H_acc;
Matrix3 vel_H_biasAccInit = -vel_H_acc; Matrix3 vel_H_biasAccInit = -vel_H_acc;
// overall Jacobian wrt preintegrated measurements (df/dx) // overall Jacobian wrt preintegrated measurements (df/dx)
@ -143,45 +142,44 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
Eigen::Matrix<double, 15, 15> G_measCov_Gt; Eigen::Matrix<double, 15, 15> G_measCov_Gt;
G_measCov_Gt.setZero(15, 15); G_measCov_Gt.setZero(15, 15);
const Matrix3& bInitCov11 = bInitCov.block<3, 3>(0, 0) / dt;
const Matrix3& bInitCov12 = bInitCov.block<3, 3>(0, 3) / dt;
const Matrix3& bInitCov21 = bInitCov.block<3, 3>(3, 0) / dt;
const Matrix3& bInitCov22 = bInitCov.block<3, 3>(3, 3) / dt;
// BLOCK DIAGONAL TERMS // BLOCK DIAGONAL TERMS
D_R_R(&G_measCov_Gt) = D_R_R(&G_measCov_Gt) =
(theta_H_omega * (wCov / dt) * theta_H_omega.transpose()) // (theta_H_omega * (wCov / dt) * theta_H_omega.transpose()) //
+ (theta_H_biasOmegaInit * (bInitCov.block<3, 3>(3, 3) / dt) * +
theta_H_biasOmegaInit.transpose()); (theta_H_biasOmegaInit * bInitCov22 * theta_H_biasOmegaInit.transpose());
D_t_t(&G_measCov_Gt) = D_t_t(&G_measCov_Gt) =
(pos_H_acc * (aCov / dt) * pos_H_acc.transpose()) // (pos_H_acc * (aCov / dt) * pos_H_acc.transpose()) //
+ (pos_H_biasAccInit * (bInitCov.block<3, 3>(0, 0) / dt) * + (pos_H_biasAccInit * bInitCov11 * pos_H_biasAccInit.transpose()) //
pos_H_biasAccInit.transpose()) //
+ (dt * iCov); + (dt * iCov);
D_v_v(&G_measCov_Gt) = D_v_v(&G_measCov_Gt) =
(vel_H_acc * (aCov / dt) * vel_H_acc.transpose()) // (vel_H_acc * (aCov / dt) * vel_H_acc.transpose()) //
+ (vel_H_biasAccInit * (bInitCov.block<3, 3>(0, 0) / dt) * + (vel_H_biasAccInit * bInitCov11 * vel_H_biasAccInit.transpose());
vel_H_biasAccInit.transpose());
D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance; D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance;
D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance; D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance;
// OFF BLOCK DIAGONAL TERMS // OFF BLOCK DIAGONAL TERMS
D_R_t(&G_measCov_Gt) = theta_H_biasOmegaInit * D_R_t(&G_measCov_Gt) =
(bInitCov.block<3, 3>(3, 0) / dt) * theta_H_biasOmegaInit * bInitCov21 * pos_H_biasAccInit.transpose();
pos_H_biasAccInit.transpose(); D_R_v(&G_measCov_Gt) =
D_R_v(&G_measCov_Gt) = theta_H_biasOmegaInit * theta_H_biasOmegaInit * bInitCov21 * vel_H_biasAccInit.transpose();
(bInitCov.block<3, 3>(3, 0) / dt) * D_t_R(&G_measCov_Gt) =
vel_H_biasAccInit.transpose(); pos_H_biasAccInit * bInitCov12 * theta_H_biasOmegaInit.transpose();
D_t_R(&G_measCov_Gt) = pos_H_biasAccInit * (bInitCov.block<3, 3>(0, 3) / dt) *
theta_H_biasOmegaInit.transpose();
D_t_v(&G_measCov_Gt) = D_t_v(&G_measCov_Gt) =
(pos_H_acc * (aCov / dt) * vel_H_acc.transpose()) + (pos_H_acc * (aCov / dt) * vel_H_acc.transpose()) +
(pos_H_biasAccInit * (bInitCov.block<3, 3>(0, 0) / dt) * (pos_H_biasAccInit * bInitCov11 * vel_H_biasAccInit.transpose());
vel_H_biasAccInit.transpose()); D_v_R(&G_measCov_Gt) =
D_v_R(&G_measCov_Gt) = vel_H_biasAccInit * (bInitCov.block<3, 3>(0, 3) / dt) * vel_H_biasAccInit * bInitCov12 * theta_H_biasOmegaInit.transpose();
theta_H_biasOmegaInit.transpose();
D_v_t(&G_measCov_Gt) = D_v_t(&G_measCov_Gt) =
(vel_H_acc * (aCov / dt) * pos_H_acc.transpose()) + (vel_H_acc * (aCov / dt) * pos_H_acc.transpose()) +
(vel_H_biasAccInit * (bInitCov.block<3, 3>(0, 0) / dt) * (vel_H_biasAccInit * bInitCov11 * pos_H_biasAccInit.transpose());
pos_H_biasAccInit.transpose());
preintMeasCov_.noalias() += G_measCov_Gt; preintMeasCov_.noalias() += G_measCov_Gt;
} }
@ -291,6 +289,5 @@ std::ostream& operator<<(std::ostream& os, const CombinedImuFactor& f) {
os << " noise model sigmas: " << f.noiseModel_->sigmas().transpose(); os << " noise model sigmas: " << f.noiseModel_->sigmas().transpose();
return os; return os;
} }
}
/// namespace gtsam
} // namespace gtsam