diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 0990c45..965484d 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -1047,7 +1047,7 @@ void MsckfVio::PhotometricMeasurementJacobian( //d{}^Gp_P{ij} / \rho_i double rho = feature.anchor_rho; - dGpi_drhoj = feature.T_anchor_w.linear() * Eigen::Vector3d(-feature.anchorPatch_ideal[count].x/(rho*rho), -feature.anchorPatch_ideal[count].y/(rho*rho), 1/(rho*rho)); + dGpi_drhoj = feature.T_anchor_w.linear() * Eigen::Vector3d(-feature.anchorPatch_ideal[count].x/(rho*rho), -feature.anchorPatch_ideal[count].y/(rho*rho), -1/(rho*rho)); dGpi_XpAj.block<3, 3>(0, 0) = skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), feature.anchorPatch_ideal[count].y/(rho), 1/(rho))); dGpi_XpAj.block<3, 3>(0, 3) = Matrix::Identity();