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

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.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols),
model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows)); model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows));
pcl::IndicesPtr indices(new std::vector<int>);
pclCloud = rtabmap::util3d::cloudFromDepth( pclCloud = rtabmap::util3d::cloudFromDepth(
cv::Mat(imageDepthPtr->image, roi), cv::Mat(imageDepthPtr->image, roi),
m, m,
decimation_); decimation_,
processAndPublish(pclCloud, depth->header); maxDepth_,
minDepth_,
indices.get());
processAndPublish(pclCloud, indices, depth->header);
NODELET_DEBUG("point_cloud_xyz from depth time = %f s", (ros::WallTime::now() - time).toSec()); 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; pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
rtabmap::CameraModel leftModel = rtabmap_ros::cameraModelFromROS(*cameraInfo); 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); 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( pclCloud = rtabmap::util3d::cloudFromDisparity(
cv::Mat(disparity, roi), cv::Mat(disparity, roi),
stereoModel, 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()); 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) if(pclCloud->size() && voxelSize_ > 0.0)
{ {
pclCloud = rtabmap::util3d::voxelize(pclCloud, voxelSize_); pclCloud = rtabmap::util3d::voxelize(pclCloud, voxelSize_);
@@ -286,10 +289,13 @@ private:
// Do radius filtering after voxel filtering ( a lot faster) // Do radius filtering after voxel filtering ( a lot faster)
if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0) if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
{ {
if(voxelSize_ <= 0.0 && !(minDepth_ != 0.0 || maxDepth_ > minDepth_)) if(pclCloud->is_dense)
{ {
// remove NaN values indices = rtabmap::util3d::radiusFiltering(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud); }
else
{
indices = rtabmap::util3d::radiusFiltering(pclCloud, indices, noiseFilterRadius_, noiseFilterMinNeighbors_);
} }
pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(pclCloud, 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.fy(),
model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols), model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols),
model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows)); model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows));
pcl::IndicesPtr indices(new std::vector<int>);
pclCloud = rtabmap::util3d::cloudFromDepthRGB( pclCloud = rtabmap::util3d::cloudFromDepthRGB(
cv::Mat(imagePtr->image, roi), cv::Mat(imagePtr->image, roi),
cv::Mat(imageDepthPtr->image, roi), cv::Mat(imageDepthPtr->image, roi),
m, 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()); 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::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
pcl::IndicesPtr indices(new std::vector<int>);
pclCloud = rtabmap::util3d::cloudFromStereoImages( pclCloud = rtabmap::util3d::cloudFromStereoImages(
ptrLeftImage->image, ptrLeftImage->image,
ptrRightImage->image, ptrRightImage->image,
rtabmap_ros::stereoCameraModelFromROS(*camInfoLeft, *camInfoRight), 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()); 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) 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) // Do radius filtering after voxel filtering ( a lot faster)
if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0) if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
{ {
if(voxelSize_ <= 0.0 && !(minDepth_ != 0.0 || maxDepth_ > minDepth_)) if(pclCloud->is_dense)
{ {
// remove NaN values indices = rtabmap::util3d::radiusFiltering(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud); }
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::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*pclCloud, *indices, *tmp); pcl::copyPointCloud(*pclCloud, *indices, *tmp);
pclCloud = tmp; pclCloud = tmp;