A Class to Project Laser Scan. More...
#include <laser_geometry.h>
Public Member Functions | |
LaserProjection () | |
void | projectLaser (const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud &cloud_out, double range_cutoff=-1.0, int channel_options=channel_option::Default) |
Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud. | |
void | projectLaser (const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud2 &cloud_out, double range_cutoff=-1.0, int channel_options=channel_option::Default) |
Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2. | |
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) |
Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud in target frame. | |
void | transformLaserScanToPointCloud (const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud &cloud_out, tf::Transformer &tf, int channel_options=channel_option::Default) |
Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud in target frame. | |
void | transformLaserScanToPointCloud (const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud2 &cloud_out, tf::Transformer &tf, double range_cutoff=-1.0, int channel_options=channel_option::Default) |
Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2 in target frame. | |
~LaserProjection () | |
Destructor to deallocate stored unit vectors. | |
Protected Member Functions | |
const boost::numeric::ublas::matrix < double > & | getUnitVectors_ (double angle_min, double angle_max, double angle_increment, unsigned int length) |
Internal protected representation of getUnitVectors. | |
Private Member Functions | |
void | projectLaser_ (const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud &cloud_out, double range_cutoff, bool preservative, int channel_options) |
Internal hidden representation of projectLaser. | |
void | projectLaser_ (const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud2 &cloud_out, double range_cutoff, int channel_options) |
Internal hidden representation of projectLaser. | |
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) |
Internal hidden representation of transformLaserScanToPointCloud. | |
void | 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) |
Internal hidden representation of transformLaserScanToPointCloud2. | |
Private Attributes | |
float | angle_max_ |
float | angle_min_ |
Eigen::ArrayXXd | co_sine_map_ |
boost::mutex | guv_mutex_ |
std::map< std::string, boost::numeric::ublas::matrix < double > * > | unit_vector_map_ |
Internal map of pointers to stored values. |
A Class to Project Laser Scan.
This class will project laser scans into point clouds. It caches unit vectors between runs (provided the angular resolution of your scanner is not changing) to avoid excess computation.
By default all range values less than the scanner min_range, and greater than the scanner max_range are removed from the generated point cloud, as these are assumed to be invalid.
If it is important to preserve a mapping between the index of range values and points in the cloud, the recommended approach is to pre-filter your laser_scan message to meet the requiremnt that all ranges are between min and max_range.
The generated PointClouds have a number of channels which can be enabled through the use of ChannelOption.
Definition at line 98 of file laser_geometry.h.
laser_geometry::LaserProjection::LaserProjection | ( | ) | [inline] |
Definition at line 103 of file laser_geometry.h.
Destructor to deallocate stored unit vectors.
Definition at line 176 of file laser_geometry.cpp.
const boost::numeric::ublas::matrix< double > & laser_geometry::laser_geometry.LaserProjection::getUnitVectors_ | ( | double | angle_min, |
double | angle_max, | ||
double | angle_increment, | ||
unsigned int | length | ||
) | [protected] |
Internal protected representation of getUnitVectors.
This function should not be used by external users, however, it is left protected so that test code can evaluate it appropriately.
Definition at line 150 of file laser_geometry.cpp.
void laser_geometry::LaserProjection::projectLaser | ( | const sensor_msgs::LaserScan & | scan_in, |
sensor_msgs::PointCloud & | cloud_out, | ||
double | range_cutoff = -1.0 , |
||
int | channel_options = channel_option::Default |
||
) | [inline] |
Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud.
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.
scan_in | The input laser scan |
cloud_out | The output point cloud |
range_cutoff | An additional range cutoff which can be applied to discard everything above it. Defaults to -1.0, which means the laser scan max range. |
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. |
Definition at line 124 of file laser_geometry.h.
void laser_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 |
||
) | [inline] |
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.
scan_in | The input laser scan |
cloud_out | The output point cloud |
range_cutoff | An additional range cutoff which can be applied to discard everything above it. Defaults to -1.0, which means the laser scan max range. |
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. |
Definition at line 148 of file laser_geometry.h.
void laser_geometry::LaserProjection::projectLaser_ | ( | const sensor_msgs::LaserScan & | scan_in, |
sensor_msgs::PointCloud & | cloud_out, | ||
double | range_cutoff, | ||
bool | preservative, | ||
int | channel_options | ||
) | [private] |
Internal hidden representation of projectLaser.
void laser_geometry::laser_geometry.LaserProjection::projectLaser_ | ( | const sensor_msgs::LaserScan & | scan_in, |
sensor_msgs::PointCloud2 & | cloud_out, | ||
double | range_cutoff, | ||
int | channel_options | ||
) | [private] |
Internal hidden representation of projectLaser.
Definition at line 274 of file laser_geometry.cpp.
void laser_geometry::LaserProjection::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 |
||
) | [inline] |
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.
target_frame | The frame of the resulting point cloud |
scan_in | The input laser scan |
cloud_out | The output point cloud |
tf | a tf::Transformer object to use to perform the transform |
range_cutoff | An additional range cutoff which can be applied to discard everything above it. |
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. |
Definition at line 178 of file laser_geometry.h.
void laser_geometry::LaserProjection::transformLaserScanToPointCloud | ( | const std::string & | target_frame, |
const sensor_msgs::LaserScan & | scan_in, | ||
sensor_msgs::PointCloud & | cloud_out, | ||
tf::Transformer & | tf, | ||
int | channel_options = channel_option::Default |
||
) | [inline] |
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.
target_frame | The frame of the resulting point cloud |
scan_in | The input laser scan |
cloud_out | The output point cloud |
tf | a tf::Transformer object to use to perform the transform |
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. |
Definition at line 207 of file laser_geometry.h.
void laser_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 = -1.0 , |
||
int | channel_options = channel_option::Default |
||
) | [inline] |
Transform a sensor_msgs::LaserScan into a sensor_msgs::PointCloud2 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.
target_frame | The frame of the resulting point cloud |
scan_in | The input laser scan |
cloud_out | The output point cloud |
tf | a tf::Transformer object to use to perform the transform |
range_cutoff | An additional range cutoff which can be applied to discard everything above it. Defaults to -1.0, which means the laser scan max range. |
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. |
Definition at line 238 of file laser_geometry.h.
void laser_geometry::LaserProjection::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 | ||
) | [private] |
Internal hidden representation of transformLaserScanToPointCloud.
void laser_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 | ||
) | [private] |
Internal hidden representation of transformLaserScanToPointCloud2.
Definition at line 497 of file laser_geometry.cpp.
float laser_geometry::LaserProjection::angle_max_ [private] |
Definition at line 295 of file laser_geometry.h.
float laser_geometry::LaserProjection::angle_min_ [private] |
Definition at line 294 of file laser_geometry.h.
Eigen::ArrayXXd laser_geometry::LaserProjection::co_sine_map_ [private] |
Definition at line 296 of file laser_geometry.h.
boost::mutex laser_geometry::LaserProjection::guv_mutex_ [private] |
Definition at line 297 of file laser_geometry.h.
std::map<std::string,boost::numeric::ublas::matrix<double>* > laser_geometry::LaserProjection::unit_vector_map_ [private] |
Internal map of pointers to stored values.
Definition at line 293 of file laser_geometry.h.