diff --git a/include/laser_geometry/laser_geometry.h b/include/laser_geometry/laser_geometry.h index 9d60a88..55fb4e7 100644 --- a/include/laser_geometry/laser_geometry.h +++ b/include/laser_geometry/laser_geometry.h @@ -82,7 +82,7 @@ namespace laser_geometry * By default all range values less than the scanner min_range, and * greater than the scanner max_range are removed from the generated * point cloud, as these are assumed to be invalid. - * + * * If it is important to preserve a mapping between the index of * range values and points in the cloud, the recommended approach is * to pre-filter your laser_scan message to meet the requiremnt that all @@ -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,13 +138,14 @@ 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, * channel_option::Distance, channel_option::Timestamp. */ - void projectLaser (const sensor_msgs::LaserScan& scan_in, + void projectLaser (const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud2 &cloud_out, double range_cutoff = -1.0, int channel_options = channel_option::Default) @@ -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,14 +228,15 @@ 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, * channel_option::Distance, channel_option::Timestamp. */ - void transformLaserScanToPointCloud(const std::string &target_frame, - const sensor_msgs::LaserScan &scan_in, + void transformLaserScanToPointCloud(const std::string &target_frame, + const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud2 &cloud_out, tf::Transformer &tf, double range_cutoff = -1.0, @@ -266,7 +268,7 @@ namespace laser_geometry int channel_options); //! Internal hidden representation of projectLaser - void projectLaser_ (const sensor_msgs::LaserScan& scan_in, + void projectLaser_ (const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud2 &cloud_out, double range_cutoff, int channel_options); @@ -280,10 +282,10 @@ namespace laser_geometry int channel_options); //! Internal hidden representation of transformLaserScanToPointCloud2 - void transformLaserScanToPointCloud_ (const std::string &target_frame, + void transformLaserScanToPointCloud_ (const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, - sensor_msgs::PointCloud2 &cloud_out, - tf::Transformer &tf, + sensor_msgs::PointCloud2 &cloud_out, + tf::Transformer &tf, double range_cutoff, int channel_options); diff --git a/src/laser_geometry.cpp b/src/laser_geometry.cpp index 050bc41..56d2ecd 100644 --- a/src/laser_geometry.cpp +++ b/src/laser_geometry.cpp @@ -1,10 +1,10 @@ /* * Copyright (c) 2008, Willow Garage, Inc. * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above copyright @@ -13,7 +13,7 @@ * * Neither the name of the Willow Garage, Inc. nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE @@ -35,7 +35,7 @@ namespace laser_geometry { void - LaserProjection::projectLaser_ (const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud & cloud_out, double range_cutoff, + LaserProjection::projectLaser_ (const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud & cloud_out, double range_cutoff, bool preservative, int mask) { boost::numeric::ublas::matrix ranges(2, scan_in.ranges.size()); @@ -46,7 +46,6 @@ namespace laser_geometry ranges(0,index) = (double) scan_in.ranges[index]; 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)); @@ -59,7 +58,7 @@ namespace laser_geometry // Define 4 indices in the channel array for each possible value type int idx_intensity = -1, idx_index = -1, idx_distance = -1, idx_timestamp = -1; - cloud_out.channels.resize(0); + cloud_out.channels.resize(0); // Check if the intensity bit is set if ((mask & channel_option::Intensity) && scan_in.intensities.size() > 0) @@ -70,7 +69,7 @@ namespace laser_geometry cloud_out.channels[0].values.resize (scan_in.intensities.size()); idx_intensity = 0; } - + // Check if the index bit is set if (mask & channel_option::Index) { @@ -102,19 +101,17 @@ 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; - + //double x = cloud_out.points[count].x; //double y = cloud_out.points[count].y; //if(x*x + y*y < scan_in.range_min * scan_in.range_min){ @@ -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) @@ -143,7 +140,6 @@ namespace laser_geometry count++; } } - //downsize if necessary cloud_out.points.resize (count); @@ -170,7 +166,7 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do (*tempPtr)(0,index) = cos(angle_min + (double) index * angle_increment); (*tempPtr)(1,index) = sin(angle_min + (double) index * angle_increment); } - //store + //store unit_vector_map_[anglestring.str()] = tempPtr; //and return return *tempPtr; @@ -275,7 +271,7 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do cloud_out.channels.erase(cloud_out.channels.begin() + index_channel_idx); } - void LaserProjection::projectLaser_ (const sensor_msgs::LaserScan& scan_in, + void LaserProjection::projectLaser_ (const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud2 &cloud_out, double range_cutoff, int channel_options) @@ -414,14 +410,13 @@ const boost::numeric::ublas::matrix& 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& 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) @@ -496,10 +491,10 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do cloud_out.data.resize (cloud_out.row_step * cloud_out.height); } - void LaserProjection::transformLaserScanToPointCloud_ (const std::string &target_frame, + void LaserProjection::transformLaserScanToPointCloud_ (const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, - sensor_msgs::PointCloud2 &cloud_out, - tf::Transformer &tf, + sensor_msgs::PointCloud2 &cloud_out, + tf::Transformer &tf, double range_cutoff, int channel_options) { @@ -519,7 +514,7 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do uint32_t vp_x_offset = 0; //we need to find the offset of the intensity field in the point cloud - //we also know that the index field is guaranteed to exist since we + //we also know that the index field is guaranteed to exist since we //set the channel option above. To be really safe, it might be worth //putting in a check at some point, but I'm just going to put in an //assert for now @@ -565,7 +560,7 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do //find the index of the point uint32_t pt_index; memcpy(&pt_index, &cloud_out.data[i * cloud_out.point_step + index_offset], sizeof(uint32_t)); - + // Assume constant motion during the laser-scan, and use slerp to compute intermediate transforms tfScalar ratio = pt_index * ranges_norm;