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

util3d_filtering.h: more tests and doc

Browse Source
This commit is contained in:
matlabbe
2025-05-04 16:17:36 -07:00
parent 79dec73030
commit 8d3b78e760
4 changed files with 954 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/util3d_filtering.h
+240 -2
View File
@@ -97,50 +97,134 @@ RTABMAP_DEPRECATED LaserScan RTABMAP_CORE_EXPORT commonFiltering(
float normalRadius,
bool forceGroundNormalsUp);
/**
* @brief Filters a LaserScan data on a minimum and maximum Euclidean range.
*
* This function removes scan points from the input LaserScan that fall
* outside the specified minimum and maximum range (in meters) from the LaserScan's viewpoint.
*
* @param scan The input LaserScan object containing the scan data.
* @param rangeMin The minimum range threshold. Points closer than this will be excluded.
* @param rangeMax The maximum range threshold. Points farther than this will be excluded.
* @return A new LaserScan object containing only the points within the specified range.
* If the input scan is empty or the range limits are both zero, the original scan is returned.
*
* @note The function handles both 2D and 3D scans based on the `scan.is2d()` flag.
* The function doesn't keep the scan organized if the input is.
* @throws Assertion failure if either `rangeMin` or `rangeMax` is negative.
*/
LaserScan RTABMAP_CORE_EXPORT rangeFiltering(
const LaserScan & scan,
float rangeMin,
float rangeMax);
/**
* @defgroup PointCloudRangeFiltering Range Filtering of PCL Point Clouds
* @brief Filters a point cloud based on a minimum and maximum Euclidean range.
*
* This templated function computes the squared Euclidean distance of each point
* in the given point cloud and returns indices of points whose distance falls
* within the specified range limits. If no indices are provided, the entire cloud
* is evaluated.
*
* @param cloud The input point cloud to be filtered.
* @param indices The subset of point indices to evaluate. If empty, the full cloud is used.
* @param rangeMin Minimum distance threshold. Points closer than this are excluded.
* @param rangeMax Maximum distance threshold. Points farther than this are excluded.
* @return pcl::IndicesPtr Pointer to a vector of indices that passed the range filter.
*
* @note Both rangeMin and rangeMax must be non-negative. If both are zero, no filtering is applied.
* @throws Assertion failure if rangeMin or rangeMax is negative.
*/
/**
* @ingroup PointCloudRangeFiltering
* @brief Filters a point cloud of type `pcl::PointXYZ`.
*/
pcl::IndicesPtr RTABMAP_CORE_EXPORT rangeFiltering(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float rangeMin,
float rangeMax);
/**
* @ingroup PointCloudRangeFiltering
* @brief Filters a point cloud of type `pcl::PointXYZRGB`.
*/
pcl::IndicesPtr RTABMAP_CORE_EXPORT rangeFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float rangeMin,
float rangeMax);
/**
* @ingroup PointCloudRangeFiltering
* @brief Filters a point cloud of type `pcl::PointNormal`.
*/
pcl::IndicesPtr RTABMAP_CORE_EXPORT rangeFiltering(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float rangeMin,
float rangeMax);
/**
* @ingroup PointCloudRangeFiltering
* @brief Filters a point cloud of type `pcl::PointXYZRGBNormal`.
*/
pcl::IndicesPtr RTABMAP_CORE_EXPORT rangeFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float rangeMin,
float rangeMax);
/**
* @defgroup PointCloudSplitRangeFiltering Split Range Filtering of PCL Point Clouds
* @brief Splits a point cloud into two groups based on a distance threshold.
*
* This function divides the input point cloud (or a subset specified by indices)
* into two sets of indices: points closer than the given range (`closeIndices`)
* and points farther than or equal to the range (`farIndices`), using Euclidean
* distance.
*
* @param cloud The input point cloud.
* @param indices The subset of point indices to consider. If empty, the full cloud is used.
* @param range The distance threshold in meters used to classify points as "close" or "far".
* @param[out] closeIndices Output indices for points with distance < range.
* @param[out] farIndices Output indices for points with distance >= range.
*
* @note The function resets and fills both output index containers.
* If the input cloud is empty, both outputs will also be empty.
*/
/**
* @ingroup PointCloudSplitRangeFiltering
* @brief Splits a point cloud of type `pcl::PointXYZ`.
*/
void RTABMAP_CORE_EXPORT rangeSplitFiltering(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float range,
pcl::IndicesPtr & closeIndices,
pcl::IndicesPtr & farIndices);
/**
* @ingroup PointCloudSplitRangeFiltering
* @brief Splits a point cloud of type `pcl::PointXYZRGB`.
*/
void RTABMAP_CORE_EXPORT rangeSplitFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float range,
pcl::IndicesPtr & closeIndices,
pcl::IndicesPtr & farIndices);
/**
* @ingroup PointCloudSplitRangeFiltering
* @brief Splits a point cloud of type `pcl::PointNormal`.
*/
void RTABMAP_CORE_EXPORT rangeSplitFiltering(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float range,
pcl::IndicesPtr & closeIndices,
pcl::IndicesPtr & farIndices);
/**
* @ingroup PointCloudSplitRangeFiltering
* @brief Splits a point cloud of type `pcl::PointXYZRGBNormal`.
*/
void RTABMAP_CORE_EXPORT rangeSplitFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
@@ -148,83 +232,237 @@ void RTABMAP_CORE_EXPORT rangeSplitFiltering(
pcl::IndicesPtr & closeIndices,
pcl::IndicesPtr & farIndices);
/**
* @defgroup PointCloudDownSampling Point Cloud Downsampling
* @brief Downsamples a point cloud or LaserScan by keeping only every N-th point.
*
* This function downsamples a point cloud based on a specified step size. The method adapts based on
* whether the cloud is organized (2D) or unorganized (1D), and whether it's structured like a depth image or LiDAR scan.
*
* - For **unorganized point clouds**: Every `step`-th point is retained linearly.
* - For **organized point clouds** with LiDAR-like layout (e.g., 2048x64): Downsampling is performed along the "long" dimension,
* preserving the ring structure.
* - For **depth-image-like organized clouds** (e.g., 640x480): Downsampling is performed in both row and column directions,
* similar to image decimation. Step size must divide both width and height exactly.
*
* @param cloud The input point cloud to downsample.
* @param step The decimation step. Must be greater than 0.
* @return A new point cloud containing only the sampled points.
*
* @note If `step <= 1` or the cloud has fewer points than `step`, the function returns a copy of the input cloud.
* @throws Assertion failure if `step <= 0` or, in the case of depth-image-style clouds, if the width and height are not divisible by `step`.
*/
/**
* @ingroup PointCloudDownSampling
* @brief Downsamples a LaserScan.
*/
LaserScan RTABMAP_CORE_EXPORT downsample(
const LaserScan & cloud,
int step);
const LaserScan & cloud,
int step);
/**
* @ingroup PointCloudDownSampling
* @brief Downsamples a point cloud of type `pcl::PointXYZ`.
*/
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT downsample(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
int step);
/**
* @ingroup PointCloudDownSampling
* @brief Downsamples a point cloud of type `pcl::PointXYZRGB`.
*/
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT downsample(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
int step);
/**
* @ingroup PointCloudDownSampling
* @brief Downsamples a point cloud of type `pcl::PointXYZI`.
*/
pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_CORE_EXPORT downsample(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
int step);
/**
* @ingroup PointCloudDownSampling
* @brief Downsamples a point cloud of type `pcl::PointNormal`.
*/
pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_CORE_EXPORT downsample(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
int step);
/**
* @ingroup PointCloudDownSampling
* @brief Downsamples a point cloud of type `pcl::PointXYZRGBNormal`.
*/
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_CORE_EXPORT downsample(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
int step);
/**
* @ingroup PointCloudDownSampling
* @brief Downsamples a point cloud of type `pcl::PointXYZINormal`.
*/
pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_CORE_EXPORT downsample(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
int step);
/**
* @defgroup VoxelFiltering Voxel Filtering
* @brief Performs voxel grid downsampling on a point cloud with optional index filtering.
*
* This function downsamples a point cloud using a voxel grid filter. It optionally accepts a set of point indices to limit
* the operation to a subset of the cloud. For very large point clouds or very small voxel sizes that would cause integer index
* overflow in the voxel grid structure, the function automatically partitions the space into smaller regions and applies the voxel
* filtering recursively.
*
* @param cloud The input point cloud to be voxelized.
* @param indices (Optional) A shared pointer to a vector of indices indicating which points to consider. If empty, the entire cloud is used.
* @param voxelSize The voxel (leaf) size for downsampling. Must be greater than zero.
* @return A new downsampled point cloud as a shared pointer.
*
* @note If the computed voxel grid exceeds 32-bit integer capacity, the function splits the bounding volume into smaller regions
* (using a quadtree or octree-like subdivision depending on the Z-axis span) and processes each region recursively.
* @note If the cloud is not dense and no indices are provided, a warning is issued and an empty cloud is returned.
*
* @throws Assertion failure if voxelSize <= 0.
*
* @see pcl::VoxelGrid, util3d::cropBox
*/
/**
* @ingroup VoxelFiltering
* @brief Performs voxel grid downsampling on a point cloud of type `pcl::PointXYZ` on provided indices.
*/
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT voxelize(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float voxelSize);
/**
* @ingroup VoxelFiltering
* @brief Performs voxel grid downsampling on a point cloud of type `pcl::PointNormal` on provided indices.
*/
pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_CORE_EXPORT voxelize(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float voxelSize);
/**
* @ingroup VoxelFiltering
* @brief Performs voxel grid downsampling on a point cloud of type `pcl::PointXYZRGB` on provided indices.
*/
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT voxelize(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float voxelSize);
/**
* @ingroup VoxelFiltering
* @brief Performs voxel grid downsampling on a point cloud of type `pcl::PointXYZRGBNormal` on provided indices.
*/
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_CORE_EXPORT voxelize(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float voxelSize);
/**
* @ingroup VoxelFiltering
* @brief Performs voxel grid downsampling on a point cloud of type `pcl::PointXYZI` on provided indices.
*/
pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_CORE_EXPORT voxelize(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float voxelSize);
/**
* @ingroup VoxelFiltering
* @brief Performs voxel grid downsampling on a point cloud of type `pcl::PointXYZINormal` on provided indices.
*/
pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_CORE_EXPORT voxelize(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float voxelSize);
/**
* @ingroup VoxelFiltering
* @brief Performs voxel grid downsampling on a point cloud of type `pcl::PointXYZ`.
*/
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT voxelize(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
float voxelSize);
/**
* @ingroup VoxelFiltering
* @brief Performs voxel grid downsampling on a point cloud of type `pcl::PointNormal`.
*/
pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_CORE_EXPORT voxelize(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
float voxelSize);
/**
* @ingroup VoxelFiltering
* @brief Performs voxel grid downsampling on a point cloud of type `pcl::PointXYZRGB`.
*/
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT voxelize(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float voxelSize);
/**
* @ingroup VoxelFiltering
* @brief Performs voxel grid downsampling on a point cloud of type `pcl::PointXYZRGBNormal`.
*/
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_CORE_EXPORT voxelize(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
float voxelSize);
/**
* @ingroup VoxelFiltering
* @brief Performs voxel grid downsampling on a point cloud of type `pcl::PointXYZI`.
*/
pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_CORE_EXPORT voxelize(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
float voxelSize);
/**
* @ingroup VoxelFiltering
* @brief Performs voxel grid downsampling on a point cloud of type `pcl::PointXYZINormal`.
*/
pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_CORE_EXPORT voxelize(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
float voxelSize);
/**
* @brief DEPRECATED: Use voxelize() instead.
*
* Performs uniform sampling of a point cloud by applying voxel grid filtering
* with the specified voxel size. This is a legacy wrapper for `voxelize()`.
*
* @param cloud The input point cloud (pcl::PointXYZ).
* @param voxelSize The voxel size (resolution) used for downsampling.
* @return A downsampled point cloud using voxel grid filtering.
*
* @deprecated This function is deprecated. Use voxelize() for equivalent behavior.
*/
inline pcl::PointCloud<pcl::PointXYZ>::Ptr uniformSampling(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
float voxelSize)
{
return voxelize(cloud, voxelSize);
}
/**
* @brief DEPRECATED: Use voxelize() instead.
*
* Performs uniform sampling of a point cloud by applying voxel grid filtering
* with the specified voxel size. This is a legacy wrapper for `voxelize()`.
*
* @param cloud The input point cloud (pcl::PointXYZRGB).
* @param voxelSize The voxel size (resolution) used for downsampling.
* @return A downsampled point cloud using voxel grid filtering.
*
* @deprecated This function is deprecated. Use voxelize() for equivalent behavior.
*/
inline pcl::PointCloud<pcl::PointXYZRGB>::Ptr uniformSampling(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float voxelSize)
{
return voxelize(cloud, voxelSize);
}
/**
* @brief DEPRECATED: Use voxelize() instead.
*
* Performs uniform sampling of a point cloud by applying voxel grid filtering
* with the specified voxel size. This is a legacy wrapper for `voxelize()`.
*
* @param cloud The input point cloud (pcl::PointXYZRGBNormal).
* @param voxelSize The voxel size (resolution) used for downsampling.
* @return A downsampled point cloud using voxel grid filtering.
*
* @deprecated This function is deprecated. Use voxelize() for equivalent behavior.
*/
inline pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr uniformSampling(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
float voxelSize)
corelib/src/util3d_filtering.cpp
+122 -32
View File
@@ -552,27 +552,97 @@ LaserScan downsample(
int step)
{
UASSERT(step > 0);
if(step <= 1 || scan.size() <= step)
cv::Mat output;
if(step <= 1 ||
(scan.data().cols > scan.data().rows ? scan.data().cols <= step : scan.data().rows <= step))
{
// no sampling
return scan;
}
else if(scan.isOrganized())
{
// Organized LaserScan
if(scan.data().rows <= scan.data().cols/4 ||
scan.data().cols<= scan.data().rows/4)
{
// Assuming LiDAR point cloud (e.g, 2048x64 or 32x1024),
// for which the lower dimension is the number of rings.
// Downsample each ring by the step.
// Example data packed:
// <ringA-1, ringB-1, ringC-1, ringD-1;
// ringA-2, ringB-2, ringC-2, ringD-2;
// ringA-3, ringB-3, ringC-3, ringD-3;
// ringA-4, ringB-4, ringC-4, ringD-4;
// ringA-#, ringB-#, ringC-#, ringD-#>
// or
// <ringA-1, ringA-2, ringA-3, ringA-4, ringA-#;
// ringB-1, ringB-2, ringB-3, ringB-4, ringB-#;
// ringC-1, ringC-2, ringC-3, ringC-4, ringC-#;
// ringD-1, ringD-2, ringD-3, ringD-4, ringD-#;>
bool ringsOnRows = scan.data().rows < scan.data().cols;
unsigned int rings = ringsOnRows ? scan.data().rows : scan.data().cols;
unsigned int pts = ringsOnRows ? scan.data().cols : scan.data().rows;
unsigned int outputPts = pts/step;
output = cv::Mat(
ringsOnRows ? rings : outputPts,
ringsOnRows ? outputPts : rings,
scan.data().type());
if(ringsOnRows) {
for(unsigned int j=0; j<rings; ++j)
{
for(unsigned int i=0; i<outputPts; ++i)
{
cv::Mat(scan.data(), cv::Range(j,j+1), cv::Range(i*step,i*step+1)).copyTo(cv::Mat(output, cv::Range(j,j+1), cv::Range(i,i+1)));
}
}
}
else {
for(unsigned int j=0; j<outputPts; ++j)
{
for(unsigned int i=0; i<rings; ++i)
{
cv::Mat(scan.data(), cv::Range(j*step,j*step+1), cv::Range(i,i+1)).copyTo(cv::Mat(output, cv::Range(j,j+1), cv::Range(i,i+1)));
}
}
}
}
else
{
// assume depth image (e.g., 640x480), downsample like an image (step in both dimensions)
UASSERT_MSG(scan.data().rows % step == 0 && scan.data().cols % step == 0,
uFormat("Decimation of image-like scans should be exact! (decimation=%d, size=%dx%d)",
step, scan.data().cols, scan.data().rows).c_str());
output = cv::Mat(scan.data().rows/step, scan.data().cols/step, scan.data().type());
for(int j=0; j<output.rows; ++j)
{
for(int i=0; i<output.cols; ++i)
{
cv::Mat(scan.data(), cv::Range(j*step,j*step+1), cv::Range(i*step,i*step+1)).copyTo(cv::Mat(output, cv::Range(j,j+1), cv::Range(i,i+1)));
}
}
}
}
else
{
// Dense LaserScan
int finalSize = scan.size()/step;
cv::Mat output = cv::Mat(1, finalSize, scan.dataType());
output = cv::Mat(1, finalSize, scan.dataType());
int oi = 0;
for(int i=0; i<scan.size()-step+1; i+=step)
{
cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(output, cv::Range::all(), cv::Range(oi,oi+1)));
++oi;
}
if(scan.angleIncrement() > 0.0f)
{
return LaserScan(output, scan.format(), scan.rangeMin(), scan.rangeMax(), scan.angleMin(), scan.angleMax(), scan.angleIncrement()*step, scan.localTransform());
}
return LaserScan(output, scan.maxPoints()/step, scan.rangeMax(), scan.format(), scan.localTransform());
}
if(scan.angleIncrement() > 0.0f)
{
return LaserScan(output, scan.format(), scan.rangeMin(), scan.rangeMax(), scan.angleMin(), scan.angleMax(), scan.angleIncrement()*step, scan.localTransform());
}
return LaserScan(output, scan.maxPoints()/step, scan.rangeMax(), scan.format(), scan.localTransform());
}
template<typename PointT>
typename pcl::PointCloud<PointT>::Ptr downsampleImpl(
@@ -581,42 +651,62 @@ typename pcl::PointCloud<PointT>::Ptr downsampleImpl(
{
UASSERT(step > 0);
typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
if(step <= 1 || (int)cloud->size() <= step)
if(step <= 1 ||
(cloud->width > cloud->height ? (int)cloud->width <= step : (int)cloud->height <= step))
{
// no sampling
*output = *cloud;
}
else
else if(cloud->height > 1) // Organized point cloud
{
if(cloud->height > 1 && cloud->height < cloud->width/4)
if(cloud->height <= cloud->width/4 ||
cloud->width <= cloud->height/4)
{
// Assuming ouster point cloud (e.g, 2048x64),
// Assuming LiDAR point cloud (e.g, 2048x64 or 32x1024),
// for which the lower dimension is the number of rings.
// Downsample each ring by the step.
// Example data packed:
// <ringA-1, ringB-1, ringC-1, ringD-1;
// ringA-2, ringB-2, ringC-2, ringD-2;
// ringA-3, ringB-3, ringC-3, ringD-3;
// ringA-4, ringB-4, ringC-4, ringD-4>
unsigned int rings = cloud->height<cloud->width?cloud->height:cloud->width;
unsigned int pts = cloud->height>cloud->width?cloud->height:cloud->width;
// ringA-4, ringB-4, ringC-4, ringD-4;
// ringA-#, ringB-#, ringC-#, ringD-#>
// or
// <ringA-1, ringA-2, ringA-3, ringA-4, ringA-#;
// ringB-1, ringB-2, ringB-3, ringB-4, ringB-#;
// ringC-1, ringC-2, ringC-3, ringC-4, ringC-#;
// ringD-1, ringD-2, ringD-3, ringD-4, ringD-#;>
bool ringsOnRows = cloud->height < cloud->width;
unsigned int rings = ringsOnRows ? cloud->height : cloud->width;
unsigned int pts = ringsOnRows ? cloud->width : cloud->height;
unsigned int finalSize = rings * pts/step;
unsigned int outputPts = pts/step;
output->resize(finalSize);
output->width = rings;
output->height = pts/step;
output->height = ringsOnRows ? rings : outputPts;
output->width = ringsOnRows ? outputPts : rings;
for(unsigned int j=0; j<rings; ++j)
{
for(unsigned int i=0; i<output->height; ++i)
if(ringsOnRows) {
for(unsigned int j=0; j<rings; ++j)
{
(*output)[i*rings + j] = cloud->at(i*step*rings + j);
for(unsigned int i=0; i<outputPts; ++i)
{
(*output)[j*outputPts + i] = cloud->at(j*pts + i*step);
}
}
}
else {
for(unsigned int j=0; j<outputPts; ++j)
{
for(unsigned int i=0; i<rings; ++i)
{
(*output)[j*rings + i] = cloud->at(j*step*rings + i);
}
}
}
}
else if(cloud->height > 1)
else
{
// assume depth image (e.g., 640x480), downsample like an image
// assume depth image (e.g., 640x480), downsample like an image (step in both dimensions)
UASSERT_MSG(cloud->height % step == 0 && cloud->width % step == 0,
uFormat("Decimation of depth images should be exact! (decimation=%d, size=%dx%d)",
step, cloud->width, cloud->height).c_str());
@@ -630,19 +720,19 @@ typename pcl::PointCloud<PointT>::Ptr downsampleImpl(
{
for(unsigned int i=0; i<output->width; ++i)
{
output->at(j*output->width + i) = cloud->at(j*output->width*step + i*step);
output->at(j*output->width + i) = cloud->at(j*cloud->width*step + i*step);
}
}
}
else
}
else // Dense cloud
{
int finalSize = int(cloud->size())/step;
output->resize(finalSize);
int oi = 0;
for(unsigned int i=0; i<cloud->size()-step+1; i+=step)
{
int finalSize = int(cloud->size())/step;
output->resize(finalSize);
int oi = 0;
for(unsigned int i=0; i<cloud->size()-step+1; i+=step)
{
(*output)[oi++] = cloud->at(i);
}
(*output)[oi++] = cloud->at(i);
}
}
return output;
corelib/test/test_util3d_filtering.cpp
+591
View File
@@ -416,4 +416,595 @@ TEST(Util3dFiltering, commonFilteringGroundNormalsUp) {
EXPECT_EQ(result.field(3, nz), -1.0f);
EXPECT_FLOAT_EQ(result.field(4, nz), sin(M_PI/4));
EXPECT_FLOAT_EQ(result.field(5, nz), -sin(M_PI/4));
}
TEST(Util3dFiltering, rangeFilteringNoFilteringAppliedWhenEmpty)
{
LaserScan emptyScan; // Assuming default constructor gives empty scan
LaserScan result = util3d::rangeFiltering(emptyScan, 1.0f, 5.0f);
EXPECT_TRUE(result.isEmpty());
}
TEST(Util3dFiltering, rangeFilteringNoFilteringWhenMinMaxZero)
{
// Simulate a simple 2D scan with 3 points: (1,0), (3,0), (6,0)
cv::Mat data = (cv::Mat_<float>(1, 3*2) << 1.0f, 0.0f, 3.0f, 0.0f, 6.0f, 0.0f);
data = data.reshape(2, 1); // Reshape to 1 row, 3 columns with 2 floats per point
LaserScan scan(data, 0, 0, LaserScan::kXY); // assuming kXY means 2D points
LaserScan result = util3d::rangeFiltering(scan, 0.0f, 0.0f);
EXPECT_EQ(result.size(), scan.size());
EXPECT_EQ(result.data().at<cv::Vec2f>(0,0), scan.data().at<cv::Vec2f>(0,0));
EXPECT_EQ(result.data().at<cv::Vec2f>(0,1), scan.data().at<cv::Vec2f>(0,1));
EXPECT_EQ(result.data().at<cv::Vec2f>(0,2), scan.data().at<cv::Vec2f>(0,2));
}
TEST(Util3dFiltering, rangeFilteringFilteringRemovesOutOfRangePoints)
{
// 2D points: (1,0) [1m], (3,0) [3m], (6,0) [6m]
cv::Mat data = (cv::Mat_<float>(1, 3*2) << 1.0f, 0.0f, 3.0f, 0.0f, 6.0f, 0.0f);
data = data.reshape(2, 1);
LaserScan scan(data, 0, 0, LaserScan::kXY);
LaserScan result = util3d::rangeFiltering(scan, 2.0f, 5.0f);
ASSERT_EQ(result.size(), 1); // Only the (3,0) point should remain
EXPECT_FLOAT_EQ(result.data().at<cv::Vec3f>(0)[0], 3.0f); // x
EXPECT_FLOAT_EQ(result.data().at<cv::Vec3f>(0)[1], 0.0f); // y
// 3D points: (1,0) [1m], (3,0) [3m], (6,0) [6m]
data = (cv::Mat_<float>(1, 3*3) << 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 3.0f, 0.0f, 0.0f, 6.0f);
data = data.reshape(3, 1);
scan = LaserScan(data, 0, 0, LaserScan::kXYZ);
result = util3d::rangeFiltering(scan, 2.0f, 5.0f);
ASSERT_EQ(result.size(), 1); // Only the (0,0,3) point should remain
EXPECT_FLOAT_EQ(result.data().at<cv::Vec3f>(0)[0], 0.0f); // x
EXPECT_FLOAT_EQ(result.data().at<cv::Vec3f>(0)[1], 0.0f); // y
EXPECT_FLOAT_EQ(result.data().at<cv::Vec3f>(0)[2], 3.0f); // z
}
TEST(Util3dFiltering, rangeFilteringFilteringWithOnlyMinOrMax)
{
// 2D points: (1,0) [1m], (4,0) [4m]
cv::Mat data = (cv::Mat_<float>(1, 2*2) << 1.0f, 0.0f, 4.0f, 0.0f);
data = data.reshape(2, 1);
LaserScan scan(data, 0, 0, LaserScan::kXY);
// Only apply minimum range
LaserScan resultMin = util3d::rangeFiltering(scan, 2.0f, 0.0f);
ASSERT_EQ(resultMin.size(), 1);
EXPECT_FLOAT_EQ(resultMin.data().at<float>(0, 0), 4.0f);
// Only apply maximum range
LaserScan resultMax = util3d::rangeFiltering(scan, 0.0f, 2.5f);
ASSERT_EQ(resultMax.size(), 1);
EXPECT_FLOAT_EQ(resultMax.data().at<float>(0, 0), 1.0f);
}
TEST(Util3dFiltering, rangeFilteringPointXYZ) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->push_back(pcl::PointXYZ(1, 0, 0)); // Distance: 1
cloud->push_back(pcl::PointXYZ(3, 0, 0)); // Distance: 3
cloud->push_back(pcl::PointXYZ(6, 0, 0)); // Distance: 6
pcl::IndicesPtr indices(new std::vector<int>); // Empty means use whole cloud
auto filtered = util3d::rangeFiltering(cloud, indices, 2.0f, 5.0f);
ASSERT_EQ(filtered->size(), 1);
EXPECT_EQ(filtered->at(0), 1); // Only point at index 1 is within range
}
TEST(Util3dFiltering, rangeFilteringPointXYZRGB) {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
cloud->push_back(pcl::PointXYZRGB()); cloud->back().x = 2; cloud->back().y = 0; cloud->back().z = 0; // Distance: 2
cloud->push_back(pcl::PointXYZRGB()); cloud->back().x = 4; cloud->back().y = 0; cloud->back().z = 0; // Distance: 4
pcl::IndicesPtr indices(new std::vector<int>);
indices->push_back(0);
indices->push_back(1);
auto filtered = util3d::rangeFiltering(cloud, indices, 0.0f, 3.0f);
ASSERT_EQ(filtered->size(), 1);
EXPECT_EQ(filtered->at(0), 0);
}
TEST(Util3dFiltering, rangeFilteringPointNormal) {
pcl::PointCloud<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal>);
cloud->push_back(pcl::PointNormal()); cloud->back().x = 1.0f; // Distance: 1
cloud->push_back(pcl::PointNormal()); cloud->back().x = 10.0f; // Distance: 10
pcl::IndicesPtr indices(new std::vector<int>);
auto filtered = util3d::rangeFiltering(cloud, indices, 0.0f, 5.0f);
ASSERT_EQ(filtered->size(), 1);
EXPECT_EQ(filtered->at(0), 0);
}
TEST(Util3dFiltering, rangeFilteringPointXYZRGBNormal) {
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::PointXYZRGBNormal pt1; pt1.x = 0.5f; pt1.y = 0; pt1.z = 0; // Distance: 0.5
pcl::PointXYZRGBNormal pt2; pt2.x = 2.5f; pt2.y = 0; pt2.z = 0; // Distance: 2.5
cloud->push_back(pt1);
cloud->push_back(pt2);
pcl::IndicesPtr indices(new std::vector<int>); // All points
auto filtered = util3d::rangeFiltering(cloud, indices, 1.0f, 3.0f);
ASSERT_EQ(filtered->size(), 1);
EXPECT_EQ(filtered->at(0), 1);
}
TEST(Util3dFiltering, rangeSplitFilteringPointXYZ)
{
auto cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
cloud->push_back(pcl::PointXYZ(1, 0, 0)); // dist = 1
cloud->push_back(pcl::PointXYZ(4, 0, 0)); // dist = 4
cloud->push_back(pcl::PointXYZ(2, 0, 0)); // dist = 2
pcl::IndicesPtr indices(new std::vector<int>); // Empty = use all
pcl::IndicesPtr closeIndices, farIndices;
util3d::rangeSplitFiltering(cloud, indices, 3.0f, closeIndices, farIndices);
EXPECT_EQ(closeIndices->size(), 2);
EXPECT_EQ(closeIndices->at(0), 0);
EXPECT_EQ(closeIndices->at(1), 2);
EXPECT_EQ(farIndices->size(), 1);
EXPECT_EQ(farIndices->at(0), 1);
indices->push_back(1);
indices->push_back(2);
util3d::rangeSplitFiltering(cloud, indices, 3.0f, closeIndices, farIndices);
EXPECT_EQ(closeIndices->size(), 1);
EXPECT_EQ(closeIndices->at(0), 2);
EXPECT_EQ(farIndices->size(), 1);
EXPECT_EQ(farIndices->at(0), 1);
}
TEST(Util3dFiltering, rangeSplitFilteringPointXYZRGB)
{
auto cloud = pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointXYZRGB pt1, pt2;
pt1.x = 1; pt1.y = 0; pt1.z = 0; pt1.r = 255;
pt2.x = 5; pt2.y = 0; pt2.z = 0; pt2.g = 255;
cloud->push_back(pt1);
cloud->push_back(pt2);
pcl::IndicesPtr indices(new std::vector<int>); // All points
pcl::IndicesPtr closeIndices, farIndices;
util3d::rangeSplitFiltering(cloud, indices, 3.0f, closeIndices, farIndices);
EXPECT_EQ(closeIndices->size(), 1);
EXPECT_EQ(closeIndices->at(0), 0);
EXPECT_EQ(farIndices->size(), 1);
EXPECT_EQ(farIndices->at(0), 1);
indices->push_back(1);
util3d::rangeSplitFiltering(cloud, indices, 3.0f, closeIndices, farIndices);
EXPECT_EQ(closeIndices->size(), 0);
EXPECT_EQ(farIndices->size(), 1);
EXPECT_EQ(farIndices->at(0), 1);
}
TEST(Util3dFiltering, rangeSplitFilteringPointNormal)
{
auto cloud = pcl::PointCloud<pcl::PointNormal>::Ptr(new pcl::PointCloud<pcl::PointNormal>);
pcl::PointNormal pt1, pt2;
pt1.x = 0.5f; pt1.y = 0; pt1.z = 0;
pt2.x = 3.5f; pt2.y = 0; pt2.z = 0;
cloud->push_back(pt1);
cloud->push_back(pt2);
pcl::IndicesPtr indices(new std::vector<int>); // Use full cloud
pcl::IndicesPtr closeIndices, farIndices;
util3d::rangeSplitFiltering(cloud, indices, 2.0f, closeIndices, farIndices);
EXPECT_EQ(closeIndices->size(), 1);
EXPECT_EQ(closeIndices->at(0), 0);
EXPECT_EQ(farIndices->size(), 1);
EXPECT_EQ(farIndices->at(0), 1);
indices->push_back(1);
util3d::rangeSplitFiltering(cloud, indices, 2.0f, closeIndices, farIndices);
EXPECT_EQ(closeIndices->size(), 0);
EXPECT_EQ(farIndices->size(), 1);
EXPECT_EQ(farIndices->at(0), 1);
}
TEST(Util3dFiltering, rangeSplitFilteringPointXYZRGBNormal)
{
auto cloud = pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::PointXYZRGBNormal pt1, pt2;
pt1.x = 1.0f; pt1.y = 0; pt1.z = 0;
pt2.x = 4.0f; pt2.y = 0; pt2.z = 0;
cloud->push_back(pt1);
cloud->push_back(pt2);
pcl::IndicesPtr indices(new std::vector<int>);
pcl::IndicesPtr closeIndices, farIndices;
util3d::rangeSplitFiltering(cloud, indices, 3.0f, closeIndices, farIndices);
EXPECT_EQ(closeIndices->size(), 1);
EXPECT_EQ(closeIndices->at(0), 0);
EXPECT_EQ(farIndices->size(), 1);
EXPECT_EQ(farIndices->at(0), 1);
indices->push_back(1);
util3d::rangeSplitFiltering(cloud, indices, 3.0f, closeIndices, farIndices);
EXPECT_EQ(closeIndices->size(), 0);
EXPECT_EQ(farIndices->size(), 1);
EXPECT_EQ(farIndices->at(0), 1);
}
TEST(Util3dFiltering, downsampleImplUnorganizedCloud) {
auto cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
// Create 10 points along X axis
for (int i = 0; i < 10; ++i) {
cloud->push_back(pcl::PointXYZ(float(i), 0, 0));
}
auto result = util3d::downsample(cloud, 3);
ASSERT_EQ(result->size(), 3);
EXPECT_FLOAT_EQ(result->at(0).x, 0);
EXPECT_FLOAT_EQ(result->at(1).x, 3);
EXPECT_FLOAT_EQ(result->at(2).x, 6);
//LaserScan version
{
// 2D scan
cv::Mat data(1, 10, CV_32FC2);
// Create 10 points along X axis
for (int i = 0; i < 10; ++i) {
data.at<cv::Vec2f>(i)[0] = float(i); // x
}
auto scan = LaserScan(data, LaserScan::kXY, 0.0f, 40.0f, -M_PI/2.0f, M_PI/2.0f, M_PI/10.0f);
auto result = util3d::downsample(scan, 3);
ASSERT_EQ(result.data().cols, 3);
ASSERT_EQ(result.angleIncrement(), scan.angleIncrement()*3);
EXPECT_FLOAT_EQ(result.field(0, 0), 0);
EXPECT_FLOAT_EQ(result.field(1, 0), 3);
EXPECT_FLOAT_EQ(result.field(2, 0), 6);
}
}
TEST(Util3dFiltering, downsampleImplStepLargerThanCloudSize) {
auto cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
for (int i = 0; i < 5; ++i)
cloud->push_back(pcl::PointXYZ(float(i), 0, 0));
auto result = util3d::downsample(cloud, 10);
ASSERT_EQ(result->size(), cloud->size());
for (size_t i = 0; i < cloud->size(); ++i)
EXPECT_EQ(result->at(i).x, cloud->at(i).x);
//LaserScan version
{
cv::Mat data(1, 10, CV_32FC3);
for (int i = 0; i < 5; ++i)
data.at<cv::Vec3f>(i)[0] = float(i); // x
auto scan = LaserScan(data, 0, 0, LaserScan::kXYZ);
auto result = util3d::downsample(scan, 10);
ASSERT_EQ(result.size(), scan.size());
for (int i = 0; i < scan.size(); ++i)
EXPECT_EQ(result.field(i,0), scan.field(i,0));
}
}
TEST(Util3dFiltering, downsampleImplStepOne) {
auto cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
cloud->push_back(pcl::PointXYZ(1, 2, 3));
auto result = util3d::downsample(cloud, 1);
ASSERT_EQ(result->size(), cloud->size());
EXPECT_EQ(result->at(0).x, 1);
EXPECT_EQ(result->at(0).y, 2);
EXPECT_EQ(result->at(0).z, 3);
//LaserScan version
{
cv::Mat data = (cv::Mat_<float>(1, 3) << 1.0f, 2.0f, 3.0f);
data = data.reshape(3, 1);
auto scan = LaserScan(data, 0, 0, LaserScan::kXYZ);
auto result = util3d::downsample(scan, 1);
ASSERT_EQ(result.size(), scan.size());
EXPECT_EQ(result.field(0,0), 1);
EXPECT_EQ(result.field(0,1), 2);
EXPECT_EQ(result.field(0,2), 3);
}
}
TEST(Util3dFiltering, downsampleImplOrganizedDepthImage) {
auto cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
cloud->width = 4;
cloud->height = 4;
cloud->is_dense = false;
cloud->resize(cloud->width * cloud->height);
// Fill organized cloud with row-major increasing values
for (int j = 0; j < 4; ++j) {
for (int i = 0; i < 4; ++i) {
cloud->at(j * 4 + i) = pcl::PointXYZ(float(i + j * 4), 0, 0);
}
}
auto result = util3d::downsample(cloud, 2);
ASSERT_EQ(result->width, 2);
ASSERT_EQ(result->height, 2);
ASSERT_EQ(result->size(), 4);
EXPECT_EQ(result->at(0).x, cloud->at(0).x); // (0,0)
EXPECT_EQ(result->at(1).x, cloud->at(2).x); // (0,2)
EXPECT_EQ(result->at(2).x, cloud->at(8).x); // (2,0)
EXPECT_EQ(result->at(3).x, cloud->at(10).x); // (2,2)
//LaserScan version
{
cv::Mat data(4, 4, CV_32FC3);
// Fill organized cloud with row-major increasing values
for (int j = 0; j < 4; ++j) {
for (int i = 0; i < 4; ++i) {
data.at<cv::Vec3f>(j * 4 + i)[0] = float(i + j * 4);
}
}
auto scan = LaserScan(data, data.total(), 0, LaserScan::kXYZ);
auto result = util3d::downsample(scan, 2);
ASSERT_EQ(result.data().cols, 2);
ASSERT_EQ(result.data().rows, 2);
ASSERT_EQ(result.size(), 4);
EXPECT_EQ(result.field(0,0), scan.field(0,0)); // (0,0)
EXPECT_EQ(result.field(1,0), scan.field(2,0)); // (0,2)
EXPECT_EQ(result.field(2,0), scan.field(8,0)); // (2,0)
EXPECT_EQ(result.field(3,0), scan.field(10,0)); // (2,2)
}
}
TEST(Util3dFiltering, downsampleImplInvalidStep) {
auto cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
cloud->push_back(pcl::PointXYZ(0, 0, 0));
EXPECT_THROW(util3d::downsample(cloud, 0), UException);
// just checking if other point type functions are there,
// note that they all call the exact same implemented function
// under the hood, so we son't duplicate all tests already
// done with pcl::PointXYZ type.
EXPECT_THROW(util3d::downsample(pcl::PointCloud<pcl::PointXYZRGB>::Ptr(), 0), UException);
EXPECT_THROW(util3d::downsample(pcl::PointCloud<pcl::PointXYZI>::Ptr(), 0), UException);
EXPECT_THROW(util3d::downsample(pcl::PointCloud<pcl::PointNormal>::Ptr(), 0), UException);
EXPECT_THROW(util3d::downsample(pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr(), 0), UException);
EXPECT_THROW(util3d::downsample(pcl::PointCloud<pcl::PointXYZINormal>::Ptr(), 0), UException);
// LaserScan version
EXPECT_THROW(util3d::downsample(LaserScan(), 0), UException);
}
TEST(Util3dFiltering, downsampleImplLiDARStyleRingsInRows) {
const int width = 8;
const int height = 2; // 2 rings, 8 points per ring
const int step = 2;
auto cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
cloud->width = width;
cloud->height = height;
cloud->is_dense = true;
cloud->resize(width * height);
// Fill with values (row-major): pt(i,j) = i + j*width
for (int j = 0; j < height; ++j) {
for (int i = 0; i < width; ++i) {
cloud->at(j * width + i) = pcl::PointXYZ(float(i + j * width), 0, 0);
}
}
auto result = util3d::downsample(cloud, step);
EXPECT_EQ(result->width, width / step);
EXPECT_EQ(result->height, height);
EXPECT_EQ(result->size(), (width / step) * height);
// Expect values at (0, 2, 4, 6) for each row
EXPECT_FLOAT_EQ(result->at(0).x, 0); // row 0, col 0
EXPECT_FLOAT_EQ(result->at(1).x, 2); // row 0, col 2
EXPECT_FLOAT_EQ(result->at(2).x, 4); // row 0, col 4
EXPECT_FLOAT_EQ(result->at(3).x, 6); // row 0, col 6
EXPECT_FLOAT_EQ(result->at(4).x, 8); // row 1, col 0
EXPECT_FLOAT_EQ(result->at(5).x, 10); // row 1, col 2
EXPECT_FLOAT_EQ(result->at(6).x, 12); // row 1, col 4
EXPECT_FLOAT_EQ(result->at(7).x, 14); // row 1, col 6
// LaserScan version
{
cv::Mat data(height, width, CV_32FC3);
// Fill with values (row-major): pt(i,j) = i + j*width
for (int j = 0; j < height; ++j) {
for (int i = 0; i < width; ++i) {
data.at<cv::Vec3f>(j * width + i)[0] = float(i + j * width);
}
}
auto scan = LaserScan(data, data.total(), 0, LaserScan::kXYZ);
auto result = util3d::downsample(scan, step);
EXPECT_EQ(result.data().cols, width / step);
EXPECT_EQ(result.data().rows, height);
EXPECT_EQ(result.size(), (width / step) * height);
// Expect values at (0, 2, 4, 6) for each row
EXPECT_FLOAT_EQ(result.field(0,0), 0); // row 0, col 0
EXPECT_FLOAT_EQ(result.field(1,0), 2); // row 0, col 2
EXPECT_FLOAT_EQ(result.field(2,0), 4); // row 0, col 4
EXPECT_FLOAT_EQ(result.field(3,0), 6); // row 0, col 6
EXPECT_FLOAT_EQ(result.field(4,0), 8); // row 1, col 0
EXPECT_FLOAT_EQ(result.field(5,0), 10); // row 1, col 2
EXPECT_FLOAT_EQ(result.field(6,0), 12); // row 1, col 4
EXPECT_FLOAT_EQ(result.field(7,0), 14); // row 1, col 6
}
}
TEST(Util3dFiltering, downsampleImplLiDARStyleRingsInColumns) {
const int width = 2; // 2 columns = 2 rings
const int height = 8; // 8 points per ring
const int step = 2;
auto cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
cloud->width = width;
cloud->height = height;
cloud->is_dense = true;
cloud->resize(width * height);
// Fill with values (row-major): pt(i,j) = i + j*width
for (int j = 0; j < height; ++j) {
for (int i = 0; i < width; ++i) {
cloud->at(j * width + i) = pcl::PointXYZ(float(i + j * width), 0, 0);
}
}
auto result = util3d::downsample(cloud, step);
EXPECT_EQ(result->width, width);
EXPECT_EQ(result->height, height / step);
EXPECT_EQ(result->size(), (height / step) * width);
// Expect values at rows 0,2,4,6 for each column
EXPECT_FLOAT_EQ(result->at(0).x, 0); // row 0, col 0
EXPECT_FLOAT_EQ(result->at(1).x, 1); // row 0, col 1
EXPECT_FLOAT_EQ(result->at(2).x, 4); // row 2, col 0
EXPECT_FLOAT_EQ(result->at(3).x, 5); // row 2, col 1
EXPECT_FLOAT_EQ(result->at(4).x, 8); // row 4, col 0
EXPECT_FLOAT_EQ(result->at(5).x, 9); // row 4, col 1
EXPECT_FLOAT_EQ(result->at(6).x, 12); // row 6, col 0
EXPECT_FLOAT_EQ(result->at(7).x, 13); // row 6, col 1
// LaserScan version
{
cv::Mat data(height, width, CV_32FC3);
// Fill with values (row-major): pt(i,j) = i + j*width
for (int j = 0; j < height; ++j) {
for (int i = 0; i < width; ++i) {
data.at<cv::Vec3f>(j * width + i)[0] = float(i + j * width);
}
}
auto scan = LaserScan(data, data.total(), 0, LaserScan::kXYZ);
auto result = util3d::downsample(scan, step);
EXPECT_EQ(result.data().cols, width);
EXPECT_EQ(result.data().rows, height/step);
EXPECT_EQ(result.size(), (width / step) * height);
// Expect values at rows 0,2,4,6 for each column
EXPECT_FLOAT_EQ(result.field(0,0), 0); // row 0, col 0
EXPECT_FLOAT_EQ(result.field(1,0), 1); // row 0, col 1
EXPECT_FLOAT_EQ(result.field(2,0), 4); // row 2, col 0
EXPECT_FLOAT_EQ(result.field(3,0), 5); // row 2, col 1
EXPECT_FLOAT_EQ(result.field(4,0), 8); // row 4, col 0
EXPECT_FLOAT_EQ(result.field(5,0), 9); // row 4, col 1
EXPECT_FLOAT_EQ(result.field(6,0), 12); // row 6, col 0
EXPECT_FLOAT_EQ(result.field(7,0), 13); // row 6, col 1
}
}
TEST(Util3dFiltering, voxelizeFullCloud) {
auto cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
// Create a 3D grid of points (e.g., 10x10x1)
for (int x = 0; x < 10; ++x) {
for (int y = 0; y < 10; ++y) {
cloud->push_back(pcl::PointXYZ(float(x), float(y), 0.0f));
}
}
float voxelSize = 2.0f;
auto result = util3d::voxelize(cloud, voxelSize);
EXPECT_EQ(result->size(), 25);
}
TEST(Util3dFiltering, voxelizeWithIndices) {
auto cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
pcl::IndicesPtr indices(new std::vector<int>);
// Add 100 points, only even-indexed ones will be used
for (int i = 0; i < 100; ++i) {
cloud->push_back(pcl::PointXYZ(float(i % 10), float(i / 10), 0.0f));
if (i % 2 == 0) {
indices->push_back(i);
}
}
float voxelSize = 2.0f;
auto result = util3d::voxelize(cloud, indices, voxelSize);
EXPECT_EQ(result->size(), 25);
}
TEST(Util3dFiltering, voxelizeSmallCloud) {
auto cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
for (int i = 0; i < 3; ++i) {
cloud->push_back(pcl::PointXYZ(float(i), 0, 0));
}
float voxelSize = 0.0001f; // Very small voxel size (no points get merged)
auto result = util3d::voxelize(cloud, voxelSize);
EXPECT_EQ(result->size(), cloud->size());
}
TEST(Util3dFiltering, voxelizeEmptyInput) {
auto cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
float voxelSize = 1.0f;
auto result = util3d::voxelize(cloud, voxelSize);
EXPECT_TRUE(result->empty());
pcl::IndicesPtr indices(new std::vector<int>);
auto result2 = util3d::voxelize(cloud, indices, voxelSize);
EXPECT_TRUE(result2->empty());
}
TEST(Util3dFiltering, voxelizeInvalidVoxelSize) {
auto cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
cloud->push_back(pcl::PointXYZ(0, 0, 0));
pcl::IndicesPtr indices(new std::vector<int>{0});
EXPECT_THROW(util3d::voxelize(cloud, 0.0f), UException);
EXPECT_THROW(util3d::voxelize(cloud, indices, 0.0f), UException);
// just checking if other point type functions are there,
// note that they all call the exact same implemented function
// under the hood, so we son't duplicate all tests already
// done with pcl::PointXYZ type.
EXPECT_THROW(util3d::voxelize(pcl::PointCloud<pcl::PointNormal>::Ptr(new pcl::PointCloud<pcl::PointNormal>()), 0.0f), UException);
EXPECT_THROW(util3d::voxelize(pcl::PointCloud<pcl::PointNormal>::Ptr(new pcl::PointCloud<pcl::PointNormal>()), indices, 0.0f), UException);
EXPECT_THROW(util3d::voxelize(pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>()), 0.0f), UException);
EXPECT_THROW(util3d::voxelize(pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>()), indices, 0.0f), UException);
EXPECT_THROW(util3d::voxelize(pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>()), 0.0f), UException);
EXPECT_THROW(util3d::voxelize(pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>()), indices, 0.0f), UException);
EXPECT_THROW(util3d::voxelize(pcl::PointCloud<pcl::PointXYZI>::Ptr(new pcl::PointCloud<pcl::PointXYZI>()), 0.0f), UException);
EXPECT_THROW(util3d::voxelize(pcl::PointCloud<pcl::PointXYZI>::Ptr(new pcl::PointCloud<pcl::PointXYZI>()), indices, 0.0f), UException);
EXPECT_THROW(util3d::voxelize(pcl::PointCloud<pcl::PointXYZINormal>::Ptr(new pcl::PointCloud<pcl::PointXYZINormal>()), 0.0f), UException);
EXPECT_THROW(util3d::voxelize(pcl::PointCloud<pcl::PointXYZINormal>::Ptr(new pcl::PointCloud<pcl::PointXYZINormal>()), indices, 0.0f), UException);
}
guilib/src/DatabaseViewer.cpp
+1 -1
View File
@@ -7140,7 +7140,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
}
if(!allNodesAreInWM)
{
ui_->graphViewer->updatePosterior(colors, 1, 1);
ui_->graphViewer->updateNodeColorByValue("In WM", colors, 1, false, 1);
}
}
QGraphicsRectItem * rectScaleItem = 0;