mirror/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2025-06-29 16:11:00 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge pull request #20784 from No-Plane-Cannot-Be-Detected:next

Browse Source 
Add 3D point cloud sampling functions to branch next

* Add several 3D point cloud sampling functions: Random, VoxelGrid, FarthestPoint.

* Made some code detail changes and exposed the random number generator parameters at the interface.

* Add simple tests for sampling.

* Modify interface output parameters.

* Modify interface return value.

* The sampling test is modified for the new changes of function interface.

* Improved test of VoxelGridFilterSampling

* Improved test of VoxelGridFilterSampling and FPS.

* Add test for the dist_lower_limit arguments of FPS function.

* Optimization function _getMatFromInputArray.

* Optimize the code style and some details according to the suggestions.

* Clear prefix cv: in the source code.

* Change the initialization of Mat in the sampling test.

* 1. Unified code style
2. Optimize randomSampling method

* 1. Optimize code comments.
2. Remove unused local variables.

* Rebuild the structure of the test, make the test case more reliable, and change the code style.

* Update test_sampling.cpp

Fix a warning.
This commit is contained in:
Ruan 2021-10-29 06:41:21 +08:00 committed by GitHub
parent 6544c7b787
commit 1470f90c2a
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 733 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
modules/3d/include/opencv2/3d.hpp
View File

@ -10,6 +10,7 @@
#include "opencv2/3d/depth.hpp"
#include "opencv2/3d/volume.hpp"
#include "opencv2/3d/ptcloud.hpp"
/**
@defgroup _3d 3D vision functionality

108
modules/3d/include/opencv2/3d/ptcloud.hpp Normal file
View File

@ -0,0 +1,108 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
#ifndef OPENCV_3D_PTCLOUD_HPP
#define OPENCV_3D_PTCLOUD_HPP
namespace cv {
//! @addtogroup _3d
//! @{
/**
* @brief Point cloud sampling by Voxel Grid filter downsampling.
*
* Creates a 3D voxel grid (a set of tiny 3D boxes in space) over the input
* point cloud data, in each voxel (i.e., 3D box), all the points present will be
* approximated (i.e., downsampled) with the point closest to their centroid.
*
* @param sampled_point_flags (Output) Flags of the sampled point, (pass in std::vector<int> or std::vector<char> etc.)
* sampled_point_flags[i] is 1 means i-th point selected, 0 means it is not selected.
* @param input_pts Original point cloud, vector of Point3 or Mat of size Nx3/3xN.
* @param length Grid length.
* @param width Grid width.
* @param height Grid height.
* @return The number of points actually sampled.
*/
CV_EXPORTS int voxelGridSampling(OutputArray sampled_point_flags, InputArray input_pts,
float length, float width, float height);
/**
* @brief Point cloud sampling by randomly select points.
*
* Use cv::randShuffle to shuffle the point index list,
* then take the points corresponding to the front part of the list.
*
* @param sampled_pts Point cloud after sampling.
* Support cv::Mat(sampled_pts_size, 3, CV_32F), std::vector<cv::Point3f>.
* @param input_pts Original point cloud, vector of Point3 or Mat of size Nx3/3xN.
* @param sampled_pts_size The desired point cloud size after sampling.
* @param rng Optional random number generator used for cv::randShuffle;
* if it is nullptr, theRNG () is used instead.
*/
CV_EXPORTS void randomSampling(OutputArray sampled_pts, InputArray input_pts,
int sampled_pts_size, RNG *rng = nullptr);
/**
* @overload
*
* @param sampled_pts Point cloud after sampling.
* Support cv::Mat(size * sampled_scale, 3, CV_32F), std::vector<cv::Point3f>.
* @param input_pts Original point cloud, vector of Point3 or Mat of size Nx3/3xN.
* @param sampled_scale Range (0, 1), the percentage of the sampled point cloud to the original size,
* that is, sampled size = original size * sampled_scale.
* @param rng Optional random number generator used for cv::randShuffle;
* if it is nullptr, theRNG () is used instead.
*/
CV_EXPORTS void randomSampling(OutputArray sampled_pts, InputArray input_pts,
float sampled_scale, RNG *rng = nullptr);
/**
* @brief Point cloud sampling by Farthest Point Sampling(FPS).
*
* FPS Algorithm:
* Input: Point cloud *C*, *sampled_pts_size*, *dist_lower_limit*
* Initialize: Set sampled point cloud S to the empty set
* Step:
* 1. Randomly take a seed point from C and take it from C to S;
* 2. Find a point in C that is the farthest away from S and take it from C to S;
* (The distance from point to set S is the smallest distance from point to all points in S)
* 3. Repeat *step 2* until the farthest distance of the point in C from S
* is less than *dist_lower_limit*, or the size of S is equal to *sampled_pts_size*.
* Output: Sampled point cloud S
*
* @param sampled_point_flags (Output) Flags of the sampled point, (pass in std::vector<int> or std::vector<char> etc.)
* sampled_point_flags[i] is 1 means i-th point selected, 0 means it is not selected.
* @param input_pts Original point cloud, vector of Point3 or Mat of size Nx3/3xN.
* @param sampled_pts_size The desired point cloud size after sampling.
* @param dist_lower_limit Sampling is terminated early if the distance from
* the farthest point to S is less than dist_lower_limit, default 0.
* @param rng Optional random number generator used for selecting seed point for FPS;
* if it is nullptr, theRNG () is used instead.
* @return The number of points actually sampled.
*/
CV_EXPORTS int farthestPointSampling(OutputArray sampled_point_flags, InputArray input_pts,
int sampled_pts_size, float dist_lower_limit = 0, RNG *rng = nullptr);
/**
* @overload
*
* @param sampled_point_flags (Output) Flags of the sampled point, (pass in std::vector<int> or std::vector<char> etc.)
* sampled_point_flags[i] is 1 means i-th point selected, 0 means it is not selected.
* @param input_pts Original point cloud, vector of Point3 or Mat of size Nx3/3xN.
* @param sampled_scale Range (0, 1), the percentage of the sampled point cloud to the original size,
* that is, sampled size = original size * sampled_scale.
* @param dist_lower_limit Sampling is terminated early if the distance from
* the farthest point to S is less than dist_lower_limit, default 0.
* @param rng Optional random number generator used for selecting seed point for FPS;
* if it is nullptr, theRNG () is used instead.
* @return The number of points actually sampled.
*/
CV_EXPORTS int farthestPointSampling(OutputArray sampled_point_flags, InputArray input_pts,
float sampled_scale, float dist_lower_limit = 0, RNG *rng = nullptr);
//! @} _3d
} //end namespace cv
#endif //OPENCV_3D_PTCLOUD_HPP

344
modules/3d/src/ptcloud/sampling.cpp Normal file
View File

@ -0,0 +1,344 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
#include "../precomp.hpp"
#include "opencv2/3d/ptcloud.hpp"
#include <unordered_map>
namespace cv {
//! @addtogroup _3d
//! @{
template<typename Tp>
static inline void _swap(Tp &n, Tp &m)
{
Tp tmp = n;
n = m;
m = tmp;
}
/**
* Get cv::Mat with type N×3 CV_32FC1 from cv::InputArray
* Use different interpretations for the same memory data.
*/
static inline void _getMatFromInputArray(InputArray input_pts, Mat &mat)
{
CV_Check(input_pts.dims(), input_pts.dims() < 3,
"Only support data with dimension less than 3.");
// Guaranteed data can construct N×3 point clouds
int rows = input_pts.rows(), cols = input_pts.cols(), channels = input_pts.channels();
size_t total = rows * cols * channels;
CV_Check(total, total % 3 == 0,
"total = input_pts.rows() * input_pts.cols() * input_pts.channels() must be an integer multiple of 3");
if (channels == 1 && rows == 3 && cols != 3)
{
// Layout of point cloud data in memory space:
// x1, ..., xn, y1, ..., yn, z1, ..., zn
// For example, the input is cv::Mat with type 3×N CV_32FC1
transpose(input_pts, mat);
}
else
{
// Layout of point cloud data in memory space:
// x1, y1, z1, ..., xn, yn, zn
// For example, the input is std::vector<Point3d>, or std::vector<int>, or cv::Mat with type N×1 CV_32FC3
mat = input_pts.getMat().reshape(1, (int) (total / 3));
}
if (mat.type() != CV_32F)
{
Mat tmp;
mat.convertTo(tmp, CV_32F); // Use float to store data
swap(mat, tmp);
}
if (!mat.isContinuous())
{
mat = mat.clone();
}
}
int voxelGridSampling(OutputArray sampled_point_flags, InputArray input_pts,
const float length, const float width, const float height)
{
CV_CheckGT(length, 0.0f, "Invalid length of grid");
CV_CheckGT(width, 0.0f, "Invalid width of grid");
CV_CheckGT(height, 0.0f, "Invalid height of grid");
// Get input point cloud data
Mat ori_pts;
_getMatFromInputArray(input_pts, ori_pts);
const int ori_pts_size = ori_pts.rows;
float *const ori_pts_ptr = (float *) ori_pts.data;
// Compute the minimum and maximum bounding box values
float x_min, x_max, y_min, y_max, z_min, z_max;
x_max = x_min = ori_pts_ptr[0];
y_max = y_min = ori_pts_ptr[1];
z_max = z_min = ori_pts_ptr[2];
for (int i = 1; i < ori_pts_size; ++i)
{
float *const ptr_base = ori_pts_ptr + 3 * i;
float x = ptr_base[0], y = ptr_base[1], z = ptr_base[2];
x_min = std::min(x_min, x);
x_max = std::max(x_max, x);
y_min = std::min(y_min, y);
y_max = std::max(y_max, y);
z_min = std::min(z_min, z);
z_max = std::max(z_max, z);
}
// Up to 2^64 grids for key type int64_t
// For larger point clouds or smaller grid sizes, use string or hierarchical map
typedef int64_t keyType;
std::unordered_map<keyType, std::vector<int>> grids;
// int init_size = ori_pts_size * 0.02;
// grids.reserve(init_size);
// Divide points into different grids
keyType offset_y = static_cast<keyType>(cvCeil((x_max - x_min) / length));
keyType offset_z = offset_y * static_cast<keyType>(cvCeil((y_max - y_min) / width));
for (int i = 0; i < ori_pts_size; ++i)
{
int ii = 3 * i;
keyType hx = static_cast<keyType>((ori_pts_ptr[ii] - x_min) / length);
keyType hy = static_cast<keyType>((ori_pts_ptr[ii + 1] - y_min) / width);
keyType hz = static_cast<keyType>((ori_pts_ptr[ii + 2] - z_min) / height);
// Convert three-dimensional coordinates to one-dimensional coordinates key
// Place the stacked three-dimensional grids(boxes) into one dimension (placed in a straight line)
keyType key = hx + hy * offset_y + hz * offset_z;
std::unordered_map<keyType, std::vector<int>>::iterator iter = grids.find(key);
if (iter == grids.end())
grids[key] = {i};
else
iter->second.push_back(i);
}
const int pts_new_size = static_cast<int>(grids.size());
std::vector<char> _sampled_point_flags(ori_pts_size, 0);
// Take out the points in the grid and calculate the point closest to the centroid
std::unordered_map<keyType, std::vector<int>>::iterator grid_iter = grids.begin();
for (int label_id = 0; label_id < pts_new_size; ++label_id, ++grid_iter)
{
std::vector<int> grid_pts = grid_iter->second;
int grid_pts_cnt = static_cast<int>(grid_iter->second.size());
int sampled_point_idx = grid_pts[0];
// 1. one point in the grid, select it directly, no need to calculate.
// 2. two points in the grid, the distance from these two points to their centroid is the same,
// can directly select one of them, also do not need to calculate
if (grid_pts_cnt > 2)
{
// Calculate the centroid position
float sum_x = 0, sum_y = 0, sum_z = 0;
for (const int &item: grid_pts)
{
float *const ptr_base = ori_pts_ptr + 3 * item;
sum_x += ptr_base[0];
sum_y += ptr_base[1];
sum_z += ptr_base[2];
}
float centroid_x = sum_x / grid_pts_cnt, centroid_y = sum_y / grid_pts_cnt, centroid_z =
sum_z / grid_pts_cnt;
// Find the point closest to the centroid
float min_dist_square = FLT_MAX;
for (const int &item: grid_pts)
{
float *const ptr_base = ori_pts_ptr + item * 3;
float x = ptr_base[0], y = ptr_base[1], z = ptr_base[2];
float dist_square = (x - centroid_x) * (x - centroid_x) +
(y - centroid_y) * (y - centroid_y) +
(z - centroid_z) * (z - centroid_z);
if (dist_square < min_dist_square)
{
min_dist_square = dist_square;
sampled_point_idx = item;
}
}
}
_sampled_point_flags[sampled_point_idx] = 1;
}
Mat(_sampled_point_flags).copyTo(sampled_point_flags);
return pts_new_size;
} // voxelGrid()
void randomSampling(OutputArray sampled_pts, InputArray input_pts, const int sampled_pts_size, RNG *rng)
{
CV_CheckGT(sampled_pts_size, 0, "The point cloud size after sampling must be greater than 0.");
CV_CheckDepth(sampled_pts.depth(), sampled_pts.isMat() || sampled_pts.depth() == CV_32F,
"The output data type only supports Mat, vector<Point3f>.");
// Get input point cloud data
Mat ori_pts;
_getMatFromInputArray(input_pts, ori_pts);
const int ori_pts_size = ori_pts.rows;
CV_CheckLT(sampled_pts_size, ori_pts_size,
"The sampled point cloud size must be smaller than the original point cloud size.");
std::vector<int> pts_idxs(ori_pts_size);
for (int i = 0; i < ori_pts_size; ++i) pts_idxs[i] = i;
randShuffle(pts_idxs, 1, rng);
int channels = input_pts.channels();
if (channels == 3 && sampled_pts.isVector())
{
// std::vector<cv::Point3f>
sampled_pts.create(1, sampled_pts_size, CV_32FC3);
}
else
{
// std::vector<float> or cv::Mat
sampled_pts.create(sampled_pts_size, 3, CV_32F);
}
Mat out = sampled_pts.getMat();
float *const ori_pts_ptr = (float *) ori_pts.data;
float *const sampled_pts_ptr = (float *) out.data;
for (int i = 0; i < sampled_pts_size; ++i)
{
float *const ori_pts_ptr_base = ori_pts_ptr + pts_idxs[i] * 3;
float *const sampled_pts_ptr_base = sampled_pts_ptr + i * 3;
sampled_pts_ptr_base[0] = ori_pts_ptr_base[0];
sampled_pts_ptr_base[1] = ori_pts_ptr_base[1];
sampled_pts_ptr_base[2] = ori_pts_ptr_base[2];
}
} // randomSampling()
void randomSampling(OutputArray sampled_pts, InputArray input_pts, const float sampled_scale, RNG *rng)
{
CV_CheckGT(sampled_scale, 0.0f, "The point cloud sampled scale must greater than 0.");
CV_CheckLT(sampled_scale, 1.0f, "The point cloud sampled scale must less than 1.");
Mat ori_pts;
_getMatFromInputArray(input_pts, ori_pts);
randomSampling(sampled_pts, input_pts, cvCeil(sampled_scale * ori_pts.rows), rng);
} // randomSampling()
/**
* FPS Algorithm:
* Input: Point cloud *C*, *sampled_pts_size*, *dist_lower_limit*
* Initialize: Set sampled point cloud S to the empty set
* Step:
* 1. Randomly take a seed point from C and take it from C to S;
* 2. Find a point in C that is the farthest away from S and take it from C to S;
* (The distance from point to set S is the smallest distance from point to all points in S)
* 3. Repeat *step 2* until the farthest distance of the point in C from S
* is less than *dist_lower_limit*, or the size of S is equal to *sampled_pts_size*.
* Output: Sampled point cloud S
*/
int farthestPointSampling(OutputArray sampled_point_flags, InputArray input_pts,
const int sampled_pts_size, const float dist_lower_limit, RNG *rng)
{
CV_CheckGT(sampled_pts_size, 0, "The point cloud size after sampling must be greater than 0.");
CV_CheckGE(dist_lower_limit, 0.0f, "The distance lower bound must be greater than or equal to 0.");
// Get input point cloud data
Mat ori_pts;
_getMatFromInputArray(input_pts, ori_pts);
const int ori_pts_size = ori_pts.rows;
CV_CheckLT(sampled_pts_size, ori_pts_size,
"The sampled point cloud size must be smaller than the original point cloud size.");
// idx arr [ . . . . . . . . . ] --- sampling ---> [ . . . . . . . . . ]
// C S | C
AutoBuffer<int> _idxs(ori_pts_size);
AutoBuffer<float> _dist_square(ori_pts_size);
int *idxs = _idxs.data();
float *dist_square = _dist_square.data();
for (int i = 0; i < ori_pts_size; ++i)
{
idxs[i] = i;
dist_square[i] = FLT_MAX;
}
// Randomly take a seed point from C and put it into S
int seed = (int)((rng? rng->next(): theRNG().next()) % ori_pts_size);
idxs[0] = seed;
idxs[seed] = 0;
std::vector<char> _sampled_point_flags(ori_pts_size, 0);
_sampled_point_flags[seed] = 1;
float *const ori_pts_ptr = (float *) ori_pts.data;
int sampled_cnt = 1;
const float dist_lower_limit_square = dist_lower_limit * dist_lower_limit;
while (sampled_cnt < sampled_pts_size)
{
int last_pt = sampled_cnt - 1;
float *const last_pt_ptr_base = ori_pts_ptr + 3 * idxs[last_pt];
float last_pt_x = last_pt_ptr_base[0], last_pt_y = last_pt_ptr_base[1], last_pt_z = last_pt_ptr_base[2];
// Calculate the distance from point in C to set S
float max_dist_square = 0;
for (int i = sampled_cnt; i < ori_pts_size; ++i)
{
float *const ori_pts_ptr_base = ori_pts_ptr + 3 * idxs[i];
float x_diff = (last_pt_x - ori_pts_ptr_base[0]);
float y_diff = (last_pt_y - ori_pts_ptr_base[1]);
float z_diff = (last_pt_z - ori_pts_ptr_base[2]);
float next_dist_square = x_diff * x_diff + y_diff * y_diff + z_diff * z_diff;
if (next_dist_square < dist_square[i])
{
dist_square[i] = next_dist_square;
}
if (dist_square[i] > max_dist_square)
{
last_pt = i;
max_dist_square = dist_square[i];
}
}
if (max_dist_square < dist_lower_limit_square)
break;
_sampled_point_flags[idxs[last_pt]] = 1;
_swap(idxs[sampled_cnt], idxs[last_pt]);
_swap(dist_square[sampled_cnt], dist_square[last_pt]);
++sampled_cnt;
}
Mat(_sampled_point_flags).copyTo(sampled_point_flags);
return sampled_cnt;
} // farthestPointSampling()
int farthestPointSampling(OutputArray sampled_point_flags, InputArray input_pts,
const float sampled_scale, const float dist_lower_limit, RNG *rng)
{
CV_CheckGT(sampled_scale, 0.0f, "The point cloud sampled scale must greater than 0.");
CV_CheckLT(sampled_scale, 1.0f, "The point cloud sampled scale must less than 1.");
CV_CheckGE(dist_lower_limit, 0.0f, "The distance lower bound must be greater than or equal to 0.");
Mat ori_pts;
_getMatFromInputArray(input_pts, ori_pts);
return farthestPointSampling(sampled_point_flags, input_pts,
cvCeil(sampled_scale * ori_pts.rows), dist_lower_limit, rng);
} // farthestPointSampling()
//! @} _3d
} //end namespace cv

280
modules/3d/test/test_sampling.cpp Normal file
View File

@ -0,0 +1,280 @@
// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
#include "test_precomp.hpp"
namespace opencv_test { namespace {
class SamplingTest : public ::testing::Test {
public:
int ori_pts_size = 0;
// Different point clouds to test InputArray compatibility.
vector<Point3f> pts_vec_Pt3f;
Mat pts_mat_32F_Nx3;
Mat pts_mat_64F_3xN;
Mat pts_mat_32FC3_Nx1;
// Different masks to test OutputArray compatibility.
vector<char> mask_vec_char;
vector<int> mask_vec_int;
Mat mask_mat_Nx1;
Mat mask_mat_1xN;
// Combination of InputArray and OutputArray as information to mention where the test fail.
string header = "\n===================================================================\n";
string combination1 = "OutputArray: vector<char>\nInputArray: vector<point3f>\n";
string combination2 = "OutputArray: Nx1 Mat\nInputArray: Nx3 float Mat\n";
string combination3 = "OutputArray: 1xN Mat\nInputArray: 3xN double Mat\n";
string combination4 = "OutputArray: vector<int>\nInputArray: Nx1 3 channels float Mat\n";
// Initialize point clouds with different data type.
void dataInitialization(vector<float> &pts_data)
{
ori_pts_size = (int) pts_data.size() / 3;
// point cloud use Nx3 mat as data structure and float as value type.
pts_mat_32F_Nx3 = Mat(ori_pts_size, 3, CV_32F, pts_data.data());
// point cloud use vector<Point3f> as data structure.
pts_vec_Pt3f.clear();
for (int i = 0; i < ori_pts_size; i++)
{
int i3 = i * 3;
pts_vec_Pt3f.emplace_back(
Point3f(pts_data[i3], pts_data[i3 + 1], pts_data[i3 + 2]));
}
// point cloud use 3xN mat as data structure and double as value type.
pts_mat_64F_3xN = pts_mat_32F_Nx3.t();
pts_mat_64F_3xN.convertTo(pts_mat_64F_3xN, CV_64F);
// point cloud use Nx1 mat with 3 channels as data structure and float as value type.
pts_mat_32F_Nx3.convertTo(pts_mat_32FC3_Nx1, CV_32FC3);
}
};
// Get 1xN mat of mask from OutputArray.
void getMatFromMask(OutputArray &mask, Mat &mat)
{
int rows = mask.rows(), cols = mask.cols(), channels = mask.channels();
if (channels == 1 && cols == 1 && rows != 1)
mat = mask.getMat().t();
else
mat = mask.getMat().reshape(1, 1);
// Use int to store data.
if (mat.type() != CV_32S)
mat.convertTo(mat, CV_32S);
}
// Compare 2 rows in different point clouds.
bool compareRows(const Mat &m, int rm, const Mat &n, int rn)
{
Mat diff = m.row(rm) != n.row(rn);
return countNonZero(diff) == 0;
}
TEST_F(SamplingTest, VoxelGridFilterSampling)
{
vector<float> pts_info = {0.0f, 0.0f, 0.0f,
-3.0f, -3.0f, -3.0f, 3.0f, -3.0f, -3.0f, 3.0f, 3.0f, -3.0f, -3.0f, 3.0f, -3.0f,
-3.0f, -3.0f, 3.0f, 3.0f, -3.0f, 3.0f, 3.0f, 3.0f, 3.0f, -3.0f, 3.0f, 3.0f,
-0.9f, -0.9f, -0.9f, 0.9f, -0.9f, -0.9f, 0.9f, 0.9f, -0.9f, -0.9f, 0.9f, -0.9f,
-0.9f, -0.9f, 0.9f, 0.9f, -0.9f, 0.9f, 0.9f, 0.9f, 0.9f, -0.9f, 0.9f, 0.9f,};
dataInitialization(pts_info);
auto testVoxelGridFilterSampling = [this](OutputArray mask, InputArray pts, float *side,
int sampled_pts_size, const Mat &ans, const string &info) {
// Check the number of point cloud after sampling.
int res = voxelGridSampling(mask, pts, side[0], side[1], side[2]);
EXPECT_EQ(sampled_pts_size, res)
<< header << info << "The side length is " << side[0]
<< "\nThe return value of voxelGridSampling() is not equal to " << sampled_pts_size
<< endl;
EXPECT_EQ(sampled_pts_size, countNonZero(mask))
<< header << info << "The side length is " << side[0]
<< "\nThe number of selected points of mask is not equal to " << sampled_pts_size
<< endl;
// The mask after sampling should be equal to the answer.
Mat _mask;
getMatFromMask(mask, _mask);
ASSERT_TRUE(compareRows(_mask, 0, ans, 0))
<< header << info << "The side length is " << side[0]
<< "\nThe mask should be " << ans << endl;
};
// Set 6.1 as the side1 length, only the center point left.
Mat ans1({1, (int) (pts_info.size() / 3)}, {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0});
float side1[3] = {6.1f, 6.1f, 6.1f};
testVoxelGridFilterSampling(mask_vec_char, pts_vec_Pt3f, side1, 1, ans1, combination1);
testVoxelGridFilterSampling(mask_mat_Nx1, pts_mat_32F_Nx3, side1, 1, ans1, combination2);
testVoxelGridFilterSampling(mask_mat_1xN, pts_mat_64F_3xN, side1, 1, ans1, combination3);
testVoxelGridFilterSampling(mask_vec_int, pts_mat_32FC3_Nx1, side1, 1, ans1, combination4);
// Set 2 as the side1 length, only the center point and the vertexes of big cube left.
Mat ans2({1, (int) (pts_info.size() / 3)}, {1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0});
float side2[3] = {2.0f, 2.0f, 2.0f};
testVoxelGridFilterSampling(mask_vec_char, pts_vec_Pt3f, side2, 9, ans2, combination1);
testVoxelGridFilterSampling(mask_mat_Nx1, pts_mat_32F_Nx3, side2, 9, ans2, combination2);
testVoxelGridFilterSampling(mask_mat_1xN, pts_mat_64F_3xN, side2, 9, ans2, combination3);
testVoxelGridFilterSampling(mask_vec_int, pts_mat_32FC3_Nx1, side2, 9, ans2, combination4);
// Set 1.1 as the side1 length, all points should be left.
Mat ans3({1, (int) (pts_info.size() / 3)}, {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1});
float side3[3] = {1.1f, 1.1f, 1.1f};
testVoxelGridFilterSampling(mask_vec_char, pts_vec_Pt3f, side3, 17, ans3, combination1);
testVoxelGridFilterSampling(mask_mat_Nx1, pts_mat_32F_Nx3, side3, 17, ans3, combination2);
testVoxelGridFilterSampling(mask_mat_1xN, pts_mat_64F_3xN, side3, 17, ans3, combination3);
testVoxelGridFilterSampling(mask_vec_int, pts_mat_32FC3_Nx1, side3, 17, ans3, combination4);
}
TEST_F(SamplingTest, RandomSampling)
{
vector<float> pts_info = {0, 0, 0, 1, 0, 0, 1, 2, 0, 0, 2, 0,
0, 0, 3, 1, 0, 3, 1, 2, 3, 0, 2, 3};
dataInitialization(pts_info);
auto testRandomSampling = [this](InputArray pts, int sampled_pts_size, const string &info) {
// Check the number of point cloud after sampling.
Mat sampled_pts;
randomSampling(sampled_pts, pts, sampled_pts_size);
EXPECT_EQ(sampled_pts_size, sampled_pts.rows)
<< header << info << "The sampled points size is " << sampled_pts_size
<< "\nThe number of sampled points is not equal to " << sampled_pts_size << endl;
// Convert InputArray to Mat of original point cloud.
int rows = pts.rows(), cols = pts.cols(), channels = pts.channels();
int total = rows * cols * channels;
Mat ori_pts;
if (channels == 1 && rows == 3 && cols != 3)
ori_pts = pts.getMat().t();
else
ori_pts = pts.getMat().reshape(1, (int) (total / 3));
if (ori_pts.type() != CV_32F)
ori_pts.convertTo(ori_pts, CV_32F);
// Each point should be in the original point cloud.
for (int i = 0; i < sampled_pts_size; i++)
{
// Check whether a point exists in the point cloud.
bool flag = false;
for (int j = 0; j < ori_pts.rows; j++)
flag |= compareRows(sampled_pts, i, ori_pts, j);
ASSERT_TRUE(flag)
<< header << info << "The sampled points size is " << sampled_pts_size
<< "\nThe sampled point " << sampled_pts.row(i)
<< " is not in original point cloud" << endl;
}
};
testRandomSampling(pts_vec_Pt3f, 3, combination1);
testRandomSampling(pts_mat_32F_Nx3, 4, combination2);
testRandomSampling(pts_mat_64F_3xN, 5, combination3);
testRandomSampling(pts_mat_32FC3_Nx1, 6, combination4);
}
TEST_F(SamplingTest, FarthestPointSampling)
{
vector<float> pts_info = {0, 0, 0, 1, 0, 0, 1, 2, 0, 0, 2, 0,
0, 0, 3, 1, 0, 3, 1, 2, 3, 0, 2, 3};
dataInitialization(pts_info);
auto testFarthestPointSampling = [this](OutputArray mask, InputArray pts, int sampled_pts_size,
const vector<Mat> &ans, const string &info) {
// Check the number of point cloud after sampling.
int res = farthestPointSampling(mask, pts, sampled_pts_size);
EXPECT_EQ(sampled_pts_size, res)
<< header << info << "The sampled points size is " << sampled_pts_size
<< "\nThe return value of farthestPointSampling() is not equal to "
<< sampled_pts_size << endl;
EXPECT_EQ(sampled_pts_size, countNonZero(mask))
<< header << info << "The sampled points size is " << sampled_pts_size
<< "\nThe number of selected points of mask is not equal to " << sampled_pts_size
<< endl;
// The mask after sampling should be one of the answers.
Mat _mask;
getMatFromMask(mask, _mask);
bool flag = false;
for (const Mat &a: ans)
flag |= compareRows(a, 0, _mask, 0);
string ans_info;
ASSERT_TRUE(flag)
<< header << info << "The sampled points size is " << sampled_pts_size
<< "\nThe mask should be one of the answer list" << endl;
};
// Set 2 as the size, and there should be 2 diagonal points after sampling.
vector<Mat> ans_list1;
ans_list1.emplace_back(Mat({1, ori_pts_size}, {1, 0, 0, 0, 0, 0, 1, 0}));
ans_list1.emplace_back(Mat({1, ori_pts_size}, {0, 1, 0, 0, 0, 0, 0, 1}));
ans_list1.emplace_back(Mat({1, ori_pts_size}, {0, 0, 1, 0, 1, 0, 0, 0}));
ans_list1.emplace_back(Mat({1, ori_pts_size}, {0, 0, 0, 1, 0, 1, 0, 0}));
testFarthestPointSampling(mask_vec_char, pts_vec_Pt3f, 2, ans_list1, combination1);
testFarthestPointSampling(mask_mat_Nx1, pts_mat_32F_Nx3, 2, ans_list1, combination2);
testFarthestPointSampling(mask_mat_1xN, pts_mat_64F_3xN, 2, ans_list1, combination3);
testFarthestPointSampling(mask_vec_int, pts_mat_32FC3_Nx1, 2, ans_list1, combination4);
// Set 4 as the size, and there should be 4 specific points form a plane perpendicular to the X and Y axes after sampling.
vector<Mat> ans_list2;
ans_list2.emplace_back(Mat({1, ori_pts_size}, {1, 0, 1, 0, 1, 0, 1, 0}));
ans_list2.emplace_back(Mat({1, ori_pts_size}, {0, 1, 0, 1, 0, 1, 0, 1}));
testFarthestPointSampling(mask_vec_char, pts_vec_Pt3f, 4, ans_list2, combination1);
testFarthestPointSampling(mask_mat_Nx1, pts_mat_32F_Nx3, 4, ans_list2, combination2);
testFarthestPointSampling(mask_mat_1xN, pts_mat_64F_3xN, 4, ans_list2, combination3);
testFarthestPointSampling(mask_vec_int, pts_mat_32FC3_Nx1, 4, ans_list2, combination4);
}
TEST_F(SamplingTest, FarthestPointSamplingDoublePtCloud)
{
// After doubling the point cloud, 8 points are sampled and each vertex is displayed only once.
vector<float> pts_info = {0, 0, 0, 1, 0, 0, 1, 2, 0, 0, 2, 0,
0, 0, 3, 1, 0, 3, 1, 2, 3, 0, 2, 3};
// Double the point cloud.
pts_info.insert(pts_info.end(), pts_info.begin(), pts_info.end());
dataInitialization(pts_info);
auto testFarthestPointSamplingDoublePtCloud = [this](OutputArray mask, InputArray pts, int sampled_pts_size,
const string &info) {
// Check the number of point cloud after sampling.
int res = farthestPointSampling(mask, pts, sampled_pts_size);
EXPECT_EQ(sampled_pts_size, res)
<< header << info << "The sampled points size is " << sampled_pts_size
<< "\nThe return value of farthestPointSampling() is not equal to "
<< sampled_pts_size << endl;
EXPECT_EQ(sampled_pts_size, countNonZero(mask))
<< header << info << "The sampled points size is " << sampled_pts_size
<< "\nThe number of selected points of mask is not equal to " << sampled_pts_size
<< endl;
// One and only one of mask[index] and mask[index + size/2] is true for first eight index.
Mat _mask;
getMatFromMask(mask, _mask);
auto *mask_ptr = (int *) _mask.data;
int offset = ori_pts_size / 2;
bool flag = true;
for (int i = 0; i < offset; i++)
flag &= (mask_ptr[i] == 0 && mask_ptr[i + offset] != 0) || (mask_ptr[i] != 0 && mask_ptr[i + offset] == 0);
ASSERT_TRUE(flag)
<< header << info << "The sampled points size is " << sampled_pts_size
<< "\nThe mask should be one of the answer list" << endl;
};
testFarthestPointSamplingDoublePtCloud(mask_vec_char, pts_vec_Pt3f, 8, combination1);
testFarthestPointSamplingDoublePtCloud(mask_mat_Nx1, pts_mat_32F_Nx3, 8, combination2);
testFarthestPointSamplingDoublePtCloud(mask_mat_1xN, pts_mat_64F_3xN, 8, combination3);
testFarthestPointSamplingDoublePtCloud(mask_vec_int, pts_mat_32FC3_Nx1, 8, combination4);
}
} // namespace
} // opencv_test