Skip to content
Snippets Groups Projects
Commit b54ca10b authored by acoromin's avatar acoromin
Browse files

Non ready / non tested update. Come back to r13768 for a ready to use revision from wolf

parent e8db7bd5
No related branches found
No related tags found
No related merge requests found
......@@ -14,49 +14,58 @@ void laserscanutils::ExtractCornerParams::print() const
unsigned int laserscanutils::extractLines(const laserscanutils::ScanParams & _params, const ExtractCornerParams & _alg_params, const std::vector<float> & _ranges, 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
//local variables
Eigen::MatrixXs points
ScalarT cos_theta, theta;
Line line;
std::list<Line>::iterator line_it1, line_it2;
std::list<unsigned int> jumps;
bool update_iterators;
unsigned int ii_from;
ScalarT max_distance_sq(_alg_params.max_distance*_alg_params.max_distance); //init to squared value to avoid root() at comparisons
//init random generator
std::default_random_engine generator(1);
std::uniform_int_distribution<int> rand_window_overlapping(1,_alg_params.segment_window_size);
// std::default_random_engine generator(1);
// std::uniform_int_distribution<int> rand_window_overlapping(1,_alg_params.segment_window_size);
Eigen::MatrixXs points = laserPolar2xy(_params, _ranges, _alg_params.max_distance, jumps);
//laserPolar2xy(_params, _ranges, _alg_params.max_distance, points, jumps);
ranges2xy(_params, _ranges, _alg_params.max_distance, points, 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=ii+rand_window_overlapping(generator) )
for (unsigned int ii=0, ii_from=0; ii<points.cols(); ii++)//ii=ii+rand_window_overlapping(generator) )
{
//unsigned int i_from = ii - _alg_params.segment_window_size + 1;
//if current point (ii) still close to ii_from, jump to next ii
if ( (points.col(ii_from)-points.col(ii)).squaredNorm() < max_distance_sq ) continue; //jump to for loop again
//advance ii_from up to being closer than max_distance_sq
while ( (points.col(ii_from+1)-points.col(ii)).squaredNorm() > max_distance_sq ) ii_from ++;
// update the jump to be checked
while (!jumps.empty() && ii_from > jumps.front()) jumps.pop_front();
// STEP 1: find line segments running over the scan
for (unsigned int ii = _alg_params.segment_window_size-1; ii<points.cols(); ii++)//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;
// check if there is a jump inside the window (not fitting if it is the case)
if ( (jumps.front() > ii_from) && (jumps.front() <= ii) ) continue;
//Found the best fitting line over points within the window [ii - segment_window_size + 1, ii]
fitLine(points.block(0,i_from, 3, _alg_params.segment_window_size), line);
//Found the best fitting line over points within the window [ii - segment_window_size + 1, ii]
fitLine(points.block(0,ii_from, 3, _alg_params.segment_window_size), 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);
}
}
//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
......
......@@ -37,19 +37,6 @@ namespace laserscanutils
//just a print method
void print() const;
};
/** \brief Extract corners from a given scan. Result as a list of Vector4s
*
* Extract corners from a given scan.
* Returns corners as a list of Vector-4, where each corner is parametrized as:
* - corner(0): x coordinate
* - corner(1): y coordinate
* - corner(2): orientation
* - corner(3): aperture
*
*/
//unsigned int extractCorners(const laserscanutils::ScanParams & _params, const ExtractCornerParams & _alg_params, const Eigen::VectorXs & _ranges, std::list<Eigen::Vector4s> & _corner_list);
/** \brief Extract lines from a given scan. Result as a list of Line
*
......
......@@ -12,12 +12,41 @@ void laserscanutils::ScanParams::print() const
<< " Range std dev: " << range_std_dev_ << std::endl;
}
void laserscanutils::polar2xy(const ScanParams & _params, Eigen::VectorXs & _ranges, Eigen::MatrixXs _points )
unsigned int laserscanutils::ranges2xy(const ScanParams & _params, const std::vector<float> & _ranges, const ScalarT& _jump_theshold, Eigen::MatrixXs & _points, std::list<unsigned int> & _jumps)
{
ScalarT azimuth = 0.0;
ScalarT prev_range = -1;
unsigned int valid_rays = 0;
unsigned int ii, ii_ok;
//resize to all points case
_points.resize(3,_ranges.size());
//jumps_id_.clear(); //std::queue has no member clear(). why it is a queue ?
_jumps.clear();
//for each range, check correctness of value and translate from polar to xy coordinates
for(unsigned int ii=0, ii_ok=0; ii<_ranges.size(); ++ii)
{
if ( _ranges[ii] >=_params.range_min_ && _ranges[ii] <= _params.range_max_ && !isnan(_ranges[ii]) && !isinf(_ranges[ii]) )
{
azimuth = _params.angle_min_+ _params.angle_step_*ii;
_points(0,ii_ok) = _ranges[ii] * cos(azimuth);
_points(1,ii_ok) = _ranges[ii] * sin(azimuth);
_points(2,ii_ok) = 1;
if ( (prev_range !=-1) && (fabs(_ranges[ii]-prev_range) > _jump_theshold) )
_jumps.push_back(ii_ok);
prev_range = _ranges[ii];
ii_ok ++;
}
}
//resize the output matrix to the number of correct points, while keeping values
_points.conservativeResize(3, ii_ok);
//_points.conservativeResize(Eigen::NoChange_t, ii_ok); //does not compile ... why ?
}
// Just a test!
Eigen::MatrixXs laserscanutils::laserPolar2xy(const ScanParams & _params, std::vector<float> & _ranges)
{
......@@ -47,40 +76,34 @@ Eigen::MatrixXs laserscanutils::laserPolar2xy(const ScanParams & _params, std::v
return matrixReturn; // Remap the vector to the Matrix
}
Eigen::MatrixXs laserscanutils::laserPolar2xy(const ScanParams & _params, const std::vector<float> & _ranges, const ScalarT& _jump_theshold, std::queue<unsigned int> & jumps_id_)
{
while (!jumps_id_.empty())
jumps_id_.pop();
std::vector<ScalarT> points;
ScalarT azimuth = 0.0;
std::vector<ScalarT> points;
ScalarT azimuth = 0.0;
ScalarT prev_range = -1;
while (!jumps_id_.empty())
jumps_id_.pop();
float prev_range = -1;
for(unsigned int i=0; i<_ranges.size(); ++i)
{
if ( _ranges[i] >=_params.range_min_ && _ranges[i] <= _params.range_max_ && !isnan(_ranges[i]) && !isinf(_ranges[i]) )
for(unsigned int i=0; i<_ranges.size(); ++i)
{
azimuth = _params.angle_min_+ _params.angle_step_*i;
points.push_back(_ranges[i] * cos(azimuth));
points.push_back(_ranges[i] * sin(azimuth));
points.push_back(1);
if (prev_range !=-1 && fabs(_ranges[i]-prev_range) > _jump_theshold)
jumps_id_.push(points.size() / 3);
prev_range = _ranges[i];
if ( _ranges[i] >=_params.range_min_ && _ranges[i] <= _params.range_max_ && !isnan(_ranges[i]) && !isinf(_ranges[i]) )
{
azimuth = _params.angle_min_+ _params.angle_step_*i;
points.push_back(_ranges[i] * cos(azimuth));
points.push_back(_ranges[i] * sin(azimuth));
points.push_back(1);
if (prev_range !=-1 && fabs(_ranges[i]-prev_range) > _jump_theshold)
jumps_id_.push(points.size() / 3);
prev_range = _ranges[i];
}
}
}
return Eigen::Map<Eigen::MatrixXs>(points.data(),3, points.size()/3);
return Eigen::Map<Eigen::MatrixXs>(points.data(),3, points.size()/3);
}
// 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)
{
......
......@@ -6,6 +6,7 @@
#include <iostream>
#include <vector>
#include <queue>
#include <list>
//laserscanutils
#include "types_laser_scan_utils.h"
......@@ -27,9 +28,20 @@ namespace laserscanutils
void print() const;
};
void polar2xy(const ScanParams & _params, Eigen::VectorXs & _ranges, Eigen::MatrixXs _points );
/** \brief Transforms from ranges (polar) to euclidean (xy)
*
* Transforms from polar (ranges) to euclidean (xy), while checking correctness of raw data.
* Invalid values (inf's and nan's) and out of range measurements are not transformed.
* Valid measurements are returned in homogeneous coordinates through reference.
* A list of jumps indices is also returned by reference.
* \param _params laser_scan_utils structure of the laser scan parameters.
* \param _ranges raw range measurements from the laser scanner device. Size Nr.
* \param _jump_theshold distance threshold to classify a jump in the range.
* \param _points matrix of homogeneous coordinates of hit points. Size 3xNp, where Np can be <= than Nr (due to possible inf's and nan's from raw data)
* \param jumps_id_ list of indexes of jumps in ranges. Indexes correpond to _points vector and
**/
void ranges2xy(const ScanParams & _params, const std::vector<float> & _ranges, const ScalarT& _jump_theshold, Eigen::MatrixXs & _points, std::list<unsigned int> & _jumps);
/** \brief Receives the ranges from a laser Scanner and returns the valid Homogenian coordinates (X,Y,1)
This means that invalid scanner measures are filtered above and beyond the range limits and the valid
......@@ -53,8 +65,7 @@ namespace laserscanutils
\return _points is the returned Eigen Matrix of homogeneous points.
**/
Eigen::MatrixXs laserPolar2xy(const ScanParams & _params, const std::vector<float> & _ranges, const ScalarT& _jump_theshold, std::queue<unsigned int> & jumps_id_);
/** \brief Receives the ranges from a laser Scanner and returns the valid Homogenian coordinates (X,Y,1)
This means that invalid scanner measures are filtered above and beyond the range limits and the valid
......
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