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

point_cloud_xyzrgb: fixed assert on voxelize if cloud is organized

Browse Source
This commit is contained in:
matlabbe
2017-07-13 16:05:14 -04:00
parent 7f392f3796
commit 96dd78dc67
2 changed files with 39 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/point_cloud_xyz.cpp
+19 -13
View File
@@ -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<int>);
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<pcl::PointXYZ>::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<int>);
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<pcl::PointXYZ>::Ptr & pclCloud, const std_msgs::Header & header)
void processAndPublish(pcl::PointCloud<pcl::PointXYZ>::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<float>::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_);
src/nodelets/point_cloud_xyzrgb.cpp
+20 -16
View File
@@ -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<int>);
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<pcl::PointXYZRGB>::Ptr pclCloud;
pcl::IndicesPtr indices(new std::vector<int>);
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<pcl::PointXYZRGB>::Ptr & pclCloud, const std_msgs::Header & header)
void processAndPublish(pcl::PointCloud<pcl::PointXYZRGB>::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<float>::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<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*pclCloud, *indices, *tmp);
pclCloud = tmp;