Solved wrong size jacobian vector bug due to fix() + removed std couts

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