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

Merging in remaining missing contents for laser_piple that svn ignored on the first merge.

Browse Source 
git-svn-id: https://code.ros.org/svn/ros-pkg/pkg/trunk/stacks/laser_pipeline@23510 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
This commit is contained in:
Jeremy Leibs 2009-09-01 08:18:14 +00:00
commit 58e93bb190
9 changed files with 1309 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
CMakeLists.txt Normal file
View File

@ -0,0 +1,16 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
set(ROS_BUILD_TYPE Debug)
rospack(laser_geometry)
rospack_add_boost_directories()
#todo integrate this
rospack_add_library(laser_processor src/laser_processor.cpp)
rospack_add_library(laser_scan src/laser_scan.cc )
rospack_link_boost(laser_scan thread)
rospack_add_gtest(test/projection_test test/projection_test.cpp)
target_link_libraries (test/projection_test laser_scan)

1
Makefile Normal file
View File

@ -0,0 +1 @@
include $(shell rospack find mk)/cmake.mk

99
include/laser_geometry/laser_geometry.h Normal file
View File

@ -0,0 +1,99 @@
/*
* 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
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * 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
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef LASER_SCAN_UTILS_LASERSCAN_H
#define LASER_SCAN_UTILS_LASERSCAN_H
#include <map>
#include <iostream>
#include <sstream>
#include "boost/numeric/ublas/matrix.hpp"
#include "tf/tf.h"
#include "sensor_msgs/LaserScan.h"
#include "sensor_msgs/PointCloud.h"
#include "sensor_msgs/PointCloud.h"
/* \mainpage
* This is a class for laser scan utilities.
* \todo The first goal will be to project laser scans into point clouds efficiently.
* The second goal is to provide median filtering.
* \todo Other potential additions are upsampling and downsampling algorithms for the scans.
*/
namespace laser_geometry
{
/** \brief Define masks for output channels */
const int MASK_INTENSITY = 0x01;
const int MASK_INDEX = 0x02;
const int MASK_DISTANCE = 0x04;
const int MASK_TIMESTAMP = 0x08;
const int DEFAULT_MASK = (MASK_INTENSITY | MASK_INDEX);
/** \brief A Class to Project Laser Scan
* This class will project laser scans into point clouds, and caches unit vectors
* between runs so as not to need to recalculate.
*/
class LaserProjection
{
public:
/** \brief Destructor to deallocate stored unit vectors */
~LaserProjection();
/** \brief Project Laser Scan
* This will project a laser scan from a linear array into a 3D point cloud
* \param scan_in The input laser scan
* \param cloudOut 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.
* \param preservative Default: false If true all points in scan will be projected, including out of range values. Otherwise they will not be added to the cloud.
*/
void projectLaser (const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud & cloud_out, double range_cutoff=-1.0, bool preservative = false, int mask = DEFAULT_MASK);
/** \brief Transform a sensor_msgs::LaserScan into a PointCloud in target frame */
void transformLaserScanToPointCloud (const std::string& target_frame, sensor_msgs::PointCloud & cloudOut, const sensor_msgs::LaserScan & scanIn, tf::Transformer & tf, int mask = DEFAULT_MASK, bool preservative = false);
/** \brief Return the unit vectors for this configuration
* Return the unit vectors for this configuration.
* These are dynamically generated and allocated on first request
* and will be valid until destruction of this node. */
const boost::numeric::ublas::matrix<double>& getUnitVectors(float angle_max, float angle_min, float angle_increment, unsigned int length);
private:
///The map of pointers to stored values
std::map<std::string,boost::numeric::ublas::matrix<double>* > unit_vector_map_;
};
}
#endif //LASER_SCAN_UTILS_LASERSCAN_H

145
include/laser_geometry/laser_processor.h Normal file
View File

@ -0,0 +1,145 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* 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 notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage 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 ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/*! \mainpage
* \htmlinclude manifest.html
*/
//! A namespace containing the laser processor helper classes
#ifndef LASER_SCAN_LASERPROCESSOR_HH
#define LASER_SCAN_LASERPROCESSOR_HH
#include <unistd.h>
#include <math.h>
#include "ros/node.h"
#include "sensor_msgs/LaserScan.h"
#include "sensor_msgs/PointCloud.h"
#include "geometry_msgs/Point.h"
#include <list>
#include <set>
#include <vector>
#include <map>
#include <utility>
#include <algorithm>
#include "tf/transform_datatypes.h"
namespace laser_geometry
{
//! A struct representing a single sample from the laser.
class Sample
{
public:
int index;
float range;
float intensity;
float x;
float y;
static Sample* Extract(int ind, const sensor_msgs::LaserScan& scan);
private:
Sample() {};
};
//! The comparator allowing the creation of an ordered "SampleSet"
struct CompareSample
{
inline bool operator() (const Sample* a, const Sample* b)
{
return (a->index < b->index);
}
};
//! An ordered set of Samples
class SampleSet : public std::set<Sample*, CompareSample>
{
public:
~SampleSet() { clear(); }
void clear();
void appendToCloud(sensor_msgs::PointCloud& cloud, int r = 0, int g = 0, int b = 0);
tf::Point center();
};
//! A mask for filtering out Samples based on range
class ScanMask
{
SampleSet mask_;
bool filled;
float angle_min;
float angle_max;
uint32_t size;
public:
ScanMask() : filled(false), angle_min(0), angle_max(0), size(0) { }
inline void clear() { mask_.clear(); filled = false; }
void addScan(sensor_msgs::LaserScan& scan);
bool hasSample(Sample* s, float thresh);
};
class ScanProcessor
{
std::list<SampleSet*> clusters_;
sensor_msgs::LaserScan scan_;
public:
std::list<SampleSet*>& getClusters() { return clusters_; }
ScanProcessor(const sensor_msgs::LaserScan& scan, ScanMask& mask_, float mask_threshold = 0.03);
~ScanProcessor();
void removeLessThan(uint32_t num);
void splitConnected(float thresh);
};
};
#endif

29
mainpage.dox Normal file
View File

@ -0,0 +1,29 @@
/**
\mainpage
This package is designed to provide helper functions/classes for working with laser scans. It provides library functionality only.
\section projection Laser Scan Projection to Point Cloud
laser_scan::LaserProjection provides a simple method to project laser scans into point clouds while discarding out-of-range measurements.
\section filters Laser Scan Filters
There are a number of filters that this class provides.
There is also a factory class to provide easy creation and chaining of laser scan filter
as a input to any process.
\todo implement and document (they're currently basically standalone nodes)
\subsection median Median Filter
laser_scan::LaserMedianFilter applies a median filter to scans both in range and intensity.
\subsection mean Mean Filter
\todo Document
\subsection shadows Scan Shadows Filter
\todo document
\subsection intensity Intensity Filter
\todo document
*/

22
manifest.xml Normal file
View File

@ -0,0 +1,22 @@
<package>
<description brief='Utilities for converting laser scans to pointclouds'>
This package contains a class for converting from a 2D laser scan as
defined by sensor_msgs/LaserScan into a point cloud as defined by
sensor_msgs/PointCloud. In particular, it contains functionality to
account for the skew resulting from moving robots or tilting laser
scanners.
</description>
<author>Tully Foote</author>
<license>BSD</license>
<review status="API cleared" notes=""/>
<url>http://ros.org/wiki/laser_scan</url>
<depend package="sensor_msgs"/>
<depend package="roscpp"/>
<depend package="tf"/>
<depend package="angles"/>
<export>
<cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -llaser_scan -llaser_processor"/>
</export>
</package>

288
src/laser_processor.cpp Normal file
View File

@ -0,0 +1,288 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* 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 notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage 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 ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#include "laser_geometry/laser_processor.h"
#include <stdexcept>
using namespace ros;
using namespace std;
using namespace laser_geometry;
Sample* Sample::Extract(int ind, const sensor_msgs::LaserScan& scan)
{
Sample* s = new Sample;
s->index = ind;
s->range = scan.ranges[ind];
s->intensity = scan.intensities[ind];
s->x = cos( scan.angle_min + ind*scan.angle_increment ) * s->range;
s->y = sin( scan.angle_min + ind*scan.angle_increment ) * s->range;
if (s->range > scan.range_min && s->range < scan.range_max)
return s;
else
{
delete s;
return NULL;
}
}
void SampleSet::clear()
{
for (SampleSet::iterator i = begin();
i != end();
i++)
{
delete (*i);
}
set<Sample*, CompareSample>::clear();
}
void SampleSet::appendToCloud(sensor_msgs::PointCloud& cloud, int r, int g, int b)
{
float color_val = 0;
int rgb = (r << 16) | (g << 8) | b;
color_val = *(float*)&(rgb);
for (iterator sample_iter = begin();
sample_iter != end();
sample_iter++)
{
geometry_msgs::Point32 point;
point.x = (*sample_iter)->x;
point.y = (*sample_iter)->y;
point.z = 0;
cloud.points.push_back(point);
if (cloud.channels[0].name == "rgb")
cloud.channels[0].values.push_back(color_val);
if (cloud.channels[0].name == "intensity")
cloud.channels[0].values.push_back((*sample_iter)->intensity);
}
}
tf::Point SampleSet::center()
{
float x_mean = 0.0;
float y_mean = 0.0;
for (iterator i = begin();
i != end();
i++)
{
x_mean += ((*i)->x)/size();
y_mean += ((*i)->y)/size();
}
return tf::Point (x_mean, y_mean, 0.0);
}
void ScanMask::addScan(sensor_msgs::LaserScan& scan)
{
if (!filled)
{
angle_min = scan.angle_min;
angle_max = scan.angle_max;
size = scan.ranges.size();
filled = true;
} else if (angle_min != scan.angle_min ||
angle_max != scan.angle_max ||
size != scan.ranges.size())
{
throw std::runtime_error("laser_scan::ScanMask::addScan: inconsistantly sized scans added to mask");
}
for (uint32_t i = 0; i < scan.ranges.size(); i++)
{
Sample* s = Sample::Extract(i, scan);
if (s != NULL)
{
SampleSet::iterator m = mask_.find(s);
if (m != mask_.end())
{
if ((*m)->range > s->range)
{
delete (*m);
mask_.erase(m);
mask_.insert(s);
} else {
delete s;
}
}
else
{
mask_.insert(s);
}
}
}
}
bool ScanMask::hasSample(Sample* s, float thresh)
{
if (s != NULL)
{
SampleSet::iterator m = mask_.find(s);
if ( m != mask_.end())
if (((*m)->range - thresh) < s->range)
return true;
}
return false;
}
ScanProcessor::ScanProcessor(const sensor_msgs::LaserScan& scan, ScanMask& mask_, float mask_threshold)
{
scan_ = scan;
SampleSet* cluster = new SampleSet;
for (uint32_t i = 0; i < scan.ranges.size(); i++)
{
Sample* s = Sample::Extract(i, scan);
if (s != NULL)
{
if (!mask_.hasSample(s, mask_threshold))
{
cluster->insert(s);
} else {
delete s;
}
}
}
clusters_.push_back(cluster);
}
ScanProcessor::~ScanProcessor()
{
for ( list<SampleSet*>::iterator c = clusters_.begin();
c != clusters_.end();
c++)
delete (*c);
}
void
ScanProcessor::removeLessThan(uint32_t num)
{
list<SampleSet*>::iterator c_iter = clusters_.begin();
while (c_iter != clusters_.end())
{
if ( (*c_iter)->size() < num )
{
delete (*c_iter);
clusters_.erase(c_iter++);
} else {
++c_iter;
}
}
}
void
ScanProcessor::splitConnected(float thresh)
{
list<SampleSet*> tmp_clusters;
list<SampleSet*>::iterator c_iter = clusters_.begin();
// For each cluster
while (c_iter != clusters_.end())
{
// Go through the entire list
while ((*c_iter)->size() > 0 )
{
// Take the first element
SampleSet::iterator s_first = (*c_iter)->begin();
// Start a new queue
list<Sample*> sample_queue;
sample_queue.push_back(*s_first);
(*c_iter)->erase(s_first);
// Grow until we get to the end of the queue
list<Sample*>::iterator s_q = sample_queue.begin();
while (s_q != sample_queue.end())
{
int expand = (int)(asin( thresh / (*s_q)->range ) / scan_.angle_increment);
SampleSet::iterator s_rest = (*c_iter)->begin();
while ( (s_rest != (*c_iter)->end() &&
(*s_rest)->index < (*s_q)->index + expand ) )
{
if ( (*s_rest)->range - (*s_q)->range > thresh)
{
break;
}
else if (sqrt( pow( (*s_q)->x - (*s_rest)->x, 2.0f) + pow( (*s_q)->y - (*s_rest)->y, 2.0f)) < thresh)
{
sample_queue.push_back(*s_rest);
(*c_iter)->erase(s_rest++);
break;
} else {
++s_rest;
}
}
s_q++;
}
// Move all the samples into the new cluster
SampleSet* c = new SampleSet;
for (s_q = sample_queue.begin(); s_q != sample_queue.end(); s_q++)
c->insert(*s_q);
// Store the temporary clusters
tmp_clusters.push_back(c);
}
//Now that c_iter is empty, we can delete
delete (*c_iter);
//And remove from the map
clusters_.erase(c_iter++);
}
clusters_.insert(clusters_.begin(), tmp_clusters.begin(), tmp_clusters.end());
}

337
src/laser_scan.cc Normal file
View File

@ -0,0 +1,337 @@
/*
* 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
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * 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
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "laser_geometry/laser_geometry.h"
#include <algorithm>
namespace laser_geometry
{
void
LaserProjection::projectLaser (const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud & cloud_out, double range_cutoff,
bool preservative, int mask)
{
boost::numeric::ublas::matrix<double> ranges(2, scan_in.get_ranges_size());
// Fill the ranges matrix
for (unsigned int index = 0; index < scan_in.get_ranges_size(); index++)
{
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));
boost::numeric::ublas::matrix<double> output = element_prod(ranges, getUnitVectors(scan_in.angle_min, scan_in.angle_max, scan_in.angle_increment, scan_in.get_ranges_size()));
//Stuff the output cloud
cloud_out.header = scan_in.header;
cloud_out.set_points_size (scan_in.get_ranges_size ());
// 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.set_channels_size(0);
// Check if the intensity bit is set
if ((mask & MASK_INTENSITY) && scan_in.get_intensities_size () > 0)
{
int chan_size = cloud_out.get_channels_size ();
cloud_out.set_channels_size (chan_size + 1);
cloud_out.channels[0].name = "intensities";
cloud_out.channels[0].set_values_size (scan_in.get_intensities_size ());
idx_intensity = 0;
}
// Check if the index bit is set
if (mask & MASK_INDEX)
{
int chan_size = cloud_out.get_channels_size ();
cloud_out.set_channels_size (chan_size +1);
cloud_out.channels[chan_size].name = "index";
cloud_out.channels[chan_size].set_values_size (scan_in.get_ranges_size ());
idx_index = chan_size;
}
// Check if the distance bit is set
if (mask & MASK_DISTANCE)
{
int chan_size = cloud_out.get_channels_size ();
cloud_out.set_channels_size (chan_size + 1);
cloud_out.channels[chan_size].name = "distances";
cloud_out.channels[chan_size].set_values_size (scan_in.get_ranges_size ());
idx_distance = chan_size;
}
if (mask & MASK_TIMESTAMP)
{
int chan_size = cloud_out.get_channels_size ();
cloud_out.set_channels_size (chan_size + 1);
cloud_out.channels[chan_size].name = "stamps";
cloud_out.channels[chan_size].set_values_size (scan_in.get_ranges_size ());
idx_timestamp = chan_size;
}
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.get_ranges_size(); index++)
{
if (preservative || ((ranges(0,index) < range_cutoff) && (ranges(0,index) >= 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){
// ROS_INFO("(%.2f, %.2f)", cloud_out.points[count].x, cloud_out.points[count].y);
//}
// Save the original point index
if (idx_index != -1)
cloud_out.channels[idx_index].values[count] = index;
// Save the original point distance
if (idx_distance != -1)
cloud_out.channels[idx_distance].values[count] = ranges (0, index);
// Save intensities channel
if (scan_in.get_intensities_size() >= index)
{ /// \todo optimize and catch length difference better
if (idx_intensity != -1)
cloud_out.channels[idx_intensity].values[count] = scan_in.intensities[index];
}
// Save timestamps to seperate channel if asked for
if( idx_timestamp != -1)
cloud_out.channels[idx_timestamp].values[count] = (float)index*scan_in.time_increment;
count++;
}
}
//downsize if necessary
cloud_out.set_points_size (count);
for (unsigned int d = 0; d < cloud_out.get_channels_size (); d++)
cloud_out.channels[d].set_values_size(count);
};
const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors(float angle_min, float angle_max, float angle_increment, unsigned int length)
{
if (angle_min >= angle_max)
{
std::stringstream ss;
ss << "LaserProjection min angle " << angle_min << " greater than max angle "<< angle_max;
ROS_ERROR("%s", ss.str().c_str());
throw std::runtime_error(ss.str()); //This would result in a bad alloc anyway so throwing instead
}
//construct string for lookup in the map
std::stringstream anglestring;
anglestring <<angle_min<<","<<angle_max<<","<<angle_increment<<","<<length;
std::map<std::string, boost::numeric::ublas::matrix<double>* >::iterator it;
it = unit_vector_map_.find(anglestring.str());
//check the map for presense
if (it != unit_vector_map_.end())
return *((*it).second); //if present return
boost::numeric::ublas::matrix<double> * tempPtr = new boost::numeric::ublas::matrix<double>(2,length);
for (unsigned int index = 0;index < length; index++)
{
(*tempPtr)(0,index) = cos(angle_min + (double) index * angle_increment);
(*tempPtr)(1,index) = sin(angle_min + (double) index * angle_increment);
}
//store
unit_vector_map_[anglestring.str()] = tempPtr;
//and return
return *tempPtr;
};
LaserProjection::~LaserProjection()
{
std::map<std::string, boost::numeric::ublas::matrix<double>*>::iterator it;
it = unit_vector_map_.begin();
while (it != unit_vector_map_.end())
{
delete (*it).second;
it++;
}
};
void
LaserProjection::transformLaserScanToPointCloud (const std::string &target_frame, sensor_msgs::PointCloud &cloud_out, const sensor_msgs::LaserScan &scan_in,
tf::Transformer& tf, int mask, bool preservative)
{
cloud_out.header = scan_in.header;
cloud_out.header.frame_id = target_frame;
cloud_out.set_points_size (scan_in.get_ranges_size());
// 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.set_channels_size(0);
// Check if the intensity bit is set
if ((mask & MASK_INTENSITY) && scan_in.get_intensities_size () > 0)
{
int chan_size = cloud_out.get_channels_size ();
cloud_out.set_channels_size (chan_size + 1);
cloud_out.channels[0].name = "intensities";
cloud_out.channels[0].set_values_size (scan_in.get_intensities_size ());
idx_intensity = 0;
}
// Check if the index bit is set
if (mask & MASK_INDEX)
{
int chan_size = cloud_out.get_channels_size ();
cloud_out.set_channels_size (chan_size + 1);
cloud_out.channels[chan_size].name = "index";
cloud_out.channels[chan_size].set_values_size (scan_in.get_ranges_size ());
idx_index = chan_size;
}
// Check if the distance bit is set
if (mask & MASK_DISTANCE)
{
int chan_size = cloud_out.get_channels_size ();
cloud_out.set_channels_size (chan_size + 1);
cloud_out.channels[chan_size].name = "distances";
cloud_out.channels[chan_size].set_values_size (scan_in.get_ranges_size ());
idx_distance = chan_size;
}
if (mask & MASK_TIMESTAMP)
{
int chan_size = cloud_out.get_channels_size ();
cloud_out.set_channels_size (chan_size + 1);
cloud_out.channels[chan_size].name = "stamps";
cloud_out.channels[chan_size].set_values_size (scan_in.get_ranges_size ());
idx_timestamp = chan_size;
}
tf::Stamped<tf::Point> pointIn;
tf::Stamped<tf::Point> pointOut;
pointIn.frame_id_ = scan_in.header.frame_id;
///\todo this can be optimized
sensor_msgs::PointCloud intermediate; //optimize out
projectLaser (scan_in, intermediate, -1.0, true, mask);
// Extract transforms for the beginning and end of the laser scan
ros::Time start_time = scan_in.header.stamp ;
ros::Time end_time = scan_in.header.stamp + ros::Duration().fromSec(scan_in.get_ranges_size()*scan_in.time_increment) ;
tf::Stamped<tf::Transform> start_transform ;
tf::Stamped<tf::Transform> end_transform ;
tf::Stamped<tf::Transform> cur_transform ;
tf.lookupTransform(target_frame, scan_in.header.frame_id, start_time, start_transform) ;
tf.lookupTransform(target_frame, scan_in.header.frame_id, end_time, end_transform) ;
unsigned int count = 0;
for (unsigned int i = 0; i < scan_in.get_ranges_size(); i++)
{
if (preservative || (scan_in.ranges[i] < scan_in.range_max && scan_in.ranges[i] > scan_in.range_min)) //only when valid
{
// Looking up transforms in tree is too expensive. Need more optimized way
/*
pointIn = tf::Stamped<tf::Point>(btVector3(intermediate.points[i].x, intermediate.points[i].y, intermediate.points[i].z),
ros::Time(scan_in.header.stamp.to_ull() + (uint64_t) (scan_in.time_increment * 1000000000)),
pointIn.frame_id_ = scan_in.header.frame_id);///\todo optimize to no copy
transformPoint(target_frame, pointIn, pointOut);
*/
// Instead, assume constant motion during the laser-scan, and use slerp to compute intermediate transforms
btScalar ratio = i / ( (double) scan_in.get_ranges_size() - 1.0) ;
//! \todo Make a function that performs both the slerp and linear interpolation needed to interpolate a Full Transform (Quaternion + Vector)
//Interpolate translation
btVector3 v (0, 0, 0);
v.setInterpolate3(start_transform.getOrigin(), end_transform.getOrigin(), ratio) ;
cur_transform.setOrigin(v) ;
//Interpolate rotation
btQuaternion q1, q2 ;
start_transform.getBasis().getRotation(q1) ;
end_transform.getBasis().getRotation(q2) ;
// Compute the slerp-ed rotation
cur_transform.setRotation( slerp( q1, q2 , ratio) ) ;
// Apply the transform to the current point
btVector3 pointIn(intermediate.points[i].x, intermediate.points[i].y, intermediate.points[i].z) ;
btVector3 pointOut = cur_transform * pointIn ;
// Copy transformed point into cloud
cloud_out.points[count].x = pointOut.x();
cloud_out.points[count].y = pointOut.y();
cloud_out.points[count].z = pointOut.z();
// Copy index over from projected point cloud
if (idx_index != -1)
cloud_out.channels[idx_index].values[count] = intermediate.channels[idx_index].values[i];
// Save the original point distance
if (idx_distance != -1)
cloud_out.channels[idx_distance].values[count] = scan_in.ranges[i];
if (scan_in.get_intensities_size() >= i)
{ /// \todo optimize and catch length difference better
if (idx_intensity != -1)
cloud_out.channels[idx_intensity].values[count] = scan_in.intensities[i];
}
if (idx_timestamp != -1)
cloud_out.channels[idx_timestamp].values[count] = (float)i * scan_in.time_increment;
count++;
}
}
//downsize if necessary
cloud_out.set_points_size (count);
for (unsigned int d = 0; d < cloud_out.get_channels_size (); d++)
cloud_out.channels[d].set_values_size (count);
}
} //laser_scan

372
test/projection_test.cpp Normal file
View File

@ -0,0 +1,372 @@
/*
* 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
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * 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
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <gtest/gtest.h>
#include <sys/time.h>
#include "laser_scan/laser_scan.h"
#include "sensor_msgs/PointCloud.h"
#include <math.h>
#include "angles/angles.h"
#include "rostest/permuter.h"
#define PROJECTION_TEST_RANGE_MIN (0.23)
#define PROJECTION_TEST_RANGE_MAX (40.0)
sensor_msgs::LaserScan build_constant_scan(double range, double intensity,
double ang_min, double ang_max, double ang_increment,
ros::Duration scan_time)
{
sensor_msgs::LaserScan scan;
scan.header.stamp = ros::Time::now();
scan.header.frame_id = "laser_frame";
scan.angle_min = ang_min;
scan.angle_max = ang_max;
scan.angle_increment = ang_increment;
scan.scan_time = scan_time.toSec();
scan.range_min = PROJECTION_TEST_RANGE_MIN;
scan.range_max = PROJECTION_TEST_RANGE_MAX;
unsigned int i = 0;
for(; ang_min + i * ang_increment < ang_max; i++)
{
scan.ranges.push_back(range);
scan.intensities.push_back(intensity);
}
scan.time_increment = scan_time.toSec()/(double)i;
return scan;
};
void test_getUnitVectors (float angle_min, float angle_max, float angle_increment, unsigned int length)
{
double tolerance = 1e-6;
laser_geometry::LaserProjection projector;
const boost::numeric::ublas::matrix<double> & mat = projector.getUnitVectors(angle_min, angle_max, angle_increment, length);
for (unsigned int i = 0; i < mat.size2(); i++)
{
EXPECT_NEAR(angles::shortest_angular_distance(atan2(mat(1,i), mat(0,i)),
angle_min + i * angle_increment),
0,
tolerance); // check expected angle
EXPECT_NEAR(1.0, mat(1,i)*mat(1,i) + mat(0,i)*mat(0,i), tolerance); //make sure normalized
}
}
TEST(laser_geometry, getUnitVectors)
{
double min_angle = -M_PI/2;
double max_angle = M_PI/2;
double angle_increment = M_PI/180;
std::vector<double> min_angles, max_angles, angle_increments;
rostest::Permuter permuter;
min_angles.push_back(-M_PI);
min_angles.push_back(-M_PI/1.5);
min_angles.push_back(-M_PI/2);
min_angles.push_back(-M_PI/4);
min_angles.push_back(-M_PI/8);
min_angles.push_back(M_PI);
min_angles.push_back(M_PI/1.5);
min_angles.push_back(M_PI/2);
min_angles.push_back(M_PI/4);
min_angles.push_back(M_PI/8);
permuter.addOptionSet(min_angles, &min_angle);
max_angles.push_back(M_PI);
max_angles.push_back(M_PI/1.5);
max_angles.push_back(M_PI/2);
max_angles.push_back(M_PI/4);
max_angles.push_back(M_PI/8);
max_angles.push_back(-M_PI);
max_angles.push_back(-M_PI/1.5);
max_angles.push_back(-M_PI/2);
max_angles.push_back(-M_PI/4);
max_angles.push_back(-M_PI/8);
permuter.addOptionSet(max_angles, &max_angle);
angle_increments.push_back(M_PI/180); // one degree
angle_increments.push_back(M_PI/360); // half degree
angle_increments.push_back(M_PI/720); // quarter degree
permuter.addOptionSet(angle_increments, &angle_increment);
while (permuter.step())
{
try
{
test_getUnitVectors(min_angle, max_angle, angle_increment, round((max_angle - min_angle)/ angle_increment));
test_getUnitVectors(min_angle, max_angle, angle_increment, (max_angle - min_angle)/ angle_increment);
test_getUnitVectors(min_angle, max_angle, angle_increment, (max_angle - min_angle)/ angle_increment + 1);
}
catch (std::runtime_error &ex)
{
if (min_angle < max_angle)
EXPECT_FALSE(ex.what());
}
}
}
TEST(laser_geometry, projectLaser)
{
double tolerance = 1e-5;
laser_geometry::LaserProjection projector;
double min_angle = -M_PI/2;
double max_angle = M_PI/2;
double angle_increment = M_PI/180;
double range = 1.0;
double intensity = 1.0;
ros::Duration scan_time = ros::Duration(1/40);
ros::Duration increment_time = ros::Duration(1/400);
std::vector<double> ranges, intensities, min_angles, max_angles, angle_increments;
std::vector<ros::Duration> increment_times, scan_times;
rostest::Permuter permuter;
ranges.push_back(-1.0);
ranges.push_back(1.0);
ranges.push_back(2.0);
ranges.push_back(3.0);
ranges.push_back(4.0);
ranges.push_back(5.0);
ranges.push_back(100.0);
permuter.addOptionSet(ranges, &range);
intensities.push_back(1.0);
intensities.push_back(2.0);
intensities.push_back(3.0);
intensities.push_back(4.0);
intensities.push_back(5.0);
permuter.addOptionSet(intensities, &intensity);
min_angles.push_back(-M_PI);
min_angles.push_back(-M_PI/1.5);
min_angles.push_back(-M_PI/2);
min_angles.push_back(-M_PI/4);
min_angles.push_back(-M_PI/8);
permuter.addOptionSet(min_angles, &min_angle);
max_angles.push_back(M_PI);
max_angles.push_back(M_PI/1.5);
max_angles.push_back(M_PI/2);
max_angles.push_back(M_PI/4);
max_angles.push_back(M_PI/8);
permuter.addOptionSet(max_angles, &max_angle);
angle_increments.push_back(M_PI/180); // one degree
angle_increments.push_back(M_PI/360); // half degree
angle_increments.push_back(M_PI/720); // quarter degree
permuter.addOptionSet(angle_increments, &angle_increment);
scan_times.push_back(ros::Duration(1/40));
scan_times.push_back(ros::Duration(1/20));
permuter.addOptionSet(scan_times, &scan_time);
while (permuter.step())
{
try
{
//printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec());
sensor_msgs::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time);
sensor_msgs::PointCloud cloud_out;
projector.projectLaser(scan, cloud_out, -1.0, false, laser_geometry::MASK_INDEX);
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
projector.projectLaser(scan, cloud_out, -1.0, false, laser_geometry::MASK_INTENSITY);
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
projector.projectLaser(scan, cloud_out, -1.0, false);
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2);
projector.projectLaser(scan, cloud_out, -1.0, false, laser_geometry::MASK_INTENSITY | laser_geometry::MASK_INDEX);
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2);
projector.projectLaser(scan, cloud_out, -1.0, false, laser_geometry::MASK_INTENSITY | laser_geometry::MASK_INDEX | laser_geometry::MASK_DISTANCE);
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)3);
projector.projectLaser(scan, cloud_out, -1.0, false, laser_geometry::MASK_INTENSITY | laser_geometry::MASK_INDEX | laser_geometry::MASK_DISTANCE | laser_geometry::MASK_TIMESTAMP);
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)4);
unsigned int valid_points = 0;
for (unsigned int i = 0; i < scan.ranges.size(); i++)
if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN)
valid_points ++;
EXPECT_EQ(valid_points, cloud_out.get_points_size());
for (unsigned int i = 0; i < cloud_out.points.size(); i++)
{
EXPECT_NEAR(cloud_out.points[i].x , scan.ranges[i] * cos(scan.angle_min + i * scan.angle_increment), tolerance);
EXPECT_NEAR(cloud_out.points[i].y , scan.ranges[i] * sin(scan.angle_min + i * scan.angle_increment), tolerance);
EXPECT_NEAR(cloud_out.points[i].z, 0, tolerance);
EXPECT_NEAR(cloud_out.channels[0].values[i], scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order
EXPECT_NEAR(cloud_out.channels[1].values[i], i, tolerance);//index
EXPECT_NEAR(cloud_out.channels[2].values[i], scan.ranges[i], tolerance);//ranges
EXPECT_NEAR(cloud_out.channels[3].values[i], (float)i * scan.time_increment, tolerance);//timestamps
};
}
catch (std::runtime_error &ex)
{
if (min_angle < max_angle)
EXPECT_FALSE(ex.what());
}
}
}
TEST(laser_geometry, transformLaserScanToPointCloud)
{
tf::Transformer tf;
double tolerance = 1e-5;
laser_geometry::LaserProjection projector;
double min_angle = -M_PI/2;
double max_angle = M_PI/2;
double angle_increment = M_PI/180;
double range = 1.0;
double intensity = 1.0;
ros::Duration scan_time = ros::Duration(1/40);
ros::Duration increment_time = ros::Duration(1/400);
std::vector<double> ranges, intensities, min_angles, max_angles, angle_increments;
std::vector<ros::Duration> increment_times, scan_times;
rostest::Permuter permuter;
ranges.push_back(-1.0);
ranges.push_back(1.0);
ranges.push_back(2.0);
ranges.push_back(3.0);
ranges.push_back(4.0);
ranges.push_back(5.0);
ranges.push_back(100.0);
permuter.addOptionSet(ranges, &range);
intensities.push_back(1.0);
intensities.push_back(2.0);
intensities.push_back(3.0);
intensities.push_back(4.0);
intensities.push_back(5.0);
permuter.addOptionSet(intensities, &intensity);
min_angles.push_back(-M_PI);
min_angles.push_back(-M_PI/1.5);
min_angles.push_back(-M_PI/2);
min_angles.push_back(-M_PI/4);
min_angles.push_back(-M_PI/8);
max_angles.push_back(M_PI);
max_angles.push_back(M_PI/1.5);
max_angles.push_back(M_PI/2);
max_angles.push_back(M_PI/4);
max_angles.push_back(M_PI/8);
permuter.addOptionSet(min_angles, &min_angle);
permuter.addOptionSet(max_angles, &max_angle);
angle_increments.push_back(M_PI/180); // one degree
angle_increments.push_back(M_PI/360); // half degree
angle_increments.push_back(M_PI/720); // quarter degree
permuter.addOptionSet(angle_increments, &angle_increment);
scan_times.push_back(ros::Duration(1/40));
scan_times.push_back(ros::Duration(1/20));
permuter.addOptionSet(scan_times, &scan_time);
while (permuter.step())
{
try
{
//printf("%f %f %f %f %f %f\n", range, intensity, min_angle, max_angle, angle_increment, scan_time.toSec());
sensor_msgs::LaserScan scan = build_constant_scan(range, intensity, min_angle, max_angle, angle_increment, scan_time);
scan.header.frame_id = "laser_frame";
sensor_msgs::PointCloud cloud_out;
projector.transformLaserScanToPointCloud(scan.header.frame_id, cloud_out, scan, tf, laser_geometry::MASK_INDEX);
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
projector.transformLaserScanToPointCloud(scan.header.frame_id, cloud_out, scan, tf, laser_geometry::MASK_INTENSITY);
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)1);
projector.transformLaserScanToPointCloud(scan.header.frame_id, cloud_out, scan, tf);
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2);
projector.transformLaserScanToPointCloud(scan.header.frame_id, cloud_out, scan, tf, laser_geometry::MASK_INTENSITY | laser_geometry::MASK_INDEX);
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)2);
projector.transformLaserScanToPointCloud(scan.header.frame_id, cloud_out, scan, tf, laser_geometry::MASK_INTENSITY | laser_geometry::MASK_INDEX | laser_geometry::MASK_DISTANCE);
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)3);
projector.transformLaserScanToPointCloud(scan.header.frame_id, cloud_out, scan, tf, laser_geometry::MASK_INTENSITY | laser_geometry::MASK_INDEX | laser_geometry::MASK_DISTANCE | laser_geometry::MASK_TIMESTAMP);
EXPECT_EQ(cloud_out.channels.size(), (unsigned int)4);
unsigned int valid_points = 0;
for (unsigned int i = 0; i < scan.ranges.size(); i++)
if (scan.ranges[i] <= PROJECTION_TEST_RANGE_MAX && scan.ranges[i] >= PROJECTION_TEST_RANGE_MIN)
valid_points ++;
EXPECT_EQ(valid_points, cloud_out.get_points_size());
for (unsigned int i = 0; i < cloud_out.points.size(); i++)
{
EXPECT_NEAR(cloud_out.points[i].x , scan.ranges[i] * cos(scan.angle_min + i * scan.angle_increment), tolerance);
EXPECT_NEAR(cloud_out.points[i].y , scan.ranges[i] * sin(scan.angle_min + i * scan.angle_increment), tolerance);
EXPECT_NEAR(cloud_out.points[i].z, 0, tolerance);
EXPECT_NEAR(cloud_out.channels[0].values[i], scan.intensities[i], tolerance);//intensity \todo determine this by lookup not hard coded order
EXPECT_NEAR(cloud_out.channels[1].values[i], i, tolerance);//index
EXPECT_NEAR(cloud_out.channels[2].values[i], scan.ranges[i], tolerance);//ranges
EXPECT_NEAR(cloud_out.channels[3].values[i], (float)i * scan.time_increment, tolerance);//timestamps
};
}
catch (std::runtime_error &ex)
{
if (min_angle < max_angle)
EXPECT_FALSE(ex.what());
}
}
}
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}