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

28
include/laser_geometry/laser_geometry.h
View File

@ -82,7 +82,7 @@ namespace laser_geometry
* By default all range values less than the scanner min_range, and * By default all range values less than the scanner min_range, and
* greater than the scanner max_range are removed from the generated * greater than the scanner max_range are removed from the generated
* point cloud, as these are assumed to be invalid. * point cloud, as these are assumed to be invalid.
* *
* If it is important to preserve a mapping between the index of * If it is important to preserve a mapping between the index of
* range values and points in the cloud, the recommended approach is * range values and points in the cloud, the recommended approach is
* to pre-filter your laser_scan message to meet the requiremnt that all * 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 //! 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,13 +138,14 @@ 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,
* channel_option::Distance, channel_option::Timestamp. * 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, sensor_msgs::PointCloud2 &cloud_out,
double range_cutoff = -1.0, double range_cutoff = -1.0,
int channel_options = channel_option::Default) int channel_options = channel_option::Default)
@ -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,14 +228,15 @@ 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,
* channel_option::Distance, channel_option::Timestamp. * channel_option::Distance, channel_option::Timestamp.
*/ */
void transformLaserScanToPointCloud(const std::string &target_frame, void transformLaserScanToPointCloud(const std::string &target_frame,
const sensor_msgs::LaserScan &scan_in, const sensor_msgs::LaserScan &scan_in,
sensor_msgs::PointCloud2 &cloud_out, sensor_msgs::PointCloud2 &cloud_out,
tf::Transformer &tf, tf::Transformer &tf,
double range_cutoff = -1.0, double range_cutoff = -1.0,
@ -266,7 +268,7 @@ namespace laser_geometry
int channel_options); int channel_options);
//! Internal hidden representation of projectLaser //! 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, sensor_msgs::PointCloud2 &cloud_out,
double range_cutoff, double range_cutoff,
int channel_options); int channel_options);
@ -280,10 +282,10 @@ namespace laser_geometry
int channel_options); int channel_options);
//! Internal hidden representation of transformLaserScanToPointCloud2 //! 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, const sensor_msgs::LaserScan &scan_in,
sensor_msgs::PointCloud2 &cloud_out, sensor_msgs::PointCloud2 &cloud_out,
tf::Transformer &tf, tf::Transformer &tf,
double range_cutoff, double range_cutoff,
int channel_options); int channel_options);

47
src/laser_geometry.cpp
View File

@ -1,10 +1,10 @@
/* /*
* Copyright (c) 2008, Willow Garage, Inc. * Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved. * All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met: * modification, are permitted provided that the following conditions are met:
* *
* * Redistributions of source code must retain the above copyright * * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer. * notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright * * 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 * * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from * contributors may be used to endorse or promote products derived from
* this software without specific prior written permission. * this software without specific prior written permission.
* *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
@ -35,7 +35,7 @@ namespace laser_geometry
{ {
void 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) bool preservative, int mask)
{ {
boost::numeric::ublas::matrix<double> ranges(2, scan_in.ranges.size()); boost::numeric::ublas::matrix<double> ranges(2, scan_in.ranges.size());
@ -46,7 +46,6 @@ namespace laser_geometry
ranges(0,index) = (double) scan_in.ranges[index]; ranges(0,index) = (double) scan_in.ranges[index];
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));
@ -59,7 +58,7 @@ namespace laser_geometry
// Define 4 indices in the channel array for each possible value type // 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; 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 // Check if the intensity bit is set
if ((mask & channel_option::Intensity) && scan_in.intensities.size() > 0) 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()); cloud_out.channels[0].values.resize (scan_in.intensities.size());
idx_intensity = 0; idx_intensity = 0;
} }
// Check if the index bit is set // Check if the index bit is set
if (mask & channel_option::Index) if (mask & channel_option::Index)
{ {
@ -102,19 +101,17 @@ 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;
//double x = cloud_out.points[count].x; //double x = cloud_out.points[count].x;
//double y = cloud_out.points[count].y; //double y = cloud_out.points[count].y;
//if(x*x + y*y < scan_in.range_min * scan_in.range_min){ //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 // 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)
@ -143,7 +140,6 @@ namespace laser_geometry
count++; count++;
} }
} }
//downsize if necessary //downsize if necessary
cloud_out.points.resize (count); cloud_out.points.resize (count);
@ -170,7 +166,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
(*tempPtr)(0,index) = cos(angle_min + (double) index * angle_increment); (*tempPtr)(0,index) = cos(angle_min + (double) index * angle_increment);
(*tempPtr)(1,index) = sin(angle_min + (double) index * angle_increment); (*tempPtr)(1,index) = sin(angle_min + (double) index * angle_increment);
} }
//store //store
unit_vector_map_[anglestring.str()] = tempPtr; unit_vector_map_[anglestring.str()] = tempPtr;
//and return //and return
return *tempPtr; return *tempPtr;
@ -275,7 +271,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
cloud_out.channels.erase(cloud_out.channels.begin() + index_channel_idx); 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, sensor_msgs::PointCloud2 &cloud_out,
double range_cutoff, double range_cutoff,
int channel_options) int channel_options)
@ -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)
@ -496,10 +491,10 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
cloud_out.data.resize (cloud_out.row_step * cloud_out.height); 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, const sensor_msgs::LaserScan &scan_in,
sensor_msgs::PointCloud2 &cloud_out, sensor_msgs::PointCloud2 &cloud_out,
tf::Transformer &tf, tf::Transformer &tf,
double range_cutoff, double range_cutoff,
int channel_options) int channel_options)
{ {
@ -519,7 +514,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
uint32_t vp_x_offset = 0; uint32_t vp_x_offset = 0;
//we need to find the offset of the intensity field in the point cloud //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 //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 //putting in a check at some point, but I'm just going to put in an
//assert for now //assert for now
@ -565,7 +560,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
//find the index of the point //find the index of the point
uint32_t pt_index; uint32_t pt_index;
memcpy(&pt_index, &cloud_out.data[i * cloud_out.point_step + index_offset], sizeof(uint32_t)); 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 // Assume constant motion during the laser-scan, and use slerp to compute intermediate transforms
tfScalar ratio = pt_index * ranges_norm; tfScalar ratio = pt_index * ranges_norm;