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

Just using internal co_sine_map for LaserProjection

Browse Source 
git-svn-id: https://code.ros.org/svn/ros-pkg/stacks/laser_pipeline/trunk@35260 eb33c2ac-9c88-4c90-87e0-44a10359b0c3
This commit is contained in:
Jeremy Leibs 2011-01-14 21:32:56 +00:00
parent 4fafc67ab6
commit 92e6c455c6
2 changed files with 17 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
include/laser_geometry/laser_geometry.h
View File

@ -68,7 +68,7 @@ namespace laser_geometry
Distance = 0x04, //!< Enable "distances" channel Distance = 0x04, //!< Enable "distances" channel
Timestamp = 0x08, //!< Enable "stamps" channel Timestamp = 0x08, //!< Enable "stamps" channel
Viewpoint = 0x16, //!< Enable "viewpoint" 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: public:
LaserProjection() : angle_min_(0), angle_max_(0) {}
//! Destructor to deallocate stored unit vectors //! Destructor to deallocate stored unit vectors
~LaserProjection(); ~LaserProjection();
//! Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud //! Project a sensor_msgs::LaserScan into a sensor_msgs::PointCloud
/*! /*!
* Project a single laser scan from a linear array into a 3D * Project a single laser scan from a linear array into a 3D
@ -147,7 +148,7 @@ namespace laser_geometry
double range_cutoff = -1.0, double range_cutoff = -1.0,
int channel_options = channel_option::Default) 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, const sensor_msgs::LaserScan &scan_in,
sensor_msgs::PointCloud2 &cloud_out, sensor_msgs::PointCloud2 &cloud_out,
tf::Transformer &tf, tf::Transformer &tf,
double range_cutoff, double range_cutoff = -1.0,
int channel_options = channel_option::Default) 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: protected:
//! Internal protected representation of getUnitVectors //! Internal protected representation of getUnitVectors
@ -295,7 +267,6 @@ namespace laser_geometry
//! Internal hidden representation of projectLaser //! Internal hidden representation of projectLaser
void projectLaser_ (const sensor_msgs::LaserScan& scan_in, void projectLaser_ (const sensor_msgs::LaserScan& scan_in,
sensor_msgs::PointCloud2 &cloud_out, sensor_msgs::PointCloud2 &cloud_out,
Eigen3::ArrayXXd &co_sine_map,
double range_cutoff, double range_cutoff,
int channel_options); int channel_options);
@ -313,14 +284,14 @@ namespace laser_geometry
const sensor_msgs::LaserScan &scan_in, const sensor_msgs::LaserScan &scan_in,
sensor_msgs::PointCloud2 &cloud_out, sensor_msgs::PointCloud2 &cloud_out,
tf::Transformer &tf, tf::Transformer &tf,
Eigen3::ArrayXXd &co_sine_map,
double range_cutoff, double range_cutoff,
int channel_options); int channel_options);
//! Internal map of pointers to stored values //! Internal map of pointers to stored values
std::map<std::string,boost::numeric::ublas::matrix<double>* > unit_vector_map_; std::map<std::string,boost::numeric::ublas::matrix<double>* > unit_vector_map_;
float angle_min_;
float angle_max_;
Eigen3::ArrayXXd co_sine_map_; Eigen3::ArrayXXd co_sine_map_;
boost::mutex guv_mutex_; boost::mutex guv_mutex_;
}; };

18
src/laser_geometry.cpp
View File

@ -276,7 +276,6 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
void LaserProjection::projectLaser_ (const sensor_msgs::LaserScan& scan_in, void LaserProjection::projectLaser_ (const sensor_msgs::LaserScan& scan_in,
sensor_msgs::PointCloud2 &cloud_out, sensor_msgs::PointCloud2 &cloud_out,
Eigen3::ArrayXXd &co_sine_map,
double range_cutoff, double range_cutoff,
int channel_options) int channel_options)
{ {
@ -291,20 +290,22 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
ranges (i, 1) = (double) scan_in.ranges[i]; ranges (i, 1) = (double) scan_in.ranges[i];
} }
// Check if we have a precomputed co_sine_map // Check if our existing co_sine_map is valid
if (co_sine_map.rows () != (int)n_pts) 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."); 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 // Spherical->Cartesian projection
for (size_t i = 0; i < n_pts; ++i) 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, 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, 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 // Set the output cloud accordingly
cloud_out.header = scan_in.header; cloud_out.header = scan_in.header;
@ -486,7 +487,6 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
const sensor_msgs::LaserScan &scan_in, const sensor_msgs::LaserScan &scan_in,
sensor_msgs::PointCloud2 &cloud_out, sensor_msgs::PointCloud2 &cloud_out,
tf::Transformer &tf, tf::Transformer &tf,
Eigen3::ArrayXXd &co_sine_map,
double range_cutoff, double range_cutoff,
int channel_options) int channel_options)
{ {
@ -499,7 +499,7 @@ const boost::numeric::ublas::matrix<double>& LaserProjection::getUnitVectors_(do
//ensure that we use the correct timestamps //ensure that we use the correct timestamps
channel_options |= channel_option::Index; 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 //we'll assume no associated viewpoint by default
bool has_viewpoint = false; bool has_viewpoint = false;