From e970b211bb291bf1241a3eb9cac486ca18fba8de Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?M=C3=A9d=C3=A9ric=20Fourmy?= <mfourmy@laas.fr>
Date: Mon, 30 Mar 2020 12:20:34 +0200
Subject: [PATCH] Solved wrong size jacobian vector bug due to fix() + removed
std couts
---
include/core/factor/factor_block_difference.h | 41 ++++++++-----------
1 file changed, 18 insertions(+), 23 deletions(-)
diff --git a/include/core/factor/factor_block_difference.h b/include/core/factor/factor_block_difference.h
index 13d74058c..5dd1086aa 100644
--- a/include/core/factor/factor_block_difference.h
+++ b/include/core/factor/factor_block_difference.h
@@ -37,12 +37,12 @@ class FactorBlockDifference : public FactorAnalytic
*
*/
FactorBlockDifference(
- StateBlockPtr _sb1_ptr,
- StateBlockPtr _sb2_ptr,
- FrameBasePtr _frame_other = nullptr,
- CaptureBasePtr _cap_other = nullptr,
- FeatureBasePtr _feat_other = nullptr,
- LandmarkBasePtr _lmk_other = nullptr,
+ const StateBlockPtr& _sb1_ptr,
+ const StateBlockPtr& _sb2_ptr,
+ const FrameBasePtr& _frame_other = nullptr,
+ const CaptureBasePtr& _cap_other = nullptr,
+ const FeatureBasePtr& _feat_other = nullptr,
+ const LandmarkBasePtr& _lmk_other = nullptr,
unsigned int _start_idx1 = 0,
int _size1 = -1,
unsigned int _start_idx2 = 0,
@@ -153,16 +153,13 @@ 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");
- std::cout << "\nevaluateJacobians" << std::endl;
- std::cout << getMeasurementSquareRootInformationUpper().rows() << " " << getMeasurementSquareRootInformationUpper().cols() << std::endl;
- std::cout << J_res_sb1_.rows() << " " << J_res_sb1_.cols() << std::endl;
- std::cout << J_res_sb2_.rows() << " " << J_res_sb2_.cols() << std::endl;
- std::cout << _jacobians[0].rows() << " " << _jacobians[0].cols() << std::endl;
- std::cout << _jacobians[1].rows() << " " << _jacobians[1].cols() << std::endl;
-
// normalized jacobian
- _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
- _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
+ if (_jacobians[0].size() > 0){
+ _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
+ }
+ if (_jacobians[1].size() > 0){
+ _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
+ }
}
inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
@@ -179,15 +176,13 @@ 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");
- std::cout << "\nevaluateJacobians" << std::endl;
- std::cout << getMeasurementSquareRootInformationUpper().rows() << " " << getMeasurementSquareRootInformationUpper().cols() << std::endl;
- std::cout << J_res_sb1_.rows() << " " << J_res_sb1_.cols() << std::endl;
- std::cout << J_res_sb2_.rows() << " " << J_res_sb2_.cols() << std::endl;
- std::cout << _jacobians[0].rows() << " " << _jacobians[0].cols() << std::endl;
- std::cout << _jacobians[1].rows() << " " << _jacobians[1].cols() << std::endl;
// normalized jacobian
- _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
- _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
+ if (_jacobians[0].size() > 0){
+ _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
+ }
+ if (_jacobians[1].size() > 0){
+ _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
+ }
}
inline void FactorBlockDifference::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& _pure_jacobians) const
--
GitLab