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

pointcloud_to_depthimage: Handling the case of empty input cloud to avoid an assert #364

Browse Source
This commit is contained in:
matlabbe
2019-11-19 08:53:02 -05:00
parent b19a82ef34
commit 26b8fb5027
1 changed files with 13 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/pointcloud_to_depthimage.cpp
+13 -4
View File
@@ -200,13 +200,22 @@ private:
pcl_conversions::toPCL(*pointCloud2Msg, *cloud);
cv_bridge::CvImage depthImage;
depthImage.image = rtabmap::util3d::projectCloudToCamera(model.imageSize(), model.K(), cloud, model.localTransform());
if(fillHolesSize_ > 0 && fillIterations_ > 0)
if(cloud->data.empty())
{
for(int i=0; i<fillIterations_;++i)
ROS_WARN("Received an empty cloud on topic \"%s\"! A depth image with all zeros is returned.", pointCloudSub_.getTopic().c_str());
depthImage.image = cv::Mat::zeros(model.imageSize(), CV_32FC1);
}
else
{
depthImage.image = rtabmap::util3d::projectCloudToCamera(model.imageSize(), model.K(), cloud, model.localTransform());
if(fillHolesSize_ > 0 && fillIterations_ > 0)
{
depthImage.image = rtabmap::util2d::fillDepthHoles(depthImage.image, fillHolesSize_, fillHolesError_);
for(int i=0; i<fillIterations_;++i)
{
depthImage.image = rtabmap::util2d::fillDepthHoles(depthImage.image, fillHolesSize_, fillHolesError_);
}
}
}