Removing laser_scan_geometry files from the package
git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@35239 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
This commit is contained in:
Eitan Marder-Eppstein
parent
063550e67c
commit
e5a51cf5c3
|
|
@ -7,7 +7,6 @@ rosbuild_add_boost_directories()
|
||||||
|
|
||||||
rosbuild_add_library(laser_geometry src/laser_geometry.cpp )
|
rosbuild_add_library(laser_geometry src/laser_geometry.cpp )
|
||||||
rosbuild_link_boost(laser_geometry thread)
|
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)
|
rosbuild_add_gtest(test/projection_test test/projection_test.cpp)
|
||||||
target_link_libraries (test/projection_test laser_geometry)
|
target_link_libraries (test/projection_test laser_geometry)
|
||||||
|
|
|
||||||
|
|
@ -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 <Eigen3/Core>
|
|
||||||
#include <sensor_msgs/LaserScan.h>
|
|
||||||
#include <sensor_msgs/PointCloud2.h>
|
|
||||||
#include <tf/tf.h>
|
|
||||||
|
|
||||||
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
|
|
||||||
|
|
@ -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<float>::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<float>::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_);
|
|
||||||
}
|
|
||||||
|
|
||||||
Loading…
Reference in New Issue
Block a user