diff --git a/src/object_detector.cpp b/src/object_detector.cpp index f8f754508f3cdf51fd6dfe2f6047461f40596f30..35607fc5e1e8baf9d63d0fc7c9f58425d9fba91e 100644 --- a/src/object_detector.cpp +++ b/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_; }