diff --git a/include/core/factor/factor_block_difference.h b/include/core/factor/factor_block_difference.h index 5dd1086aaaad4a33a2d8b35909d2686274676894..d3558ef21aa6e6577fc54d26015db3f9e3193312 100644 --- a/include/core/factor/factor_block_difference.h +++ b/include/core/factor/factor_block_difference.h @@ -153,11 +153,11 @@ inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Ma assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb1_.rows() && "Wrong jacobian sb1 or covariance size"); assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb2_.rows() && "Wrong jacobian sb2 or covariance size"); - // normalized jacobian - if (_jacobians[0].size() > 0){ + // normalized jacobian, computed according to the _compute_jacobian flag + if (_compute_jacobian[0]){ _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_; } - if (_jacobians[1].size() > 0){ + if (_compute_jacobian[1]){ _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_; } } @@ -176,11 +176,11 @@ inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Ma assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb1_.rows() && "Wrong jacobian sb1 or covariance size"); assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb2_.rows() && "Wrong jacobian sb2 or covariance size"); - // normalized jacobian - if (_jacobians[0].size() > 0){ + // normalized jacobian, computed according to the _compute_jacobian flag + if (_compute_jacobian[0]){ _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_; } - if (_jacobians[1].size() > 0){ + if (_compute_jacobian[1]){ _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_; } }