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

[clustering] - extraction of lines

parent b7537f50
No related branches found
No related tags found
No related merge requests found
......@@ -247,6 +247,50 @@ void laserscanutils::findClusters(const LaserDetectionParams & _alg_params,
void laserscanutils::findLinesInClusters(const laserscanutils::ScanParams & _params,
const ExtractCornerParams & _alg_params,
Eigen::MatrixXs & _cluster)
{
Line line;
std::list<Line> line_list;
// std::list<Line>::iterator line_it1, line_it2;
//init random generator
std::default_random_engine generator(1);
std::uniform_int_distribution<int> rand_window_overlapping(1,_alg_params.segment_window_size);
for (unsigned int ii = _alg_params.segment_window_size-1; ii<_cluster.cols(); ii=ii+rand_window_overlapping(generator) )
{
unsigned int i_from = ii - _alg_params.segment_window_size + 1;
//Found the best fitting line over points within the window [ii - segment_window_size + 1, ii]
//fitLine(i_from, ii, points, line);
std::cout << "Fitting Line: from " << i_from << " to " << ii-i_from+1 << std::endl;
fitLine(_cluster.block(0,i_from, 3, ii-i_from+1), line);
//if error below stdev, add line to ScalarT cornerAperture(const Eigen::Vector3s & _p1, const Eigen::Vector3s & _c, const Eigen::Vector3s & _p2);result set
if ( line.error_ < _params.range_std_dev_*_alg_params.k_sigmas )
{
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);
}
}
std::cout << "Lines fitted: " << line_list.size() << std::endl;
for
std::cout << "Lines fitted: " << line_list.size() << std::endl;
}
......
......@@ -102,6 +102,18 @@ namespace laserscanutils
void findLinesInClusters(const laserscanutils::ScanParams & _params,
const ExtractCornerParams & _alg_params,
Eigen::MatrixXs & _cluster);
/** \brief Converts ranges from a laserScan to homogeneous coordinates. Check validity of the ranges and look cluster jumps.
*
* 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.
......
......@@ -361,12 +361,12 @@ unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _
// update_iterators = false;
// std::cout << "lines concatenated" << std::endl;
// std::cout << "line 1: " << std::endl << line_it1->first << std::endl <<
// line_it1->last << std::endl << line_it1->vector_.transpose() << std::endl << line_it1->error << std::endl;
// std::cout << "line 2: " << std::endl << line_it2->first << std::endl <<
// line_it2->last << std::endl << line_it2->vector_.transpose() << std::endl << line_it2->error << std::endl;
// std::cout << "line resultant: "<< std::endl << new_line.first << std::endl <<
// new_line.last << std::endl << new_line.vector_.transpose() << std::endl << new_line.error << std::endl;
// std::cout << "line 1: " << std::endl << line_it1->first_ << std::endl <<
// line_it1->last_ << std::endl << line_it1->vector_.transpose() << std::endl << line_it1->error_ << std::endl;
// std::cout << "line 2: " << std::endl << line_it2->first_ << std::endl <<
// line_it2->last_ << std::endl << line_it2->vector_.transpose() << std::endl << line_it2->error_ << std::endl;
// std::cout << "line resultant: "<< std::endl << new_line.first_ << std::endl <<
// new_line.last_ << std::endl << new_line.vector_.transpose() << std::endl << new_line.error_ << std::endl;
}
}
}
......
......@@ -17,18 +17,40 @@
#include "clustering.h"
/* //TEST For graphical output
void setup() { glClearColor(1.0f, 1.0f, 1.0f, 1.0f); }
void display()
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glColor3f(0.0f, 0.0f, 0.0f);
glRectf(-0.75f,0.75f, 0.75f, -0.75f);
glutSwapBuffers();
}
*/
void readOneScanRanges(const char * _file_name, std::vector<float> & ranges_read)
{
// "/home/vvaquero/iri-lab/matlab/laser_detection_mat/TestOneScanRanges.txt"
std::ifstream inputFile(_file_name); // input file stream with name
ranges_read.clear();
std::string lineTest;
std::getline( inputFile, lineTest );
// use a stringstream to separate the fields out of the line
std::stringstream ss( lineTest);
std::string field;
while (getline( ss, field, ',' ))
{
// for each field convert it to a double
std::stringstream fs( field );
double f = 0.0; // (default value is 0.0)
fs >> f;
ranges_read.push_back( f );
}
inputFile.close();
// printing
// std::cout << " Vector Ranges [ " ;
// for (unsigned int k = 0; k < 10; k++)
// {
// std::cout << ranges_read[k] << ", ";
// }
// std::cout << " ] " << std::endl;
}
void initParameters(laserscanutils::LaserDetectionParams & _parameters)
......@@ -79,9 +101,19 @@ int main()
scan_params.scan_time_ = 0.01;//not relevant
scan_params.range_min_ = 0.1;
scan_params.range_max_ = 200;
scan_params.range_std_dev_ = 6.91555e-310;
scan_params.range_std_dev_ = 0.01;
scan_params.print();
laserscanutils::ExtractCornerParams algExtractCornerParams;
algExtractCornerParams.segment_window_size = 10;
algExtractCornerParams.theta_min = 0.01;
algExtractCornerParams.theta_max_parallel = 0.1;
algExtractCornerParams.k_sigmas = 2;
algExtractCornerParams.max_beam_distance = 500;
algExtractCornerParams.max_distance = 0.9;
algExtractCornerParams.print();
// ***** READING LASER POINTS FOR TEST ***** //
......@@ -140,11 +172,12 @@ int main()
std::pair<int,int> tempSeg = alg_stats.cluster_indxs_[j];
std::cout << "(" << tempSeg.first << "," << tempSeg.second << ") | " ;
}
std::cout << std::endl;
// Prin Info to Matlab Script
laserscanutils::exportData2Matlab(result, alg_parameters, alg_stats,
"/home/vvaquero/iri-lab/matlab/laser_detection_mat/auto_laserDetection_ClustersScript.m");
// // Prin to Matlab Script
// laserscanutils::exportData2Matlab(result, alg_parameters, alg_stats,
// "/home/vvaquero/iri-lab/matlab/laser_detection_mat/auto_laserDetection_ClustersScript.m");
......@@ -153,13 +186,35 @@ int main()
// // TESTING LINE EXTRACTION
// ************** TESTING LINE EXTRACTION **************
//read Ranges file
std::vector<float> ranges_read;
std::vector<float> ranges_read2;
readOneScanRanges("/home/vvaquero/iri-lab/matlab/laser_detection_mat/TestOneScanRanges.txt", ranges_read);
ranges_read2.clear();
for (std::vector<float>::iterator h= ranges_read.begin()+1 ; h < ranges_read.end(); h=h+2 )
{
//std::cout << ' ' << *h;
ranges_read2.push_back(*h);
}
// extract corners
std::list<laserscanutils::Corner> corner_list;
//laserscanutils::extractCorners(scan_params, algExtractCornerParams, ranges_read2, corner_list);
std::pair<int,int> tempSeg = alg_stats.cluster_indxs_[0];
Eigen::MatrixXs tempCluster = result.block(0,tempSeg.first, 3, tempSeg.second);
laserscanutils::findLinesInClusters(scan_params,algExtractCornerParams,tempCluster);
// Printing lines
// // extract corners
// std::list<laserscanutils::Corner> corner_list;
// std::vector<double> corner_vector;
// laserscanutils::extractCorners(scan_params, alg_parameters, myScan, corner_list);
......
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