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

Fixed matrix block index of jacobians in CombinedIMUFactor.

release/4.3a0
krunalchande 2014-12-19 14:15:14 -05:00
parent d7ba38e476
commit 6120bf0846
1 changed files with 9 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -111,7 +111,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
// Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3; // Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3;
F.setZero(); F.setZero();
F.block<9,9>(0,0) = F_9x9; F.block<9,9>(0,0) = F_9x9;
F.block<6,6>(9,9) = eye(9); F.block<6,6>(9,9) = eye(6);
F.block<3,3>(3,9) = H_vel_biasacc; F.block<3,3>(3,9) = H_vel_biasacc;
F.block<3,3>(6,12) = H_angles_biasomega; F.block<3,3>(6,12) = H_angles_biasomega;
@ -219,37 +219,37 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_
H1->resize(15,6); H1->resize(15,6);
H1->block<9,6>(0,0) = H1_pvR; H1->block<9,6>(0,0) = H1_pvR;
// adding: [dBiasAcc/dPi ; dBiasOmega/dPi] // adding: [dBiasAcc/dPi ; dBiasOmega/dPi]
H1->block<6,6>(0,9) = Matrix::Zero(6,6); H1->block<6,6>(9,0) = Matrix::Zero(6,6);
} }
if(H2) { if(H2) {
H2->resize(15,3); H2->resize(15,3);
H2->block<9,3>(0,0) = H2_pvR; H2->block<9,3>(0,0) = H2_pvR;
// adding: [dBiasAcc/dVi ; dBiasOmega/dVi] // adding: [dBiasAcc/dVi ; dBiasOmega/dVi]
H2->block<6,3>(0,9) = Matrix::Zero(6,3); H2->block<6,3>(9,0) = Matrix::Zero(6,3);
} }
if(H3) { if(H3) {
H3->resize(15,6); H3->resize(15,6);
H3->block<9,6>(0,0) = H3_pvR; H3->block<9,6>(0,0) = H3_pvR;
// adding: [dBiasAcc/dPj ; dBiasOmega/dPj] // adding: [dBiasAcc/dPj ; dBiasOmega/dPj]
H3->block<6,6>(0,9) = Matrix::Zero(6,6); H3->block<6,6>(9,0) = Matrix::Zero(6,6);
} }
if(H4) { if(H4) {
H4->resize(15,3); H4->resize(15,3);
H4->block<9,3>(0,0) = H4_pvR; H4->block<9,3>(0,0) = H4_pvR;
// adding: [dBiasAcc/dVi ; dBiasOmega/dVi] // adding: [dBiasAcc/dVi ; dBiasOmega/dVi]
H4->block<6,3>(0,9) = Matrix::Zero(6,3); H4->block<6,3>(9,0) = Matrix::Zero(6,3);
} }
if(H5) { if(H5) {
H5->resize(15,6); H5->resize(15,6);
H5->block<9,6>(0,0) = H5_pvR; H5->block<9,6>(0,0) = H5_pvR;
// adding: [dBiasAcc/dBias_i ; dBiasOmega/dBias_i] // adding: [dBiasAcc/dBias_i ; dBiasOmega/dBias_i]
H5->block<6,6>(0,9) = Hbias_i; H5->block<6,6>(9,0) = Hbias_i;
} }
if(H6) { if(H6) {
H6->resize(15,6); H6->resize(15,6);
H6->block<9,6>(0,0) = Matrix::Zero(6,6); H6->block<9,6>(0,0) = Matrix::Zero(9,6);
// adding: [dBiasAcc/dBias_j ; dBiasOmega/dBias_j] // adding: [dBiasAcc/dBias_j ; dBiasOmega/dBias_j]
H6->block<6,6>(0,9) = Hbias_j; H6->block<6,6>(9,0) = Hbias_j;
} }
Vector r(15); r << r_pvR, fbias; // vector of size 15 Vector r(15); r << r_pvR, fbias; // vector of size 15
return r; return r;