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

OccupancyGrid/voxelize: if organized cloud, indices should not be null

Browse Source
This commit is contained in:
matlabbe
2017-07-13 12:23:03 -04:00
parent b4cfa0e844
commit 85f7c1e73c
2 changed files with 6 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/OccupancyGrid.cpp
+2 -1
View File
@@ -299,7 +299,8 @@ void OccupancyGrid::createLocalMap(
viewPoint.z = viewpointRotated.z(); viewPoint.z = viewpointRotated.z();
} }
if(cloud->size()) if((cloud->is_dense && cloud->size()) ||
(!cloud->is_dense && indices->size()))
{ {
pcl::IndicesPtr groundIndices(new std::vector<int>); pcl::IndicesPtr groundIndices(new std::vector<int>);
pcl::IndicesPtr obstaclesIndices(new std::vector<int>); pcl::IndicesPtr obstaclesIndices(new std::vector<int>);
corelib/src/util3d_filtering.cpp
+4
View File
@@ -142,6 +142,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(
float voxelSize) float voxelSize)
{ {
UASSERT(voxelSize > 0.0f); UASSERT(voxelSize > 0.0f);
UASSERT((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()));
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
pcl::VoxelGrid<pcl::PointXYZ> filter; pcl::VoxelGrid<pcl::PointXYZ> filter;
filter.setLeafSize(voxelSize, voxelSize, voxelSize); filter.setLeafSize(voxelSize, voxelSize, voxelSize);
@@ -159,6 +160,7 @@ pcl::PointCloud<pcl::PointNormal>::Ptr voxelize(
float voxelSize) float voxelSize)
{ {
UASSERT(voxelSize > 0.0f); UASSERT(voxelSize > 0.0f);
UASSERT((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()));
pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>); pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
pcl::VoxelGrid<pcl::PointNormal> filter; pcl::VoxelGrid<pcl::PointNormal> filter;
filter.setLeafSize(voxelSize, voxelSize, voxelSize); filter.setLeafSize(voxelSize, voxelSize, voxelSize);
@@ -176,6 +178,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(
float voxelSize) float voxelSize)
{ {
UASSERT(voxelSize > 0.0f); UASSERT(voxelSize > 0.0f);
UASSERT((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()));
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::VoxelGrid<pcl::PointXYZRGB> filter; pcl::VoxelGrid<pcl::PointXYZRGB> filter;
filter.setLeafSize(voxelSize, voxelSize, voxelSize); filter.setLeafSize(voxelSize, voxelSize, voxelSize);
@@ -193,6 +196,7 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelize(
float voxelSize) float voxelSize)
{ {
UASSERT(voxelSize > 0.0f); UASSERT(voxelSize > 0.0f);
UASSERT((cloud->is_dense && cloud->size()) || (!cloud->is_dense && indices->size()));
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>); pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::VoxelGrid<pcl::PointXYZRGBNormal> filter; pcl::VoxelGrid<pcl::PointXYZRGBNormal> filter;
filter.setLeafSize(voxelSize, voxelSize, voxelSize); filter.setLeafSize(voxelSize, voxelSize, voxelSize);