i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

commonFiltering(): warn when normals could not be computed (returned all NaNs)

Browse Source
This commit is contained in:
matlabbe
2021-05-16 11:58:17 -04:00
parent 56bc15d7cd
commit fe896260c5
1 changed files with 20 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
corelib/src/util3d_filtering.cpp
View File

@@ -176,6 +176,10 @@ LaserScan commonFiltering(
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, normalK, normalRadius);
scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), scan.localTransform());
UDEBUG("Normals computed (k=%d radius=%f)", normalK, normalRadius);
if(scan.empty())
{
UWARN("Only NaNs returned after normals estimation! The returned point cloud is empty. Normal k (%d) and/or radius (%f) may be too small.", normalK, normalRadius);
}
}
else
{
@@ -214,11 +218,19 @@ LaserScan commonFiltering(
{
scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), scan.localTransform());
}
if(scan.empty())
{
UWARN("Only NaNs returned after normals estimation! The returned point cloud is empty. Normal k (%d) and/or radius (%f) may be too small.", normalK, normalRadius);
}
}
else
{
normals = util3d::computeNormals(cloud, normalK, normalRadius);
scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), scan.localTransform());
if(scan.empty())
{
UWARN("Only NaNs returned after normals estimation! The returned point cloud is empty. Normal k (%d) and/or radius (%f) may be too small.", normalK, normalRadius);
}
}
UDEBUG("Normals computed (k=%d radius=%f)", normalK, normalRadius);
}
@@ -266,11 +278,19 @@ LaserScan commonFiltering(
{
scan = LaserScan(laserScan2dFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), scan.localTransform());
}
if(scan.empty())
{
UWARN("Only NaNs returned after normals estimation! The returned point cloud is empty. Normal k (%d) and/or radius (%f) may be too small.", normalK, normalRadius);
}
}
else
{
normals = util3d::computeNormals(cloud, normalK, normalRadius);
scan = LaserScan(laserScanFromPointCloud(*cloud, *normals), scanMaxPts, scan.rangeMax(), scan.localTransform());
if(scan.empty())
{
UWARN("Only NaNs returned after normals estimation! The returned point cloud is empty. Normal k (%d) and/or radius (%f) may be too small.", normalK, normalRadius);
}
}
UDEBUG("Normals computed (k=%d radius=%f)", normalK, normalRadius);
}