From 58e93bb190cc024d4a3f68427a50092435a4ed5e Mon Sep 17 00:00:00 2001 From: Jeremy Leibs Date: Tue, 1 Sep 2009 08:18:14 +0000 Subject: [PATCH] Merging in remaining missing contents for laser_piple that svn ignored on the first merge. git-svn-id: https://code.ros.org/svn/ros-pkg/pkg/trunk/stacks/laser_pipeline@23510 eb33c2ac-9c88-4c90-87e0-44a10359b0c3 --- CMakeLists.txt | 16 + Makefile | 1 + include/laser_geometry/laser_geometry.h | 99 ++++++ include/laser_geometry/laser_processor.h | 145 +++++++++ mainpage.dox | 29 ++ manifest.xml | 22 ++ src/laser_processor.cpp | 288 ++++++++++++++++++ src/laser_scan.cc | 337 ++++++++++++++++++++ test/projection_test.cpp | 372 +++++++++++++++++++++++ 9 files changed, 1309 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 Makefile create mode 100644 include/laser_geometry/laser_geometry.h create mode 100644 include/laser_geometry/laser_processor.h create mode 100644 mainpage.dox create mode 100644 manifest.xml create mode 100644 src/laser_processor.cpp create mode 100644 src/laser_scan.cc create mode 100644 test/projection_test.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..f330578 --- /dev/null +++ b/CMakeLists.txt @@ -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) + diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..bbd3fc6 --- /dev/null +++ b/Makefile @@ -0,0 +1 @@ +include $(shell rospack find mk)/cmake.mk diff --git a/include/laser_geometry/laser_geometry.h b/include/laser_geometry/laser_geometry.h new file mode 100644 index 0000000..bb1774b --- /dev/null +++ b/include/laser_geometry/laser_geometry.h @@ -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 +#include +#include + +#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& getUnitVectors(float angle_max, float angle_min, float angle_increment, unsigned int length); + + private: + + ///The map of pointers to stored values + std::map* > unit_vector_map_; + + }; + +} +#endif //LASER_SCAN_UTILS_LASERSCAN_H diff --git a/include/laser_geometry/laser_processor.h b/include/laser_geometry/laser_processor.h new file mode 100644 index 0000000..c17f2b6 --- /dev/null +++ b/include/laser_geometry/laser_processor.h @@ -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 +#include +#include "ros/node.h" +#include "sensor_msgs/LaserScan.h" +#include "sensor_msgs/PointCloud.h" +#include "geometry_msgs/Point.h" + +#include +#include +#include +#include +#include +#include + +#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 + { + 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 clusters_; + sensor_msgs::LaserScan scan_; + + public: + + std::list& 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 diff --git a/mainpage.dox b/mainpage.dox new file mode 100644 index 0000000..1a8dc22 --- /dev/null +++ b/mainpage.dox @@ -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 + +*/ diff --git a/manifest.xml b/manifest.xml new file mode 100644 index 0000000..6ca721c --- /dev/null +++ b/manifest.xml @@ -0,0 +1,22 @@ + + + +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. + + +Tully Foote +BSD + +http://ros.org/wiki/laser_scan + + + + + + + + diff --git a/src/laser_processor.cpp b/src/laser_processor.cpp new file mode 100644 index 0000000..b788d82 --- /dev/null +++ b/src/laser_processor.cpp @@ -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 + +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::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::iterator c = clusters_.begin(); + c != clusters_.end(); + c++) + delete (*c); +} + +void +ScanProcessor::removeLessThan(uint32_t num) +{ + list::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 tmp_clusters; + + list::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_queue; + sample_queue.push_back(*s_first); + + (*c_iter)->erase(s_first); + + // Grow until we get to the end of the queue + list::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()); +} diff --git a/src/laser_scan.cc b/src/laser_scan.cc new file mode 100644 index 0000000..3b900dd --- /dev/null +++ b/src/laser_scan.cc @@ -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 + +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 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 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& 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 <* >::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 * tempPtr = new boost::numeric::ublas::matrix(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*>::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 pointIn; + tf::Stamped 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 start_transform ; + tf::Stamped end_transform ; + tf::Stamped 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(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 diff --git a/test/projection_test.cpp b/test/projection_test.cpp new file mode 100644 index 0000000..67f1923 --- /dev/null +++ b/test/projection_test.cpp @@ -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 +#include + +#include "laser_scan/laser_scan.h" +#include "sensor_msgs/PointCloud.h" +#include + + +#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 & 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 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 ranges, intensities, min_angles, max_angles, angle_increments; + std::vector 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 ranges, intensities, min_angles, max_angles, angle_increments; + std::vector 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(); +}