diff --git a/CMakeLists.txt b/CMakeLists.txt index 75303c7..8cd44b7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,7 +7,6 @@ rosbuild_add_boost_directories() rosbuild_add_library(laser_geometry src/laser_geometry.cpp ) rosbuild_link_boost(laser_geometry thread) -#rosbuild_add_library (laser_scan_geometry src/laser_scan_geometry.cpp) rosbuild_add_gtest(test/projection_test test/projection_test.cpp) target_link_libraries (test/projection_test laser_geometry) diff --git a/include/laser_geometry/laser_scan_geometry.h b/include/laser_geometry/laser_scan_geometry.h deleted file mode 100644 index 34ae801..0000000 --- a/include/laser_geometry/laser_scan_geometry.h +++ /dev/null @@ -1,90 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2010, 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 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. - * - * $Id: point_types.h 33238 2010-03-11 00:46:58Z rusu $ - * - */ - -#ifndef LASER_SCAN_GEOMETRY_H -#define LASER_SCAN_GEOMETRY_H - -#include -#include -#include -#include - -namespace laser_scan_geometry -{ - // NOTE: invalid scan errors (will be present in LaserScan.msg in D-Turtle) - const float LASER_SCAN_INVALID = -1.0; - const float LASER_SCAN_MIN_RANGE = -2.0; - const float LASER_SCAN_MAX_RANGE = -3.0; - - /** \brief Project a single laser scan from a linear array into a 3D point cloud. - * The generated cloud will be in the same frame as the original laser scan. - * \param scan_in The input laser scan - * \param cloud_out The output point cloud - */ - void projectLaser (const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud2 &cloud_out, Eigen3::ArrayXXd &co_sine_map); - - /** \brief Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud in target frame - * Transform a single laser scan from a linear array into a 3D - * point cloud, accounting for movement of the laser over the - * course of the scan. In order for this transform to be - * meaningful at a single point in time, the target_frame must - * be a fixed reference frame. See the tf documentation for - * more information on fixed frames. - * - * \param target_frame The frame of the resulting point cloud - * \param scan_in The input laser scan - * \param cloud_out The output point cloud - * \param tf a tf::Transformer object to use to perform the transform - */ - void transformLaserScanToPointCloud (const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, tf::Transformer &tf, sensor_msgs::PointCloud2 &cloud_out, Eigen3::ArrayXXd &co_sine_map); - - class LaserProjection - { - public: - - void projectLaser (const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud2 &cloud_out); - - void transformLaserScanToPointCloud (const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, tf::Transformer &tf, sensor_msgs::PointCloud2 &cloud_out); - - private: - - Eigen3::ArrayXXd co_sine_map_; - boost::mutex guv_mutex_; - }; -} - -#endif diff --git a/src/laser_scan_geometry.cpp b/src/laser_scan_geometry.cpp deleted file mode 100644 index 38331ca..0000000 --- a/src/laser_scan_geometry.cpp +++ /dev/null @@ -1,277 +0,0 @@ -/* - * 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_scan_geometry.h" - -void - laser_scan_geometry::projectLaser (const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud2 &cloud_out, - Eigen3::ArrayXXd &co_sine_map) -{ - bool store_intensity = true; // By default, we assume we have intensity data - - size_t n_pts = scan_in.ranges.size (); - - Eigen3::ArrayXXd ranges (n_pts, 2), output (n_pts, 2); - - // Get the ranges into Eigen format - for (size_t i = 0; i < n_pts; ++i) - { - ranges (i, 0) = (double) scan_in.ranges[i]; - ranges (i, 1) = (double) scan_in.ranges[i]; - } - - // Check if we have a precomputed co_sine_map - if (co_sine_map.rows () != (int)n_pts) - { - ROS_DEBUG ("[projectLaser] No precomputed map given. Computing one."); - co_sine_map = Eigen3::ArrayXXd (n_pts, 2); - // Spherical->Cartesian projection - for (size_t i = 0; i < n_pts; ++i) - { - co_sine_map (i, 0) = cos (scan_in.angle_min + (double) i * scan_in.angle_increment); - co_sine_map (i, 1) = sin (scan_in.angle_min + (double) i * scan_in.angle_increment); - } - } - - output = ranges * co_sine_map; - - // Set the output cloud accordingly - cloud_out.header = scan_in.header; - cloud_out.height = 1; - cloud_out.width = scan_in.ranges.size (); - cloud_out.fields.resize (9); - cloud_out.fields[0].name = "x"; - cloud_out.fields[1].name = "y"; - cloud_out.fields[2].name = "z"; - if (scan_in.intensities.size () == scan_in.ranges.size ()) - { - cloud_out.fields[3].name = "intensity"; - cloud_out.fields[4].name = "distance"; - cloud_out.fields[5].name = "timestamp"; - cloud_out.fields[6].name = "vp_x"; - cloud_out.fields[7].name = "vp_y"; - cloud_out.fields[8].name = "vp_z"; - } - else - { - cloud_out.fields.resize (8); - cloud_out.fields[3].name = "distance"; - cloud_out.fields[4].name = "timestamp"; - cloud_out.fields[5].name = "vp_x"; - cloud_out.fields[6].name = "vp_y"; - cloud_out.fields[7].name = "vp_z"; - store_intensity = false; - } - - // Set all the fields types accordingly - int offset = 0; - for (size_t s = 0; s < cloud_out.fields.size (); ++s, offset += 4) - { - cloud_out.fields[s].offset = offset; - cloud_out.fields[s].count = 1; - cloud_out.fields[s].datatype = sensor_msgs::PointField::FLOAT32; - } - - cloud_out.point_step = offset; - cloud_out.row_step = cloud_out.point_step * cloud_out.width; - cloud_out.data.resize (cloud_out.row_step * cloud_out.height); - cloud_out.is_dense = true; - - float bad_point = std::numeric_limits::quiet_NaN (); - - for (size_t i = 0; i < n_pts; ++i) - { - int d = 0; - float *pstep = (float*)&cloud_out.data[i * cloud_out.point_step]; - - // Copy XYZ - pstep[d++] = output (i, 0); - pstep[d++] = output (i, 1); - pstep[d++] = 0; - - // Copy intensity - if (store_intensity) - pstep[d++] = scan_in.intensities[i]; - - // Copy distance - pstep[d++] = scan_in.ranges[i]; - - // Copy timestamp - pstep[d++] = i * scan_in.time_increment; - - // Copy viewpoint (0, 0, 0) - pstep[d++] = 0; - pstep[d++] = 0; - pstep[d++] = 0; - - // Invalid measurement? - if (scan_in.ranges[i] >= scan_in.range_max || scan_in.ranges[i] <= scan_in.range_min) - { - if (scan_in.ranges[i] != LASER_SCAN_MAX_RANGE) - { - for (size_t s = 0; s < cloud_out.fields.size (); ++s) - pstep[s] = bad_point; - } - else - { - // Kind of nasty thing: - // We keep the oringinal point information for max ranges but set x to NAN to mark the point as invalid. - // Since we still might need the x value we store it in the distance field - pstep[0] = bad_point; // X -> NAN to mark a bad point - pstep[1] = co_sine_map (i, 1); // Y - pstep[2] = 0; // Z - - if (store_intensity) - { - pstep[3] = bad_point; // Intensity -> NAN to mark a bad point - pstep[4] = co_sine_map (i, 0); // Distance -> Misused to store the originnal X - } - else - pstep[3] = co_sine_map (i, 0); // Distance -> Misused to store the originnal X - } - } - } -} - -void - laser_scan_geometry::transformLaserScanToPointCloud (const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, - tf::Transformer &tf, sensor_msgs::PointCloud2 &cloud_out, Eigen3::ArrayXXd &co_sine_map) -{ - bool store_intensity = true; // By default, we assume we have intensity data - if (scan_in.intensities.size () != scan_in.ranges.size ()) - store_intensity = false; - - projectLaser (scan_in, cloud_out, co_sine_map); - - cloud_out.header.frame_id = target_frame; - - // 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.ranges.size () * scan_in.time_increment); - - tf::StampedTransform start_transform, end_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); - - double ranges_norm = 1 / ((double) scan_in.ranges.size () - 1.0); - - int distance_idx = 4; - int vp_idx_x = 6, vp_idx_y = 7, vp_idx_z = 8; - // Special case when intensity data doesn't exist - if (!store_intensity) - { - distance_idx -= 1; - vp_idx_x -= 1; vp_idx_y -= 1; vp_idx_z -= 1; - } - - for (size_t i = 0; i < scan_in.ranges.size (); ++i) - { - // Apply the transform to the current point - float *pstep = (float*)&cloud_out.data[i * cloud_out.point_step + 0]; - - // If the input point is NaN, perform no transformation - bool max_range_point = false; - if (!std::isfinite (pstep[0]) || !std::isfinite (pstep[1]) || !std::isfinite (pstep[2])) - { - // Check for special case max range, where x==NAN, but distance=originalX - if (std::isfinite (pstep[distance_idx])) - { - // copy the x-value back from range - pstep[0] = pstep[distance_idx]; - max_range_point = true; - } - else - continue; - } - - // Assume constant motion during the laser-scan, and use slerp to compute intermediate transforms - btScalar ratio = i * ranges_norm; - - //! \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)); - - btVector3 point_in (pstep[0], pstep[1], pstep[2]); - btVector3 point_out = cur_transform * point_in; - - // Copy transformed point into cloud - pstep[0] = point_out.x (); - pstep[1] = point_out.y (); - pstep[2] = point_out.z (); - - // Convert the viewpoint as well - point_in = btVector3 (pstep[vp_idx_x], pstep[vp_idx_y], pstep[vp_idx_z]); - point_out = cur_transform * point_in; - - // Copy transformed point into cloud - pstep[vp_idx_x] = point_out.x (); - pstep[vp_idx_y] = point_out.y (); - pstep[vp_idx_z] = point_out.z (); - - // Max range point? Make it invalid again, and set the transformed X to distance - if (max_range_point) - { - pstep[distance_idx] = pstep[0]; - pstep[0] = std::numeric_limits::quiet_NaN (); - } - } -} - -void - laser_scan_geometry::LaserProjection::projectLaser (const sensor_msgs::LaserScan& scan_in, - sensor_msgs::PointCloud2 &cloud_out, - double range_cutoff = -1.0, - int channel_options = channel_option::Default) -{ - boost::mutex::scoped_lock guv_lock (guv_mutex_); - laser_scan_geometry::projectLaser (scan_in, cloud_out, co_sine_map_); -} - -void - laser_scan_geometry::LaserProjection::transformLaserScanToPointCloud (const std::string &target_frame, - const sensor_msgs::LaserScan &scan_in, - sensor_msgs::PointCloud2 &cloud_out, - tf::Transformer &tf, - double range_cutoff, - int channel_options = channel_option::Default) -{ - laser_scan_geometry::transformLaserScanToPointCloud (target_frame, scan_in, tf, cloud_out, co_sine_map_); -} -