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

Merge pull request #9 from efernandez/allow_cutoff_higher_than_range_max

Browse Source 
allows to have range_cutoff > range_max
This commit is contained in:
Vincent Rabaud 2014-06-05 04:08:21 +02:00
commit 43bb1c4191
2 changed files with 36 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
include/laser_geometry/laser_geometry.h
View File

@ -105,7 +105,6 @@ namespace laser_geometry
//! Destructor to deallocate stored unit vectors
~LaserProjection();
//! Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud
/*!
* Project a single laser scan from a linear array into a 3D
@ -115,7 +114,8 @@ namespace laser_geometry
* \param scan_in The input laser scan
* \param cloud_out The output point cloud
* \param range_cutoff An additional range cutoff which can be
* applied which is more limiting than max_range in the scan.
* applied to discard everything above it.
* Defaults to -1.0, which means the laser scan max range.
* \param channel_option An OR'd set of channels to include.
* Options include: channel_option::Default,
* channel_option::Intensity, channel_option::Index,
@ -138,7 +138,8 @@ namespace laser_geometry
* \param scan_in The input laser scan
* \param cloud_out The output point cloud
* \param range_cutoff An additional range cutoff which can be
* applied which is more limiting than max_range in the scan.
* applied to discard everything above it.
* Defaults to -1.0, which means the laser scan max range.
* \param channel_option An OR'd set of channels to include.
* Options include: channel_option::Default,
* channel_option::Intensity, channel_option::Index,
@ -168,7 +169,7 @@ namespace laser_geometry
* \param tf a tf::Transformer object to use to perform the
* transform
* \param range_cutoff An additional range cutoff which can be
* applied which is more limiting than max_range in the scan.
* applied to discard everything above it.
* \param channel_option An OR'd set of channels to include.
* Options include: channel_option::Default,
* channel_option::Intensity, channel_option::Index,
@ -227,7 +228,8 @@ namespace laser_geometry
* \param tf a tf::Transformer object to use to perform the
* transform
* \param range_cutoff An additional range cutoff which can be
* applied which is more limiting than max_range in the scan.
* applied to discard everything above it.
* Defaults to -1.0, which means the laser scan max range.
* \param channel_option An OR'd set of channels to include.
* Options include: channel_option::Default,
* channel_option::Intensity, channel_option::Index,

17
src/laser_geometry.cpp
View File

@ -47,7 +47,6 @@ namespace laser_geometry
ranges(1,index) = (double) scan_in.ranges[index];
}
//Do the projection
// NEWMAT::Matrix output = NEWMAT::SP(ranges, getUnitVectors(scan_in.angle_min, scan_in.angle_max, scan_in.angle_increment));
boost::numeric::ublas::matrix<double> output = element_prod(ranges, getUnitVectors_(scan_in.angle_min, scan_in.angle_max, scan_in.angle_increment, scan_in.ranges.size()));
@ -102,15 +101,13 @@ namespace laser_geometry
if (range_cutoff < 0)
range_cutoff = scan_in.range_max;
else
range_cutoff = std::min(range_cutoff, (double)scan_in.range_max);
unsigned int count = 0;
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].y = output(1,index);
cloud_out.points[count].z = 0.0;
@ -127,7 +124,7 @@ namespace laser_geometry
// Save the original point distance
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
if (scan_in.intensities.size() >= index)
@ -144,7 +141,6 @@ namespace laser_geometry
}
}
//downsize if necessary
cloud_out.points.resize (count);
for (unsigned int d = 0; d < cloud_out.channels.size(); d++)
@ -414,14 +410,13 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
if (range_cutoff < 0)
range_cutoff = scan_in.range_max;
else
range_cutoff = std::min(range_cutoff, (double)scan_in.range_max);
unsigned int count = 0;
for (size_t i = 0; i < n_pts; ++i)
{
//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];
@ -440,7 +435,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
// Copy distance
if(idx_distance != -1)
pstep[idx_distance] = scan_in.ranges[i];
pstep[idx_distance] = range;
// Copy timestamp
if(idx_timestamp != -1)