diff --git a/include/laser_geometry/laser_geometry.h b/include/laser_geometry/laser_geometry.h index e2cdc4a..dc7392b 100644 --- a/include/laser_geometry/laser_geometry.h +++ b/include/laser_geometry/laser_geometry.h @@ -68,7 +68,7 @@ namespace laser_geometry Distance = 0x04, //!< Enable "distances" channel Timestamp = 0x08, //!< Enable "stamps" channel Viewpoint = 0x16, //!< Enable "viewpoint" channel - Default = (Intensity | Index) //!< Enable "intensities" and "stamps" channels + Default = (Intensity | Index) //!< Enable "intensities" and "index" channels }; } @@ -99,11 +99,12 @@ namespace laser_geometry public: + LaserProjection() : angle_min_(0), angle_max_(0) {} + //! Destructor to deallocate stored unit vectors ~LaserProjection(); - //! Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud /*! * Project a single laser scan from a linear array into a 3D @@ -147,7 +148,7 @@ namespace laser_geometry double range_cutoff = -1.0, int channel_options = channel_option::Default) { - projectLaser_(scan_in, cloud_out, co_sine_map_, range_cutoff, channel_options); + projectLaser_(scan_in, cloud_out, range_cutoff, channel_options); } @@ -235,41 +236,12 @@ namespace laser_geometry const sensor_msgs::LaserScan &scan_in, sensor_msgs::PointCloud2 &cloud_out, tf::Transformer &tf, - double range_cutoff, + double range_cutoff = -1.0, int channel_options = channel_option::Default) { - transformLaserScanToPointCloud2_(target_frame, scan_in, cloud_out, tf, co_sine_map_, range_cutoff, channel_options); + transformLaserScanToPointCloud2_(target_frame, scan_in, cloud_out, tf, range_cutoff, channel_options); } - //! 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. - * - * \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. - */ - 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) - { - transformLaserScanToPointCloud2(target_frame, scan_in, cloud_out, tf, -1.0, channel_options); - } - - protected: //! Internal protected representation of getUnitVectors @@ -295,7 +267,6 @@ namespace laser_geometry //! 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); @@ -313,14 +284,14 @@ namespace laser_geometry 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* > unit_vector_map_; + float angle_min_; + float angle_max_; Eigen3::ArrayXXd co_sine_map_; - boost::mutex guv_mutex_; }; diff --git a/src/laser_geometry.cpp b/src/laser_geometry.cpp index 71779c0..bdd05ec 100644 --- a/src/laser_geometry.cpp +++ b/src/laser_geometry.cpp @@ -276,7 +276,6 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do 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) { @@ -291,20 +290,22 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do 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) + // Check if our existing co_sine_map is valid + if (co_sine_map_.rows () != (int)n_pts || angle_min_ != scan_in.angle_min || angle_max_ != scan_in.angle_max ) { ROS_DEBUG ("[projectLaser] No precomputed map given. Computing one."); - co_sine_map = Eigen3::ArrayXXd (n_pts, 2); + co_sine_map_ = Eigen3::ArrayXXd (n_pts, 2); + angle_min_ = scan_in.angle_min; + angle_max_ = scan_in.angle_max; // 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); + 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; + output = ranges * co_sine_map_; // Set the output cloud accordingly cloud_out.header = scan_in.header; @@ -486,7 +487,6 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do 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) { @@ -499,7 +499,7 @@ const boost::numeric::ublas::matrix& LaserProjection::getUnitVectors_(do //ensure that we use the correct timestamps channel_options |= channel_option::Index; - projectLaser_(scan_in, cloud_out, co_sine_map, -1.0, channel_options); + projectLaser_(scan_in, cloud_out, -1.0, channel_options); //we'll assume no associated viewpoint by default bool has_viewpoint = false;