diff --git a/include/core/factor/factor_block_difference.h b/include/core/factor/factor_block_difference.h index 2ece75d51225d3c163244fd5d39b0dd2ace08e13..13d74058c5731e46e1f4482788a5525b3c3693c7 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_;