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

Adding support for PointCloud2 to the laser projector, removing a bunch of deprecated functions, and fixing a ton of warnings

Browse Source 
git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@35237 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
This commit is contained in:
Eitan Marder-Eppstein 2011-01-14 08:05:45 +00:00
parent 9287eff456
commit 3f5ba8926a
5 changed files with 509 additions and 97 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -7,7 +7,7 @@ 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_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)

170
include/laser_geometry/laser_geometry.h
View File

@ -43,14 +43,16 @@
#include "sensor_msgs/PointCloud.h"
#include "sensor_msgs/PointCloud.h"
#if defined(__GNUC__)
#define DEPRECATED __attribute__((deprecated))
#else
#define DEPRECATED
#endif
#include <Eigen3/Core>
#include <sensor_msgs/PointCloud2.h>
namespace laser_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;
namespace channel_option
{
//! Enumerated output channels options.
@ -65,17 +67,11 @@ namespace laser_geometry
Index = 0x02, //!< Enable "index" channel
Distance = 0x04, //!< Enable "distances" channel
Timestamp = 0x08, //!< Enable "stamps" channel
Viewpoint = 0x16, //!< Enable "viewpoint" channel
Default = (Intensity | Index) //!< Enable "intensities" and "stamps" channels
};
}
DEPRECATED const int MASK_INTENSITY = channel_option::Intensity;
DEPRECATED const int MASK_INDEX = channel_option::Index;
DEPRECATED const int MASK_DISTANCE = channel_option::Distance;
DEPRECATED const int MASK_TIMESTAMP = channel_option::Timestamp;
DEPRECATED const int DEFAULT_MASK = (channel_option::Intensity | channel_option::Index);
//! \brief A Class to Project Laser Scan
/*!
* This class will project laser scans into point clouds. It caches
@ -131,6 +127,61 @@ namespace laser_geometry
return projectLaser_ (scan_in, cloud_out, range_cutoff, false, channel_options);
}
//! Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2
/*!
* 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
* \param range_cutoff An additional range cutoff which can be
* applied which is more limiting than max_range in the scan.
* \param channel_option An OR'd set of channels to include.
* Options include: channel_option::Default,
* channel_option::Intensity, channel_option::Index,
* channel_option::Distance, channel_option::Timestamp.
*/
void projectLaser (const sensor_msgs::LaserScan& scan_in,
sensor_msgs::PointCloud2 &cloud_out,
double range_cutoff = -1.0,
int channel_options = channel_option::Default)
{
projectLaser_(scan_in, cloud_out, co_sine_map_, range_cutoff, channel_options);
}
//! 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
* \param range_cutoff An additional range cutoff which can be
* applied which is more limiting than max_range in the scan.
* \param channel_option An OR'd set of channels to include.
* Options include: channel_option::Default,
* channel_option::Intensity, channel_option::Index,
* channel_option::Distance, channel_option::Timestamp.
*/
void transformLaserScanToPointCloud (const std::string& target_frame,
const sensor_msgs::LaserScan& scan_in,
sensor_msgs::PointCloud& cloud_out,
tf::Transformer& tf,
double range_cutoff,
int channel_options = channel_option::Default)
{
return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, range_cutoff, channel_options, false);
}
//! 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
@ -156,51 +207,68 @@ namespace laser_geometry
tf::Transformer& tf,
int channel_options = channel_option::Default)
{
return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, channel_options, false);
return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, -1.0, channel_options, false);
}
//! Deprecated version of projectLaser
//! Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2 in target frame
/*!
* - The preservative argument has been removed.
* 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
* \param range_cutoff An additional range cutoff which can be
* applied which is more limiting than max_range in the scan.
* \param channel_option An OR'd set of channels to include.
* Options include: channel_option::Default,
* channel_option::Intensity, channel_option::Index,
* channel_option::Distance, channel_option::Timestamp.
*/
DEPRECATED
void projectLaser (const sensor_msgs::LaserScan& scan_in,
sensor_msgs::PointCloud & cloud_out,
void transformLaserScanToPointCloud2(const std::string &target_frame,
const sensor_msgs::LaserScan &scan_in,
sensor_msgs::PointCloud2 &cloud_out,
tf::Transformer &tf,
double range_cutoff,
bool preservative,
int channel_options = channel_option::Default)
{
return projectLaser_ (scan_in, cloud_out, range_cutoff, preservative, channel_options);
transformLaserScanToPointCloud2_(target_frame, scan_in, cloud_out, tf, co_sine_map_, range_cutoff, channel_options);
}
//! Deprecated version of projectLaser
//! Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2 in target frame
/*!
* - The preservative argument has been removed.
* - The ordering of cloud_out and scan_in have been reversed
* 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
* \param channel_option An OR'd set of channels to include.
* Options include: channel_option::Default,
* channel_option::Intensity, channel_option::Index,
* channel_option::Distance, channel_option::Timestamp.
*/
DEPRECATED
void transformLaserScanToPointCloud (const std::string& target_frame,
sensor_msgs::PointCloud & cloud_out,
void transformLaserScanToPointCloud2(const std::string &target_frame,
const sensor_msgs::LaserScan &scan_in,
sensor_msgs::PointCloud2 &cloud_out,
tf::Transformer &tf,
int channel_options = channel_option::Default,
bool preservative = false)
int channel_options = channel_option::Default)
{
return transformLaserScanToPointCloud_ (target_frame, cloud_out, scan_in, tf, channel_options, preservative);
transformLaserScanToPointCloud2(target_frame, scan_in, cloud_out, tf, -1.0, channel_options);
}
//! Deprecated version of getUnitVectors
/*!
* - This is now assumed to be an internal, protected API. Do not call externally.
*/
DEPRECATED
const boost::numeric::ublas::matrix<double>& getUnitVectors(float angle_min,
float angle_max,
float angle_increment,
unsigned int length)
{
return getUnitVectors_(angle_min, angle_max, angle_increment, length);
}
protected:
@ -224,22 +292,38 @@ namespace laser_geometry
bool preservative,
int channel_options);
//! Internal hidden representation of projectLaser
void projectLaser_ (const sensor_msgs::LaserScan& scan_in,
sensor_msgs::PointCloud2 &cloud_out,
Eigen3::ArrayXXd &co_sine_map,
double range_cutoff,
int channel_options);
//! Internal hidden representation of transformLaserScanToPointCloud
void transformLaserScanToPointCloud_ (const std::string& target_frame,
sensor_msgs::PointCloud& cloud_out,
const sensor_msgs::LaserScan& scan_in,
tf::Transformer & tf,
double range_cutoff,
int channel_options,
bool preservative);
//! Internal hidden representation of transformLaserScanToPointCloud2
void transformLaserScanToPointCloud2_ (const std::string &target_frame,
const sensor_msgs::LaserScan &scan_in,
sensor_msgs::PointCloud2 &cloud_out,
tf::Transformer &tf,
Eigen3::ArrayXXd &co_sine_map,
double range_cutoff,
int channel_options);
//! Internal map of pointers to stored values
std::map<std::string,boost::numeric::ublas::matrix<double>* > unit_vector_map_;
Eigen3::ArrayXXd co_sine_map_;
boost::mutex guv_mutex_;
};
}
#undef DEPRECATED
#endif //LASER_SCAN_UTILS_LASERSCAN_H

1
manifest.xml
View File

@ -15,6 +15,7 @@ for the skew resulting from moving robots or tilting laser scanners.
<depend package="roscpp"/>
<depend package="tf"/>
<depend package="angles"/>
<depend package="eigen3"/>
<export>
<cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -llaser_geometry"/>
</export>

413
src/laser_geometry.cpp
View File

@ -37,10 +37,10 @@ namespace laser_geometry
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());
boost::numeric::ublas::matrix<double> ranges(2, scan_in.ranges.size());
// Fill the ranges matrix
for (unsigned int index = 0; index < scan_in.get_ranges_size(); index++)
for (unsigned int index = 0; index < scan_in.ranges.size(); index++)
{
ranges(0,index) = (double) scan_in.ranges[index];
ranges(1,index) = (double) scan_in.ranges[index];
@ -49,53 +49,53 @@ namespace laser_geometry
//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()));
boost::numeric::ublas::matrix<double> output = element_prod(ranges, getUnitVectors_(scan_in.angle_min, scan_in.angle_max, scan_in.angle_increment, scan_in.ranges.size()));
//Stuff the output cloud
cloud_out.header = scan_in.header;
cloud_out.set_points_size (scan_in.get_ranges_size ());
cloud_out.points.resize (scan_in.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);
cloud_out.channels.resize(0);
// Check if the intensity bit is set
if ((mask & channel_option::Intensity) && scan_in.get_intensities_size () > 0)
if ((mask & channel_option::Intensity) && scan_in.intensities.size() > 0)
{
int chan_size = cloud_out.get_channels_size ();
cloud_out.set_channels_size (chan_size + 1);
int chan_size = cloud_out.channels.size();
cloud_out.channels.resize (chan_size + 1);
cloud_out.channels[0].name = "intensities";
cloud_out.channels[0].set_values_size (scan_in.get_intensities_size ());
cloud_out.channels[0].values.resize (scan_in.intensities.size());
idx_intensity = 0;
}
// Check if the index bit is set
if (mask & channel_option::Index)
{
int chan_size = cloud_out.get_channels_size ();
cloud_out.set_channels_size (chan_size +1);
int chan_size = cloud_out.channels.size();
cloud_out.channels.resize (chan_size +1);
cloud_out.channels[chan_size].name = "index";
cloud_out.channels[chan_size].set_values_size (scan_in.get_ranges_size ());
cloud_out.channels[chan_size].values.resize (scan_in.ranges.size());
idx_index = chan_size;
}
// Check if the distance bit is set
if (mask & channel_option::Distance)
{
int chan_size = cloud_out.get_channels_size ();
cloud_out.set_channels_size (chan_size + 1);
int chan_size = cloud_out.channels.size();
cloud_out.channels.resize (chan_size + 1);
cloud_out.channels[chan_size].name = "distances";
cloud_out.channels[chan_size].set_values_size (scan_in.get_ranges_size ());
cloud_out.channels[chan_size].values.resize (scan_in.ranges.size());
idx_distance = chan_size;
}
if (mask & channel_option::Timestamp)
{
int chan_size = cloud_out.get_channels_size ();
cloud_out.set_channels_size (chan_size + 1);
int chan_size = cloud_out.channels.size();
cloud_out.channels.resize (chan_size + 1);
cloud_out.channels[chan_size].name = "stamps";
cloud_out.channels[chan_size].set_values_size (scan_in.get_ranges_size ());
cloud_out.channels[chan_size].values.resize (scan_in.ranges.size());
idx_timestamp = chan_size;
}
@ -105,7 +105,7 @@ namespace laser_geometry
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++)
for (unsigned int index = 0; index< scan_in.ranges.size(); index++)
{
if (preservative || ((ranges(0,index) < range_cutoff) && (ranges(0,index) >= scan_in.range_min))) //if valid or preservative
{
@ -129,7 +129,7 @@ namespace laser_geometry
cloud_out.channels[idx_distance].values[count] = ranges (0, index);
// Save intensities channel
if (scan_in.get_intensities_size() >= index)
if (scan_in.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];
@ -145,9 +145,9 @@ namespace laser_geometry
//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);
cloud_out.points.resize (count);
for (unsigned int d = 0; d < cloud_out.channels.size(); d++)
cloud_out.channels[d].values.resize(count);
};
const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(double angle_min, double angle_max, double angle_increment, unsigned int length)
@ -189,54 +189,54 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
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)
tf::Transformer& tf, double range_cutoff, 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());
cloud_out.points.resize (scan_in.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);
cloud_out.channels.resize(0);
// Check if the intensity bit is set
if ((mask & channel_option::Intensity) && scan_in.get_intensities_size () > 0)
if ((mask & channel_option::Intensity) && scan_in.intensities.size() > 0)
{
int chan_size = cloud_out.get_channels_size ();
cloud_out.set_channels_size (chan_size + 1);
int chan_size = cloud_out.channels.size();
cloud_out.channels.resize (chan_size + 1);
cloud_out.channels[0].name = "intensities";
cloud_out.channels[0].set_values_size (scan_in.get_intensities_size ());
cloud_out.channels[0].values.resize (scan_in.intensities.size());
idx_intensity = 0;
}
// Check if the index bit is set
if (mask & channel_option::Index)
{
int chan_size = cloud_out.get_channels_size ();
cloud_out.set_channels_size (chan_size + 1);
int chan_size = cloud_out.channels.size();
cloud_out.channels.resize (chan_size + 1);
cloud_out.channels[chan_size].name = "index";
cloud_out.channels[chan_size].set_values_size (scan_in.get_ranges_size ());
cloud_out.channels[chan_size].values.resize (scan_in.ranges.size());
idx_index = chan_size;
}
// Check if the distance bit is set
if (mask & channel_option::Distance)
{
int chan_size = cloud_out.get_channels_size ();
cloud_out.set_channels_size (chan_size + 1);
int chan_size = cloud_out.channels.size();
cloud_out.channels.resize (chan_size + 1);
cloud_out.channels[chan_size].name = "distances";
cloud_out.channels[chan_size].set_values_size (scan_in.get_ranges_size ());
cloud_out.channels[chan_size].values.resize (scan_in.ranges.size());
idx_distance = chan_size;
}
if (mask & channel_option::Timestamp)
{
int chan_size = cloud_out.get_channels_size ();
cloud_out.set_channels_size (chan_size + 1);
int chan_size = cloud_out.channels.size();
cloud_out.channels.resize (chan_size + 1);
cloud_out.channels[chan_size].name = "stamps";
cloud_out.channels[chan_size].set_values_size (scan_in.get_ranges_size ());
cloud_out.channels[chan_size].values.resize (scan_in.ranges.size());
idx_timestamp = chan_size;
}
@ -252,7 +252,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
// 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) ;
ros::Time end_time = scan_in.header.stamp + ros::Duration().fromSec(scan_in.ranges.size()*scan_in.time_increment) ;
tf::StampedTransform start_transform ;
tf::StampedTransform end_transform ;
@ -261,10 +261,15 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
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) ;
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 i = 0; i < scan_in.get_ranges_size(); i++)
for (unsigned int i = 0; i < scan_in.ranges.size(); i++)
{
if (preservative || (scan_in.ranges[i] < scan_in.range_max && scan_in.ranges[i] > scan_in.range_min)) //only when valid
if (preservative || (scan_in.ranges[i] < range_cutoff && scan_in.ranges[i] > scan_in.range_min)) //only when valid
{
// Looking up transforms in tree is too expensive. Need more optimized way
/*
@ -275,7 +280,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
*/
// 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) ;
btScalar ratio = i / ( (double) scan_in.ranges.size() - 1.0) ;
//! \todo Make a function that performs both the slerp and linear interpolation needed to interpolate a Full Transform (Quaternion + Vector)
@ -309,7 +314,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
if (idx_distance != -1)
cloud_out.channels[idx_distance].values[count] = scan_in.ranges[i];
if (scan_in.get_intensities_size() >= i)
if (scan_in.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];
@ -323,9 +328,323 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
}
//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);
cloud_out.points.resize (count);
for (unsigned int d = 0; d < cloud_out.channels.size(); d++)
cloud_out.channels[d].values.resize (count);
}
void LaserProjection::projectLaser_ (const sensor_msgs::LaserScan& scan_in,
sensor_msgs::PointCloud2 &cloud_out,
Eigen3::ArrayXXd &co_sine_map,
double range_cutoff,
int channel_options)
{
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 (3);
cloud_out.fields[0].name = "x";
cloud_out.fields[1].name = "y";
cloud_out.fields[2].name = "z";
// 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, idx_vpx = -1, idx_vpy = -1, idx_vpz = -1;
//now, we need to check what fields we need to store
int offset = 0;
if ((channel_options & channel_option::Intensity) && scan_in.intensities.size() > 0)
{
int field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 1);
cloud_out.fields[field_size].name = "intensity";
cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32;
cloud_out.fields[field_size].offset = offset;
offset += 4;
idx_intensity = field_size;
}
if ((channel_options & channel_option::Index))
{
int field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 1);
cloud_out.fields[field_size].name = "index";
cloud_out.fields[field_size].datatype = sensor_msgs::PointField::INT32;
cloud_out.fields[field_size].offset = offset;
offset += 4;
idx_index = field_size;
}
if ((channel_options & channel_option::Distance))
{
int field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 1);
cloud_out.fields[field_size].name = "distances";
cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32;
cloud_out.fields[field_size].offset = offset;
offset += 4;
idx_distance = field_size;
}
if ((channel_options & channel_option::Timestamp))
{
int field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 1);
cloud_out.fields[field_size].name = "stamps";
cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32;
cloud_out.fields[field_size].offset = offset;
offset += 4;
idx_timestamp = field_size;
}
if ((channel_options & channel_option::Viewpoint))
{
int field_size = cloud_out.fields.size();
cloud_out.fields.resize(field_size + 3);
cloud_out.fields[field_size].name = "vp_x";
cloud_out.fields[field_size].datatype = sensor_msgs::PointField::FLOAT32;
cloud_out.fields[field_size].offset = offset;
offset += 4;
cloud_out.fields[field_size + 1].name = "vp_y";
cloud_out.fields[field_size + 1].datatype = sensor_msgs::PointField::FLOAT32;
cloud_out.fields[field_size + 1].offset = offset;
offset += 4;
cloud_out.fields[field_size + 2].name = "vp_z";
cloud_out.fields[field_size + 2].datatype = sensor_msgs::PointField::FLOAT32;
cloud_out.fields[field_size + 2].offset = offset;
offset += 4;
idx_vpx = field_size;
idx_vpy = field_size + 1;
idx_vpz = field_size + 2;
}
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;
//TODO: Find out why this was needed
//float bad_point = std::numeric_limits<float>::quiet_NaN ();
unsigned int count = 0;
for (size_t i = 0; i < n_pts; ++i)
{
if (range_cutoff < 0)
range_cutoff = scan_in.range_max;
else
range_cutoff = std::min(range_cutoff, (double)scan_in.range_max);
//check to see if we want to keep the point
if (scan_in.ranges[i] >= range_cutoff || scan_in.ranges[i] <= scan_in.range_min)
{
float *pstep = (float*)&cloud_out.data[count * cloud_out.point_step];
// Copy XYZ
pstep[0] = output (i, 0);
pstep[1] = output (i, 1);
pstep[2] = 0;
// Copy intensity
if(idx_intensity != -1)
pstep[idx_intensity] = scan_in.intensities[i];
//Copy index
if(idx_index != -1)
pstep[idx_index] = i;
// Copy distance
if(idx_distance != -1)
pstep[idx_distance] = scan_in.ranges[i];
// Copy timestamp
if(idx_timestamp != -1)
pstep[idx_timestamp] = i * scan_in.time_increment;
// Copy viewpoint (0, 0, 0)
if(idx_vpx != -1 && idx_vpy != -1 && idx_vpz != -1)
{
pstep[idx_vpx] = 0;
pstep[idx_vpy] = 0;
pstep[idx_vpz] = 0;
}
//make sure to increment count
++count;
}
/* TODO: Why was this done in this way, I don't get this at all, you end up with a ton of points with NaN values
* why can't you just leave them out?
*
// Invalid measurement?
if (scan_in.ranges[i] >= range_cutoff || 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
}
}
*/
}
//resize if necessary
cloud_out.width = count;
cloud_out.row_step = cloud_out.point_step * cloud_out.width;
cloud_out.data.resize (cloud_out.row_step * cloud_out.height);
}
void LaserProjection::transformLaserScanToPointCloud2_ (const std::string &target_frame,
const sensor_msgs::LaserScan &scan_in,
sensor_msgs::PointCloud2 &cloud_out,
tf::Transformer &tf,
Eigen3::ArrayXXd &co_sine_map,
double range_cutoff,
int channel_options)
{
//we'll enforce that we get index values for the laser scan so that we
//ensure that we use the correct timestamps
channel_options |= channel_option::Index;
projectLaser_(scan_in, cloud_out, co_sine_map, -1.0, channel_options);
//we'll assume no associated viewpoint by default
bool has_viewpoint = false;
uint32_t vp_x_offset = 0;
//we need to find the offset of the intensity field in the point cloud
//we also know that the index field is guaranteed to exist since we
//set the channel option above. To be really safe, it might be worth
//putting in a check at some point, but I'm just going to put in an
//assert for now
uint32_t index_offset = 0;
for(unsigned int i = 0; i < cloud_out.fields.size(); ++i)
{
if(cloud_out.fields[i].name == "index")
{
index_offset = cloud_out.fields[i].offset;
}
//we want to check if the cloud has a viewpoint associated with it
//checking vp_x should be sufficient since vp_x, vp_y, and vp_z all
//get put in together
if(cloud_out.fields[i].name == "vp_x")
{
has_viewpoint = true;
vp_x_offset = cloud_out.fields[i].offset;
}
}
ROS_ASSERT(index_offset > 0);
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);
//we want to loop through all the points in the cloud
for(size_t i = 0; i < cloud_out.width; ++i)
{
// Apply the transform to the current point
float *pstep = (float*)&cloud_out.data[i * cloud_out.point_step + 0];
//find the index of the point
uint32_t pt_index;
memcpy(&pt_index, &cloud_out.data[i * cloud_out.point_step + index_offset], sizeof(uint32_t));
// Assume constant motion during the laser-scan, and use slerp to compute intermediate transforms
btScalar ratio = pt_index * 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
if(has_viewpoint)
{
float *vpstep = (float*)&cloud_out.data[i * cloud_out.point_step + vp_x_offset];
point_in = btVector3 (vpstep[0], vpstep[1], vpstep[2]);
point_out = cur_transform * point_in;
// Copy transformed point into cloud
vpstep[0] = point_out.x ();
vpstep[1] = point_out.y ();
vpstep[2] = point_out.z ();
}
}
}

12
src/laser_scan_geometry.cpp
View File

@ -255,14 +255,22 @@ void
}
void
laser_scan_geometry::LaserProjection::projectLaser (const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud2 &cloud_out)
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, tf::Transformer &tf, sensor_msgs::PointCloud2 &cloud_out)
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_);
}