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

Clustering tests

parent e0e14926
No related branches found
No related tags found
No related merge requests found
...@@ -16,7 +16,8 @@ SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib) ...@@ -16,7 +16,8 @@ SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
SET(CMAKE_INSTALL_PREFIX /usr/local) SET(CMAKE_INSTALL_PREFIX /usr/local)
IF (NOT CMAKE_BUILD_TYPE) IF (NOT CMAKE_BUILD_TYPE)
SET(CMAKE_BUILD_TYPE "RELEASE") #SET(CMAKE_BUILD_TYPE "DEBUG")
SET(CMAKE_BUILD_TYPE "RELEASE")
ENDIF (NOT CMAKE_BUILD_TYPE) ENDIF (NOT CMAKE_BUILD_TYPE)
message(STATUS "Configured to compile in ${CMAKE_BUILD_TYPE} mode.") message(STATUS "Configured to compile in ${CMAKE_BUILD_TYPE} mode.")
......
#edit the following line to add the librarie's header files #edit the following line to add the librarie's header files
FIND_PATH( FIND_PATH(
laser_scan_utils_INCLUDE_DIRS laser_scan_utils_INCLUDE_DIRS
NAMES corner_detector.h entities.h line_fitting.h scan_basics.h types_laser_scan_utils.h NAMES corner_detector.h entities.h line_fitting.h scan_basics.h types_laser_scan_utils.h clustering.h
PATHS /usr/local/include/iri-algorithms/laser_scan_utils PATHS /usr/local/include/iri-algorithms/laser_scan_utils
) )
......
#include "clustering.h" #include "clustering.h"
#include <fstream> // std::ofstream
void laserscanutils::LaserDetectionParams::print() const
{
std::cout << "--------------------------------------------" << std::endl
<< "-- Laser Detection Algorithm Parameters --" << std::endl
<< "--------------------------------------------" << std::endl
<< " * Maximum Distance Between Points: " << jump_thr << std::endl
<< " * Minimum Number of Points for a Cluster: " << min_num_cluster_points << std::endl
<< " * Maximum Number of Points for a Cluster: " << max_num_cluster_points << std::endl;
}
Eigen::MatrixXs laserscanutils::loadScanMatrix()
{
std::vector<std::vector<float> > values;
std::ifstream fin("/home/vvaquero/testOneScan2.txt");
for (std::string line; std::getline(fin, line); )
{
std::replace(line.begin(), line.end(), ',', ' ');
std::istringstream in(line);
values.push_back(
std::vector<float>(std::istream_iterator<float>(in),
std::istream_iterator<float>()));
}
unsigned int rows = values.size();
unsigned int columns = 0;
for (unsigned int k = 0; k < rows; k++)
{
if (values[k].size() > columns)
columns = values[k].size();
}
std::cout << "Matrix Size = " << rows << "x" << columns <<std::endl;
Eigen::MatrixXs result(rows,columns);
for (unsigned int i = 0; i < rows; i++)
for (unsigned int j = 0; j < columns; j++)
{
result(i,j) = values[i][j];
//std::cout << "(" << result(i,j) << ") " ;
}
return result;
}
void laserscanutils::exportData2Matlab(const Eigen::MatrixXs & _points,
const LaserDetectionParams & _alg_params,
const LaserDetectionStats & _stats,
const char * _file_name)
{
std::ofstream fileMatlab;
fileMatlab.open (_file_name, std::ofstream::out); // | std::ofstream::app);
// FILE HEADER
fileMatlab << "% ***** Autogenerated File - vvg ***** \n\n";
fileMatlab << " clc,\n clear all,\n close all \n\n\n";
// Algorithm Params
fileMatlab << "% Algorithm Params \n";
fileMatlab << "alg_params = struct("
" 'jump_thr', " << _alg_params.jump_thr << ","
"'min_num_cluster_points', " << _alg_params.min_num_cluster_points << ","
"'max_num_cluster_points', " << _alg_params.max_num_cluster_points << "); \n\n";
// Algorithm Status Variables
fileMatlab << "% Algorithm Status Variables \n";
fileMatlab << "alg_stats = struct("
"'filteredTotalPoints', " << _stats.filteredTotalPoints << ","
"'numberOfClusters', " << _stats.numberOfClusters << ","
"'numberOfValidClusters', " << _stats.numberOfValidClusters << ","
"'scanTotalPoints', " << _stats.scanTotalPoints << ","
"'cluster_indxs_', [" ;
for (unsigned int cluster = 0; cluster < _stats.cluster_indxs_.size(); cluster++)
{
std::pair<int,int> tempCluster = _stats.cluster_indxs_[cluster];
fileMatlab << tempCluster.first << "; " << tempCluster.second << "; ";
}
fileMatlab << "]);\n";
fileMatlab << "alg_stats.cluster_indxs_ = reshape(alg_stats.cluster_indxs_,[2,length(alg_stats.cluster_indxs_)/2]); \n\n";
// Scanner Points in Homogeneous coordinates
fileMatlab << "% Writing Scanner Points in Homogeneous coordinates \n";
fileMatlab << "scanMatrixHomo = [";
for (unsigned int row = 0; row < 3; row++)
{
for (unsigned int col = 0; col < _stats.filteredTotalPoints; col++)
{
fileMatlab << _points(row,col) << ",";
}
fileMatlab << "\n";
}
fileMatlab << "]; \n\n";
// Rotating Points Matrix
fileMatlab << "% Rotating Points Matrix \n";
fileMatlab << "scanMatrix = scanMatrixHomo(1:2, :); \n" ;
fileMatlab << "alfa = pi/2; \n";
fileMatlab << "rotateM = [cos(alfa), -sin(alfa); sin(alfa), -cos(alfa)]; \n";
fileMatlab << "scanMatrixRot = rotateM*scanMatrix; \n\n";
// Drawing Rotated Scan
fileMatlab << "% Drawing Rotated Scan \n";
fileMatlab << "scatter(scanMatrixRot(1,:), scanMatrixRot(2,:),30,'filled'); axis equal; grid on; \n\n";
fileMatlab.close();
}
//****************************************************
//********** preFilterScan
//****************************************************
void laserscanutils::preFilterScan(const ScanParams & _params,
const std::vector<float> & _ranges,
Eigen::MatrixXs & _points,
LaserDetectionStats &_stats)
{
//std::cout << "[PreFiltering SCAN... ";
//assert( (_ranges.size() == _points.cols() ) &&(_points.rows() == (unsigned int)3) &&("Error: [laserPolar2xy] points Matrix size is different than number of ranges. Wrong Matrix Size."));
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
_stats.scanTotalPoints = _ranges.size();
_stats.filteredTotalPoints = matPosition;
//std::cout << "..DONE] " << std::endl;
}
//****************************************************
//********** findClusters
//****************************************************
void laserscanutils::findClusters(const LaserDetectionParams & _alg_params,
const Eigen::MatrixXs & _points,
LaserDetectionStats & _stats)
{
_stats.cluster_indxs_.clear();
std::pair<int,int> seg; // saves the first and last point for each segment
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.jump_thr,2.0);
for (unsigned int col = 1; col < _points.cols(); 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.min_num_cluster_points ) && ( (seg.second - seg.first)+1 <= _alg_params.max_num_cluster_points) )
{
_stats.cluster_indxs_.push_back(seg);
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;
if (seg.second - seg.first >= _alg_params.min_num_cluster_points && seg.second - seg.first <= _alg_params.max_num_cluster_points)
{
_stats.cluster_indxs_.push_back(seg);
count_num_valid_cluster++; // count cluster as valid
std::cout << "CLUSTER IS VALID" << std::endl;
}
count_num_cluster++; // count any cluster
// Stats
_stats.numberOfClusters = count_num_cluster;
_stats.numberOfValidClusters = count_num_valid_cluster;
std::cout << "numberOfValidClusters = " << count_num_valid_cluster << std::endl;
}
// NOT USED!!!!
// VV - 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. // VV - 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.
unsigned int laserscanutils::scanProcess(const ScanParams & _params, const std::vector<float> & _ranges, const float _jump_threshold, Eigen::MatrixXs & _points, std::vector<unsigned int> & _jumps_id_) unsigned int laserscanutils::scanProcess(const ScanParams & _params,
const std::vector<float> & _ranges,
const float _jump_threshold,
Eigen::MatrixXs & _points,
std::vector<unsigned int> & _jumps_id_)
{ {
assert( (_ranges.size() == _points.cols() ) &&(_points.rows() == 3) &&("Error: [laserPolar2xy] points Matrix size is different than number of ranges. Wrong Matrix Size.")); //assert( (_ranges.size() == _points.cols() ) &&(_points.rows() == 3) &&("Error: [laserPolar2xy] points Matrix size is different than number of ranges. Wrong Matrix Size."));
_jumps_id_.clear(); _jumps_id_.clear();
...@@ -59,6 +314,45 @@ unsigned int laserscanutils::scanProcess(const ScanParams & _params, const std:: ...@@ -59,6 +314,45 @@ unsigned int laserscanutils::scanProcess(const ScanParams & _params, const std::
// VV - test to check results in txt
void laserscanutils::printMatrix2File(const unsigned int _num_points,
Eigen::MatrixXs & _points,
std::vector<unsigned int> & _jumps_id_)
{
std::ofstream filePoints;
filePoints.open ("/home/vvaquero/TestInfoScanPoints.txt", std::ofstream::out | std::ofstream::app);
// TO PRINT IN ONE ROW
// for (int numpoint = 0; numpoint < _num_points; numpoint++)
// {
// filePoints << _points.col(numpoint) << "\n";
// }
// filePoints.close();
for (unsigned int row = 0; row < 3; row++)
{
for (unsigned int col = 0; col < _num_points; col++)
{
filePoints << _points(row,col) << ",";
}
filePoints << "\n";
}
filePoints.close();
std::ofstream fileSegments;
fileSegments.open ("/home/vvaquero/TestInfoScanSegments.txt", std::ofstream::out | std::ofstream::app);
for (unsigned int segnum = 1; segnum<_jumps_id_.size(); segnum++)
{
if (segnum == 1)
fileSegments << "Segment Number: 0 - from 0 " << " to " << _jumps_id_[segnum] << "\n";
else
fileSegments << "Segment Number: "<< segnum << " - from " << _jumps_id_[segnum-1] << " to " << _jumps_id_[segnum] << "\n";
}
fileSegments.close();
//std::cout << "PRINTING OUT " << std::endl;
}
...@@ -70,132 +364,6 @@ unsigned int laserscanutils::scanProcess(const ScanParams & _params, const std:: ...@@ -70,132 +364,6 @@ unsigned int laserscanutils::scanProcess(const ScanParams & _params, const std::
//unsigned int laserscanutils::extractLineInSegs(const Segment & _mySegment , std::list<laserscanutils::Line> & _line_list)
//{
// //local variables
// ScalarT cos_theta, theta;
// Line line;
// std::list<Line>::iterator line_it1, line_it2;
// std::queue<unsigned int> jumps;
// bool update_iterators;
// ScalarT max_distance_sq(_alg_params.max_distance*_alg_params.max_distance); //init max distance as the squared value to avoid sqroot on comparisons
// //init random generator
// std::default_random_engine generator(1);
// std::uniform_int_distribution<int> rand_window_overlapping(1,_alg_params.segment_window_size);
// //STEP 0: convert range polar data to cartesian points, and keep range jumps
//// for (unsigned int ii = 0; ii<_ranges.size(); ii++)
//// {
//// //range 2 polar
//// azimuth = _params.angle_max_ - _params.angle_step_ * ii;
//// points.col(ii) << _ranges(ii)*cos(azimuth), _ranges(ii)*sin(azimuth), 1; //points.row0-> x coordinate, points.row1->y coordinate
////
//// //check range jump and keep it
//// if (ii > 0 && fabs(_ranges(ii)-_ranges(ii-1)) > _alg_params.max_distance) jumps.push(ii);
//// }
// Eigen::MatrixXs points = laserPolar2xy(_params, _ranges, _alg_params.max_distance, jumps);
// std::cout << "jumps: " << jumps.size() << std::endl;
// std::cout << "points: " << points.rows() << "x" << points.cols()<< std::endl;
// // STEP 1: find line segments running over the scan
// for (unsigned int ii = _alg_params.segment_window_size-1; ii<points.cols(); ii=ii+rand_window_overlapping(generator) )
// {
// unsigned int i_from = ii - _alg_params.segment_window_size + 1;
// // update the jump to be checked
// while (!jumps.empty() && i_from > jumps.front())
// jumps.pop();
// // check if there is a jump inside the window (not fitting if it is the case)
// if (jumps.front() > i_from && jumps.front() <= ii)
// continue;
// //Found the best fitting line over points within the window [ii - segment_window_size + 1, ii]
// //fitLine(i_from, ii, points, line);
// fitLine(points.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_ = points.col(line.first_);
// line.point_last_ = points.col(line.last_);
// line.np_ = line.last_ - line.first_ + 1;
// _line_list.push_back(line);
// }
// }
// std::cout << "Lines fitted: " << _line_list.size() << std::endl;
// //In case just less than two lines found, return with 0 corners
// if ( _line_list.size() < 2 ) return 0;
// //STEP 2: concatenate lines
// line_it1 = _line_list.begin();
// line_it2 = line_it1;
// line_it2 ++;
// while ( line_it1 != _line_list.end() )
// {
// //if no concatenation found, iterators will run forward
// update_iterators = true;
// // check if last of line1 and first of line2 are within the threshold
// if ( ( line_it2->first_ - line_it1->last_ ) <= _alg_params.max_beam_distance )
// {
// //compute angle between lines 1 and 2
// cos_theta = (line_it1->vector_).dot(line_it2->vector_) / ( (line_it1->vector_).norm()*(line_it2->vector_).norm() );
// theta = acos (cos_theta); //theta is in [0,PI]
// if (theta > M_PI/2) theta = M_PI - theta;//assures theta>0 AND either theta close to 0 or close to PI will always map to theta close to 0.
//// std::cout << std::endl << "cos_theta: " << cos_theta << std::endl << "theta: " << theta << std::endl <<
//// "*index_it1: " << *index_it1 << std::endl << "*index_it2: " << *index_it2 << std::endl;
//// " (*line_it1).dot(*line_it2): " << (*line_it1).dot(*line_it2) << std::endl <<
//// " (*line_it1).norm()*(*line_it2).norm(): " << (*line_it1).norm()*(*line_it2).norm() << std::endl;
// //Check angle threshold
// if ( theta < _alg_params.theta_max_parallel ) //fabs(theta) not required since theta>0
// {
// //compute a new line with all involved points
// Line new_line;
// fitLine(points.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_ < _params.range_std_dev_*_alg_params.k_sigmas )
// {
// //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_ = points.col(new_line.first_);
// new_line.point_last_ = points.col(new_line.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);
// //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;
//// 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;
// }
// }
// }
// //update iterators
// if (update_iterators)
// {
// line_it1 ++;
// line_it2 = line_it1;
// line_it2 ++;
// }
// }
// std::cout << "Lines after concatenation: " << _line_list.size() << std::endl;
// return _line_list.size();
//}
...@@ -7,6 +7,17 @@ ...@@ -7,6 +7,17 @@
#include "scan_basics.h" #include "scan_basics.h"
#include "corner_detector.h" #include "corner_detector.h"
#include <fstream>
#include <iostream>
#include <iterator>
#include <sstream>
#include <string>
#include <vector>
#include <eigen3/Eigen/Dense>
//#include <fstream> // File Printing for tests
//#include "segment.h" //#include "segment.h"
...@@ -15,12 +26,109 @@ ...@@ -15,12 +26,109 @@
namespace laserscanutils namespace laserscanutils
{ {
/** \brief TODO /** \brief Set of tunning 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. * Set of tunning parameters for corner extraction
* *
**/ */
unsigned int scanProcess(const ScanParams & _params, const std::vector<float> & _ranges, const float _jump_threshold, Eigen::MatrixXs & _points, std::vector<unsigned int> & _jumps_id_); struct LaserDetectionParams
{
//members
float jump_thr; //max distance between points
float min_num_cluster_points; //minimum number of points for a cluster
float max_num_cluster_points; //maximum number of points for a cluster
void print() const;
};
struct LaserDetectionStats
{
// Mid variables for the algorithm, storing information needed for the correct working
unsigned int scanTotalPoints; //Total number of points in the raw scan
unsigned int filteredTotalPoints; //Total number of valid points after pre filter
unsigned int numberOfClusters; //Total number of clusters found in the scan
unsigned int numberOfValidClusters; //Total number of valid clusters found in the scan
std::vector< std::pair<int,int> > cluster_indxs_; // Contains initial and final columns (of the filtered homogeneous matrix) for clusters.
};
// TEST
Eigen::MatrixXs loadScanMatrix();
/** \brief Test Porpuses to create a Matlab script with all the variables.
**/
void exportData2Matlab(const Eigen::MatrixXs & _points,
const LaserDetectionParams & _alg_params,
const LaserDetectionStats & _stats,
const char * _file_name);
/** \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 preFilterScan(const ScanParams & _params,
const std::vector<float> & _ranges,
Eigen::MatrixXs & _points,
LaserDetectionStats & _stats);
/** \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 Detection Algorithm.
* \param _points is the returned points from the conversion of ranges to homogeneous coordinates
* \param _stats is the statistics structure (TODO: What is used here? - Number of Clusters)
**/
void findClusters(const LaserDetectionParams & _alg_params,
const Eigen::MatrixXs & _points,
LaserDetectionStats & _stats);
/** \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.
* Valid ranges are filtered and at the same time cluster jumps are checked.
* TODO (jumpThreshold)
*
* \param _params is the laser_scan_utils structure of the laser parameters.
* \param _ranges is the raw scan measures. Vector os ranges.
* \param _jump_threshold is the jump to diferenciate between clusters -> TODO in algorithm parameters
* \param _points is the returned points from the conversion of ranges to homogeneous coordinates
* \param _jumps_id_ is the vector containing in which column of the _points matrix exist a cluster jump.
**/// NOT USED!!
unsigned int scanProcess(const ScanParams & _params,
const std::vector<float> & _ranges,
const float _jump_threshold,
Eigen::MatrixXs & _points,
std::vector<unsigned int> & _jumps_id_);
void printMatrix2File(const unsigned int _num_points,
Eigen::MatrixXs & _points,
std::vector<unsigned int> & _jumps_id_);
/** \brief Analyse clusters and extract the geometry primitives - line, corner... /** \brief Analyse clusters and extract the geometry primitives - line, corner...
......
...@@ -7,3 +7,7 @@ ENDIF(faramotics_FOUND) ...@@ -7,3 +7,7 @@ ENDIF(faramotics_FOUND)
# corner aperture test # corner aperture test
ADD_EXECUTABLE(test_corner_aperture test_corner_aperture.cpp) ADD_EXECUTABLE(test_corner_aperture test_corner_aperture.cpp)
TARGET_LINK_LIBRARIES(test_corner_aperture ${PROJECT_NAME}) TARGET_LINK_LIBRARIES(test_corner_aperture ${PROJECT_NAME})
# Object Detector test
ADD_EXECUTABLE(test_laser_detector test_laser_detector.cpp)
TARGET_LINK_LIBRARIES(test_laser_detector ${PROJECT_NAME})
2.47857,2.40308,2.46936,2.49425,2.58269,2.6075,2.6976,2.67785,2.75212,2.81579,2.88573,2.93293,2.9802,3.02752,3.07489,3.12231,3.20072,3.26732,3.31537,3.36345,3.43738,3.51185,3.56706,3.58244,3.68457,3.78771,3.81647,3.88631,3.91461,4.01289,4.09067,4.14022,4.21861,4.29739,4.38391,4.42651,4.50619,4.61629,4.6969,4.71678,4.85908,4.94062,4.96786,5.07315,5.21855,5.26954,5.38442,5.49995,5.55117,5.67566,5.78439,5.86879,6.01984,6.10489,6.19005,6.34295,6.42867,6.58285,6.66897,6.85886,6.94558,7.13698,7.22426,7.41708,7.50468,7.69889,7.78679,8.018,8.17831,8.3029,8.53652,8.66171,8.89682,9.13282,9.36953,9.50533,9.77104,10.0097,10.2865,10.5641,10.8424,11.1214,11.4011,11.6813,12.0002,12.3675,12.8314,13.1143,13.6283,13.8832,14.409,15.6434,16.1826,16.6058,17.1172,17.6488,18.3473,19.0567,19.7473,20.6452,21.5048,22.4048,23.5033,24.6522,25.8219,27.3595,76.0194,75.946,30.8241,75.9103,75.8487,75.8609,75.8273,75.6581,76.332,76.1013,91.4294,91.4046,91.4127,91.3939,91.3481,91.4349,146.41,145.369,145.355,54.1523,49.9382,46.2363,44.6039,42.9663,42.9377,42.9911,42.8599,42.9149,42.89,42.8329,42.9128,42.8853,42.8537,42.8924,42.834,42.7902,42.8156,42.7908,42.8072,42.7463,42.6903,42.7922,42.7359,42.7112,42.7083,
-4.25007,-4.03921,-4.06938,-4.03067,-4.09332,-4.05381,-4.11453,-4.00768,-4.04203,-4.05894,-4.08321,-4.0741,-4.06448,-4.05435,-4.04369,-4.03252,-4.06009,-4.071,-4.05782,-4.04411,-4.0604,-4.0757,-4.06745,-4.01379,-4.0564,-4.09752,-4.05703,-4.05969,-4.01846,-4.04806,-4.05513,-4.03323,-4.03844,-4.04258,-4.05244,-4.02076,-4.02193,-4.04838,-4.04711,-3.99305,-4.04126,-4.03666,-3.98713,-3.99935,-4.04063,-4.00705,-4.02074,-4.03273,-3.99627,-4.01116,-4.01278,-3.99591,-4.02233,-4.00254,-3.98158,-4.00211,-3.97818,-3.99457,-3.96762,-3.99997,-3.96972,-3.9969,-3.96337,-3.98534,-3.94841,-3.96519,-3.92486,-3.95404,-3.94475,-3.9159,-3.93539,-3.90182,-3.91469,-3.92377,-3.92897,-3.88873,-3.89825,-3.89258,-3.89719,-3.89729,-3.89281,-3.88376,-3.87014,-3.85191,-3.84129,-3.84021,-3.86182,-3.82248,-3.84359,-3.78501,-3.79356,-3.97291,-3.95986,-3.91015,-3.87323,-3.83194,-3.81625,-3.79061,-3.74914,-3.73326,-3.69521,-3.64886,-3.6175,-3.5744,-3.51421,-3.48057,-8.99749,-8.31744,-3.10383,-6.97518,-6.30254,-5.63744,-4.96998,-4.29618,-3.66649,-2.99003,3.59227,4.39048,5.19079,5.99027,6.78833,7.59765,18.6256,22.3744,23.6725,13.251,13.1476,13.04,13.0008,12.9314,13.3325,13.7616,14.1331,14.5676,14.9778,15.3786,15.8314,16.2478,16.6649,17.1123,17.5239,17.9434,18.395,18.8284,19.2833,19.7063,20.134,20.6405,21.075,21.5282,21.9962,
1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,
#include <algorithm>
#include <fstream>
#include <iostream>
#include <iterator>
#include <sstream>
#include <string>
#include <vector>
#include <eigen3/Eigen/Dense>
//#include <graphics.h>
#include <GL/gl.h>
#include <GL/glut.h>
#include <GL/glu.h>
//laserscanutils
#include "entities.h"
#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 initParameters(laserscanutils::LaserDetectionParams & _parameters)
{
_parameters.jump_thr = 0.9;
_parameters.max_num_cluster_points = 200;
_parameters.min_num_cluster_points = 5;
//_parameters.print();
}
int main()
{
/* //TEST For graphical output
glutInit(&argc, argv);
glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
glutInitWindowSize(800,600);
glutCreateWindow("Hello World");
setup();
glutDisplayFunc(display);
glutMainLoop();
*/
// ***** INITIALIZATION ***** //
laserscanutils::LaserDetectionParams alg_parameters; // structure with the parameters to tune the algorithm
laserscanutils::LaserDetectionStats alg_stats; // structure with the internal variables of the algorithm
initParameters(alg_parameters);
alg_parameters.print();
// get laser Scan Parameters from real laser rosbag
// Laser 2D Scan Parameters :
// Angle min: -1.0472
// Angle max: 0.610865
// Angle step: 0.00436332
// Scan time: 6.95332e-310
// Range min: 0.1
// Range max: 200
// Range std dev: 6.91555e-310
laserscanutils::ScanParams scan_params;
scan_params.angle_min_ = -1.0472;
scan_params.angle_max_ = 0.610865;
scan_params.angle_step_ = 0.00436332;
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.print();
// ***** READING LASER POINTS FOR TEST ***** //
std::vector<std::vector<float> > values; // temp for reading the file values
std::ifstream fin("/home/vvaquero/iri-lab/matlab/laser_detection_mat/testOneScanMatrix.txt"); // input file stream with name
for (std::string line; std::getline(fin, line); )
{
std::replace(line.begin(), line.end(), ',', ' ');
std::istringstream in(line);
values.push_back(
std::vector<float>(std::istream_iterator<float>(in),
std::istream_iterator<float>()));
}
unsigned int rows = values.size();
unsigned int columns = 0;
for (unsigned int k = 0; k < rows; k++)
{
if (values[k].size() > columns)
columns = values[k].size();
}
std::cout << "Matrix Size = " << rows << "x" << columns <<std::endl;
Eigen::MatrixXs result(rows,columns);
for (unsigned int i = 0; i < rows; i++)
for (unsigned int j = 0; j < columns; j++)
{
result(i,j) = values[i][j];
}
alg_stats.scanTotalPoints = 380;
alg_stats.filteredTotalPoints = columns;
//std::cout << "Matrix = " << std::endl << result <<std::endl;
// |
// "
// Till here, everything is like a scan has been received and prefiltered
// Finding clusters on the scan
laserscanutils::findClusters(alg_parameters, result,alg_stats);
std::cout << "Clusters: " << std::endl <<
" TotalFound = " << alg_stats.numberOfClusters << std::endl <<
" Valid Ones = " << alg_stats.numberOfValidClusters << std::endl <<
" Indexes = " ;
for (unsigned int j = 0; j < alg_stats.cluster_indxs_.size(); j++)
{
std::pair<int,int> tempSeg = alg_stats.cluster_indxs_[j];
std::cout << "(" << tempSeg.first << "," << tempSeg.second << ") | " ;
}
// Prin Info to Matlab Script
laserscanutils::exportData2Matlab(result, alg_parameters, alg_stats,
"/home/vvaquero/iri-lab/matlab/laser_detection_mat/auto_laserDetection_ClustersScript.m");
// // TESTING LINE EXTRACTION
// // extract corners
// std::list<laserscanutils::Corner> corner_list;
// std::vector<double> corner_vector;
// laserscanutils::extractCorners(scan_params, alg_parameters, myScan, corner_list);
}
...@@ -44,9 +44,7 @@ Eigen::MatrixXs laserscanutils::laserPolar2xy(const ScanParams & _params, std::v ...@@ -44,9 +44,7 @@ Eigen::MatrixXs laserscanutils::laserPolar2xy(const ScanParams & _params, std::v
Eigen::Map<Eigen::MatrixXs> matrixReturn(xy_scan_points.data(),3, xy_scan_points.size()/3); Eigen::Map<Eigen::MatrixXs> matrixReturn(xy_scan_points.data(),3, xy_scan_points.size()/3);
// std::cout << "EIGEN Matrix return: " << matrixReturn.col(3).transpose() << std::endl;
return matrixReturn; // Remap the vector to the Matrix return matrixReturn; // Remap the vector to the Matrix
} }
...@@ -83,10 +81,10 @@ Eigen::MatrixXs laserscanutils::laserPolar2xy(const ScanParams & _params, const ...@@ -83,10 +81,10 @@ Eigen::MatrixXs laserscanutils::laserPolar2xy(const ScanParams & _params, const
// VV 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. // VV [To be deleted] 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.
unsigned int laserscanutils::ranges2xyh(const ScanParams & _params, std::vector<float> & _ranges, Eigen::MatrixXs & _points) unsigned int laserscanutils::ranges2xyh(const ScanParams & _params, std::vector<float> & _ranges, Eigen::MatrixXs & _points)
{ {
assert( (_ranges.size() == _points.cols() ) &&(_points.rows() == 3) &&("Error: [laserPolar2xy] points Matrix size is different than number of ranges. Wrong Matrix Size.")); //assert( (_ranges.size() == _points.cols() ) &&(_points.rows() == 3) &&("Error: [laserPolar2xy] points Matrix size is different than number of ranges. Wrong Matrix Size."));
double thetaPos = 0.0; double thetaPos = 0.0;
unsigned int matPosition = 0; unsigned int matPosition = 0;
...@@ -109,8 +107,6 @@ unsigned int laserscanutils::ranges2xyh(const ScanParams & _params, std::vector< ...@@ -109,8 +107,6 @@ unsigned int laserscanutils::ranges2xyh(const ScanParams & _params, std::vector<
}//END for }//END for
return matPosition-1; // Remap the vector to the Matrix - Accounting for 0 return matPosition-1; // Remap the vector to the Matrix - Accounting for 0
} }
......
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