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

Some prints in FactroBlockDifference

parent 8d742451
No related branches found
No related tags found
1 merge request!339Adapting to multiple processor motion 285
Pipeline #4992 passed
Hide whitespace changes
Inline Side-by-side
include/core/factor/factor_block_difference.h
+ 13
− 0
View file @ bd566ce1
...@@ -153,6 +153,13 @@ inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Ma ...@@ -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_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");
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 // normalized jacobian
_jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_; _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
_jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_; _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
...@@ -172,6 +179,12 @@ inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Ma ...@@ -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_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");
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 // normalized jacobian
_jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_; _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
_jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_; _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment