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 //! Destructor to deallocate stored unit vectors
~LaserProjection(); ~LaserProjection();
//! Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud //! Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud
/*! /*!
* Project a single laser scan from a linear array into a 3D * 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 scan_in The input laser scan
* \param cloud_out The output point cloud * \param cloud_out The output point cloud
* \param range_cutoff An additional range cutoff which can be * \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. * \param channel_option An OR'd set of channels to include.
* Options include: channel_option::Default, * Options include: channel_option::Default,
* channel_option::Intensity, channel_option::Index, * channel_option::Intensity, channel_option::Index,
@ -138,7 +138,8 @@ namespace laser_geometry
* \param scan_in The input laser scan * \param scan_in The input laser scan
* \param cloud_out The output point cloud * \param cloud_out The output point cloud
* \param range_cutoff An additional range cutoff which can be * \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. * \param channel_option An OR'd set of channels to include.
* Options include: channel_option::Default, * Options include: channel_option::Default,
* channel_option::Intensity, channel_option::Index, * channel_option::Intensity, channel_option::Index,
@ -168,7 +169,7 @@ namespace laser_geometry
* \param tf a tf::Transformer object to use to perform the * \param tf a tf::Transformer object to use to perform the
* transform * transform
* \param range_cutoff An additional range cutoff which can be * \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. * \param channel_option An OR'd set of channels to include.
* Options include: channel_option::Default, * Options include: channel_option::Default,
* channel_option::Intensity, channel_option::Index, * channel_option::Intensity, channel_option::Index,
@ -227,7 +228,8 @@ namespace laser_geometry
* \param tf a tf::Transformer object to use to perform the * \param tf a tf::Transformer object to use to perform the
* transform * transform
* \param range_cutoff An additional range cutoff which can be * \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. * \param channel_option An OR'd set of channels to include.
* Options include: channel_option::Default, * Options include: channel_option::Default,
* channel_option::Intensity, channel_option::Index, * 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]; ranges(1,index) = (double) scan_in.ranges[index];
} }
//Do the projection //Do the projection
// NEWMAT::Matrix output = NEWMAT::SP(ranges, getUnitVectors(scan_in.angle_min, scan_in.angle_max, scan_in.angle_increment)); // 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())); 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) 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 +124,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)
@ -144,7 +141,6 @@ namespace laser_geometry
} }
} }
//downsize if necessary //downsize if necessary
cloud_out.points.resize (count); cloud_out.points.resize (count);
for (unsigned int d = 0; d < cloud_out.channels.size(); d++) 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) 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];
@ -440,7 +435,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)