Skip to content
Snippets Groups Projects
Commit 8d00651a authored by Médéric Fourmy's avatar Médéric Fourmy
Browse files

Conform to _compute_jacobian in factor block difference

parent 9afbdf83
No related branches found
No related tags found
1 merge request!339Adapting to multiple processor motion 285
Pipeline #5005 passed
...@@ -153,11 +153,11 @@ inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Ma ...@@ -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_sb1_.rows() && "Wrong jacobian sb1 or covariance size");
assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb2_.rows() && "Wrong jacobian sb2 or covariance size"); assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb2_.rows() && "Wrong jacobian sb2 or covariance size");
// normalized jacobian // normalized jacobian, computed according to the _compute_jacobian flag
if (_jacobians[0].size() > 0){ if (_compute_jacobian[0]){
_jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_; _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
} }
if (_jacobians[1].size() > 0){ if (_compute_jacobian[1]){
_jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_; _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
} }
} }
...@@ -176,11 +176,11 @@ inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Ma ...@@ -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_sb1_.rows() && "Wrong jacobian sb1 or covariance size");
assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb2_.rows() && "Wrong jacobian sb2 or covariance size"); assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb2_.rows() && "Wrong jacobian sb2 or covariance size");
// normalized jacobian // normalized jacobian, computed according to the _compute_jacobian flag
if (_jacobians[0].size() > 0){ if (_compute_jacobian[0]){
_jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_; _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
} }
if (_jacobians[1].size() > 0){ if (_compute_jacobian[1]){
_jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_; _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
} }
} }
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment