Skip to content
Snippets Groups Projects
Commit 174a46cb authored by vvaquero's avatar vvaquero
Browse files

conversion to Object_detector Files. Clustering is not used anymore. Leave there as reference

parent ffbca1a0
No related branches found
No related tags found
No related merge requests found
......@@ -553,126 +553,123 @@ void laserscanutils::preFilterScan(const laserscanutils::ScanParams & _params,
unsigned int laserscanutils::findCornersInClusterLines(const laserscanutils::ScanParams & _params,
const ExtractCornerParams & _alg_params,
const std::list<laserscanutils::Line> & _line_list,
std::list<laserscanutils::Corner> & _corner_list)
{
//local variables
ScalarT theta;
Corner corner;
std::list<Line>::const_iterator line_it1, line_it2, line_it3, last_line;
std::list<laserscanutils::Corner>::iterator corner_it1, corner_it2;
ScalarT max_distance_sq(_alg_params.line_params_.jump_dist_ut_*_alg_params.line_params_.jump_dist_ut_); //init max distance as the squared value to avoid sqroot on comparisons
//std::cout << "CORNER DETECTION FROM LINES: " << std::endl;
// _alg_params.print();
//unsigned int laserscanutils::findCornersInClusterLines(const laserscanutils::ScanParams & _params,
// const ExtractCornerParams & _alg_params,
// const std::list<laserscanutils::Line> & _line_list,
// std::list<laserscanutils::Corner> & _corner_list)
//{
// //local variables
// ScalarT theta;
// Corner corner;
// std::list<Line>::const_iterator line_it1, line_it2, line_it3, last_line;
// std::list<laserscanutils::Corner>::iterator corner_it1, corner_it2;
// ScalarT max_distance_sq(_alg_params.line_params_.jump_dist_ut_*_alg_params.line_params_.jump_dist_ut_); //init max distance as the squared value to avoid sqroot on comparisons
line_it1 = _line_list.begin();
// //std::cout << "CORNER DETECTION FROM LINES: " << std::endl;
// // _alg_params.print();
if (_line_list.size() <2)
{
//std::cout << "DEBUG: --> NOT ENOUGH LINES IN CLUSTER TO EXTRACT CORNERS" << std::endl;
return 0;
}
while ( line_it1 != _line_list.end()--)
{
line_it2 = line_it1;
line_it2 ++;
// line_it1 = _line_list.begin();
// if (_line_list.size() <2)
// {
// //std::cout << "DEBUG: --> NOT ENOUGH LINES IN CLUSTER TO EXTRACT CORNERS" << std::endl;
// return 0;
// }
// std::cout << std::endl << "line 1: last idx: " << (int)line_it1->last_ << std::endl;
// std::cout << "line 2: first idx: " << (int)line_it2->first_ << std::endl;
// std::cout << "substraction " << static_cast<int>(line_it2->first_) - (int)line_it1->last_ << std::endl;
// while ( line_it1 != _line_list.end()--)
// {
// line_it2 = line_it1;
// line_it2 ++;
// std::cout << "line 1: point first: " << line_it1->point_first_.transpose() << std::endl;
// std::cout << "line 1: point last: " << line_it1->point_last_.transpose() << std::endl;
// std::cout << "line 2: point first: " << line_it2->point_first_.transpose() << std::endl;
// std::cout << "line 2: point last: " << line_it2->point_last_.transpose() << std::endl;
//compute angle between lines 1 and 2
theta = angleBetweenLines(*line_it1, *line_it2);
std::cout << "Angle Between Lines #" << std::distance(_line_list.begin(),line_it1) << " - "
<< std::distance(_line_list.begin(),line_it2) << " = " << theta;
// // std::cout << std::endl << "line 1: last idx: " << (int)line_it1->last_ << std::endl;
// // std::cout << "line 2: first idx: " << (int)line_it2->first_ << std::endl;
// // std::cout << "substraction " << static_cast<int>(line_it2->first_) - (int)line_it1->last_ << std::endl;
//Check angle threshold
if ( theta > _alg_params.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_);
// // std::cout << "line 1: point first: " << line_it1->point_first_.transpose() << std::endl;
// // std::cout << "line 1: point last: " << line_it1->point_last_.transpose() << std::endl;
// // std::cout << "line 2: point first: " << line_it2->point_first_.transpose() << std::endl;
// // std::cout << "line 2: point last: " << line_it2->point_last_.transpose() << std::endl;
// normalize corner point to set last component to 1
corner.pt_ = corner.pt_ / corner.pt_(2);
// //compute angle between lines 1 and 2
// theta = angleBetweenLines(*line_it1, *line_it2);
// std::cout << "Angle Between Lines #" << std::distance(_line_list.begin(),line_it1) << " - "
// << std::distance(_line_list.begin(),line_it2) << " = " << theta;
// Check if the corner is close to both lines ends. TODO: Move this check before computing intersection
if ( (line_it1->point_last_ - corner.pt_).head(2).squaredNorm() < max_distance_sq
&&
(line_it2->point_first_ - corner.pt_).head(2).squaredNorm() < max_distance_sq )
{
std::cout << " Corners is close enough: " << std::endl;
// //Check angle threshold
// if ( theta > _alg_params.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_);
//vector from corner to first point of l1
Eigen::Vector2s v1 = line_it1->point_first_.head(2) - corner.pt_.head(2);
// // normalize corner point to set last component to 1
// corner.pt_ = corner.pt_ / corner.pt_(2);
//compute corner orientation as the angle between v1 and local X, in [-pi,pi]
corner.orientation_ = atan2(v1(1),v1(0));
// // Check if the corner is close to both lines ends. TODO: Move this check before computing intersection
// if ( (line_it1->point_last_ - corner.pt_).head(2).squaredNorm() < max_distance_sq
// &&
// (line_it2->point_first_ - corner.pt_).head(2).squaredNorm() < max_distance_sq )
// {
// std::cout << " Corners is close enough: " << std::endl;
//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_);
// //vector from corner to first point of l1
// Eigen::Vector2s v1 = line_it1->point_first_.head(2) - corner.pt_.head(2);
//set other descriptors
corner.line_1_ = *line_it1;
corner.line_2_ = *line_it2;
// //compute corner orientation as the angle between v1 and local X, in [-pi,pi]
// corner.orientation_ = atan2(v1(1),v1(0));
//add the corner to the list
_corner_list.push_back(corner);
// //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_);
// debug: print
corner.print();
}
}
else
//std::cout << " ... No Corner! : " << std::endl;
// //set other descriptors
// corner.line_1_ = *line_it1;
// corner.line_2_ = *line_it2;
// //add the corner to the list
// _corner_list.push_back(corner);
//TODO: QUESTION: Are we checking each line against all ? ANSWER: Only with who is close enough in beam distance
// ANSWER2: as big lines are taken and exists one after another, iterators can be updated simultaneously.
line_it2 ++;
line_it1 ++;
// // debug: print
// corner.print();
// }
// }
// else
// //std::cout << " ... No Corner! : " << std::endl;
// line_it2 ++;
// line_it1 ++;
}
//std::cout << "Corners extracted: " << _corner_list.size() << std::endl;
// // Erase duplicated corners
// corner_it1 = _corner_list.begin();
// while ( corner_it1 != _corner_list.end() )
// {
// corner_it2 = corner_it1;
// corner_it2 ++;
// while (corner_it2 != _corner_list.end())
// {
// // Check if two corners are close enough. TODO: Check othe attributes also (orientation and aperture) !
// if ( ( corner_it1->pt_ - corner_it2->pt_ ).head(2).squaredNorm() < max_distance_sq )
// {
// // Keep the one with bigger product of number of points of each line
// (*corner_it1) = (corner_it1->line_1_.np_*corner_it1->line_2_.np_ > corner_it2->line_1_.np_*corner_it2->line_2_.np_? *corner_it1 : *corner_it2);
// corner_it2 = _corner_list.erase(corner_it2);
// }
// else
// corner_it2 ++;
// }
// corner_it1 ++;
// }
//std::cout << "Corners after removing duplicates: " << _corner_list.size() << std::endl;
// //std::cout << "Corners extracted: " << _corner_list.size() << std::endl;
//// // Erase duplicated corners
//// corner_it1 = _corner_list.begin();
//// while ( corner_it1 != _corner_list.end() )
//// {
//// corner_it2 = corner_it1;
//// corner_it2 ++;
//// while (corner_it2 != _corner_list.end())
//// {
//// // Check if two corners are close enough. TODO: Check othe attributes also (orientation and aperture) !
//// if ( ( corner_it1->pt_ - corner_it2->pt_ ).head(2).squaredNorm() < max_distance_sq )
//// {
//// // Keep the one with bigger product of number of points of each line
//// (*corner_it1) = (corner_it1->line_1_.np_*corner_it1->line_2_.np_ > corner_it2->line_1_.np_*corner_it2->line_2_.np_? *corner_it1 : *corner_it2);
//// corner_it2 = _corner_list.erase(corner_it2);
//// }
//// else
//// corner_it2 ++;
//// }
//// corner_it1 ++;
//// }
// //std::cout << "Corners after removing duplicates: " << _corner_list.size() << std::endl;
// for(std::list<laserscanutils::Corner>::iterator corner_it1 = _corner_list.begin(); corner_it1 != _corner_list.end(); corner_it1++)
// corner_it1->print();
// // for(std::list<laserscanutils::Corner>::iterator corner_it1 = _corner_list.begin(); corner_it1 != _corner_list.end(); corner_it1++)
// // corner_it1->print();
return _corner_list.size();
}
// return _corner_list.size();
//}
......
#include "object_detector.h"
#include <fstream> // std::ofstream
//LaserScanProcessed::LaserScanProcessed(unsigned int _num_points)
//{
// LaserScanProcessed::scan_points_matrix_.resize(3,_num_points);
//}
void laserscanutils::LaserObjectDetAlgParams::print() const
{
std::cout << "---------------------------------------------------" << std::endl
<< "-- Laser Object Detection Algorithm Parameters --" << std::endl
<< "---------------------------------------------------" << std::endl
<< " * Maximum Distance Between Points: " << this->cluster_jump_thr_ << std::endl
<< " * Minimum Number of Points for a Cluster: " << this->cluster_min_num_points_ << std::endl
<< " * Maximum Number of Points for a Cluster: " << this->cluster_max_num_points_ << std::endl
<< " * Segmnet Window Size: " << this->fit_line_window_size_ << std::endl
<< " * K_sigmas to tolerate std_dev in line: " << this->fit_line_k_sigmas_ << std::endl
<< " * theta max to concatenate segments: " << this->fit_line_theta_max_parallel_ << std::endl;
}
//****************************************************
//********** preFilterScan
//****************************************************
void laserscanutils::preFilterScan2homog(const laserscanutils::ScanParams & _params,
const std::vector<float> & _ranges,
Eigen::MatrixXs & _points,
LaserScanProcessed &_scan_processed)
{
//std::cout << "[PreFiltering SCAN... ";
unsigned int matPosition = 0;
float act_range = -1;
float act_theta = 0.0;
for(unsigned int i=0; i<_ranges.size(); ++i)
{
act_range = _ranges[i];
if ( (act_range >=_params.range_min_) && (act_range <= _params.range_max_) && !(isnan(act_range)) && !(isinf(act_range)) )
{
act_theta = _params.angle_min_+ _params.angle_step_*(i);
_points.col(matPosition) << act_range*cos(act_theta), act_range*sin(act_theta), 1;
matPosition++;
}
else
{
//std::cout << "Invalid Point: " << scan.ranges[i] << " - index: " << i << std::endl;
}
} // END FOR scan ranges
// Filling stats
_scan_processed.scanTotalPoints_ = _ranges.size();
_scan_processed.filteredTotalPoints_ = matPosition;
// Conservative resize of the eigen Matrix
_points.conservativeResize(3, matPosition);
// std::cout << std::endl << "[PRE-FILTERING]: TotalPoints: " << _scan_processed.scanTotalPoints_ << std::endl;
// std::cout << "[PRE-FILTERING]: filteredTotalPoints_: " << _scan_processed.filteredTotalPoints_ << std::endl;
// std::cout << "[PRE-FILTERING]: matrixResize: " << _points.cols() << std::endl;
// std::cout << "..DONE] " << std::endl;
}
//****************************************************
//********** extractClustersInScan
//****************************************************
void laserscanutils::extractClustersInScan(const laserscanutils::LaserObjectDetAlgParams & _alg_params,
const Eigen::MatrixXs & _points,
unsigned int _global_scan_number,
laserscanutils::LaserScanProcessed & _scan_processed
)
{
std::cout << "Extracting Clusters... " ;
_scan_processed.cluster_indxs_.clear();
_scan_processed.object_list_.clear(); // Assure the object list for the scan is empty.
std::pair<int,int> seg; // saves the first and last point for each segment
laserscanutils::Object temp_object; // objects detected initialization.
float points_distance = 0.0;
int count_num_cluster = 0;
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.filteredTotalPoints_; col++)
{
points_distance = ( pow( ( _points(0,col) -_points(0,col-1) ), 2.0 ) + pow( ( _points(1,col) - _points(1,col-1) ), 2.0) );
if ( points_distance <= sq_threshold) // same segment points
{
// Same segment -> TODO counter?
}
else // if ( col != _points.cols() ) // new segment
{
//std::cout << "P " << pos << " - A - (" << pointA.x << "-" << pointA.y << ") - B - (" << pointB.x << "-" << pointB.y << ") -> distance: " << points_distance << " More Than th" << std:endl;
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_) )
{
_scan_processed.cluster_indxs_.push_back(seg);
// fulfill and add object
temp_object.first_ = seg.first;
temp_object.last_ = seg.second;
temp_object.num_points_ = seg.second - seg.first +1;
_scan_processed.object_list_.push_back(temp_object);
_scan_processed.num_points_in_clusters_ += temp_object.num_points_;
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
seg.first = col;
}
}
// last point close last segment
// seg.second = _points.cols()-1;
seg.second = _scan_processed.filteredTotalPoints_-1;
if (seg.second - seg.first >= _alg_params.cluster_min_num_points_ && seg.second - seg.first <= _alg_params.cluster_max_num_points_)
{
_scan_processed.cluster_indxs_.push_back(seg);
// fulfill and add object
temp_object.first_ = seg.first;
temp_object.last_ = seg.second;
temp_object.num_points_ = seg.second - seg.first +1;
_scan_processed.object_list_.push_back(temp_object);
_scan_processed.num_points_in_clusters_ += temp_object.num_points_;
count_num_valid_cluster++; // count cluster as valid
//std::cout << "CLUSTER IS VALID" << std::endl;
}
count_num_cluster++; // count any cluster
// Stats
_scan_processed.number_of_clusters_ = count_num_cluster;
_scan_processed.number_of_valid_clusters_ = count_num_valid_cluster;
_scan_processed.number_of_global_scan_ = _global_scan_number;
// *** [DEBUG] ****
// 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;
}
//****************************************************
//********** findLinesInCluster
//****************************************************
void laserscanutils::findLinesInCluster(const laserscanutils::ScanParams & _scan_params,
const laserscanutils::LaserObjectDetAlgParams & _alg_params,
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
// cleaning line_list
_line_list.clear();
//init random generator
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;
}
unsigned int i_from = ii - _alg_params.fit_line_window_size_ + 1;
//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;
line.first_ = i_from;
line.last_ = ii;
line.point_first_ = _cluster.col(line.first_);
line.point_last_ = _cluster.col(line.last_);
line.np_ = line.last_ - line.first_ + 1;
_line_list.push_back(line);
}
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);
}
//std::cout << "Lines fitted: " << _line_list.size();
// concatenating lines
line_it1 = _line_list.begin();
while ( line_it1 != _line_list.end() )
{
line_it2 = line_it1;
line_it2 ++;
while (line_it2 != _line_list.end())
{
//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_)
{
//std::cout << " => lines concatenated";
//update line1 as the concatenation of l1 and l2
new_line.first_ = line_it1->first_;
new_line.last_ = line_it2->last_;
new_line.point_first_ = line_it1->point_first_;
new_line.point_last_ = line_it2->point_last_;
new_line.np_ = new_line.last_ - new_line.first_ + 1;
*line_it1 = new_line;
//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;
//<< 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
// Eliminating intermediate lines
std::list<Line>::iterator temp_line_it = line_it1;
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
// // 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++)
// {
// //std::cout << " INSIDE FIND LINES #." << std::distance(_line_list.begin(),list_it1) << std::endl;
// list_it1->first_ += initIndex;
// list_it1->last_ += initIndex;
// //list_it1->print();
// //std::cout << std::endl << std::endl;
// }
// std::cout << " --> Lines After Concatenation: " << _line_list.size() << std::endl << std::endl;
}
unsigned int laserscanutils::findCornersInClusterLines(const laserscanutils::LaserObjectDetAlgParams & _obj_det_alg_params,
const std::list<laserscanutils::Line> & _line_list,
std::list<laserscanutils::Corner> & _corner_list)
{
//local variables
ScalarT theta;
Corner corner;
std::list<Line>::const_iterator line_it1, line_it2;
_corner_list.clear();
line_it1 = _line_list.begin();
if (_line_list.size() <2)
{
//std::cout << "CORNERS DEBUG: --> NOT ENOUGH LINES IN CLUSTER TO EXTRACT CORNERS" << std::endl;
return 0;
}
while ( line_it1 != _line_list.end()--)
{
line_it2 = line_it1;
line_it2 ++;
// std::cout << std::endl << "line 1: last idx: " << (int)line_it1->last_ << std::endl;
// std::cout << "line 2: first idx: " << (int)line_it2->first_ << std::endl;
// std::cout << "substraction " << static_cast<int>(line_it2->first_) - (int)line_it1->last_ << std::endl;
// std::cout << "line 1: point first: " << line_it1->point_first_.transpose() << std::endl;
// std::cout << "line 1: point last: " << line_it1->point_last_.transpose() << std::endl;
// std::cout << "line 2: point first: " << line_it2->point_first_.transpose() << std::endl;
// std::cout << "line 2: point last: " << line_it2->point_last_.transpose() << std::endl;
//compute angle between lines 1 and 2
theta = angleBetweenLines(*line_it1, *line_it2);
// std::cout << "Angle Between Lines #" << std::distance(_line_list.begin(),line_it1) << " - "
// << std::distance(_line_list.begin(),line_it2) << " = " << theta;
//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);
// // Check if the corner is close to both lines ends. TODO: Move this check before computing intersection
// if ( (line_it1->point_last_ - corner.pt_).head(2).squaredNorm() < max_distance_sq
// &&
// (line_it2->point_first_ - corner.pt_).head(2).squaredNorm() < max_distance_sq )
// {
// std::cout << " Corners is close enough: " << std::endl;
//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 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;
//add the corner to the list
_corner_list.push_back(corner);
// debug: print
corner.print();
// }
}
else
//std::cout << " ... No Corner! : " << std::endl;
line_it2 ++;
line_it1 ++;
}
//std::cout << "Corners extracted: " << _corner_list.size() << std::endl;
// // Erase duplicated corners
// corner_it1 = _corner_list.begin();
// while ( corner_it1 != _corner_list.end() )
// {
// corner_it2 = corner_it1;
// corner_it2 ++;
// while (corner_it2 != _corner_list.end())
// {
// // Check if two corners are close enough. TODO: Check othe attributes also (orientation and aperture) !
// if ( ( corner_it1->pt_ - corner_it2->pt_ ).head(2).squaredNorm() < max_distance_sq )
// {
// // Keep the one with bigger product of number of points of each line
// (*corner_it1) = (corner_it1->line_1_.np_*corner_it1->line_2_.np_ > corner_it2->line_1_.np_*corner_it2->line_2_.np_? *corner_it1 : *corner_it2);
// corner_it2 = _corner_list.erase(corner_it2);
// }
// else
// corner_it2 ++;
// }
// corner_it1 ++;
// }
//std::cout << "Corners after removing duplicates: " << _corner_list.size() << std::endl;
// for(std::list<laserscanutils::Corner>::iterator corner_it1 = _corner_list.begin(); corner_it1 != _corner_list.end(); corner_it1++)
// corner_it1->print();
// return _corner_list.size();
//if (_corner_list.empty())
//std::cout << " ****** CORNERS LIST IS EMPTY!!!!!" << std::endl << std::endl;
std::cout << " ****** CORNERS LIST SIZE: " << _corner_list.size() << std::endl;
}
#ifndef OBJECT_DETECTOR_H
#define OBJECT_DETECTOR_H
//std
#include <iostream>
#include <vector>
#include <list>
// Eigen
#include <eigen3/Eigen/Geometry>
//laserscanutils
#include "types_laser_scan_utils.h"
#include "entities.h"
#include "scan_basics.h"
#include "line_detector.h"
#include "corner_detector.h"
namespace laserscanutils
{
struct LaserScanProcessed // Stores all the needed information from a scan
{
// Constructor
LaserScanProcessed(unsigned int _num_points=10)
{
this->scan_points_matrix_.resize(3,_num_points);
this->odom_ori_ = {0.0,0.0,0.0,0.0};
}
// Atributes
//TBR?// std::vector<float> scan_raw_ranges_; // [TBC] Raw scan ranges information
Eigen::MatrixXs scan_points_matrix_; // Homogeneous Coordinates for the valid scan points.
std::vector< std::pair<int,int> > cluster_indxs_; // Indexs of the initial and final position (of the scan_points_matrix_) for the clusters in scan.
std::list<laserscanutils::Object> object_list_; // Contains the different objects detected (clusters with associate information) in a scan.
//std::list<laserscanutils::Line> line_list_; //[TBR] Contains all the lines found in the scan (GLOBAL). -> TBR: use object lines!
std::vector<float> odom_pos_; // [X,Y,Z] position of the global odometry "nav_msg: pose.pose.position"
std::vector<float> odom_ori_; // [X,Y,Z,W] orientation of the global odometry. "nav_msg: pose.pose.orientation"
unsigned int scanTotalPoints_; //Total number of points in the raw scan
unsigned int filteredTotalPoints_; //Total number of valid points after pre filter
unsigned int number_of_clusters_; //Total number of clusters found in the scan
unsigned int num_points_in_clusters_; // Total number of points in the clusters
unsigned int number_of_valid_clusters_; //Total number of valid clusters found in the scan
unsigned int number_of_global_scan_; //Total number scan till now
};
struct LaserObjectDetAlgParams
{
//members
float cluster_jump_thr_; // max distance between points
float cluster_min_num_points_; // minimum number of points for a cluster
float cluster_max_num_points_; // maximum number of points for a cluster
// Line extraction
unsigned int fit_line_window_size_; // number of points to fit lines in the first pass
ScalarT fit_line_theta_max_parallel_; // maximum theta between consecutive segments to fuse them in a single line.
ScalarT fit_line_k_sigmas_; // How many std_dev are tolerated to count that a point is supporting a line
// Corner extraction
ScalarT corner_theta_min_; // minimum theta between consecutive segments to detect corner. PI/6=0.52
// TF Scan association
unsigned int tf_scan_window_size_; // Number of scanners on the buffer (comparison of movement btw thos step)
//ExtractLineParams extract_line_params_; // parameters for extracting lines
void print() const;
};
/** \brief Filters the valid ranges on a scan and converts them to homogeneous coordinates.
*
* Valid ranges are filtered according to scan parameters.
* Converts ranges from a laserScan to homogeneous coordinates and save it to a matrix of same size than ranges size, returning index till valid values.
*
* \param _params is the laser_scan_utils structure of the laser parameters.
* \param _ranges is the raw scan measures. Vector of float ranges.
* \param _points is the returned points from the conversion of ranges to homogeneous coordinates
* \param _stats is the statistics structure where number of valid points is saved under _stats.filteredTotalPoints
*
**/
void preFilterScan2homog(const laserscanutils::ScanParams & _params,
const std::vector<float> & _ranges,
Eigen::MatrixXs & _points,
laserscanutils::LaserScanProcessed &_scan_processed);
/** \brief Find the clusters in the valid points from the scan.
*
* Check distance between valid points in homogeneous coordinates. If it is more than the threshold, considers it as a different cluster.
*
* \param _alg_params is the structure of algorithm params for the Laser Object Detection Algorithm.
* \param _points is the returned points from the conversion of ranges to homogeneous coordinates
* \param _global_scan_number is the global number of scan processed (to be used as header id)
* \param _scan_processed is the structure where all the information of elements in a scan is stored
*
**/
void extractClustersInScan(const laserscanutils::LaserObjectDetAlgParams & _alg_params,
const Eigen::MatrixXs & _points,
unsigned int _global_scan_number,
laserscanutils::LaserScanProcessed & _scan_processed);
/** \brief Extract lines in the given cluster.
*
* From a cluster of points (EigenMatrix) extract the existing lines according to the alg parameters
*
* \param _alg_params is the structure of algorithm params for the Laser Object Detection Algorithm.
* \param _points is the returned points from the conversion of ranges to homogeneous coordinates
* \param _global_scan_number is the global number of scan processed (to be used as header id)
* \param _scan_processed is the structure where all the information of elements in a scan is stored
*
**/
void findLinesInCluster(const laserscanutils::ScanParams & _scan_params,
const laserscanutils::LaserObjectDetAlgParams &_alg_params,
const Eigen::MatrixXs &_cluster,
std::list<Line> &_line_list);
/** \brief Extract corners in the lines of a given cluster.
*
* From a list of lines belonging to the same cluster, extracts the existing corners according to the alg parameters
*
* \param _alg_params is the structure of algorithm params for the Laser Object Detection Algorithm.
* \param _line_list is the list of lines from which the corners will be extracted.
* \param _corner_list is the list of found corners in the cluster.
*
**/
unsigned int findCornersInClusterLines(const laserscanutils::LaserObjectDetAlgParams & _obj_det_alg_params,
const std::list<laserscanutils::Line> & _line_list,
std::list<laserscanutils::Corner> & _corner_list);
}
#endif // OBJECT_DETECTOR_H
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment