object detector clean version

src/object_detector.cpp

@@ -455,13 +455,13 @@ void laserscanutils::findLinesInCluster(const laserscanutils::ScanParams & _scan
         }
         unsigned int i_from = ii - _alg_params.fit_line_window_size_ + 1;
 
-        std::cout << "Fitting Line: from " << i_from << " to " << ii << "...";
+        //std::cout << "Fitting Line: from " << i_from << " to " << ii << "...";
         fitLine(_cluster.block(0,i_from, 3, _alg_params.fit_line_window_size_), line);
 
         //if error below stdev, add line to result set
-        if ( line.error_ < _scan_params.range_std_dev_*_alg_params.fit_line_k_sigmas_ )
+        if ( line.error_ < std::pow(_scan_params.range_std_dev_*_alg_params.fit_line_k_sigmas_,2) )
         {
-            std::cout << "=> YES!" << std::endl;
+            //std::cout << "=> YES!" << std::endl;
             line.first_ = i_from;
             line.last_ = ii;
             line.point_first_ = _cluster.col(line.first_);
@@ -472,7 +472,7 @@ void laserscanutils::findLinesInCluster(const laserscanutils::ScanParams & _scan
         }
         else
         {
-            std::cout << " NO! - Error = " << line.error_ << " > " << _scan_params.range_std_dev_*_alg_params.fit_line_k_sigmas_ << std::endl;
+            //std::cout << " NO! - Error = " << line.error_ << " > " << _scan_params.range_std_dev_*_alg_params.fit_line_k_sigmas_ << std::endl;
         }
         ii = ii+rand_window_overlapping(generator);
     }
@@ -499,9 +499,9 @@ void laserscanutils::findLinesInCluster(const laserscanutils::ScanParams & _scan
                 fitLine(_cluster.block(0,line_it1->first_, 3, line_it2->last_-line_it1->first_+1), new_line);
 
                 //check if error below a threshold to effectively concatenate
-                if ( new_line.error_ < _scan_params.range_std_dev_*_alg_params.fit_line_k_sigmas_* _scan_params.range_std_dev_*_alg_params.fit_line_k_sigmas_)
+                if ( new_line.error_ < std::pow(_scan_params.range_std_dev_*_alg_params.fit_line_k_sigmas_,2) )
                 {
-                    std::cout << "   => lines concatenated";
+                    //std::cout << "   => lines concatenated";
 
                     //update line1 as the concatenation of l1 and l2
                     new_line.first_ = line_it1->first_;
@@ -514,13 +514,13 @@ void laserscanutils::findLinesInCluster(const laserscanutils::ScanParams & _scan
                     //remove l2 from the list
                     line_it2 = _line_list.erase(line_it2);
 
-                    std::cout << " - line resultant: from "<< new_line.first_ << " to " << new_line.last_ << std::endl;
+                    //std::cout << " - line resultant: from "<< new_line.first_ << " to " << new_line.last_ << std::endl;
                     //<< new_line.vector_.transpose() <<  "    ERROR: " << new_line.error_ << std::endl << std::endl;
 
                 }
                 else
                 {
-                    std::cout << "   => lines NOT concatenated";
+                    //std::cout << "   => lines NOT concatenated";
 
                     line_it2++;
                 }
@@ -555,17 +555,18 @@ void laserscanutils::findLinesInCluster(const laserscanutils::ScanParams & _scan
 //        //std::cout << std::endl << std::endl;
 //    }
 
-    std::cout << " --> Lines After Concatenation: " << _line_list.size() << std::endl << std::endl;
 
         // DEBUG TEST
-    for (auto list_it1 = _line_list.begin(); list_it1 != _line_list.end(); list_it1++)
-    {
-        list_it1->print();
-        std::cout << std::endl;
-    }
+//    for (auto list_it1 = _line_list.begin(); list_it1 != _line_list.end(); list_it1++)
+//    {
+//        list_it1->print();
+//        std::cout << std::endl;
+//    }
+
+
 
+    std::cout << " --> Final Lines In Object: " << _line_list.size() << std::endl << std::endl;
 
-    std::cout << "END findLinesInCluster. Size = " << _line_list.size() << std::endl;
 }
 
 
@@ -968,8 +969,8 @@ void laserscanutils::associateObjects(std::list<laserscanutils::Object> & _prev_
                 if (act_obj_it.ref_type_ <= prev_obj_it.ref_type_)
                 {
                     act_obj_it.ref_act_point_ = prev_obj_it.ref_act_point_;
-                    std::cout << std::endl << "Change Refs:\t OLD_ = " << act_obj_it.ref_old_point_.transpose() << "("<<act_obj_it.ref_type_<<") \n\t\t NEW = "
-                                                                    << act_obj_it.ref_act_point_.transpose() << "("<<prev_obj_it.ref_type_<<") \t\t" << std::endl;
+//                    std::cout << std::endl << "Change Refs:\t OLD_ = " << act_obj_it.ref_old_point_.transpose() << "("<<act_obj_it.ref_type_<<") \n\t\t NEW = "
+//                                                                    << act_obj_it.ref_act_point_.transpose() << "("<<prev_obj_it.ref_type_<<") \t\t" << std::endl;
 
                     act_obj_it.ref_type_ = prev_obj_it.ref_type_;
 
@@ -979,8 +980,8 @@ void laserscanutils::associateObjects(std::list<laserscanutils::Object> & _prev_
                 {
                     // Added new version for integration => Not really used
                     prev_obj_it.ref_act_point_ = act_obj_it.ref_act_point_;
-                    std::cout << std::endl << "Change Refs Backward:\t OLD = " << prev_obj_it.ref_old_point_.transpose() << "("<<act_obj_it.ref_type_<<") \n\t\t NEW = "
-                                                                    << act_obj_it.ref_act_point_.transpose() << "("<<prev_obj_it.ref_type_<<") \t\t" << std::endl;
+//                    std::cout << std::endl << "Change Refs Backward:\t OLD = " << prev_obj_it.ref_old_point_.transpose() << "("<<act_obj_it.ref_type_<<") \n\t\t NEW = "
+//                                                                    << act_obj_it.ref_act_point_.transpose() << "("<<prev_obj_it.ref_type_<<") \t\t" << std::endl;
 
                     prev_obj_it.ref_type_= act_obj_it.ref_type_;
                 }