diff --git a/CMakeLists.txt b/CMakeLists.txt
index de2f6ef5b3dd9cdb90cdd0bad765687258995717..feda53e6d5abcbbf77308df98e9dfd423563e2db 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -16,7 +16,8 @@ SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
SET(CMAKE_INSTALL_PREFIX /usr/local)
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)
message(STATUS "Configured to compile in ${CMAKE_BUILD_TYPE} mode.")
diff --git a/Findlaser_scan_utils.cmake b/Findlaser_scan_utils.cmake
index 6e76479dcc4ffde8df823fbe64f1b531edcce657..1c7aa15396ad0767ea47238c60f2eeb70ced9b51 100644
--- a/Findlaser_scan_utils.cmake
+++ b/Findlaser_scan_utils.cmake
@@ -1,7 +1,7 @@
#edit the following line to add the librarie's header files
FIND_PATH(
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
)
diff --git a/src/clustering.cpp b/src/clustering.cpp
index 88b0e616b5031c33af8aab94b8a3d01266b4467f..2b75d519975f0c10c56f224e3a5b4bb1bdb619a6 100644
--- a/src/clustering.cpp
+++ b/src/clustering.cpp
@@ -1,11 +1,266 @@
#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.
-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();
@@ -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::
-//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();
-//}
diff --git a/src/clustering.h b/src/clustering.h
index 8eb060610ba3361ec179e59a6fa792add798189c..82a0b0732ea53b849eae5a4d43e870dc0c210441 100644
--- a/src/clustering.h
+++ b/src/clustering.h
@@ -7,6 +7,17 @@
#include "scan_basics.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"
@@ -15,12 +26,109 @@
namespace laserscanutils
{
- /** \brief TODO
- *
- * 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 scanProcess(const ScanParams & _params, const std::vector<float> & _ranges, const float _jump_threshold, Eigen::MatrixXs & _points, std::vector<unsigned int> & _jumps_id_);
+ /** \brief Set of tunning parameters
+ *
+ * Set of tunning parameters for corner extraction
+ *
+ */
+ 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...
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index a121632f9e66ba51897e1d8e442c95d236bf50df..330d3de351619f5d85df78dfe9a61ac97895ac29 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -7,3 +7,7 @@ ENDIF(faramotics_FOUND)
# corner aperture test
ADD_EXECUTABLE(test_corner_aperture test_corner_aperture.cpp)
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})
diff --git a/src/examples/testOneScan.txt b/src/examples/testOneScan.txt
new file mode 100644
index 0000000000000000000000000000000000000000..859c61e67dbc2064d77ae68b39e2372aa80ceeb6
--- /dev/null
+++ b/src/examples/testOneScan.txt
@@ -0,0 +1,3 @@
+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,
diff --git a/src/examples/test_laser_detector.cpp b/src/examples/test_laser_detector.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..45e05f89ad948c7cfe9e0041a812166adcf4bb8a
--- /dev/null
+++ b/src/examples/test_laser_detector.cpp
@@ -0,0 +1,166 @@
+#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);
+
+
+
+}
diff --git a/src/scan_basics.cpp b/src/scan_basics.cpp
index 94146f8d19e0e5219dcb98a2587c897fdd1cb943..962a18f029ed32ca41cd3dd079cf2f30cacf8075 100644
--- a/src/scan_basics.cpp
+++ b/src/scan_basics.cpp
@@ -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);
-// std::cout << "EIGEN Matrix return: " << matrixReturn.col(3).transpose() << std::endl;
return matrixReturn; // Remap the vector to the Matrix
-
}
@@ -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)
{
- 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;
unsigned int matPosition = 0;
@@ -109,8 +107,6 @@ unsigned int laserscanutils::ranges2xyh(const ScanParams & _params, std::vector<
}//END for
return matPosition-1; // Remap the vector to the Matrix - Accounting for 0
-
-
}