WIP logimatic

include/laser/processor/polyline_2D_utils.h

@@ -74,7 +74,8 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
                                      int min_N_overlapped,
                                      int min_N_defined_overlapped,
                                      bool both_exceeding_A_allowed_ = true,
-                                     bool both_exceeding_B_allowed_ = true);
+                                     bool both_exceeding_B_allowed_ = true,
+									 bool print = false);
 
 Eigen::Array<Scalar,Eigen::Dynamic,1> computeSqDist(const Eigen::MatrixXs& _points_A, const int& from_A, const int& to_A, bool from_A_defined, bool to_A_defined,
                                                     const Eigen::MatrixXs& _points_B, const int& from_B, const int& to_B, bool from_B_defined, bool to_B_defined,

src/landmark/landmark_match_polyline_2D.cpp

@@ -172,7 +172,6 @@ bool LandmarkMatchPolyline2D::check(bool check_partial_match) const
 
 Eigen::Array<Scalar,Eigen::Dynamic,1> LandmarkMatchPolyline2D::computeSqDistArray() const
 {
-	std::cout << "LandmarkMatchPolyline2D::computeSqDistArray\n";
     assert(pl_feature_ != nullptr);
     assert(pl_landmark_ != nullptr);
 

src/processor/polyline_2D_utils.cpp

@@ -172,9 +172,9 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
                                      int overlap_N_min,
                                      int overlap_N_defined_min,
                                      bool both_exceeding_A_allowed_,
-                                     bool both_exceeding_B_allowed_)
+                                     bool both_exceeding_B_allowed_,
+									 bool print)
 {
-    bool print = true;
     if (print)
     {
         std::cout << "computeBestSqDist...\n";
@@ -429,7 +429,6 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
         }
     }
 
-    std::cout << "returning...\n";
     return best_match;
 }
 
@@ -437,7 +436,6 @@ Eigen::Array<Scalar,Eigen::Dynamic,1> computeSqDist(const Eigen::MatrixXs& _poin
                                                     const Eigen::MatrixXs& _points_B, const int& from_B, const int& to_B, bool from_B_defined, bool to_B_defined,
                                                     const int& last_A, const int& N_overlap )
 {
-	std::cout << "computeSqDist\n";
     // POINT-POINT DISTANCES for all overlap points (in case of closed A in two parts)
     // Fill with B points
     Eigen::ArrayXXd d(_points_B.block(0,from_B, 2, N_overlap).array());
@@ -485,7 +483,7 @@ Eigen::Array<Scalar,Eigen::Dynamic,1> computeSqDist(const Eigen::MatrixXs& _poin
                                                   to_A_defined,
                                                   to_B_defined);
     }
-    std::cout << "\t\tsquared distances = " << dist2.transpose() << std::endl;
+    //std::cout << "\t\tsquared distances = " << dist2.transpose() << std::endl;
 
     return dist2;
 }