mirror of https://github.com/opencv/opencv.git
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.pull/20981/head
Ruan
3 years ago
committed by
GitHub
parent
6544c7b787
commit
1470f90c2a
4 changed files with 733 additions and 0 deletions
@ -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
|
||||
@ -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
|
||||
@ -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
|
||||
Loading…
Reference in new issue