Skip to content
Snippets Groups Projects
Commit 1a26f977 authored by jvallve's avatar jvallve
Browse files

added function polar2xy computing also the range jumps and changed ranges from...

added function polar2xy computing also the range jumps and changed ranges from eigen to std vector in corner_detector
parent a5cd203f
No related branches found
No related tags found
No related merge requests found
......@@ -13,10 +13,11 @@ void laserscanutils::ExtractCornerParams::print() const
}
//unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _params, const ExtractCornerParams & _alg_params, const Eigen::VectorXs & _ranges, std::list<Eigen::Vector4s> & _corner_list)
unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _params, const ExtractCornerParams & _alg_params, const Eigen::VectorXs & _ranges, std::list<laserscanutils::Corner> & _corner_list)
//unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _params, const ExtractCornerParams & _alg_params, const Eigen::VectorXs & _ranges, std::list<laserscanutils::Corner> & _corner_list)
unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _params, const ExtractCornerParams & _alg_params, const std::vector<float> & _ranges, std::list<laserscanutils::Corner> & _corner_list)
{
//local variables
Eigen::MatrixXs points(3,_ranges.size());
//Eigen::MatrixXs points(3,_ranges.size());
ScalarT azimuth, cos_theta, theta;
//Eigen::Vector3s corner;
Corner corner;
......@@ -32,15 +33,16 @@ unsigned int laserscanutils::extractCorners(const laserscanutils::ScanParams & _
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);
}
// 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);
// STEP 1: find line segments running over the scan
for (unsigned int ii = _alg_params.segment_window_size-1; ii<_ranges.size(); ii=ii+rand_window_overlapping(generator) )
......
......@@ -56,7 +56,7 @@ namespace laserscanutils
* Returns corners as a list of laserscanutils::Corners
*
*/
unsigned int extractCorners(const laserscanutils::ScanParams & _params, const ExtractCornerParams & _alg_params, const Eigen::VectorXs & _ranges, std::list<laserscanutils::Corner> & _corner_list);
unsigned int extractCorners(const laserscanutils::ScanParams & _params, const ExtractCornerParams & _alg_params, const std::vector<float> & _ranges, std::list<laserscanutils::Corner> & _corner_list);
/** \brief Returns corner aperture
*
......
......@@ -126,8 +126,6 @@ int main(int argc, char** argv)
//compute scan
myScan.clear();
myScanner->computeScan(devicePose,myScan);
vector<double> myScanDoubles(myScan.begin(), myScan.end());
Eigen::Map<Eigen::VectorXs> scan_reading(myScanDoubles.data(), 720);
//draws the device frame, scan hits and depth image
myRender->drawPoseAxis(devicePose);
......@@ -136,7 +134,7 @@ int main(int argc, char** argv)
// extract corners
std::list<laserscanutils::Corner> corner_list;
std::vector<double> corner_vector;
laserscanutils::extractCorners(scan_params, alg_params, scan_reading, corner_list);
laserscanutils::extractCorners(scan_params, alg_params, myScan, corner_list);
//draw corners
for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++ )
......
......@@ -53,3 +53,30 @@ Eigen::MatrixXs laserscanutils::laserPolar2xy(const ScanParams & _params, std::v
}
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;
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]) )
{
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 (i > 0 && fabs(_ranges[i]-_ranges[i-1]) > _jump_theshold)
jumps_id_.push(i);
}
}
return Eigen::Map<Eigen::MatrixXs>(points.data(),3, points.size()/3);
}
......@@ -5,6 +5,7 @@
//std
#include <iostream>
#include <vector>
#include <queue>
//laserscanutils
#include "types_laser_scan_utils.h"
......@@ -39,5 +40,18 @@ namespace laserscanutils
\param _points is the returned Eigen Matrix of homogeneous points.
**/
Eigen::MatrixXs laserPolar2xy(const ScanParams & _params, std::vector<float> & _ranges);
/** \brief Receives the ranges from a laser Scanner and returns the valid Homogenian coordinates (X,Y,1) and a vector of jumps indices via reference
This means that invalid scanner measures are filtered above and beyond the range limits and the valid
ones are returned in Homogenian Coordinates inside a MatrixXs laser_scan_util's type.
\param _params is the laser_scan_utils structure of the laser parameters.
\param _ranges is the laser range measures
\param _jump_theshold is the distance threshold to classify a jump in the range.
\param jumps_id_ is the returned indexes vector of jumps in ranges.
\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_);
}
#endif
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