Class to accumulate point clouds laser3D. More...
#include <laser3D_pointcloud.h>
Public Member Functions | |
void | accumulate_cloud (pcl::PointCloud< pcl::PointXYZ > *pc_in) |
Function to accumulate PointCloud. Receives the pointcloud with the transformed laser scan and accumulates it on the class variable pc_accumulated. | |
las3D_PointCloud () | |
int | las3D_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 using the scan time stamps. 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. | |
int | las3D_transformLaserScanToPointCloud (const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud2 &cloud_out, const sensor_msgs::JointState &state_start, const sensor_msgs::JointState &state_end, 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 using the external information of the external shaft angle. 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. | |
~las3D_PointCloud () | |
Public Attributes | |
int | accumulation_mode |
Type of accumulation wanted to be used. | |
size_t | cloud_size |
PointCloud size. | |
std::vector < sensor_msgs::JointStatePtr > | joints |
Vector with the external shaft angle mensage. | |
int | laser_count |
Number of laser scans received. | |
bool | laserscan_arrived |
bool laser scan received | |
int | max_scans_accumulated |
Max of scans to be accumulated. | |
tf::TransformListener * | p_listener |
ROS tf listener. | |
pcl::PointCloud< pcl::PointXYZ > | pc_accumulated |
PointCloud<pcl::PointXYZ> to accumulate the laser scans. | |
int | pointcloud_stamp |
Time stamp to the output cloud, when using rosbag or in real time accumulation. | |
std::vector < sensor_msgs::LaserScanPtr > | scans |
Vector with the laser scans received. | |
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 | las3D_projectLaser_ (const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud2 &cloud_out, double range_cutoff, int channel_options) |
Internal hidden representation of las3D_projectLaser_. | |
int | las3D_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) |
int | las3D_transformLaserScanToPointCloud_ (const std::string &target_frame, const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud2 &cloud_out, const sensor_msgs::JointState &state_start, const sensor_msgs::JointState &state_end, tf::Transformer &tf, double range_cutoff, int channel_options) |
Internal hidden representation of las3D_transformLaserScanToPointCloud_. | |
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. |
Class to accumulate point clouds laser3D.
Definition at line 90 of file laser3D_pointcloud.h.
las3D_PointCloud::las3D_PointCloud | ( | ) | [inline] |
Definition at line 124 of file laser3D_pointcloud.h.
las3D_PointCloud::~las3D_PointCloud | ( | ) |
Definition at line 79 of file laser3D_pointcloud.cpp.
void las3D_PointCloud::accumulate_cloud | ( | pcl::PointCloud< pcl::PointXYZ > * | pc_in | ) |
Function to accumulate PointCloud. Receives the pointcloud with the transformed laser scan and accumulates it on the class variable pc_accumulated.
pc_in | pointcloud to transform |
Definition at line 39 of file laser3D_pointcloud.cpp.
const boost::numeric::ublas::matrix< double > & las3D_PointCloud::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 53 of file laser3D_pointcloud.cpp.
void las3D_PointCloud::las3D_projectLaser_ | ( | const sensor_msgs::LaserScan & | scan_in, | |
sensor_msgs::PointCloud2 & | cloud_out, | |||
double | range_cutoff, | |||
int | channel_options | |||
) | [private] |
Internal hidden representation of las3D_projectLaser_.
Definition at line 90 of file laser3D_pointcloud.cpp.
int las3D_PointCloud::las3D_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 using the scan time stamps. 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.
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 which is more limiting than max_range in the scan. | |
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 206 of file laser3D_pointcloud.h.
int las3D_PointCloud::las3D_transformLaserScanToPointCloud | ( | const std::string & | target_frame, | |
const sensor_msgs::LaserScan & | scan_in, | |||
sensor_msgs::PointCloud2 & | cloud_out, | |||
const sensor_msgs::JointState & | state_start, | |||
const sensor_msgs::JointState & | state_end, | |||
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 using the external information of the external shaft angle. 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.
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 | |
state_start | The external shaft angle mensage at the start of the scan | |
state_end | The external shaft angle mensage at the end of the scan | |
range_cutoff | An additional range cutoff which can be applied which is more limiting than max_range in the scan. | |
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 171 of file laser3D_pointcloud.h.
int las3D_PointCloud::las3D_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] |
Definition at line 301 of file laser3D_pointcloud.cpp.
int las3D_PointCloud::las3D_transformLaserScanToPointCloud_ | ( | const std::string & | target_frame, | |
const sensor_msgs::LaserScan & | scan_in, | |||
sensor_msgs::PointCloud2 & | cloud_out, | |||
const sensor_msgs::JointState & | state_start, | |||
const sensor_msgs::JointState & | state_end, | |||
tf::Transformer & | tf, | |||
double | range_cutoff, | |||
int | channel_options | |||
) | [private] |
Internal hidden representation of las3D_transformLaserScanToPointCloud_.
Definition at line 519 of file laser3D_pointcloud.cpp.
Type of accumulation wanted to be used.
Definition at line 116 of file laser3D_pointcloud.h.
float las3D_PointCloud::angle_max_ [private] |
Definition at line 262 of file laser3D_pointcloud.h.
float las3D_PointCloud::angle_min_ [private] |
Definition at line 261 of file laser3D_pointcloud.h.
size_t las3D_PointCloud::cloud_size |
PointCloud size.
Definition at line 101 of file laser3D_pointcloud.h.
Eigen::ArrayXXd las3D_PointCloud::co_sine_map_ [private] |
Definition at line 263 of file laser3D_pointcloud.h.
boost::mutex las3D_PointCloud::guv_mutex_ [private] |
Definition at line 264 of file laser3D_pointcloud.h.
std::vector<sensor_msgs::JointStatePtr> las3D_PointCloud::joints |
Vector with the external shaft angle mensage.
Definition at line 104 of file laser3D_pointcloud.h.
Number of laser scans received.
Definition at line 107 of file laser3D_pointcloud.h.
bool laser scan received
Definition at line 110 of file laser3D_pointcloud.h.
Max of scans to be accumulated.
Definition at line 119 of file laser3D_pointcloud.h.
tf::TransformListener* las3D_PointCloud::p_listener |
ROS tf listener.
Definition at line 95 of file laser3D_pointcloud.h.
pcl::PointCloud<pcl::PointXYZ> las3D_PointCloud::pc_accumulated |
PointCloud<pcl::PointXYZ> to accumulate the laser scans.
Definition at line 98 of file laser3D_pointcloud.h.
Time stamp to the output cloud, when using rosbag or in real time accumulation.
Definition at line 122 of file laser3D_pointcloud.h.
std::vector<sensor_msgs::LaserScanPtr> las3D_PointCloud::scans |
Vector with the laser scans received.
Definition at line 113 of file laser3D_pointcloud.h.
std::map<std::string,boost::numeric::ublas::matrix<double>* > las3D_PointCloud::unit_vector_map_ [private] |
Internal map of pointers to stored values.
Definition at line 260 of file laser3D_pointcloud.h.