i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-04 00:57:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

finished util3d_filtering doc and tests

Browse Source
This commit is contained in:
matlabbe
2025-05-20 20:22:39 -07:00
parent 40f965d1d7
commit c776b9f056
3 changed files with 1401 additions and 381 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/util3d_filtering.h
+715 -319
View File
File diff suppressed because it is too large Load Diff
corelib/src/util3d_filtering.cpp
+93 -61
View File
@@ -1584,7 +1584,7 @@ pcl::IndicesPtr proportionalRadiusFilteringImpl(
{ {
std::vector<bool> kept(cloud->size()); std::vector<bool> kept(cloud->size());
tree->setInputCloud(cloud); tree->setInputCloud(cloud);
#pragma omp parallel for //#pragma omp parallel for
for(int i=0; i<(int)cloud->size(); ++i) for(int i=0; i<(int)cloud->size(); ++i)
{ {
std::vector<int> kIndices; std::vector<int> kIndices;
@@ -1595,6 +1595,7 @@ pcl::IndicesPtr proportionalRadiusFilteringImpl(
cv::Point3f point = cv::Point3f(cloud->at(i).x,cloud->at(i).y, cloud->at(i).z); cv::Point3f point = cv::Point3f(cloud->at(i).x,cloud->at(i).y, cloud->at(i).z);
float radiusSearch = factor * cv::norm(viewpoint-point); float radiusSearch = factor * cv::norm(viewpoint-point);
int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances); int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
printf("Found k=%d (radius=%f) for %d (factor=%f dist to viewpoint=%f)\n", k, radiusSearch, i, factor, cv::norm(viewpoint-point));
bool keep = k>0; bool keep = k>0;
for(int j=0; j<k && keep; ++j) for(int j=0; j<k && keep; ++j)
{ {
@@ -1607,12 +1608,14 @@ pcl::IndicesPtr proportionalRadiusFilteringImpl(
UASSERT(viewpointIter != viewpoints.end()); UASSERT(viewpointIter != viewpoints.end());
viewpoint = cv::Point3f(viewpointIter->second.x(), viewpointIter->second.y(), viewpointIter->second.z()); viewpoint = cv::Point3f(viewpointIter->second.x(), viewpointIter->second.y(), viewpointIter->second.z());
float radiusSearchTmp = factor * cv::norm(viewpoint-pointTmp) * neighborScale; float radiusSearchTmp = factor * cv::norm(viewpoint-pointTmp) * neighborScale;
printf("Check neighbor %d dist to %d = %f >? %f\n", kIndices[j], i, sqrt(distPtSqr), radiusSearchTmp);
if(distPtSqr > radiusSearchTmp*radiusSearchTmp) if(distPtSqr > radiusSearchTmp*radiusSearchTmp)
{ {
keep = false; keep = false;
} }
} }
} }
printf("keep %d = %s\n", i, keep?"true":"false");
kept[i] = keep; kept[i] = keep;
} }
pcl::IndicesPtr output(new std::vector<int>(cloud->size())); pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
@@ -1690,14 +1693,26 @@ pcl::IndicesPtr proportionalRadiusFiltering(
return proportionalRadiusFilteringImpl<pcl::PointXYZINormal>(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale); return proportionalRadiusFilteringImpl<pcl::PointXYZINormal>(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
} }
pcl::PointCloud<pcl::PointXYZ>::Ptr subtractFiltering(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & subtractCloud,
float radiusSearch,
int minNeighborsInRadius)
{
pcl::IndicesPtr indices(new std::vector<int>);
pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, subtractCloud, indices, radiusSearch, minNeighborsInRadius);
pcl::PointCloud<pcl::PointXYZ>::Ptr out(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud, *indicesOut, *out);
return out;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractFiltering( pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & subtractCloud,
float radiusSearch, float radiusSearch,
int minNeighborsInRadius) int minNeighborsInRadius)
{ {
pcl::IndicesPtr indices(new std::vector<int>); pcl::IndicesPtr indices(new std::vector<int>);
pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, minNeighborsInRadius); pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, subtractCloud, indices, radiusSearch, minNeighborsInRadius);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr out(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr out(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*cloud, *indicesOut, *out); pcl::copyPointCloud(*cloud, *indicesOut, *out);
return out; return out;
@@ -1707,8 +1722,8 @@ template<typename PointT>
pcl::IndicesPtr subtractFilteringImpl( pcl::IndicesPtr subtractFilteringImpl(
const typename pcl::PointCloud<PointT>::Ptr & cloud, const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices, const pcl::IndicesPtr & indices,
const typename pcl::PointCloud<PointT>::Ptr & substractCloud, const typename pcl::PointCloud<PointT>::Ptr & subtractCloud,
const pcl::IndicesPtr & substractIndices, const pcl::IndicesPtr & subtractIndices,
float radiusSearch, float radiusSearch,
int minNeighborsInRadius) int minNeighborsInRadius)
{ {
@@ -1719,13 +1734,13 @@ pcl::IndicesPtr subtractFilteringImpl(
{ {
pcl::IndicesPtr output(new std::vector<int>(indices->size())); pcl::IndicesPtr output(new std::vector<int>(indices->size()));
int oi = 0; // output iterator int oi = 0; // output iterator
if(substractIndices->size()) if(subtractIndices->size())
{ {
tree->setInputCloud(substractCloud, substractIndices); tree->setInputCloud(subtractCloud, subtractIndices);
} }
else else
{ {
tree->setInputCloud(substractCloud); tree->setInputCloud(subtractCloud);
} }
for(unsigned int i=0; i<indices->size(); ++i) for(unsigned int i=0; i<indices->size(); ++i)
{ {
@@ -1744,13 +1759,13 @@ pcl::IndicesPtr subtractFilteringImpl(
{ {
pcl::IndicesPtr output(new std::vector<int>(cloud->size())); pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
int oi = 0; // output iterator int oi = 0; // output iterator
if(substractIndices->size()) if(subtractIndices->size())
{ {
tree->setInputCloud(substractCloud, substractIndices); tree->setInputCloud(subtractCloud, subtractIndices);
} }
else else
{ {
tree->setInputCloud(substractCloud); tree->setInputCloud(subtractCloud);
} }
for(unsigned int i=0; i<cloud->size(); ++i) for(unsigned int i=0; i<cloud->size(); ++i)
{ {
@@ -1766,52 +1781,62 @@ pcl::IndicesPtr subtractFilteringImpl(
return output; return output;
} }
} }
pcl::IndicesPtr subtractFiltering(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & subtractCloud,
const pcl::IndicesPtr & subtractIndices,
float radiusSearch,
int minNeighborsInRadius)
{
return subtractFilteringImpl<pcl::PointXYZ>(cloud, indices, subtractCloud, subtractIndices, radiusSearch, minNeighborsInRadius);
}
pcl::IndicesPtr subtractFiltering( pcl::IndicesPtr subtractFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices, const pcl::IndicesPtr & indices,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & subtractCloud,
const pcl::IndicesPtr & substractIndices, const pcl::IndicesPtr & subtractIndices,
float radiusSearch, float radiusSearch,
int minNeighborsInRadius) int minNeighborsInRadius)
{ {
return subtractFilteringImpl<pcl::PointXYZRGB>(cloud, indices, substractCloud, substractIndices, radiusSearch, minNeighborsInRadius); return subtractFilteringImpl<pcl::PointXYZRGB>(cloud, indices, subtractCloud, subtractIndices, radiusSearch, minNeighborsInRadius);
} }
pcl::PointCloud<pcl::PointNormal>::Ptr subtractFiltering( pcl::PointCloud<pcl::PointNormal>::Ptr subtractFiltering(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const pcl::PointCloud<pcl::PointNormal>::Ptr & substractCloud, const pcl::PointCloud<pcl::PointNormal>::Ptr & subtractCloud,
float radiusSearch, float radiusSearch,
float maxAngle, float maxAngle,
int minNeighborsInRadius) int minNeighborsInRadius)
{ {
pcl::IndicesPtr indices(new std::vector<int>); pcl::IndicesPtr indices(new std::vector<int>);
pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, maxAngle, minNeighborsInRadius); pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, subtractCloud, indices, radiusSearch, maxAngle, minNeighborsInRadius);
pcl::PointCloud<pcl::PointNormal>::Ptr out(new pcl::PointCloud<pcl::PointNormal>); pcl::PointCloud<pcl::PointNormal>::Ptr out(new pcl::PointCloud<pcl::PointNormal>);
pcl::copyPointCloud(*cloud, *indicesOut, *out); pcl::copyPointCloud(*cloud, *indicesOut, *out);
return out; return out;
} }
pcl::PointCloud<pcl::PointXYZINormal>::Ptr subtractFiltering( pcl::PointCloud<pcl::PointXYZINormal>::Ptr subtractFiltering(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & substractCloud, const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & subtractCloud,
float radiusSearch, float radiusSearch,
float maxAngle, float maxAngle,
int minNeighborsInRadius) int minNeighborsInRadius)
{ {
pcl::IndicesPtr indices(new std::vector<int>); pcl::IndicesPtr indices(new std::vector<int>);
pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, maxAngle, minNeighborsInRadius); pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, subtractCloud, indices, radiusSearch, maxAngle, minNeighborsInRadius);
pcl::PointCloud<pcl::PointXYZINormal>::Ptr out(new pcl::PointCloud<pcl::PointXYZINormal>); pcl::PointCloud<pcl::PointXYZINormal>::Ptr out(new pcl::PointCloud<pcl::PointXYZINormal>);
pcl::copyPointCloud(*cloud, *indicesOut, *out); pcl::copyPointCloud(*cloud, *indicesOut, *out);
return out; return out;
} }
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr subtractFiltering( pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr subtractFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud, const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & subtractCloud,
float radiusSearch, float radiusSearch,
float maxAngle, float maxAngle,
int minNeighborsInRadius) int minNeighborsInRadius)
{ {
pcl::IndicesPtr indices(new std::vector<int>); pcl::IndicesPtr indices(new std::vector<int>);
pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, maxAngle, minNeighborsInRadius); pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, subtractCloud, indices, radiusSearch, maxAngle, minNeighborsInRadius);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr out(new pcl::PointCloud<pcl::PointXYZRGBNormal>); pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr out(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::copyPointCloud(*cloud, *indicesOut, *out); pcl::copyPointCloud(*cloud, *indicesOut, *out);
return out; return out;
@@ -1821,8 +1846,8 @@ template<typename PointT>
pcl::IndicesPtr subtractFilteringImpl( pcl::IndicesPtr subtractFilteringImpl(
const typename pcl::PointCloud<PointT>::Ptr & cloud, const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices, const pcl::IndicesPtr & indices,
const typename pcl::PointCloud<PointT>::Ptr & substractCloud, const typename pcl::PointCloud<PointT>::Ptr & subtractCloud,
const pcl::IndicesPtr & substractIndices, const pcl::IndicesPtr & subtractIndices,
float radiusSearch, float radiusSearch,
float maxAngle, float maxAngle,
int minNeighborsInRadius) int minNeighborsInRadius)
@@ -1834,13 +1859,13 @@ pcl::IndicesPtr subtractFilteringImpl(
{ {
pcl::IndicesPtr output(new std::vector<int>(indices->size())); pcl::IndicesPtr output(new std::vector<int>(indices->size()));
int oi = 0; // output iterator int oi = 0; // output iterator
if(substractIndices->size()) if(subtractIndices->size())
{ {
tree->setInputCloud(substractCloud, substractIndices); tree->setInputCloud(subtractCloud, subtractIndices);
} }
else else
{ {
tree->setInputCloud(substractCloud); tree->setInputCloud(subtractCloud);
} }
for(unsigned int i=0; i<indices->size(); ++i) for(unsigned int i=0; i<indices->size(); ++i)
{ {
@@ -1857,7 +1882,7 @@ pcl::IndicesPtr subtractFilteringImpl(
int count = k; int count = k;
for(int j=0; j<count && k >= minNeighborsInRadius; ++j) for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
{ {
Eigen::Vector4f v(substractCloud->at(kIndices.at(j)).normal_x, substractCloud->at(kIndices.at(j)).normal_y, substractCloud->at(kIndices.at(j)).normal_z, 0.0f); Eigen::Vector4f v(subtractCloud->at(kIndices.at(j)).normal_x, subtractCloud->at(kIndices.at(j)).normal_y, subtractCloud->at(kIndices.at(j)).normal_z, 0.0f);
if(uIsFinite(v[0]) && if(uIsFinite(v[0]) &&
uIsFinite(v[1]) && uIsFinite(v[1]) &&
uIsFinite(v[2])) uIsFinite(v[2]))
@@ -1891,13 +1916,13 @@ pcl::IndicesPtr subtractFilteringImpl(
{ {
pcl::IndicesPtr output(new std::vector<int>(cloud->size())); pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
int oi = 0; // output iterator int oi = 0; // output iterator
if(substractIndices->size()) if(subtractIndices->size())
{ {
tree->setInputCloud(substractCloud, substractIndices); tree->setInputCloud(subtractCloud, subtractIndices);
} }
else else
{ {
tree->setInputCloud(substractCloud); tree->setInputCloud(subtractCloud);
} }
for(unsigned int i=0; i<cloud->size(); ++i) for(unsigned int i=0; i<cloud->size(); ++i)
{ {
@@ -1914,7 +1939,7 @@ pcl::IndicesPtr subtractFilteringImpl(
int count = k; int count = k;
for(int j=0; j<count && k >= minNeighborsInRadius; ++j) for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
{ {
Eigen::Vector4f v(substractCloud->at(kIndices.at(j)).normal_x, substractCloud->at(kIndices.at(j)).normal_y, substractCloud->at(kIndices.at(j)).normal_z, 0.0f); Eigen::Vector4f v(subtractCloud->at(kIndices.at(j)).normal_x, subtractCloud->at(kIndices.at(j)).normal_y, subtractCloud->at(kIndices.at(j)).normal_z, 0.0f);
if(uIsFinite(v[0]) && if(uIsFinite(v[0]) &&
uIsFinite(v[1]) && uIsFinite(v[1]) &&
uIsFinite(v[2])) uIsFinite(v[2]))
@@ -1949,42 +1974,42 @@ pcl::IndicesPtr subtractFilteringImpl(
pcl::IndicesPtr subtractFiltering( pcl::IndicesPtr subtractFiltering(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices, const pcl::IndicesPtr & indices,
const pcl::PointCloud<pcl::PointNormal>::Ptr & substractCloud, const pcl::PointCloud<pcl::PointNormal>::Ptr & subtractCloud,
const pcl::IndicesPtr & substractIndices, const pcl::IndicesPtr & subtractIndices,
float radiusSearch, float radiusSearch,
float maxAngle, float maxAngle,
int minNeighborsInRadius) int minNeighborsInRadius)
{ {
return subtractFilteringImpl<pcl::PointNormal>(cloud, indices, substractCloud, substractIndices, radiusSearch, maxAngle, minNeighborsInRadius); return subtractFilteringImpl<pcl::PointNormal>(cloud, indices, subtractCloud, subtractIndices, radiusSearch, maxAngle, minNeighborsInRadius);
} }
pcl::IndicesPtr subtractFiltering( pcl::IndicesPtr subtractFiltering(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const pcl::IndicesPtr & indices, const pcl::IndicesPtr & indices,
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & substractCloud, const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & subtractCloud,
const pcl::IndicesPtr & substractIndices, const pcl::IndicesPtr & subtractIndices,
float radiusSearch, float radiusSearch,
float maxAngle, float maxAngle,
int minNeighborsInRadius) int minNeighborsInRadius)
{ {
return subtractFilteringImpl<pcl::PointXYZINormal>(cloud, indices, substractCloud, substractIndices, radiusSearch, maxAngle, minNeighborsInRadius); return subtractFilteringImpl<pcl::PointXYZINormal>(cloud, indices, subtractCloud, subtractIndices, radiusSearch, maxAngle, minNeighborsInRadius);
} }
pcl::IndicesPtr subtractFiltering( pcl::IndicesPtr subtractFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices, const pcl::IndicesPtr & indices,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud, const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & subtractCloud,
const pcl::IndicesPtr & substractIndices, const pcl::IndicesPtr & subtractIndices,
float radiusSearch, float radiusSearch,
float maxAngle, float maxAngle,
int minNeighborsInRadius) int minNeighborsInRadius)
{ {
return subtractFilteringImpl<pcl::PointXYZRGBNormal>(cloud, indices, substractCloud, substractIndices, radiusSearch, maxAngle, minNeighborsInRadius); return subtractFilteringImpl<pcl::PointXYZRGBNormal>(cloud, indices, subtractCloud, subtractIndices, radiusSearch, maxAngle, minNeighborsInRadius);
} }
pcl::IndicesPtr subtractAdaptiveFiltering( pcl::IndicesPtr subtractAdaptiveFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices, const pcl::IndicesPtr & indices,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & subtractCloud,
const pcl::IndicesPtr & substractIndices, const pcl::IndicesPtr & subtractIndices,
float radiusSearchRatio, float radiusSearchRatio,
int minNeighborsInRadius, int minNeighborsInRadius,
const Eigen::Vector3f & viewpoint) const Eigen::Vector3f & viewpoint)
@@ -1998,13 +2023,13 @@ pcl::IndicesPtr subtractAdaptiveFiltering(
{ {
pcl::IndicesPtr output(new std::vector<int>(indices->size())); pcl::IndicesPtr output(new std::vector<int>(indices->size()));
int oi = 0; // output iterator int oi = 0; // output iterator
if(substractIndices->size()) if(subtractIndices->size())
{ {
tree->setInputCloud(substractCloud, substractIndices); tree->setInputCloud(subtractCloud, subtractIndices);
} }
else else
{ {
tree->setInputCloud(substractCloud); tree->setInputCloud(subtractCloud);
} }
for(unsigned int i=0; i<indices->size(); ++i) for(unsigned int i=0; i<indices->size(); ++i)
{ {
@@ -2030,13 +2055,13 @@ pcl::IndicesPtr subtractAdaptiveFiltering(
{ {
pcl::IndicesPtr output(new std::vector<int>(cloud->size())); pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
int oi = 0; // output iterator int oi = 0; // output iterator
if(substractIndices->size()) if(subtractIndices->size())
{ {
tree->setInputCloud(substractCloud, substractIndices); tree->setInputCloud(subtractCloud, subtractIndices);
} }
else else
{ {
tree->setInputCloud(substractCloud); tree->setInputCloud(subtractCloud);
} }
for(unsigned int i=0; i<cloud->size(); ++i) for(unsigned int i=0; i<cloud->size(); ++i)
{ {
@@ -2062,8 +2087,8 @@ pcl::IndicesPtr subtractAdaptiveFiltering(
pcl::IndicesPtr subtractAdaptiveFiltering( pcl::IndicesPtr subtractAdaptiveFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices, const pcl::IndicesPtr & indices,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud, const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & subtractCloud,
const pcl::IndicesPtr & substractIndices, const pcl::IndicesPtr & subtractIndices,
float radiusSearchRatio, float radiusSearchRatio,
float maxAngle, float maxAngle,
int minNeighborsInRadius, int minNeighborsInRadius,
@@ -2077,13 +2102,13 @@ pcl::IndicesPtr subtractAdaptiveFiltering(
{ {
pcl::IndicesPtr output(new std::vector<int>(indices->size())); pcl::IndicesPtr output(new std::vector<int>(indices->size()));
int oi = 0; // output iterator int oi = 0; // output iterator
if(substractIndices->size()) if(subtractIndices->size())
{ {
tree->setInputCloud(substractCloud, substractIndices); tree->setInputCloud(subtractCloud, subtractIndices);
} }
else else
{ {
tree->setInputCloud(substractCloud); tree->setInputCloud(subtractCloud);
} }
for(unsigned int i=0; i<indices->size(); ++i) for(unsigned int i=0; i<indices->size(); ++i)
{ {
@@ -2106,7 +2131,7 @@ pcl::IndicesPtr subtractAdaptiveFiltering(
int count = k; int count = k;
for(int j=0; j<count && k >= minNeighborsInRadius; ++j) for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
{ {
Eigen::Vector4f v(substractCloud->at(kIndices.at(j)).normal_x, substractCloud->at(kIndices.at(j)).normal_y, substractCloud->at(kIndices.at(j)).normal_z, 0.0f); Eigen::Vector4f v(subtractCloud->at(kIndices.at(j)).normal_x, subtractCloud->at(kIndices.at(j)).normal_y, subtractCloud->at(kIndices.at(j)).normal_z, 0.0f);
if(uIsFinite(v[0]) && if(uIsFinite(v[0]) &&
uIsFinite(v[1]) && uIsFinite(v[1]) &&
uIsFinite(v[2])) uIsFinite(v[2]))
@@ -2142,13 +2167,13 @@ pcl::IndicesPtr subtractAdaptiveFiltering(
{ {
pcl::IndicesPtr output(new std::vector<int>(cloud->size())); pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
int oi = 0; // output iterator int oi = 0; // output iterator
if(substractIndices->size()) if(subtractIndices->size())
{ {
tree->setInputCloud(substractCloud, substractIndices); tree->setInputCloud(subtractCloud, subtractIndices);
} }
else else
{ {
tree->setInputCloud(substractCloud); tree->setInputCloud(subtractCloud);
} }
for(unsigned int i=0; i<cloud->size(); ++i) for(unsigned int i=0; i<cloud->size(); ++i)
{ {
@@ -2171,7 +2196,7 @@ pcl::IndicesPtr subtractAdaptiveFiltering(
int count = k; int count = k;
for(int j=0; j<count && k >= minNeighborsInRadius; ++j) for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
{ {
Eigen::Vector4f v(substractCloud->at(kIndices.at(j)).normal_x, substractCloud->at(kIndices.at(j)).normal_y, substractCloud->at(kIndices.at(j)).normal_z, 0.0f); Eigen::Vector4f v(subtractCloud->at(kIndices.at(j)).normal_x, subtractCloud->at(kIndices.at(j)).normal_y, subtractCloud->at(kIndices.at(j)).normal_z, 0.0f);
if(uIsFinite(v[0]) && if(uIsFinite(v[0]) &&
uIsFinite(v[1]) && uIsFinite(v[1]) &&
uIsFinite(v[2])) uIsFinite(v[2]))
@@ -2655,12 +2680,19 @@ pcl::IndicesPtr extractPlane(
seg.setMethodType (pcl::SAC_RANSAC); seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (distanceThreshold); seg.setDistanceThreshold (distanceThreshold);
seg.setInputCloud (cloud); try {
if(indices->size()) seg.setInputCloud (cloud);
if(indices.get() && indices->size())
{
seg.setIndices(indices);
}
seg.segment (*inliers, *coefficients);
}
catch(const pcl::PCLException& e)
{ {
seg.setIndices(indices); UWARN("PCL exception: %s", e.what());
return pcl::IndicesPtr(new std::vector<int>); // return empty indices
} }
seg.segment (*inliers, *coefficients);
if(coefficientsOut) if(coefficientsOut)
{ {
corelib/test/test_util3d_filtering.cpp
+592
View File
@@ -1343,3 +1343,595 @@ TEST(Util3dFiltering, frustumFilteringInvalidClipPlaneTriggersAssertion)
EXPECT_THROW(util3d::frustumFiltering(cloud, indices, cameraPose, 60.0f, 45.0f, 5.0f, 1.0f, false), UException); EXPECT_THROW(util3d::frustumFiltering(cloud, indices, cameraPose, 60.0f, 45.0f, 5.0f, 1.0f, false), UException);
} }
TEST(Util3dFiltering, removeNaNFromPointCloud)
{
auto cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
// Add valid points
cloud->push_back(pcl::PointXYZ(1.0f, 2.0f, 3.0f));
cloud->push_back(pcl::PointXYZ(4.0f, 5.0f, 6.0f));
// Add NaN point
cloud->push_back(pcl::PointXYZ(
std::numeric_limits<float>::quiet_NaN(),
std::numeric_limits<float>::quiet_NaN(),
std::numeric_limits<float>::quiet_NaN()));
cloud->is_dense = false;
ASSERT_EQ(cloud->size(), 3u);
auto filtered = util3d::removeNaNFromPointCloud(cloud);
// Check that the NaN point was removed
EXPECT_EQ(filtered->size(), 2u);
// Check that the remaining points match the original valid points
EXPECT_FLOAT_EQ(filtered->points[0].x, 1.0f);
EXPECT_FLOAT_EQ(filtered->points[0].y, 2.0f);
EXPECT_FLOAT_EQ(filtered->points[0].z, 3.0f);
EXPECT_FLOAT_EQ(filtered->points[1].x, 4.0f);
EXPECT_FLOAT_EQ(filtered->points[1].y, 5.0f);
EXPECT_FLOAT_EQ(filtered->points[1].z, 6.0f);
}
TEST(Util3dFiltering, removeNaNNormalsFromPointCloud)
{
auto cloud = pcl::PointCloud<pcl::PointNormal>::Ptr(new pcl::PointCloud<pcl::PointNormal>);
// Add valid point with proper normals
pcl::PointNormal pt1;
pt1.x = pt1.y = pt1.z = 0.0f;
pt1.normal_x = 1.0f; pt1.normal_y = 0.0f; pt1.normal_z = 0.0f;
cloud->push_back(pt1);
// Add point with NaN normal_x
pcl::PointNormal pt2 = pt1;
pt2.normal_x = std::numeric_limits<float>::quiet_NaN();
cloud->push_back(pt2);
// Add point with NaN normal_y
pcl::PointNormal pt3 = pt1;
pt3.normal_y = std::numeric_limits<float>::quiet_NaN();
cloud->push_back(pt3);
// Add point with NaN normal_z
pcl::PointNormal pt4 = pt1;
pt4.normal_z = std::numeric_limits<float>::quiet_NaN();
cloud->push_back(pt4);
ASSERT_EQ(cloud->size(), 4u);
auto filtered = util3d::removeNaNNormalsFromPointCloud(cloud);
// Only the valid point should remain
ASSERT_EQ(filtered->size(), 1u);
EXPECT_FLOAT_EQ(filtered->points[0].normal_x, 1.0f);
EXPECT_FLOAT_EQ(filtered->points[0].normal_y, 0.0f);
EXPECT_FLOAT_EQ(filtered->points[0].normal_z, 0.0f);
}
TEST(Util3dFiltering, radiusFilteringWithFewNeighbors)
{
auto cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
// Create a simple grid of points: 3 points within radius, 2 isolated
cloud->push_back(pcl::PointXYZ(0.0f, 0.0f, 0.0f)); // Index 0
cloud->push_back(pcl::PointXYZ(0.05f, 0.0f, 0.0f)); // Index 1
cloud->push_back(pcl::PointXYZ(0.1f, 0.0f, 0.0f)); // Index 2
cloud->push_back(pcl::PointXYZ(5.0f, 0.0f, 0.0f)); // Index 3 (isolated)
cloud->push_back(pcl::PointXYZ(-5.0f, 0.0f, 0.0f)); // Index 4 (isolated)
pcl::IndicesPtr indices(new std::vector<int>()); // Empty -> test whole cloud
// Radius 0.11, minimum 1 neighbor (excluding self, so 2 including self)
float radiusSearch = 0.11f;
int minNeighbors = 1;
auto filtered = util3d::radiusFiltering(cloud, indices, radiusSearch, minNeighbors);
// Only indices 0, 1, 2 are near each other; 3 and 4 are too far
ASSERT_EQ(filtered->size(), 3u);
EXPECT_EQ(filtered->at(0), 0);
EXPECT_EQ(filtered->at(1), 1);
EXPECT_EQ(filtered->at(2), 2);
}
TEST(Util3dFiltering, radiusFilteringProvidedIndicesSubset)
{
auto cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
// Add a mix of close and isolated points
cloud->push_back(pcl::PointXYZ(0.0f, 0.0f, 0.0f)); // 0 - neighbor
cloud->push_back(pcl::PointXYZ(0.05f, 0.0f, 0.0f)); // 1 - neighbor
cloud->push_back(pcl::PointXYZ(1.0f, 0.0f, 0.0f)); // 2 - too far
pcl::IndicesPtr subset(new std::vector<int>{0, 1}); // Only test points 0 and 1
float radiusSearch = 0.1f;
int minNeighbors = 1;
auto filtered = util3d::radiusFiltering(cloud, subset, radiusSearch, minNeighbors);
ASSERT_EQ(filtered->size(), 2u);
EXPECT_EQ(filtered->at(0), 0);
EXPECT_EQ(filtered->at(1), 1);
}
TEST(Util3dFiltering, proportionalRadiusFilteringBasic)
{
auto cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
// Points seen from 10 meters away, so ~5 cm radius
cloud->push_back(pcl::PointXYZ(0, 0, 0)); // 0, farthest is 2 (4cm < 10cm) where 10cm is error viewpoint 1 times neighborScale
cloud->push_back(pcl::PointXYZ(0.001, 0, 0)); // 1, farthest is 2 (3.9cm < 10cm) where 10cm is error viewpoint 1 times neighborScale
cloud->push_back(pcl::PointXYZ(0.04f, 0, 0)); // 2, farthest is 0 (4cm < 10cm) where 10cm is error viewpoint 1 times neighborScale
cloud->push_back(pcl::PointXYZ(0.06f, 0, 0)); // 3, farthest is 2 (2cm < 10cm) where 10cm is error viewpoint 1 times neighborScale
cloud->push_back(pcl::PointXYZ(0.15f, 0, 0)); // 4, alone, kept
// Points seen from 10 meters away, so ~50 cm radius
cloud->push_back(pcl::PointXYZ(0, 0, 0)); // 5, reject because of 4 (15cm > 10cm) where 10cm is error viewpoint 1 times neighborScale
cloud->push_back(pcl::PointXYZ(0.4f, 0, 0)); // 6, reject because of 0 (40cm > 10cm) where 10cm is error viewpoint 1 times neighborScale
cloud->push_back(pcl::PointXYZ(0.7f, 0, 0)); // 7, farthest is 6 (30cm < 100cm) where 100cm is error viewpoint 2 times neighborScale
cloud->push_back(pcl::PointXYZ(1.5f, 0, 0)); // 8, alone, kept
std::vector<int> viewpointIndices = {0, 0, 0, 0, 0, 1, 1, 1, 1};
std::map<int, Transform> viewpoints;
viewpoints[0] = Transform(-1, 0, 0, 0,0,0);
viewpoints[1] = Transform(-10, 0, 0, 0,0,0);
pcl::IndicesPtr indices(new std::vector<int>()); // process all
float factor = 0.05f; // 6 cm radius at 1 meter, 60 cm radius at 10 meters
float neighborScale = 2.0f;
pcl::IndicesPtr result = util3d::proportionalRadiusFiltering(
cloud, indices, viewpointIndices, viewpoints, factor, neighborScale
);
std::vector<int> expected = {0,1,2,3,4,7,8};
ASSERT_EQ(result->size(), expected.size());
for (int idx : *result) {
EXPECT_NE(std::find(expected.begin(), expected.end(), idx), expected.end());
}
// If we adjust neighborScale to 4, 5 would be accepted
result = util3d::proportionalRadiusFiltering(
cloud, indices, viewpointIndices, viewpoints, factor, 4.0f
);
std::vector<int> expected2 = {0,1,2,3,4,5,7,8};
ASSERT_EQ(result->size(), expected2.size());
for (int idx : *result) {
EXPECT_NE(std::find(expected2.begin(), expected2.end(), idx), expected2.end());
}
}
TEST(Util3dFiltering, proportionalRadiusFilteringUsingProvidedIndices)
{
auto cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
cloud->push_back(pcl::PointXYZ(0, 0, 0)); // 0
cloud->push_back(pcl::PointXYZ(0.05, 0, 0)); // 1
cloud->push_back(pcl::PointXYZ(0.1, 0, 0)); // 2
cloud->push_back(pcl::PointXYZ(10, 0, 0)); // 3 (far)
std::vector<int> viewpointIndices = {0, 0, 0, 1};
std::map<int, Transform> viewpoints = {
{0, Transform(0, 0, -1)},
{1, Transform(10, 0, -1)}
};
// Only test indices 0 and 3
pcl::IndicesPtr indices(new std::vector<int>{0, 3});
float factor = 0.1f;
float neighborScale = 1.5f;
auto result = util3d::proportionalRadiusFiltering(
cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
// Point 0 has no neighbors in filtered subset, 3 too far
ASSERT_EQ(result->size(), 0u);
}
TEST(Util3dFiltering, proportionalRadiusFilteringAllPointsNoNeighbors)
{
auto cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
cloud->push_back(pcl::PointXYZ(0, 0, 0));
cloud->push_back(pcl::PointXYZ(10, 0, 0));
cloud->push_back(pcl::PointXYZ(20, 0, 0));
std::vector<int> viewpointIndices = {0, 1, 2};
std::map<int, Transform> viewpoints = {
{0, Transform(0, 0, -1)},
{1, Transform(10, 0, -1)},
{2, Transform(20, 0, -1)}
};
pcl::IndicesPtr indices(new std::vector<int>);
float factor = 0.001f; // tiny radius
float neighborScale = 1.0f;
auto result = util3d::proportionalRadiusFiltering(
cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
ASSERT_EQ(result->size(), 0u);
}
TEST(Util3dFiltering, proportionalRadiusFilteringInvalidViewpointID)
{
auto cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
cloud->push_back(pcl::PointXYZ(0, 0, 0));
std::vector<int> viewpointIndices = {42}; // invalid ID
std::map<int, Transform> viewpoints = {
{0, Transform(1, 0, 0)}
};
pcl::IndicesPtr indices(new std::vector<int>);
float factor = 0.1f;
float neighborScale = 1.0f;
EXPECT_THROW(util3d::proportionalRadiusFiltering(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale), UException);
}
TEST(Util3dFiltering, subtractFilteringBasicFiltering)
{
auto cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
cloud->push_back(pcl::PointXYZ(0, 0, 0));
cloud->push_back(pcl::PointXYZ(1, 0, 0));
cloud->push_back(pcl::PointXYZ(2, 0, 0));
pcl::IndicesPtr indices(new std::vector<int>{0, 1, 2});
auto subtractCloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
subtractCloud->push_back(pcl::PointXYZ(1.05, 0, 0));
pcl::IndicesPtr subtractIndices(new std::vector<int>); // Empty = use all
float radiusSearch = 0.2f;
int minNeighborsInRadius = 1;
auto result = util3d::subtractFiltering(
cloud,
indices,
subtractCloud,
subtractIndices,
radiusSearch,
minNeighborsInRadius);
// Expected: point 1 has a neighbor within radius -> excluded
// Points 0 and 2 do not -> included
ASSERT_EQ(result->size(), 2);
EXPECT_EQ(result->at(0), 0);
EXPECT_EQ(result->at(1), 2);
}
TEST(Util3dFiltering, subtractAdaptiveFilteringXYZRGB)
{
auto cloud = pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
auto subtractCloud = pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
// Input cloud: 3 points
pcl::PointXYZRGB pt;
pt.z = 1;
cloud->push_back(pt); // (0 ,0, 1)
pt.x = 1;
cloud->push_back(pt); // (1 ,0, 1)
pt.x = 2;
cloud->push_back(pt); // (2 ,0, 1)
// Subtract cloud: one neighbor close to point 1
pt.x = 1.05;
subtractCloud->push_back(pt); // (1.05 ,0, 1)
pcl::IndicesPtr indices(new std::vector<int>{0, 1, 2});
pcl::IndicesPtr subtractIndices(new std::vector<int>); // use full subtract cloud
float radiusSearchRatio = 0.2f; // Adaptive search radius
int minNeighborsInRadius = 1;
Eigen::Vector3f viewpoint(0, 0, 0);
pcl::IndicesPtr result = util3d::subtractAdaptiveFiltering(
cloud, indices, subtractCloud, subtractIndices,
radiusSearchRatio, minNeighborsInRadius, viewpoint
);
// Point 1 should be excluded (has a neighbor), others retained
ASSERT_EQ(result->size(), 2);
EXPECT_EQ(result->at(0), 0);
EXPECT_EQ(result->at(1), 2);
}
TEST(Util3dFiltering, subtractAdaptiveFilteringXYZRGBNormal)
{
auto cloud = pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
auto subtractCloud = pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
// Input point with normal (point 0)
pcl::PointXYZRGBNormal pt;
pt.x = 0;
pt.y = 0;
pt.z = 1;
pt.normal_x = 0;
pt.normal_y = 0;
pt.normal_z = 1;
cloud->push_back(pt);
// Neighbor with almost same normal
pcl::PointXYZRGBNormal neighbor1 = pt;
neighbor1.x = 0.05f;
neighbor1.normal_x = cos(89.0f*M_PI/180.0f);
neighbor1.normal_z = sin(89.0f*M_PI/180.0f);
subtractCloud->push_back(neighbor1);
// Neighbor with very different normal
pcl::PointXYZRGBNormal neighbor2 = pt;
neighbor2.x = 0.05f;
neighbor2.normal_x = 1;
neighbor2.normal_y = 0;
neighbor2.normal_z = 0;
subtractCloud->push_back(neighbor2);
pcl::IndicesPtr indices(new std::vector<int>{0});
pcl::IndicesPtr subtractIndices(new std::vector<int>); // full cloud
float radiusSearchRatio = 0.2f;
float maxAngle = 20.0f*M_PI/180.0f; // 20 degrees max
int minNeighborsInRadius = 1;
Eigen::Vector3f viewpoint(0, 0, 0);
pcl::IndicesPtr result = util3d::subtractAdaptiveFiltering(
cloud, indices, subtractCloud, subtractIndices,
radiusSearchRatio, maxAngle, minNeighborsInRadius, viewpoint
);
// neighbor2 exceeds angle threshold and should be ignored
// neighbor1 should pass if included; result depends on number of valid neighbors
ASSERT_EQ(result->size(), 0) << "Point should be excluded due to having a valid neighbor";
// Now set maxAngle very low so even neighbor1 is excluded
result = util3d::subtractAdaptiveFiltering(
cloud, indices, subtractCloud, subtractIndices,
radiusSearchRatio, 0.5f*M_PI/180.0f, minNeighborsInRadius, viewpoint
);
// No valid neighbors left -> point should be retained
ASSERT_EQ(result->size(), 1);
EXPECT_EQ(result->at(0), 0);
}
TEST(Util3dFiltering, normalFilteringBasic)
{
// Create a flat plane of points along XY plane (normals should be along Z)
auto cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
for (float x = -1.0f; x <= 1.0f; x += 0.5f)
{
for (float y = -1.0f; y <= 1.0f; y += 0.5f)
{
cloud->push_back(pcl::PointXYZ(x, y, 0.0f));
}
}
pcl::IndicesPtr indices(new std::vector<int>()); // Use all points
// We expect normals to be [0, 0, 1]
Eigen::Vector4f refNormal(0, 0, 1, 0);
float angleMax = 10.0f*M_PI/180.0f; // Allow 10 degrees of deviation
int normalKSearch = 5;
Eigen::Vector4f viewpoint(0, 0, 1, 0);
float groundNormalsUp = 0.0f; // No flipping
auto result = util3d::normalFiltering(
cloud,
indices,
angleMax,
refNormal,
normalKSearch,
viewpoint,
groundNormalsUp);
// Expect all points to pass (normals pointing up)
ASSERT_EQ(result->size(), cloud->size());
for (int idx : *result)
{
EXPECT_GE(idx, 0);
EXPECT_LT(idx, static_cast<int>(cloud->size()));
}
}
TEST(Util3dFiltering, normalFilteringRejectNormalsWithLargeDeviation)
{
// Construct a simple slanted surface so normals will not be vertical
auto cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
for (float x = -1.0f; x <= 1.0f; x += 1.0f)
{
cloud->push_back(pcl::PointXYZ(x, 0, x)); // Diagonal line: normals ≈ 45 degrees
}
pcl::IndicesPtr indices(new std::vector<int>()); // Use all
Eigen::Vector4f refNormal(0, 0, 1, 0); // Want normals close to Z
float angleMax = 10.0f*M_PI/180.0f; // Tight filter: only near vertical normals
int normalKSearch = 3;
Eigen::Vector4f viewpoint(0, 0, 0, 0);
float groundNormalsUp = 0.0f;
auto result = util3d::normalFiltering(
cloud,
indices,
angleMax,
refNormal,
normalKSearch,
viewpoint,
groundNormalsUp);
// Expect points to be rejected due to large angle from reference normal
ASSERT_EQ(result->size(), 0);
}
// Test for PointXYZ type
TEST(Util3dFiltering, extractClustersBasic)
{
auto cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
// Generate 3 distinct clusters: centered at (0,0), (5,0), and (10,0)
for (float i = 0; i < 3; ++i)
{
float cx = i * 5.0f;
for (int j = 0; j < 10; ++j)
{
cloud->push_back(pcl::PointXYZ(cx + static_cast<float>(rand()) / RAND_MAX * 0.1f,
static_cast<float>(rand()) / RAND_MAX * 0.1f,
0.0f));
}
}
pcl::IndicesPtr indices(new std::vector<int>()); // Use all points
float clusterTolerance = 1.0f;
int minClusterSize = 3;
int maxClusterSize = 20;
int biggestClusterIdx = -1;
auto clusters = util3d::extractClusters(
cloud, indices, clusterTolerance,
minClusterSize, maxClusterSize, &biggestClusterIdx);
// We should find 3 clusters
ASSERT_EQ(clusters.size(), 3);
// Each cluster should have 10 points
for (const auto& cluster : clusters)
{
ASSERT_EQ(cluster->size(), 10);
}
// Largest cluster index should be valid
ASSERT_GE(biggestClusterIdx, 0);
ASSERT_LT(biggestClusterIdx, static_cast<int>(clusters.size()));
ASSERT_EQ(clusters[biggestClusterIdx]->size(), 10);
}
TEST(Util3dFiltering, extractClustersMinClusterSize)
{
auto cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
// One big cluster and one tiny cluster
for (int i = 0; i < 10; ++i)
cloud->push_back(pcl::PointXYZ(i * 0.05f, 0, 0)); // Close points
cloud->push_back(pcl::PointXYZ(10.0f, 0, 0)); // Isolated point
pcl::IndicesPtr indices(new std::vector<int>());
float clusterTolerance = 0.2f;
int minClusterSize = 2;
int maxClusterSize = 20;
int biggestClusterIdx = -1;
auto clusters = util3d::extractClusters(
cloud, indices, clusterTolerance,
minClusterSize, maxClusterSize, &biggestClusterIdx);
// Should only find one cluster (ignoring the single point)
ASSERT_EQ(clusters.size(), 1);
ASSERT_EQ(clusters[0]->size(), 10);
ASSERT_EQ(biggestClusterIdx, 0);
}
// Test case: Extract the points corresponding to the specified indices
TEST(Util3dFiltering, extractIndices) {
auto cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
cloud->resize(5);
// Assigning values to the cloud points
for (size_t i = 0; i < cloud->points.size(); ++i) {
cloud->points[i].x = static_cast<float>(i);
cloud->points[i].y = static_cast<float>(i * 2);
cloud->points[i].z = static_cast<float>(i * 3);
}
// Creating a set of indices to extract (e.g., indices 1 and 3)
pcl::IndicesPtr indices(new std::vector<int>({1, 3}));
pcl::IndicesPtr output = util3d::extractIndices(cloud, indices, false);
// Verify the output contains the correct indices
ASSERT_EQ(output->size(), 2);
EXPECT_EQ(output->at(0), 1); // Index 1 should be extracted
EXPECT_EQ(output->at(1), 3); // Index 3 should be extracted
// Verify the points corresponding to those indices
EXPECT_FLOAT_EQ(cloud->points[output->at(0)].x, 1.0);
EXPECT_FLOAT_EQ(cloud->points[output->at(1)].x, 3.0);
output = util3d::extractIndices(cloud, indices, true);
// Verify the output contains the correct indices (everything except 1 and 3)
ASSERT_EQ(output->size(), 3); // There should be 3 points left (0, 2, 4)
EXPECT_EQ(output->at(0), 0); // Index 0 should be included
EXPECT_EQ(output->at(1), 2); // Index 2 should be included
EXPECT_EQ(output->at(2), 4); // Index 4 should be included
// Verify that the excluded points are not in the output
EXPECT_NE(output->at(0), 1); // Index 1 should not be in the output
EXPECT_NE(output->at(1), 3); // Index 3 should not be in the output
}
TEST(Util3dFiltering, extractPlane) {
auto cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
// Define some points that should form a plane
cloud->push_back(pcl::PointXYZ(0.0, 0.0, 0.001));
cloud->push_back(pcl::PointXYZ(1.0, 0.0, 0.002));
cloud->push_back(pcl::PointXYZ(0.0, 1.0, -0.001));
cloud->push_back(pcl::PointXYZ(1.0, 1.0, 0.002)); // All points are close to z = 0 plane
// Optionally, create a subset of indices to test
pcl::IndicesPtr indices(new std::vector<int>{0, 1, 2, 3});
pcl::ModelCoefficients coefficients;
pcl::IndicesPtr inliers = util3d::extractPlane(cloud, indices, 0.01, 100, &coefficients);
// Check if the extracted plane coefficients are correct
EXPECT_LT(fabs(coefficients.values[0]), 0.01); // Plane normal x = 0
EXPECT_LT(fabs(coefficients.values[1]), 0.01); // Plane normal y = 0
EXPECT_GT(fabs(coefficients.values[2]), 0.99); // Plane normal z = 1 (for the z = 0 plane)
EXPECT_LT(fabs(coefficients.values[3]), 0.01); // Plane offset (z = 0)
// Check that all the indices were identified as inliers
EXPECT_EQ(inliers->size(), 4); // All points should be inliers
coefficients = pcl::ModelCoefficients();
pcl::IndicesPtr subsetIndices = pcl::IndicesPtr(new std::vector<int>{0, 1, 2}); // Subset of points
inliers = util3d::extractPlane(cloud, subsetIndices, 0.01, 100, &coefficients);
// Check if the extracted plane coefficients are correct
EXPECT_LT(fabs(coefficients.values[0]), 0.01); // Plane normal x = 0
EXPECT_LT(fabs(coefficients.values[1]), 0.01); // Plane normal y = 0
EXPECT_GT(fabs(coefficients.values[2]), 0.99); // Plane normal z = 1 (for the z = 0 plane)
EXPECT_LT(fabs(coefficients.values[3]), 0.01); // Plane offset (z = 0)
// Check that the correct number of inliers are found
EXPECT_EQ(inliers->size(), 3); // Only 3 points were used
// Set a very strict distance threshold to ensure no inliers
inliers = util3d::extractPlane(cloud, indices, 0.0001, 100, &coefficients);
// Check that no inliers are found
EXPECT_EQ(inliers->size(), 3); // less than 4 inliers with such a small threshold
pcl::IndicesPtr emptyIndices; // Empty indices
coefficients = pcl::ModelCoefficients();
// When indices are empty, the function should use the whole cloud
inliers = util3d::extractPlane(cloud, emptyIndices, 0.01, 100, &coefficients);
// Check if the coefficients correspond to a plane in the z = 0 plane
EXPECT_LT(fabs(coefficients.values[0]), 0.01); // Plane normal x = 0
EXPECT_LT(fabs(coefficients.values[1]), 0.01); // Plane normal y = 0
EXPECT_GT(fabs(coefficients.values[2]), 0.99); // Plane normal z = 1 (for the z = 0 plane)
EXPECT_LT(fabs(coefficients.values[3]), 0.01); // Plane offset (z = 0)
// Check if all points are inliers (since we are using the whole cloud)
EXPECT_EQ(inliers->size(), 4); // All points should be inliers
}