From 96dd78dc674084f17cf382782fd3ab6706fefb66 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 13 Jul 2017 16:05:14 -0400 Subject: [PATCH] point_cloud_xyzrgb: fixed assert on voxelize if cloud is organized --- src/nodelets/point_cloud_xyz.cpp | 32 ++++++++++++++----------- src/nodelets/point_cloud_xyzrgb.cpp | 36 ++++++++++++++++------------- 2 files changed, 39 insertions(+), 29 deletions(-) diff --git a/src/nodelets/point_cloud_xyz.cpp b/src/nodelets/point_cloud_xyz.cpp index 7274b92a..c50ca811 100644 --- a/src/nodelets/point_cloud_xyz.cpp +++ b/src/nodelets/point_cloud_xyz.cpp @@ -220,11 +220,15 @@ private: model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols), model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows)); + pcl::IndicesPtr indices(new std::vector); pclCloud = rtabmap::util3d::cloudFromDepth( cv::Mat(imageDepthPtr->image, roi), m, - decimation_); - processAndPublish(pclCloud, depth->header); + decimation_, + maxDepth_, + minDepth_, + indices.get()); + processAndPublish(pclCloud, indices, depth->header); NODELET_DEBUG("point_cloud_xyz from depth time = %f s", (ros::WallTime::now() - time).toSec()); } @@ -260,24 +264,23 @@ private: pcl::PointCloud::Ptr pclCloud; rtabmap::CameraModel leftModel = rtabmap_ros::cameraModelFromROS(*cameraInfo); rtabmap::StereoCameraModel stereoModel(disparityMsg->f, disparityMsg->f, leftModel.cx()-roiRatios_[0]*double(disparity.cols), leftModel.cy()-roiRatios_[2]*double(disparity.rows), disparityMsg->T); + pcl::IndicesPtr indices(new std::vector); pclCloud = rtabmap::util3d::cloudFromDisparity( cv::Mat(disparity, roi), stereoModel, - decimation_); + decimation_, + maxDepth_, + minDepth_, + indices.get()); - processAndPublish(pclCloud, disparityMsg->header); + processAndPublish(pclCloud, indices, disparityMsg->header); NODELET_DEBUG("point_cloud_xyz from disparity time = %f s", (ros::WallTime::now() - time).toSec()); } } - void processAndPublish(pcl::PointCloud::Ptr & pclCloud, const std_msgs::Header & header) + void processAndPublish(pcl::PointCloud::Ptr & pclCloud, pcl::IndicesPtr & indices, const std_msgs::Header & header) { - if(pclCloud->size() && (minDepth_ != 0.0 || maxDepth_ > minDepth_)) - { - pclCloud = rtabmap::util3d::passThrough(pclCloud, "z", minDepth_, maxDepth_>minDepth_?maxDepth_:std::numeric_limits::max()); - } - if(pclCloud->size() && voxelSize_ > 0.0) { pclCloud = rtabmap::util3d::voxelize(pclCloud, voxelSize_); @@ -286,10 +289,13 @@ private: // Do radius filtering after voxel filtering ( a lot faster) if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0) { - if(voxelSize_ <= 0.0 && !(minDepth_ != 0.0 || maxDepth_ > minDepth_)) + if(pclCloud->is_dense) { - // remove NaN values - pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud); + indices = rtabmap::util3d::radiusFiltering(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_); + } + else + { + indices = rtabmap::util3d::radiusFiltering(pclCloud, indices, noiseFilterRadius_, noiseFilterMinNeighbors_); } pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_); diff --git a/src/nodelets/point_cloud_xyzrgb.cpp b/src/nodelets/point_cloud_xyzrgb.cpp index 1cdd2771..5942ec8c 100644 --- a/src/nodelets/point_cloud_xyzrgb.cpp +++ b/src/nodelets/point_cloud_xyzrgb.cpp @@ -249,14 +249,18 @@ private: model.fy(), model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols), model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows)); + pcl::IndicesPtr indices(new std::vector); pclCloud = rtabmap::util3d::cloudFromDepthRGB( cv::Mat(imagePtr->image, roi), cv::Mat(imageDepthPtr->image, roi), m, - decimation_); + decimation_, + maxDepth_, + minDepth_, + indices.get()); - processAndPublish(pclCloud, imagePtr->header); + processAndPublish(pclCloud, indices, imagePtr->header); NODELET_DEBUG("point_cloud_xyzrgb from RGB-D time = %f s", (ros::WallTime::now() - time).toSec()); } @@ -302,40 +306,40 @@ private: } pcl::PointCloud::Ptr pclCloud; + pcl::IndicesPtr indices(new std::vector); pclCloud = rtabmap::util3d::cloudFromStereoImages( ptrLeftImage->image, ptrRightImage->image, rtabmap_ros::stereoCameraModelFromROS(*camInfoLeft, *camInfoRight), - decimation_); + decimation_, + maxDepth_, + minDepth_, + indices.get()); - processAndPublish(pclCloud, imageLeft->header); + processAndPublish(pclCloud, indices, imageLeft->header); NODELET_DEBUG("point_cloud_xyzrgb from stereo time = %f s", (ros::WallTime::now() - time).toSec()); } } - void processAndPublish(pcl::PointCloud::Ptr & pclCloud, const std_msgs::Header & header) + void processAndPublish(pcl::PointCloud::Ptr & pclCloud, pcl::IndicesPtr & indices, const std_msgs::Header & header) { - if(pclCloud->size() && (minDepth_ != 0.0 || maxDepth_ > minDepth_)) - { - pclCloud = rtabmap::util3d::passThrough(pclCloud, "z", minDepth_, maxDepth_>minDepth_?maxDepth_:std::numeric_limits::max()); - } - if(pclCloud->size() && voxelSize_ > 0.0) { - pclCloud = rtabmap::util3d::voxelize(pclCloud, voxelSize_); + pclCloud = rtabmap::util3d::voxelize(pclCloud, indices, voxelSize_); } // Do radius filtering after voxel filtering ( a lot faster) if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0) { - if(voxelSize_ <= 0.0 && !(minDepth_ != 0.0 || maxDepth_ > minDepth_)) + if(pclCloud->is_dense) { - // remove NaN values - pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud); + indices = rtabmap::util3d::radiusFiltering(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_); + } + else + { + indices = rtabmap::util3d::radiusFiltering(pclCloud, indices, noiseFilterRadius_, noiseFilterMinNeighbors_); } - - pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_); pcl::PointCloud::Ptr tmp(new pcl::PointCloud); pcl::copyPointCloud(*pclCloud, *indices, *tmp); pclCloud = tmp;