i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 08:47:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

ros-pkg: Added decimation parameter to point_cloud_xyzrgb nodelet

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1344 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-06-11 15:42:41 +00:00
parent a9a7ebb6b7
commit 9a2d96b99e
2 changed files with 45 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch/tests/test_pointxyzrgb.launch
+20
View File
@@ -0,0 +1,20 @@
<launch>
<!-- sync cloud with odometry and voxelize the point cloud (for fast visualization in rviz) -->
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap/point_cloud_xyzrgb">
<remap from="rgb/image" to="camera/rgb/image_rect_color"/>
<remap from="depth/image" to="camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="camera/depth_registered/camera_info"/>
<remap from="cloud" to="/voxel_cloud" />
<param name="queue_size" type="int" value="30"/>
<param name="voxel_size" type="double" value="0.02"/>
<param name="decimation" type="int" value="2"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="depth/image_transport" type="string" value="compressedDepth"/>
</node>
</launch>
src/nodelets/point_cloud_xyzrgb.cpp
+25 -22
View File
@@ -35,7 +35,7 @@ namespace rtabmap
class PointCloudXYZRGB : public nodelet::Nodelet class PointCloudXYZRGB : public nodelet::Nodelet
{ {
public: public:
PointCloudXYZRGB() : voxelSize_(0.0) {} PointCloudXYZRGB() : voxelSize_(0.0), decimation_(1) {}
virtual ~PointCloudXYZRGB() virtual ~PointCloudXYZRGB()
{ {
@@ -51,6 +51,7 @@ private:
int queueSize = 10; int queueSize = 10;
pnh.param("queue_size", queueSize, queueSize); pnh.param("queue_size", queueSize, queueSize);
pnh.param("voxel_size", voxelSize_, voxelSize_); pnh.param("voxel_size", voxelSize_, voxelSize_);
pnh.param("decimation", decimation_, decimation_);
sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_); sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
sync_->registerCallback(boost::bind(&PointCloudXYZRGB::callback, this, _1, _2, _3)); sync_->registerCallback(boost::bind(&PointCloudXYZRGB::callback, this, _1, _2, _3));
@@ -87,29 +88,30 @@ private:
return; return;
} }
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
pclCloud = rtabmap::util3d::cloudFromDepthRGB(
imagePtr->image,
imageDepthPtr->image,
cameraInfo->K[2],
cameraInfo->K[5],
cameraInfo->K[0],
cameraInfo->K[4]);
if(voxelSize_ > 0.0)
{
pclCloud = rtabmap::util3d::voxelize(pclCloud, voxelSize_);
}
//*********************
// Publish Map
//*********************
if(cloudPub_.getNumSubscribers()) if(cloudPub_.getNumSubscribers())
{ {
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
pclCloud = rtabmap::util3d::cloudFromDepthRGB(
imagePtr->image,
imageDepthPtr->image,
cameraInfo->K[2],
cameraInfo->K[5],
cameraInfo->K[0],
cameraInfo->K[4],
decimation_);
if(voxelSize_ > 0.0)
{
pclCloud = rtabmap::util3d::voxelize(pclCloud, voxelSize_);
}
//*********************
// Publish Map
//*********************
sensor_msgs::PointCloud2 rosCloud; sensor_msgs::PointCloud2 rosCloud;
pcl::toROSMsg(*pclCloud, rosCloud); pcl::toROSMsg(*pclCloud, rosCloud);
rosCloud.header.stamp = image->header.stamp; rosCloud.header.stamp = image->header.stamp;
@@ -123,6 +125,7 @@ private:
private: private:
double voxelSize_; double voxelSize_;
int decimation_;
ros::Publisher cloudPub_; ros::Publisher cloudPub_;
image_transport::SubscriberFilter imageSub_; image_transport::SubscriberFilter imageSub_;