diff --git a/src/object_detector.cpp b/src/object_detector.cpp
index ed8bdf90581831cb219575c7172c6fece17ff30b..f8f754508f3cdf51fd6dfe2f6047461f40596f30 100644
--- a/src/object_detector.cpp
+++ b/src/object_detector.cpp
@@ -432,6 +432,7 @@ void laserscanutils::findLinesInCluster(const laserscanutils::ScanParams & _scan
std::list<laserscanutils::Line>& _line_list)
{
laserscanutils::Line line;
+ laserscanutils::Line new_line;
std::list<laserscanutils::Line>::iterator line_it1, line_it2;
ScalarT theta; // for angle between lines
@@ -454,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_ )
{
- //std::cout << "=> YES!" << std::endl;
+ std::cout << "=> YES!" << std::endl;
line.first_ = i_from;
line.last_ = ii;
line.point_first_ = _cluster.col(line.first_);
@@ -471,12 +472,12 @@ void laserscanutils::findLinesInCluster(const laserscanutils::ScanParams & _scan
}
else
{
- //std::cout << " NO! - Error = " << line.error_ << " > " << _scan_params.range_std_dev_*_alg_params.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);
}
- //std::cout << "Lines fitted: " << _line_list.size();
+ std::cout << "Lines fitted: " << _line_list.size() ;
// concatenating lines
@@ -495,13 +496,12 @@ void laserscanutils::findLinesInCluster(const laserscanutils::ScanParams & _scan
if ( theta < _alg_params.fit_line_theta_max_parallel_) //fabs(theta) not required since theta>0
{
//compute a new line with all involved points
- Line new_line;
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_)
+ 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_)
{
- //std::cout << " => lines concatenated";
+ std::cout << " => lines concatenated";
//update line1 as the concatenation of l1 and l2
new_line.first_ = line_it1->first_;
@@ -514,12 +514,14 @@ 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";
+
line_it2++;
}
}
@@ -553,8 +555,17 @@ 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;
+ 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;
+ }
+
+
+ std::cout << "END findLinesInCluster. Size = " << _line_list.size() << std::endl;
}
@@ -583,8 +594,12 @@ unsigned int laserscanutils::findCornersInClusterLines(const laserscanutils::Las
// TODO: Corner as init or final point of line. -> As done in association
}
- while ( line_it1 != _line_list.end()--)
+ auto last_it = _line_list.end();
+ last_it --;
+
+ while ( line_it1 != last_it)
{
+
line_it2 = line_it1;
line_it2 ++;
@@ -819,7 +834,7 @@ void laserscanutils::associateObjects(std::list<laserscanutils::Object> & _prev_
// std::cout << std::endl << "Comparing Objects " << act_obj << " - " << prev_obj;
- // Compare Centroids
+ // Compare CENTROIDS
float temp_dist_centroid = std::pow( ( act_obj_it.ref_centroid_point_(0) - prev_obj_it.ref_centroid_point_(0) ), 2.0 ) +
pow( ( act_obj_it.ref_centroid_point_(1) - prev_obj_it.ref_centroid_point_(1) ), 2.0 ) ;
@@ -949,11 +964,11 @@ void laserscanutils::associateObjects(std::list<laserscanutils::Object> & _prev_
act_obj_it.associated_ = true;
act_obj_it.asso_to_prev_obj_in_pos_ = prev_obj;
- // Check ref_point of the association
+ // Check ref_point of the association => Forward propagation (prev -> actual)
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 = "
+ 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_;
@@ -962,8 +977,25 @@ void laserscanutils::associateObjects(std::list<laserscanutils::Object> & _prev_
}
else
{
- //act_obj_it.ref_old_point_ = prev_obj_it.ref_act_point_;
+ // 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;
+
+ prev_obj_it.ref_type_= act_obj_it.ref_type_;
}
+
+// // Propagating object features:
+// if(act_obj_it.size_x_ < prev_obj_it.size_x_)
+// {
+// act_obj_it.size_x_ = prev_obj_it.size_x_;
+// }
+
+// if(act_obj_it.size_y_ < prev_obj_it.size_y_)
+// {
+// act_obj_it.size_y_ = prev_obj_it.size_y_;
+// }
+
}