From 1470f90c2a56c7c8e60bee56d683cc35b25cb519 Mon Sep 17 00:00:00 2001 From: Ruan <47767371+Ryyyc@users.noreply.github.com> Date: Fri, 29 Oct 2021 06:41:21 +0800 Subject: [PATCH] Merge pull request #20784 from No-Plane-Cannot-Be-Detected:next 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. --- modules/3d/include/opencv2/3d.hpp | 1 + modules/3d/include/opencv2/3d/ptcloud.hpp | 108 +++++++ modules/3d/src/ptcloud/sampling.cpp | 344 ++++++++++++++++++++++ modules/3d/test/test_sampling.cpp | 280 ++++++++++++++++++ 4 files changed, 733 insertions(+) create mode 100644 modules/3d/include/opencv2/3d/ptcloud.hpp create mode 100644 modules/3d/src/ptcloud/sampling.cpp create mode 100644 modules/3d/test/test_sampling.cpp diff --git a/modules/3d/include/opencv2/3d.hpp b/modules/3d/include/opencv2/3d.hpp index 4c7aea7f28..ae0fb130f0 100644 --- a/modules/3d/include/opencv2/3d.hpp +++ b/modules/3d/include/opencv2/3d.hpp @@ -10,6 +10,7 @@ #include "opencv2/3d/depth.hpp" #include "opencv2/3d/volume.hpp" +#include "opencv2/3d/ptcloud.hpp" /** @defgroup _3d 3D vision functionality diff --git a/modules/3d/include/opencv2/3d/ptcloud.hpp b/modules/3d/include/opencv2/3d/ptcloud.hpp new file mode 100644 index 0000000000..d1dec17ca1 --- /dev/null +++ b/modules/3d/include/opencv2/3d/ptcloud.hpp @@ -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 or std::vector 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. + * @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. + * @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 or std::vector 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 or std::vector 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 diff --git a/modules/3d/src/ptcloud/sampling.cpp b/modules/3d/src/ptcloud/sampling.cpp new file mode 100644 index 0000000000..5d0b13e58d --- /dev/null +++ b/modules/3d/src/ptcloud/sampling.cpp @@ -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 + +namespace cv { + +//! @addtogroup _3d +//! @{ + +template +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, or std::vector, 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> grids; + +// int init_size = ori_pts_size * 0.02; +// grids.reserve(init_size); + + // Divide points into different grids + + keyType offset_y = static_cast(cvCeil((x_max - x_min) / length)); + keyType offset_z = offset_y * static_cast(cvCeil((y_max - y_min) / width)); + + for (int i = 0; i < ori_pts_size; ++i) + { + int ii = 3 * i; + keyType hx = static_cast((ori_pts_ptr[ii] - x_min) / length); + keyType hy = static_cast((ori_pts_ptr[ii + 1] - y_min) / width); + keyType hz = static_cast((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>::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(grids.size()); + std::vector _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>::iterator grid_iter = grids.begin(); + + for (int label_id = 0; label_id < pts_new_size; ++label_id, ++grid_iter) + { + std::vector grid_pts = grid_iter->second; + int grid_pts_cnt = static_cast(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."); + + // 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 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 + sampled_pts.create(1, sampled_pts_size, CV_32FC3); + } + else + { + // std::vector 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 _idxs(ori_pts_size); + AutoBuffer _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 _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 \ No newline at end of file diff --git a/modules/3d/test/test_sampling.cpp b/modules/3d/test/test_sampling.cpp new file mode 100644 index 0000000000..5429f6906c --- /dev/null +++ b/modules/3d/test/test_sampling.cpp @@ -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 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 mask_vec_char; + vector 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\nInputArray: vector\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\nInputArray: Nx1 3 channels float Mat\n"; + + + // Initialize point clouds with different data type. + void dataInitialization(vector &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 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 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 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 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 &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 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 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 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