From 5726c81a6b37980c5afcbe772616fa56cd2b3fdb Mon Sep 17 00:00:00 2001
From: vvaquero <vvaquero@224674b8-e365-4e73-a4a8-558dbbfec58c>
Date: Fri, 19 Jun 2015 13:49:27 +0000
Subject: [PATCH] Object_Detection cleaned up
---
src/entities.cpp | 18 +--
src/entities.h | 28 ++--
src/object_detector.cpp | 277 ++++++++++++++++++----------------------
3 files changed, 142 insertions(+), 181 deletions(-)
diff --git a/src/entities.cpp b/src/entities.cpp
index d4cab0a..6cb9112 100644
--- a/src/entities.cpp
+++ b/src/entities.cpp
@@ -43,15 +43,17 @@ void laserscanutils::Object::print() const
std::endl << "---------------------------------------------" << std::endl
<< "--- Object Detected Information ---" << std::endl
<< "---------------------------------------------" << std::endl
- << " * Initial Global Point : " << this->first_ << std::endl
- << " * Final Global Point : " << this->last_ << std::endl
- << " * Number of Points: " << this->num_points_ << std::endl
-// << " * Velocity: " << std::endl
-// << " Linear " << this->linear_velocity_.transpose() << "; " << std::endl
-// << " Angular " << this->angular_velocity_.transpose() << "; " << std::endl
- << " * Associated?: " << this->associated_<< std::endl
+ << " * Initial Global Point : \t" << this->first_ << std::endl
+ << " * Final Global Point : \t" << this->last_ << std::endl
+ << " * Number of Points: \t" << this->num_points_ << std::endl
+ << " * Associated?: \t\t" << this->associated_<< std::endl
+ << " * Structured?: \t\t" << this->structured_ << std::endl
+ << " * Bounding Box Center : \t" << this->object_center_.transpose() << std::endl
+ << " * Bounding Box Size : \t\t" << this->size_x_ << " x " << this->size_y_ << std::endl
+ << " * Bounding Box Orientation :" << this->obj_orientation_ << std::endl
+ << " * Polyline: " << std::endl << "\t\t" << this->polyline_points_ << std::endl
<< " * Lines: " << std::endl;
- //std::cout << "object Line list SIZE: " << this->line_list_.size() << std::endl;
+
if (this->line_list_.size() >= 1)
{
for (auto it1 = this->line_list_.begin(); it1 != this->line_list_.end(); it1++)
diff --git a/src/entities.h b/src/entities.h
index 8da01f1..65dc9c0 100644
--- a/src/entities.h
+++ b/src/entities.h
@@ -61,10 +61,10 @@ namespace laserscanutils
this->first_ = 0;
this->last_ = 0;
this->associated_ = false;
+ this->structured_ = false;
this->id_ = -1;
- this->ref_middle_point_ = {0.0, 0.0, 0.0};
this->ref_centroid_point_ = {0.0, 0.0, 0.0};
this->ref_act_point_ = {0.0, 0.0, 0.0};
this->ref_old_point_ = {0.0, 0.0, 0.0};
@@ -73,35 +73,32 @@ namespace laserscanutils
this->min_y_ = 1000.0;
this->max_x_ = -1000.0;
this->max_y_ = -1000.0;
- this->angle_ = 0.0;
- this->orientation_ = 0.0;
+ this->obj_orientation_ = 0.0;
this->object_center_ << 0.0, 0.0;
this->size_x_ = 0.0;
this->size_y_ = 0.0;
-// this->polyline_points_ << 0.0, 0.0, 0.0, 0.0, 0.0,
-// 0.0, 0.0, 0.0, 0.0, 0.0;
this->ref_type_ = -1;
- this->ref_position_ = -1;
+ this->ref_position_ = -1; // TBR??? Position of the corner/line in the respective list...
+ this->asso_to_prev_obj_in_pos_ = -1; // TBR??? Position of the corner/line in the respective list...
}
bool associated_;
+ bool structured_;
unsigned int num_points_; // number of points of the cluster
unsigned int first_; //index of the Homogeneous Matrix column for the first point used (GLOBAL)
unsigned int last_; //index of the Homogeneous Matrix column for the last point used (GLOBAL)
std::list<laserscanutils::Line> line_list_; // list of lines contained in the cluster (Refs Points are LOCAL)
std::list<laserscanutils::Corner> corner_list_; // list of corners contained in the cluster (Refs Points are LOCAL)
-
-// Eigen::Vector2f linear_velocity_; // WRT??
-// Eigen::Vector2f angular_velocity_; // WRT??
-
ScalarT id_; // Id of the object. -1 if unknown or MOBILE; other number if pre-associated.
+ unsigned int asso_to_prev_obj_in_pos_; // For tracking help, this object has been associated to the object in position
+ // "asso_to_obj_in_pos_" from the previous scan. (-1 if not)
+
// Description
- Eigen::Vector3s ref_middle_point_;
Eigen::Vector3s ref_centroid_point_;
Eigen::Vector3s ref_act_point_;
Eigen::Vector3s ref_old_point_;
@@ -110,25 +107,20 @@ namespace laserscanutils
float min_y_;
float max_x_;
float max_y_;
- float angle_;
- float orientation_;
+ float obj_orientation_;
float size_x_;
float size_y_;
Eigen::Vector2s object_center_;
Eigen::MatrixXs polyline_points_; //<float,2,5>
-
int ref_type_; // Type of the reference. 8 = corner; 4 = init_line ; 2 = end_line; 1 = centroid. -1 = unkonwn;
- int ref_position_; // The position of the referenced corner (or line) in its corresponding list;
+ int ref_position_; // [TODO] The position of the referenced corner (or line) in its corresponding list;
- // TODO: Reference Point
- // TODO: Bounding Box
void print() const;
float getArea();
float getPerimeter();
-
};
}
#endif
diff --git a/src/object_detector.cpp b/src/object_detector.cpp
index 10c00d2..ed8bdf9 100644
--- a/src/object_detector.cpp
+++ b/src/object_detector.cpp
@@ -267,18 +267,19 @@ void laserscanutils::preFilterScan2homog(const laserscanutils::ScanParams & _par
//std::cout << "Invalid Point: " << scan.ranges[i] << " - index: " << i << std::endl;
}
- } // END FOR scan ranges
+ }
// Filling stats
_scan_processed.scan_total_points_ = _ranges.size();
_scan_processed.filtered_total_points_ = matPosition;
-// _scan_processed.odom_ori_
// Conservative resize of the eigen Matrix
_points.conservativeResize(3, matPosition);
+
+// // DEBUG:
// std::cout << std::endl << "[PRE-FILTERING]: TotalPoints: " << _scan_processed.scan_total_points_ << std::endl;
// std::cout << "[PRE-FILTERING]: filtered_total_points_: " << _scan_processed.filtered_total_points_ << std::endl;
// std::cout << "[PRE-FILTERING]: matrixResize: " << _points.cols() << std::endl;
@@ -310,11 +311,9 @@ void laserscanutils::extractClustersInScan(const laserscanutils::LaserObjectDetA
int count_num_valid_cluster = 0;
// Initial Segment
-// count_num_cluster = 1;
seg.first = 0;
float sq_threshold = pow(_alg_params.cluster_jump_thr_,2.0);
- //for (unsigned int col = 1; col < _points.cols(); col++)
for (unsigned int col = 1; col < _scan_processed.filtered_total_points_; col++)
{
points_distance = ( pow( ( _points(0,col) -_points(0,col-1) ), 2.0 ) + pow( ( _points(1,col) - _points(1,col-1) ), 2.0) );
@@ -323,7 +322,7 @@ void laserscanutils::extractClustersInScan(const laserscanutils::LaserObjectDetA
{
// Same segment
- // Calculating extrem Points
+ // Calculating extrem Points -> Used for those unestructured objects (with no lines found) when finding the bounding Box
if(_points(0,col-1) < temp_object.min_x_ )
temp_object.min_x_ = _points(0,col-1);
if(_points(1,col-1) < temp_object.min_y_ )
@@ -335,14 +334,15 @@ void laserscanutils::extractClustersInScan(const laserscanutils::LaserObjectDetA
temp_object.max_y_ = _points(1,col-1);
}
- else // if ( col != _points.cols() ) // new segment
+ else
{
- //std::cout << "P " << pos << " - A - (" << pointA.x << "-" << pointA.y << ") - B - (" << pointB.x << "-" << pointB.y << ") -> distance: " << points_distance << " More Than th" << std:endl;
+ // new segment
- seg.second = col-1; // end of the previous segment
+ seg.second = col-1; // end of the previous segment
// check length of the cluster
- if ( ( (seg.second - seg.first)+1 >= _alg_params.cluster_min_num_points_ ) && ( (seg.second - seg.first)+1 <= _alg_params.cluster_max_num_points_) )
+ if ( ( (seg.second - seg.first)+1 >= _alg_params.cluster_min_num_points_ ) &&
+ ( (seg.second - seg.first)+1 <= _alg_params.cluster_max_num_points_) )
{
_scan_processed.cluster_indxs_.push_back(seg);
@@ -354,29 +354,27 @@ void laserscanutils::extractClustersInScan(const laserscanutils::LaserObjectDetA
_scan_processed.object_list_.push_back(temp_object);
_scan_processed.num_points_in_clusters_ += temp_object.num_points_;
- // DEBUG
- std::cout << " MIN MAX POINTS: X: (" << temp_object.min_x_ << "," << temp_object.max_x_ <<
- ") ; Y: (" << temp_object.min_y_ << "," << temp_object.max_y_ << ")" << std::endl;
+ count_num_valid_cluster++; // count cluster as valid
+
+ } // TODO OPT: else: save the invalid cluster in other vector of bad clusters to further analysis
- count_num_valid_cluster++; // count cluster as valid
- //std::cout << "CLUSTER IS VALID" << std::endl;
- }
- // TODO: else: save the invalid cluster in other vector of bad clusters to further analysis
count_num_cluster++; // count any cluster
+
// reset min max point values:
temp_object.max_x_ = -1000.0;
temp_object.max_y_ = -1000.0;
temp_object.min_x_ = 1000.0;
temp_object.min_y_ = 1000.0;
+ // Initialize next segment (cluster)
seg.first = col;
}
}
- // LAST POINT close last segment (REPEATS STEP)
-// seg.second = _points.cols()-1;
+
+ // LAST POINT close last segment (REPEATS LAST STEP)
seg.second = _scan_processed.filtered_total_points_-1;
if ( ((seg.second - seg.first)+1 >= _alg_params.cluster_min_num_points_) && ((seg.second - seg.first)+1 <= _alg_params.cluster_max_num_points_) )
{
@@ -389,11 +387,6 @@ void laserscanutils::extractClustersInScan(const laserscanutils::LaserObjectDetA
_scan_processed.object_list_.push_back(temp_object);
_scan_processed.num_points_in_clusters_ += temp_object.num_points_;
-
- // DEBUG
- std::cout << " MIN MAX POINTS: X: (" << temp_object.min_x_ << "," << temp_object.max_x_ <<
- ") ; Y: (" << temp_object.min_y_ << "," << temp_object.max_y_ << ")" << std::endl;
-
// reset min max point values:
temp_object.max_x_ = -1000.0;
temp_object.max_y_ = -1000.0;
@@ -401,8 +394,6 @@ void laserscanutils::extractClustersInScan(const laserscanutils::LaserObjectDetA
temp_object.min_y_ = 1000.0;
count_num_valid_cluster++; // count cluster as valid
- //std::cout << "CLUSTER IS VALID" << std::endl;
-
}
count_num_cluster++; // count any cluster
@@ -413,20 +404,19 @@ void laserscanutils::extractClustersInScan(const laserscanutils::LaserObjectDetA
_scan_processed.number_of_valid_clusters_ = count_num_valid_cluster;
_scan_processed.number_of_global_scan_ = _global_scan_number;
- //_scan_processed.object_asso_values_.resize(count_num_valid_cluster,0);
-
// *** [DEBUG] ****
- // DEBUG printing clusters indexes
- std::cout << " = " ;
- for (auto it_clusters : _scan_processed.cluster_indxs_ )
- {
- std::cout << "[" << it_clusters.first << ", " << it_clusters.second << "] - ";
- }
+// // DEBUG printing clusters indexes
+// std::cout << " = " ;
+// for (auto it_clusters : _scan_processed.cluster_indxs_ )
+// {
+// std::cout << "[" << it_clusters.first << ", " << it_clusters.second << "] - ";
+// }
+
+// std::cout << "Done!" << std::endl;
- std::cout << "Done!" << std::endl;
}
@@ -441,7 +431,6 @@ void laserscanutils::findLinesInCluster(const laserscanutils::ScanParams & _scan
const Eigen::MatrixXs & _cluster,
std::list<laserscanutils::Line>& _line_list)
{
- //std::cout << "TEST!!!!: = _cluster.cols() = " << _cluster.cols() << std::endl;
laserscanutils::Line line;
std::list<laserscanutils::Line>::iterator line_it1, line_it2;
ScalarT theta; // for angle between lines
@@ -449,20 +438,17 @@ void laserscanutils::findLinesInCluster(const laserscanutils::ScanParams & _scan
// cleaning line_list
_line_list.clear();
- //init random generator
+ //init random generator -> TODO: Eliminate (this is from previous version of library)
std::default_random_engine generator(1);
std::uniform_int_distribution<int> rand_window_overlapping(1,_alg_params.fit_line_window_size_);
- //std::cout << "TEST!!!!: = _cluster.cols() = " << _cluster.cols() << std::endl;
-
unsigned int ii = _alg_params.fit_line_window_size_-1;
bool last = false;
- //while ( (ii<_cluster.cols()) && (last != true) )
+
while (last != true)
{
if (ii >= _cluster.cols())
{
- //std::cout << " LAST " << std::endl;
ii = _cluster.cols()-1;
last = true;
}
@@ -486,12 +472,6 @@ 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 << " List of points: " ;
-// for (unsigned int po = i_from; po < ii; po++)
-// {
-// std::cout << "[" << _cluster(0,po) << ", " << _cluster(1,po) << "], ";
-// }
-// std::cout << std::endl;
}
ii = ii+rand_window_overlapping(generator);
}
@@ -511,17 +491,12 @@ void laserscanutils::findLinesInCluster(const laserscanutils::ScanParams & _scan
//compute angle between lines 1 and 2
theta = laserscanutils::angleBetweenLines(*line_it1, *line_it2);
-// std::cout << "LineIt1 = " << std::distance(line_list.begin(),line_it1) << " vs LineIt2 = " << std::distance(line_list.begin(), line_it2) << std::endl;
-// std::cout << "cos_theta: " << cos(theta) << " -> theta: " << theta << std::endl;
-
//Check angle threshold
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);
-// std::cout << " Checking Concatenation error of : " << line_it1->first_ << " vs " << line_it2->last_ << std::endl;
-// std::cout << " New Line error : " << new_line.error_ << " <=? " << _scan_params.range_std_dev_*_alg_params.k_sigmas << std::endl;
//check if error below a threshold to effectively concatenate
if ( new_line.error_ < _scan_params.range_std_dev_*_alg_params.fit_line_k_sigmas_)
@@ -542,19 +517,15 @@ void laserscanutils::findLinesInCluster(const laserscanutils::ScanParams & _scan
//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;
- //in that case do not update iterators to allow concatenation of the new l1 with the next line in the list, if any
- // update_iterators = false;
}
else
{
line_it2++;
- //std::cout << " <-- no concatenation...error too big" << std::endl;
}
}
else
{
line_it2++;
- //std::cout << " <-- no concatenation... theta too big " << std::endl;
}
} // end while it 2
@@ -564,14 +535,14 @@ void laserscanutils::findLinesInCluster(const laserscanutils::ScanParams & _scan
line_it1++;
while ( (temp_line_it->last_ >= line_it1->first_) && (line_it1 != _line_list.end()))
{
- //std::cout << " linea intermedia!" << std::endl;
line_it1 = _line_list.erase(line_it1);
-// line_it1++;
}
} // end while it 1
+
+ // DEBUG TEST
// // globalizing first and last point (in terms of the whole scan)
// for (auto list_it1 = _line_list.begin(); list_it1 != _line_list.end(); list_it1++)
// {
@@ -608,8 +579,8 @@ unsigned int laserscanutils::findCornersInClusterLines(const laserscanutils::Las
if (_line_list.size() <2)
{
- //std::cout << "CORNERS DEBUG: --> NOT ENOUGH LINES IN CLUSTER TO EXTRACT CORNERS" << std::endl;
- return 0;
+ return 0; // Not enough lines for a normal corner:
+ // TODO: Corner as init or final point of line. -> As done in association
}
while ( line_it1 != _line_list.end()--)
@@ -623,32 +594,27 @@ unsigned int laserscanutils::findCornersInClusterLines(const laserscanutils::Las
//Check angle threshold
if ( theta > _obj_det_alg_params.corner_theta_min_)
{
- //std::cout << " ... Corner! : " << std::endl;
// Find the corner as the cross product between lines (intersection of lines in homog)
corner.pt_ = (line_it1->vector_).cross(line_it2->vector_);
// normalize corner point to set last component to 1
corner.pt_ = corner.pt_ / corner.pt_(2);
- //vector from corner to first point of l1
- Eigen::Vector2s v1 = line_it1->point_first_.head(2) - corner.pt_.head(2);
+ //vector from corner to first point of l1
+ Eigen::Vector2s v1 = line_it1->point_first_.head(2) - corner.pt_.head(2);
- //compute corner orientation as the angle between v1 and local X, in [-pi,pi]
- corner.orientation_ = atan2(v1(1),v1(0));
+ //compute corner orientation as the angle between v1 and local X, in [-pi,pi]
+ corner.orientation_ = atan2(v1(1),v1(0));
- //Compute corner aperture with line_it1->first, corner and line_it2->last
- corner.aperture_ = cornerAperture(line_it1->point_first_, corner.pt_, line_it2->point_last_);
+ //Compute corner aperture with line_it1->first, corner and line_it2->last
+ corner.aperture_ = cornerAperture(line_it1->point_first_, corner.pt_, line_it2->point_last_);
- //set other descriptors
- corner.line_1_ = *line_it1;
- corner.line_2_ = *line_it2;
+ //set other descriptors
+ corner.line_1_ = *line_it1;
+ corner.line_2_ = *line_it2;
- //add the corner to the list
- _corner_list.push_back(corner);
-
- // debug: print
- //corner.print();
- // }
+ //add the corner to the list
+ _corner_list.push_back(corner);
}
else
//std::cout << " ... No Corner! : " << std::endl;
@@ -666,22 +632,22 @@ unsigned int laserscanutils::findCornersInClusterLines(const laserscanutils::Las
-
-
-
-
+//****************************************************
+//********** extractFeaturesInObject
+//****************************************************
void laserscanutils::extractFeaturesInObject(const Eigen::MatrixXs &_points, laserscanutils::Object &_object)
{
float longest_line_size = -1;
int longest_line_idx = -1;
+
+ // Extract points corresponding to the object
Eigen::MatrixXs points;
points = _points.block(0,_object.first_,2,_object.num_points_);
- // If principal line
+ // Find longest line
if (!_object.line_list_.empty())
{
- // * Find longest line
longest_line_size = -1;
longest_line_idx = -1;
int counter = -1;
@@ -694,82 +660,59 @@ void laserscanutils::extractFeaturesInObject(const Eigen::MatrixXs &_points, las
longest_line_size = line_it.length();
longest_line_idx = counter;
}
-
}
}
if (longest_line_size > 0.7)
{
- // DEBUG
- std::cout << " DEBUG: LONG LINE " << std::endl;
- auto longest_line_it = std::next(_object.line_list_.begin(), longest_line_idx);
+ _object.structured_ = true;
+ // Get the longest line
+ auto longest_line_it = std::next(_object.line_list_.begin(), longest_line_idx);
laserscanutils::Line longest_line = *longest_line_it;
-
-
- //vector of the line
+ //Get orientation of the longest line to get object Orientation
Eigen::Vector2s v1 = longest_line.point_first_.head(2) - longest_line.point_last_.head(2);
- _object.orientation_ = std::atan2(v1(1),v1(0));
+ _object.obj_orientation_ = std::atan2(v1(1),v1(0));
- // Rotation Matrix
+ // Defining Rectangle (Bounding Box) Size
+ // - Rotation Matrix
Eigen::Matrix2s tf;
- tf << std::cos(_object.orientation_), -1*(std::sin(_object.orientation_)),
- std::sin(_object.orientation_), std::cos(_object.orientation_);
+ tf << std::cos(_object.obj_orientation_), -1*(std::sin(_object.obj_orientation_)),
+ std::sin(_object.obj_orientation_), std::cos(_object.obj_orientation_);
- // Points Transformed
+ // - Points Transformed
Eigen::MatrixXs pointsRot;
pointsRot = tf.inverse()*points;
- // Max and Min coordinates
+ // - Max and Min coordinates
Eigen::MatrixXf::Index maxRowX, maxColX, minRowX, minColX, maxRowY, maxColY, minRowY, minColY;
auto MaxX = pointsRot.row(0).maxCoeff(&maxRowX, &maxColX);
auto MinX = pointsRot.row(0).minCoeff(&minRowX, &minColX);
auto MaxY = pointsRot.row(1).maxCoeff(&maxRowY, &maxColY);
auto MinY = pointsRot.row(1).minCoeff(&minRowY, &minColY);
-// // DEBUG
-// std::cout << "DEBUG : MAX_X = " << MaxX << " AT: ( " << maxRowX << ", " << maxColX << ")" << std::endl;
-// std::cout << "DEBUG : MIN_X = " << MinX << " AT: ( " << minRowX << ", " << minColX << ")" << std::endl;
-// std::cout << "DEBUG : MAX_Y = " << MaxY << " AT: ( " << maxRowY << ", " << maxColY << ")" << std::endl;
-// std::cout << "DEBUG : MIN_Y = " << MinY << " AT: ( " << minRowY << ", " << minColY << ")" << std::endl;
-
- std::cout << " DEBUG: DEFINING RECTANGLE SIZE" << std::endl;
-
- // Object Rectangle definition
+ // - Object Rectangle definition
_object.size_x_ = std::abs(MaxX - MinX);
_object.size_y_ = std::abs(MaxY - MinY);
-// _object.object_center_<< (MaxX - MinX)/2 + MinX, (MaxY - MinY)/2 + MinY;
- std::cout << " DEBUG: DEFINING RECTANGLE CENTRE" << std::endl;
_object.object_center_(0) = (MaxX - MinX)/2 + MinX;
_object.object_center_(1) = (MaxY - MinY)/2 + MinY;
_object.object_center_ = tf*_object.object_center_; // de-rotation the center
- // Rotate rectangle corners
- std::cout << " DEBUG: DEFINING RECTANGLE POLYLINE" << std::endl;
+ // Defining Rectangle Polyline
+ // - Rotate rectangle corners
_object.polyline_points_.resize(2,5);
- // std::cout << "Polyline SIZE = " << _object.polyline_points_.rows() << " x " << _object.polyline_points_.cols() << std::endl;
_object.polyline_points_ << MaxX, MinX, MinX, MaxX, MaxX,
MinY, MinY, MaxY, MaxY, MinY;
_object.polyline_points_ = tf*_object.polyline_points_;
- // DEBUG
- std::cout << "DEBUG : CENTER = (" << _object.object_center_.transpose() <<
- "); SIZE = " << _object.size_x_ << " x " << _object.size_y_ << "; Orientation: " << _object.orientation_ << std::endl;
- std::cout << "DEBUG : POLYLINE: " << std::endl << _object.polyline_points_ << std::endl;
-
-
-
-
-
}
else // there are no lines OR line size is not enough -> give rectangle of unstructured Object
{
-
- std::cout << " DEBUG: NOP LONG LINE " << std::endl;
+ _object.structured_ = false;
// Find x/y_min/max
Eigen::MatrixXf::Index maxRowX, maxColX, minRowX, minColX, maxRowY, maxColY, minRowY, minColY;
auto MaxX = points.row(0).maxCoeff(&maxRowX, &maxColX);
@@ -777,31 +720,50 @@ void laserscanutils::extractFeaturesInObject(const Eigen::MatrixXs &_points, las
auto MaxY = points.row(1).maxCoeff(&maxRowY, &maxColY);
auto MinY = points.row(1).minCoeff(&minRowY, &minColY);
- std::cout << " DEBUG: DEFINING RECTANGLE SIZE" << std::endl;
// Object Rectangle definition
_object.size_x_ = std::abs(MaxX - MinX);
_object.size_y_ = std::abs(MaxY - MinY);
- std::cout << " DEBUG: DEFINING RECTANGLE CENTRE" << std::endl;
+ // - Center
_object.object_center_(0) = (MaxX - MinX)/2 + MinX;
_object.object_center_(1) = (MaxY - MinY)/2 + MinY;
- std::cout << " DEBUG: DEFINING RECTANGLE POLYLINE" << std::endl;
+ // - Polyline
_object.polyline_points_.resize(2,5);
_object.polyline_points_ << MaxX, MinX, MinX, MaxX, MaxX,
MinY, MinY, MaxY, MaxY, MinY;
- // DEBUG
- std::cout << "DEBUG : CENTER = (" << _object.object_center_.transpose() <<
- "); SIZE = " << _object.size_x_ << " x " << _object.size_y_ << "; Orientation: " << _object.orientation_ << std::endl;
- std::cout << "DEBUG : POLYLINE: " << std::endl << _object.polyline_points_ << std::endl;
+ }
+
+ // Object Centroid
+ _object.ref_centroid_point_(0) = (points.row(0).sum())/_object.num_points_;
+ _object.ref_centroid_point_(1) = (points.row(1).sum())/_object.num_points_;
+ _object.ref_centroid_point_(2) = 1;
+ // Initial References
+ if (!_object.corner_list_.empty())
+ {
+ // the front corner of the object will be the reference
+ _object.ref_old_point_ = _object.corner_list_.front().pt_;
+ _object.ref_act_point_ = _object.corner_list_.front().pt_;
+ _object.ref_type_ = 8;
+ _object.ref_position_ = 0; // referenced corner position in list = 0 = front();
+ }
+ else
+ {
+ // if no corners, ref will be the centroid
+ _object.ref_old_point_ = _object.ref_centroid_point_;
+ _object.ref_act_point_ = _object.ref_centroid_point_;
+ _object.ref_type_ = 1;
+ _object.ref_position_ = -1;
}
+// // DEBUG
+// _object.print();
}
@@ -838,10 +800,10 @@ void laserscanutils::associateObjects(std::list<laserscanutils::Object> & _prev_
unsigned int prev_obj = 0;
unsigned int act_obj = 0;
- bool obj_corner_asso = false;
+ bool obj_corner_asso = false;
bool obj_init_line_asso = false;
- bool obj_end_line_asso = false;
- bool obj_centroid_asso = false;
+ bool obj_end_line_asso = false;
+ bool obj_centroid_asso = false;
for (auto&& act_obj_it : _act_objects_list)
@@ -849,10 +811,11 @@ void laserscanutils::associateObjects(std::list<laserscanutils::Object> & _prev_
prev_obj = 0;
for (auto&& prev_obj_it : _prev_objects_list)
{
- obj_corner_asso = false;
- obj_init_line_asso = false;
- obj_end_line_asso = false;
- obj_centroid_asso = false;
+ // reset flags
+ obj_corner_asso = false;
+ obj_init_line_asso = false;
+ obj_end_line_asso = false;
+ obj_centroid_asso = false;
// std::cout << std::endl << "Comparing Objects " << act_obj << " - " << prev_obj;
@@ -860,10 +823,19 @@ void laserscanutils::associateObjects(std::list<laserscanutils::Object> & _prev_
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 ) ;
+// // DEBUG
// std::cout << "DISTANCE #Centroids: " << act_obj << " - " << prev_obj << ": " << std::endl;
// std::cout << "\t\t Centroid Distance = " << temp_dist_centroid << std::endl;
+ // Updating Matrices Results
asso_results[act_obj][prev_obj].centroid_ = std::min(temp_dist_centroid, asso_results[act_obj][prev_obj].centroid_);
+ asso_results_count[act_obj][prev_obj].centroid_ += 1;
+
+ if (temp_dist_centroid < sq_corner_threshold)
+ {
+ obj_centroid_asso = true;
+ }
+
// Compare CORNERS LIST if exist
if ( (act_obj_it.corner_list_.size() != 0) && (prev_obj_it.corner_list_.size() != 0) )
@@ -882,17 +854,16 @@ void laserscanutils::associateObjects(std::list<laserscanutils::Object> & _prev_
float temp_dist_corner = std::pow( ( act_corner_it.pt_(0) - prev_corner_it.pt_(0) ), 2.0 ) +
pow( ( act_corner_it.pt_(1) - prev_corner_it.pt_(1) ), 2.0) ;
+// // DEBUG
// std::cout << "DISTANCE #corners: " << act_obj << "."<< act_pos << " - " << prev_obj << "." << prev_pos << ": " << std::endl;
// std::cout << "\t\t Corner Distance = " << temp_dist_corner << std::endl;
+ // Updating Matrices Results
+ asso_results[act_obj][prev_obj].corner_ = std::min(temp_dist_corner, asso_results[act_obj][prev_obj].corner_);
+ asso_results_count[act_obj][prev_obj].corner_ += 1;
+
if (temp_dist_corner < sq_corner_threshold)
{
-// std::cout << "Storing in asso_results["
-// << act_obj << "][" << prev_obj << "][" << prev_pos << "]: " << temp_dist_corner << std::endl;
-
- asso_results[act_obj][prev_obj].corner_ = std::min(temp_dist_corner, asso_results[act_obj][prev_obj].corner_);
- asso_results_count[act_obj][prev_obj].corner_ += 1;
-
obj_corner_asso =true;
// If are associated, prev_scan reference is the actual one too. NO!
@@ -934,32 +905,26 @@ void laserscanutils::associateObjects(std::list<laserscanutils::Object> & _prev_
float temp_dist_final_pt = std::pow( ( act_line_it.point_last_(0) - prev_line_it.point_last_(0) ), 2.0 ) +
pow( ( act_line_it.point_last_(1) - prev_line_it.point_last_(1) ), 2.0) ;
+// // DEBUG
// std::cout << "DISTANCE #lines: " << act_obj << "."<< act_pos << " - " << prev_obj << "." << prev_pos << ": " << std::endl;
// std::cout << "\t\t Init Points Distance = " << temp_dist_init_pt << std::endl;
// std::cout << "\t\t Final Points Distance = " << temp_dist_final_pt << std::endl;
// std::cout << "\t\t ANGLE = " << temp_dist_angle << std::endl;
- if ( (temp_dist_angle < _angle_thr) && (temp_dist_init_pt < sq_line_threshold) )
- {
-// std::cout << "Storing LINES INIT in asso_results["
-// << act_obj << "][" << prev_obj << "][" << prev_pos << "]: " << temp_dist_init_pt << std::endl;
+ // Updating result Matrices.
+ asso_results[act_obj][prev_obj].line_init_ = std::min(temp_dist_init_pt, asso_results[act_obj][prev_obj].line_init_);
+ asso_results_count[act_obj][prev_obj].line_init_ += 1;
- asso_results[act_obj][prev_obj].line_init_ = std::min(temp_dist_init_pt, asso_results[act_obj][prev_obj].line_init_);
-
- asso_results_count[act_obj][prev_obj].line_init_ += 1;
+ asso_results[act_obj][prev_obj].line_end_ = std::min(temp_dist_final_pt, asso_results[act_obj][prev_obj].line_end_);
+ asso_results_count[act_obj][prev_obj].line_end_ += 1;
+ if ( (temp_dist_angle < _angle_thr) && (temp_dist_init_pt < sq_line_threshold) )
+ {
obj_init_line_asso = true;
}
if ( (temp_dist_angle < _angle_thr) && (temp_dist_final_pt < sq_line_threshold) )
{
-// std::cout << "Storing LINES END in asso_results["
-// << act_obj << "][" << prev_obj << "][" << prev_pos << "]: " << temp_dist_final_pt << std::endl;
-
- asso_results[act_obj][prev_obj].line_end_ = std::min(temp_dist_final_pt, asso_results[act_obj][prev_obj].line_end_);
-
- asso_results_count[act_obj][prev_obj].line_end_ += 1;
-
obj_end_line_asso = true;
}
@@ -975,13 +940,14 @@ void laserscanutils::associateObjects(std::list<laserscanutils::Object> & _prev_
// ASSOCIATION RULES:
// Check ref_points associated and define the tracking point.
- if (obj_corner_asso || obj_init_line_asso || obj_end_line_asso)
+ if (obj_corner_asso || obj_init_line_asso || obj_end_line_asso || obj_centroid_asso)
{
std::cout << std::endl << "***** Association SUMMARY: *****" << std::endl;
std::cout << "\t OBJECTS: " << act_obj << " && " << prev_obj << std::endl;
std::cout << "***** END SUMMARY ***** " << std::endl;
act_obj_it.associated_ = true;
+ act_obj_it.asso_to_prev_obj_in_pos_ = prev_obj;
// Check ref_point of the association
if (act_obj_it.ref_type_ <= prev_obj_it.ref_type_)
@@ -1008,7 +974,8 @@ void laserscanutils::associateObjects(std::list<laserscanutils::Object> & _prev_
}
- laserscanutils::printAssociationResults(asso_results);
+ // DEBUG
+ //laserscanutils::printAssociationResults(asso_results);
//laserscanutils::printAssociationResults(asso_results_count);
}
--
GitLab