ScalarTmax_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,
// 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;
return0;
}
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;
structLaserScanProcessed// Stores all the needed information from a scan
{
// Constructor
LaserScanProcessed(unsignedint_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::MatrixXsscan_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"
unsignedintscanTotalPoints_;//Total number of points in the raw scan
unsignedintfilteredTotalPoints_;//Total number of valid points after pre filter
unsignedintnumber_of_clusters_;//Total number of clusters found in the scan
unsignedintnum_points_in_clusters_;// Total number of points in the clusters
unsignedintnumber_of_valid_clusters_;//Total number of valid clusters found in the scan
unsignedintnumber_of_global_scan_;//Total number scan till now
};
structLaserObjectDetAlgParams
{
//members
floatcluster_jump_thr_;// max distance between points
floatcluster_min_num_points_;// minimum number of points for a cluster
floatcluster_max_num_points_;// maximum number of points for a cluster
// Line extraction
unsignedintfit_line_window_size_;// number of points to fit lines in the first pass
ScalarTfit_line_theta_max_parallel_;// maximum theta between consecutive segments to fuse them in a single line.
ScalarTfit_line_k_sigmas_;// How many std_dev are tolerated to count that a point is supporting a line
// Corner extraction
ScalarTcorner_theta_min_;// minimum theta between consecutive segments to detect corner. PI/6=0.52
// TF Scan association
unsignedinttf_scan_window_size_;// Number of scanners on the buffer (comparison of movement btw thos step)
//ExtractLineParams extract_line_params_; // parameters for extracting lines
voidprint()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