Chiebot-Mirror
/
opencv
mirror of https://github.com/opencv/opencv.git
8
0
Fork 0
Code Issues 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.
pull/20981/head
Ruan 3 years ago 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
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      modules/3d/include/opencv2/3d.hpp
  2. 108
      modules/3d/include/opencv2/3d/ptcloud.hpp
  3. 344
      modules/3d/src/ptcloud/sampling.cpp
  4. 280
      modules/3d/test/test_sampling.cpp

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

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

108
modules/3d/include/opencv2/3d/ptcloud.hpp
Unescape 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
Unescape 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
Unescape 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
Write Preview
Loading…
Cancel
Save