diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 00be79f..2616c0c 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1293,9 +1293,9 @@ void MsckfVio::PhotometricMeasurementJacobian( dGpj_XpAj.block<3, 3>(0, 3) = Matrix::Identity(); // Intermediate Jakobians - H_rho = dh_dGpij * dGpj_drhoj; // 1 x 1 - H_plj = dh_dXplj; // 1 x 6 - H_pAj = dh_dGpij * dGpj_XpAj; // 1 x 6 + H_rho = dh_dGpij * dGpj_drhoj; // 2 x 1 + H_plj = dh_dXplj; // 2 x 6 + H_pAj = dh_dGpij * dGpj_XpAj; // 2 x 6 // calculate residual