From bd566ce14d2f4e03fe325b97a052f2d13e6fbd87 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?M=C3=A9d=C3=A9ric=20Fourmy?= <mfourmy@laas.fr>
Date: Fri, 27 Mar 2020 17:28:43 +0100
Subject: [PATCH] Some prints in FactroBlockDifference
---
include/core/factor/factor_block_difference.h | 13 +++++++++++++
1 file changed, 13 insertions(+)
diff --git a/include/core/factor/factor_block_difference.h b/include/core/factor/factor_block_difference.h
index 2ece75d51..13d74058c 100644
--- a/include/core/factor/factor_block_difference.h
+++ b/include/core/factor/factor_block_difference.h
@@ -153,6 +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_;
@@ -172,6 +179,12 @@ 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_;
--
GitLab