DuongTD/laser_geometry
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

allows to have range_cutoff > range_max

Browse Source 
NOTE this is required if we need to keep the range_max readings
in the point cloud.
An example application is an obstacle_layer in a costmap.
This commit is contained in:
enriquefernandez 2014-06-02 20:34:05 +02:00
parent 4bec7c1f46
commit c8fc9bf5b4
1 changed files with 8 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/laser_geometry.cpp
View File

@ -102,15 +102,13 @@ namespace laser_geometry
if (range_cutoff < 0) if (range_cutoff < 0)
range_cutoff = scan_in.range_max; range_cutoff = scan_in.range_max;
else
range_cutoff = std::min(range_cutoff, (double)scan_in.range_max);
unsigned int count = 0; unsigned int count = 0;
for (unsigned int index = 0; index< scan_in.ranges.size(); index++) for (unsigned int index = 0; index< scan_in.ranges.size(); index++)
{ {
if (preservative || ((ranges(0,index) < range_cutoff) && (ranges(0,index) >= scan_in.range_min))) //if valid or preservative const float range = ranges(0, index);
if (preservative || ((range < range_cutoff) && (range >= scan_in.range_min))) //if valid or preservative
{ {
cloud_out.points[count].x = output(0,index); cloud_out.points[count].x = output(0,index);
cloud_out.points[count].y = output(1,index); cloud_out.points[count].y = output(1,index);
cloud_out.points[count].z = 0.0; cloud_out.points[count].z = 0.0;
@ -127,7 +125,7 @@ namespace laser_geometry
// Save the original point distance // Save the original point distance
if (idx_distance != -1) if (idx_distance != -1)
cloud_out.channels[idx_distance].values[count] = ranges (0, index); cloud_out.channels[idx_distance].values[count] = range;
// Save intensities channel // Save intensities channel
if (scan_in.intensities.size() >= index) if (scan_in.intensities.size() >= index)
@ -417,14 +415,13 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
if (range_cutoff < 0) if (range_cutoff < 0)
range_cutoff = scan_in.range_max; range_cutoff = scan_in.range_max;
else
range_cutoff = std::min(range_cutoff, (double)scan_in.range_max);
unsigned int count = 0; unsigned int count = 0;
for (size_t i = 0; i < n_pts; ++i) for (size_t i = 0; i < n_pts; ++i)
{ {
//check to see if we want to keep the point //check to see if we want to keep the point
if (scan_in.ranges[i] < range_cutoff && scan_in.ranges[i] >= scan_in.range_min) const float range = scan_in.ranges[i];
if (range < range_cutoff && range >= scan_in.range_min)
{ {
float *pstep = (float*)&cloud_out.data[count * cloud_out.point_step]; float *pstep = (float*)&cloud_out.data[count * cloud_out.point_step];
@ -443,7 +440,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
// Copy distance // Copy distance
if(idx_distance != -1) if(idx_distance != -1)
pstep[idx_distance] = scan_in.ranges[i]; pstep[idx_distance] = range;
// Copy timestamp // Copy timestamp
if(idx_timestamp != -1) if(idx_timestamp != -1)