mirror of https://github.com/opencv/opencv.git
Merge pull request #21918 from No-Plane-Cannot-Be-Detected:5.x-region_growing_3d
Add normal estimation and region growing algorithm for point cloud * Add normal estimation and region growing algorithm for point cloud * 1.Modified documentation for normal estimation;2.Converted curvature in region growing to absolute values;3.Changed the data type of threshold from float to double;4.Fixed some bugs; * Finished documentation * Add tests for normal estimation. Test the normal and curvature of each point in the plane and sphere of the point cloud. * Fix some warnings caused by to small numbers in test * Change the test to calculate the average difference instead of comparing each normal and curvature * Fixed the bugs found by testing * Redesigned the interface and fixed problems: 1. Make the interface compatible with radius search. 2. Make region growing optionally sortable on results. 3. Modified the region growing interface. 4. Format reference. 5. Removed sphere test. * Fix warnings * Remove flann dependency * Move the flann dependency to the corresponding testpull/19555/head
Wanli Zhong
3 years ago
committed by
GitHub
parent
6e9ab70359
commit
b06544bd54
9 changed files with 822 additions and 89 deletions
@ -0,0 +1,87 @@ |
||||
// 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.
|
||||
//
|
||||
// Copyright (C) 2022, Wanli Zhong <zhongwl2018@mail.sustech.edu.cn>
|
||||
|
||||
#include "../precomp.hpp" |
||||
#include "ptcloud_utils.hpp" |
||||
|
||||
namespace cv { |
||||
|
||||
//! @addtogroup _3d
|
||||
//! @{
|
||||
|
||||
void |
||||
normalEstimate(OutputArray normals, OutputArray curvatures, InputArray input_pts_, |
||||
InputArrayOfArrays nn_idx_, int max_neighbor_num_) |
||||
{ |
||||
Mat ori_pts; |
||||
getPointsMatFromInputArray(input_pts_, ori_pts, 0); |
||||
int pts_size = ori_pts.rows; |
||||
|
||||
std::vector<Mat> nn_idx; |
||||
nn_idx_.getMatVector(nn_idx); |
||||
|
||||
CV_CheckEQ((int) nn_idx.size(), pts_size, |
||||
"The point number of NN search result should be equal to the size of the point cloud."); |
||||
|
||||
if (normals.channels() == 3 && normals.isVector()) |
||||
{ |
||||
// std::vector<cv::Point3f>
|
||||
normals.create(1, pts_size, CV_32FC3); |
||||
} |
||||
else |
||||
{ |
||||
// cv::Mat
|
||||
normals.create(pts_size, 3, CV_32F); |
||||
} |
||||
|
||||
curvatures.create(pts_size, 1, CV_32F); |
||||
|
||||
int max_neighbor_num = max_neighbor_num_ <= 0 ? INT_MAX : max_neighbor_num_; |
||||
|
||||
float *normals_ptr = (float *) normals.getMat().data; |
||||
float *curvatures_ptr = (float *) curvatures.getMat().data; |
||||
|
||||
parallel_for_(Range(0, pts_size), [&](const Range &range) { |
||||
// Index of current nearest neighbor point
|
||||
int cur_nei_idx; |
||||
for (int i = range.start; i < range.end; ++i) |
||||
{ |
||||
// The maximum size that may be used for this row
|
||||
int bound = max_neighbor_num > nn_idx[i].cols ? nn_idx[i].cols : max_neighbor_num; |
||||
const int *nn_idx_ptr_base = (int *) nn_idx[i].data; |
||||
// The first point should be itself
|
||||
Mat pt_set(ori_pts.row(i)); |
||||
// Push the nearest neighbor points to pt_set
|
||||
for (int j = 1; j < bound; ++j) |
||||
{ |
||||
cur_nei_idx = nn_idx_ptr_base[j]; |
||||
// If the index is less than 0,
|
||||
// the nn_idx of this row will no longer have any information.
|
||||
if (cur_nei_idx < 0) break; |
||||
pt_set.push_back(ori_pts.row(cur_nei_idx)); |
||||
} |
||||
|
||||
Mat mean; |
||||
// Calculate the mean of point set, use "reduce()" is faster than default method in PCA
|
||||
reduce(pt_set, mean, 0, REDUCE_AVG); |
||||
// Use PCA to get eigenvalues and eigenvectors of pt_set
|
||||
PCA pca(pt_set, mean, PCA::DATA_AS_ROW); |
||||
|
||||
const float *eigenvectors_ptr = (float *) pca.eigenvectors.data; |
||||
float *normals_ptr_base = normals_ptr + 3 * i; |
||||
normals_ptr_base[0] = eigenvectors_ptr[6]; |
||||
normals_ptr_base[1] = eigenvectors_ptr[7]; |
||||
normals_ptr_base[2] = eigenvectors_ptr[8]; |
||||
|
||||
const float *eigenvalues_ptr = (float *) pca.eigenvalues.data; |
||||
curvatures_ptr[i] = eigenvalues_ptr[2] / |
||||
(eigenvalues_ptr[0] + eigenvalues_ptr[1] + eigenvalues_ptr[2]); |
||||
} |
||||
}); |
||||
} |
||||
|
||||
//! @} _3d
|
||||
} //end namespace cv
|
||||
@ -0,0 +1,183 @@ |
||||
// 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.
|
||||
//
|
||||
// Copyright (C) 2022, Wanli Zhong <zhongwl2018@mail.sustech.edu.cn>
|
||||
|
||||
#include "../precomp.hpp" |
||||
#include "region_growing_3d.hpp" |
||||
|
||||
#include <queue> |
||||
#include <numeric> |
||||
|
||||
namespace cv { |
||||
// namespace _3d {
|
||||
|
||||
/////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//////////////////////////////// RegionGrowing3DImpl //////////////////////////////////////
|
||||
|
||||
int |
||||
RegionGrowing3DImpl::segment(OutputArrayOfArrays regions_idx_, OutputArray labels_, |
||||
InputArray input_pts_, InputArray normals_, InputArrayOfArrays nn_idx_) |
||||
{ |
||||
Mat ori_pts, normals; |
||||
getPointsMatFromInputArray(input_pts_, ori_pts, 0); |
||||
getPointsMatFromInputArray(normals_, normals, 0); |
||||
|
||||
std::vector<Mat> nn_idx; |
||||
nn_idx_.getMatVector(nn_idx); |
||||
|
||||
int pts_size = ori_pts.rows; |
||||
bool has_curvatures = !curvatures.empty(); |
||||
bool has_seeds = !seeds.empty(); |
||||
|
||||
CV_CheckGE(max_size, min_size, |
||||
"The maximum size should be grater than or equal to the minimum size."); |
||||
CV_CheckEQ(pts_size, (int) nn_idx.size(), |
||||
"The point number of NN search result should be equal to the size of the point cloud."); |
||||
CV_CheckEQ(pts_size, normals.rows, |
||||
"The number of points in the normals should be equal to that in the point cloud."); |
||||
if (has_curvatures) |
||||
{ |
||||
CV_CheckEQ(pts_size, curvatures.cols, |
||||
"The number of points in the curvatures should be equal to that in the point cloud."); |
||||
} |
||||
if (has_seeds) |
||||
{ |
||||
CV_CheckGE(pts_size, seeds.cols, |
||||
"The number of seed should be less than or equal to the number of points in the point cloud."); |
||||
} |
||||
|
||||
if (!has_seeds && !has_curvatures) |
||||
{ |
||||
// If the user does not set the seeds and curvatures, use the natural order of the points
|
||||
std::vector<int> *natural_order_seeds = new std::vector<int>(pts_size); |
||||
std::iota(natural_order_seeds->begin(), natural_order_seeds->end(), 0); |
||||
seeds = Mat(*natural_order_seeds); |
||||
} |
||||
else if (!has_seeds && has_curvatures) |
||||
{ |
||||
// If the user sets the curvatures without setting the seeds, seeds will be sorted in
|
||||
// ascending order of curvatures
|
||||
sortIdx(curvatures, seeds, SORT_EVERY_ROW + SORT_ASCENDING); |
||||
} |
||||
|
||||
const int *seeds_ptr = (int *) seeds.data; |
||||
const float *curvatures_ptr = (float *) curvatures.data; |
||||
|
||||
int seeds_num = seeds.cols, flag = 1; |
||||
double cos_smoothness_thr = std::cos(smoothness_thr); |
||||
double cos_smoothness_thr_square = cos_smoothness_thr * cos_smoothness_thr; |
||||
// Used to determine if the point can be grown
|
||||
std::vector<bool> has_grown(pts_size, false); |
||||
// Store the indexes of the points in each region
|
||||
std::vector<std::vector<int>> regions_idx; |
||||
for (int i = 0; i < seeds_num; ++i) |
||||
{ |
||||
int cur_seed = seeds_ptr[i]; |
||||
|
||||
// 1. If the number of regions is satisfied then stop running.
|
||||
// 2. Filter out seeds with curvature greater than the threshold.
|
||||
// If no seed points have been set, it will use seeds sorted in ascending order of curvatures.
|
||||
// When the curvature doesn't satisfy the threshold, the seed points that follow will not satisfy the threshold either.
|
||||
if (flag > region_num || |
||||
(!has_seeds && has_curvatures && curvatures_ptr[cur_seed] > curvature_thr)) |
||||
{ |
||||
break; |
||||
} |
||||
// 1. If current seed has been grown then grow the next one.
|
||||
// 2. Filter out seeds with curvature greater than the threshold.
|
||||
if (has_grown[cur_seed] || |
||||
(has_seeds && has_curvatures && curvatures_ptr[cur_seed] > curvature_thr)) |
||||
{ |
||||
continue; |
||||
} |
||||
|
||||
Mat base_normal; |
||||
if (!smooth_mode) base_normal = normals.row(cur_seed); |
||||
|
||||
has_grown[cur_seed] = true; |
||||
std::vector<int> region = {cur_seed}; |
||||
std::queue<int> grow_list; |
||||
grow_list.push(cur_seed); |
||||
while (!grow_list.empty()) |
||||
{ |
||||
int cur_idx = grow_list.front(); |
||||
grow_list.pop(); |
||||
if (smooth_mode) base_normal = normals.row(cur_idx); |
||||
|
||||
// The maximum size that may be used for this row
|
||||
int bound = max_neighbor_num > nn_idx[cur_idx].cols ? nn_idx[cur_idx].cols |
||||
: max_neighbor_num; |
||||
const int *nn_idx_ptr_base = (int *) nn_idx[cur_idx].data; |
||||
// Start from index 1 because the first one of knn_idx is itself
|
||||
for (int j = 1; j < bound; ++j) |
||||
{ |
||||
int cur_nei_idx = nn_idx_ptr_base[j]; |
||||
// If the index is less than 0,
|
||||
// the nn_idx of this row will no longer have any information.
|
||||
if (cur_nei_idx < 0) break; |
||||
// 1. If current point has been grown then grow the next one
|
||||
// 2. Filter out points with curvature greater than the threshold
|
||||
if (has_grown[cur_nei_idx] || |
||||
(has_curvatures && curvatures_ptr[cur_nei_idx] > curvature_thr)) |
||||
{ |
||||
continue; |
||||
} |
||||
|
||||
Mat cur_normal = normals.row(cur_nei_idx); |
||||
double n12 = base_normal.dot(cur_normal); |
||||
double n1m = base_normal.dot(base_normal); |
||||
double n2m = cur_normal.dot(cur_normal); |
||||
// If the smoothness threshold is satisfied, this neighbor will be pushed to the growth list
|
||||
if (n12 * n12 / (n1m * n2m) >= cos_smoothness_thr_square) |
||||
{ |
||||
has_grown[cur_nei_idx] = true; |
||||
region.emplace_back(cur_nei_idx); |
||||
grow_list.push(cur_nei_idx); |
||||
} |
||||
} |
||||
} |
||||
|
||||
int region_size = (int) region.size(); |
||||
if (min_size <= region_size && region_size <= max_size) |
||||
{ |
||||
regions_idx.emplace_back(Mat(region)); |
||||
flag++; |
||||
} |
||||
else if (region_size > max_size) |
||||
{ |
||||
break; |
||||
} |
||||
} |
||||
|
||||
if (need_sort) |
||||
{ |
||||
// Compare the size of two regions in descending order
|
||||
auto compareRegionSize = [](const std::vector<int> &r1, |
||||
const std::vector<int> &r2) -> bool |
||||
{ |
||||
return r1.size() > r2.size(); |
||||
}; |
||||
sort(regions_idx.begin(), regions_idx.end(), compareRegionSize); |
||||
} |
||||
|
||||
std::vector<std::vector<int>> &_regions_idx = *(std::vector<std::vector<int>> *) regions_idx_.getObj(); |
||||
_regions_idx.resize(regions_idx.size()); |
||||
Mat labels = Mat::zeros(pts_size, 1, CV_32S); |
||||
int *labels_ptr = (int *) labels.data; |
||||
for (int i = 0; i < (int) regions_idx.size(); ++i) |
||||
{ |
||||
Mat(1, (int) regions_idx[i].size(), CV_32S, regions_idx[i].data()).copyTo(_regions_idx[i]); |
||||
for (int j: regions_idx[i]) |
||||
{ |
||||
labels_ptr[j] = i + 1; |
||||
} |
||||
} |
||||
labels.copyTo(labels_); |
||||
|
||||
return flag - 1; |
||||
} |
||||
|
||||
// } // _3d::
|
||||
} // cv::
|
||||
@ -0,0 +1,177 @@ |
||||
// 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.
|
||||
//
|
||||
// Copyright (C) 2022, Wanli Zhong <zhongwl2018@mail.sustech.edu.cn>
|
||||
|
||||
#ifndef OPENCV_REGION_ROWING_3D_HPP |
||||
#define OPENCV_REGION_ROWING_3D_HPP |
||||
|
||||
#include "opencv2/3d/ptcloud.hpp" |
||||
#include "ptcloud_utils.hpp" |
||||
|
||||
namespace cv { |
||||
|
||||
class RegionGrowing3DImpl : public RegionGrowing3D |
||||
{ |
||||
private: |
||||
//! Threshold of the angle between normals, the value is in radian.
|
||||
double smoothness_thr; |
||||
|
||||
//! Threshold of curvature.
|
||||
double curvature_thr; |
||||
|
||||
//! The maximum number of neighbors want to use including itself.
|
||||
int max_neighbor_num; |
||||
|
||||
//! The minimum size of region.
|
||||
int min_size; |
||||
|
||||
//! The maximum size of region
|
||||
int max_size; |
||||
|
||||
//! Whether to use the smoothness mode.
|
||||
bool smooth_mode; |
||||
|
||||
//! The maximum number of regions you want.
|
||||
int region_num; |
||||
|
||||
//! Whether the results need to be sorted in descending order by the number of points.
|
||||
bool need_sort; |
||||
|
||||
//! The curvature of each point.
|
||||
Mat curvatures; |
||||
|
||||
//! Seed points.
|
||||
Mat seeds; |
||||
|
||||
public: |
||||
//! No-argument constructor using default configuration.
|
||||
RegionGrowing3DImpl() |
||||
: smoothness_thr(0.5235987756 /* 30*PI/180 */ ), curvature_thr(0.05), |
||||
max_neighbor_num(INT_MAX), min_size(1), max_size(INT_MAX), smooth_mode(true), |
||||
region_num(INT_MAX), need_sort(true) |
||||
{ |
||||
} |
||||
|
||||
int segment(OutputArrayOfArrays regions_idx, OutputArray labels, InputArray input_pts, |
||||
InputArray normals, InputArrayOfArrays nn_idx) override; |
||||
|
||||
//-------------------------- Getter and Setter -----------------------
|
||||
|
||||
void setMinSize(int min_size_) override |
||||
{ |
||||
min_size = min_size_ <= 0 ? 1 : min_size_; |
||||
} |
||||
|
||||
int getMinSize() const override |
||||
{ |
||||
return min_size; |
||||
} |
||||
|
||||
void setMaxSize(int max_size_) override |
||||
{ |
||||
max_size = max_size_ <= 0 ? INT_MAX : max_size_; |
||||
} |
||||
|
||||
int getMaxSize() const override |
||||
{ |
||||
return max_size; |
||||
} |
||||
|
||||
void setSmoothModeFlag(bool smooth_mode_) override |
||||
{ |
||||
smooth_mode = smooth_mode_; |
||||
} |
||||
|
||||
bool getSmoothModeFlag() const override |
||||
{ |
||||
return smooth_mode; |
||||
} |
||||
|
||||
void setSmoothnessThreshold(double smoothness_thr_) override |
||||
{ |
||||
CV_CheckGE(smoothness_thr_, 0.0, |
||||
"The smoothness threshold angle should be greater than or equal to 0."); |
||||
CV_CheckLT(smoothness_thr_, 1.5707963268 /* 90*PI/180 */, |
||||
"The smoothness threshold angle should be less than 90 degrees."); |
||||
smoothness_thr = smoothness_thr_; |
||||
} |
||||
|
||||
double getSmoothnessThreshold() const override |
||||
{ |
||||
return smoothness_thr; |
||||
} |
||||
|
||||
void setCurvatureThreshold(double curvature_thr_) override |
||||
{ |
||||
CV_CheckGE(curvature_thr_, 0.0, |
||||
"The curvature threshold should be greater than or equal to 0."); |
||||
curvature_thr = curvature_thr_; |
||||
} |
||||
|
||||
double getCurvatureThreshold() const override |
||||
{ |
||||
return curvature_thr; |
||||
} |
||||
|
||||
void setMaxNumberOfNeighbors(int max_neighbor_num_) override |
||||
{ |
||||
max_neighbor_num = max_neighbor_num_ <= 0 ? INT_MAX : max_neighbor_num_; |
||||
} |
||||
|
||||
int getMaxNumberOfNeighbors() const override |
||||
{ |
||||
return max_neighbor_num; |
||||
} |
||||
|
||||
void setNumberOfRegions(int region_num_) override |
||||
{ |
||||
region_num = region_num_ <= 0 ? INT_MAX : region_num_; |
||||
} |
||||
|
||||
int getNumberOfRegions() const override |
||||
{ |
||||
return region_num; |
||||
} |
||||
|
||||
void setNeedSort(bool need_sort_) override |
||||
{ |
||||
need_sort = need_sort_; |
||||
} |
||||
|
||||
bool getNeedSort() const override |
||||
{ |
||||
return need_sort; |
||||
} |
||||
|
||||
void setSeeds(InputArray seeds_) override |
||||
{ |
||||
seeds = seeds_.getMat().reshape(1, 1); |
||||
} |
||||
|
||||
void getSeeds(OutputArray seeds_) const override |
||||
{ |
||||
seeds.copyTo(seeds_); |
||||
} |
||||
|
||||
void setCurvatures(InputArray curvatures_) override |
||||
{ |
||||
curvatures = abs(curvatures_.getMat().reshape(1, 1)); |
||||
} |
||||
|
||||
void getCurvatures(OutputArray curvatures_) const override |
||||
{ |
||||
curvatures.copyTo(curvatures_); |
||||
} |
||||
|
||||
}; |
||||
|
||||
Ptr<RegionGrowing3D> RegionGrowing3D::create() |
||||
{ |
||||
return makePtr<RegionGrowing3DImpl>(); |
||||
} |
||||
|
||||
} //end namespace cv
|
||||
|
||||
#endif //OPENCV_REGION_ROWING_3D_HPP
|
||||
@ -0,0 +1,64 @@ |
||||
// 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.
|
||||
//
|
||||
// Copyright (C) 2022, Wanli Zhong <zhongwl2018@mail.sustech.edu.cn>
|
||||
|
||||
#include "test_precomp.hpp" |
||||
#include "test_ptcloud_utils.hpp" |
||||
#include "opencv2/flann.hpp" |
||||
|
||||
namespace opencv_test { namespace { |
||||
|
||||
TEST(NormalEstimationTest, PlaneNormalEstimation) |
||||
{ |
||||
// generate a plane for test
|
||||
Mat plane_pts; |
||||
vector<float> model({1, 2, 3, 4}); |
||||
float thr = 0.f; |
||||
int num = 1000; |
||||
vector<float> limit({5, 55, 5, 55, 0, 0}); |
||||
generatePlane(plane_pts, model, thr, num, limit); |
||||
|
||||
// get knn search result
|
||||
int k = 10; |
||||
Mat knn_idx(num, k, CV_32S); |
||||
// build kdtree
|
||||
flann::Index tree(plane_pts, flann::KDTreeIndexParams()); |
||||
tree.knnSearch(plane_pts, knn_idx, noArray(), k); |
||||
// estimate normal and curvature
|
||||
vector<Point3f> normals; |
||||
vector<float> curvatures; |
||||
normalEstimate(normals, curvatures, plane_pts, knn_idx, k); |
||||
|
||||
float theta_thr = 1.f; // threshold for degree of angle between normal of plane and normal of point
|
||||
float curvature_thr = 0.01f; // threshold for curvature and actual curvature of the point
|
||||
float actual_curvature = 0.f; |
||||
|
||||
Point3f n1(model[0], model[1], model[2]); |
||||
float n1m = n1.dot(n1); |
||||
float total_theta = 0.f; |
||||
float total_diff_curvature = 0.f; |
||||
for (int i = 0; i < num; ++i) |
||||
{ |
||||
float n12 = n1.dot(normals[i]); |
||||
float n2m = normals[i].dot(normals[i]); |
||||
float cos_theta = n12 / sqrt(n1m * n2m); |
||||
// accuracy problems caused by float numbers, need to be fixed
|
||||
cos_theta = cos_theta > 1 ? 1 : cos_theta; |
||||
cos_theta = cos_theta < -1 ? -1 : cos_theta; |
||||
float theta = acos(abs(cos_theta)); |
||||
|
||||
total_theta += theta; |
||||
total_diff_curvature += abs(curvatures[i] - actual_curvature); |
||||
} |
||||
|
||||
float avg_theta = total_theta / (float) num; |
||||
ASSERT_LE(avg_theta, theta_thr); |
||||
|
||||
float avg_diff_curvature = total_diff_curvature / (float) num; |
||||
ASSERT_LE(avg_diff_curvature, curvature_thr); |
||||
} |
||||
|
||||
} // namespace
|
||||
} // opencv_test
|
||||
@ -0,0 +1,109 @@ |
||||
// 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.
|
||||
//
|
||||
// Copyright (C) 2021, Wanli Zhong <zhongwl2018@mail.sustech.edu.cn>
|
||||
|
||||
#include "test_ptcloud_utils.hpp" |
||||
|
||||
namespace opencv_test { |
||||
|
||||
void generatePlane(OutputArray plane_pts, const vector<float> &model, float thr, int num, |
||||
const vector<float> &limit) |
||||
{ |
||||
if (plane_pts.channels() == 3 && plane_pts.isVector()) |
||||
{ |
||||
// std::vector<cv::Point3f>
|
||||
plane_pts.create(1, num, CV_32FC3); |
||||
} |
||||
else |
||||
{ |
||||
// cv::Mat
|
||||
plane_pts.create(num, 3, CV_32F); |
||||
} |
||||
|
||||
cv::RNG rng(0); |
||||
auto *plane_pts_ptr = (float *) plane_pts.getMat().data; |
||||
|
||||
// Part of the points are generated for the specific model
|
||||
// The other part of the points are used to increase the thickness of the plane
|
||||
int std_num = (int) (num / 2); |
||||
// Difference of maximum d between two parallel planes
|
||||
float d_thr = thr * sqrt(model[0] * model[0] + model[1] * model[1] + model[2] * model[2]); |
||||
|
||||
for (int i = 0; i < num; i++) |
||||
{ |
||||
// Let d change then generate thickness
|
||||
float d = i < std_num ? model[3] : rng.uniform(model[3] - d_thr, model[3] + d_thr); |
||||
float x, y, z; |
||||
// c is 0 means the plane is vertical
|
||||
if (model[2] == 0) |
||||
{ |
||||
z = rng.uniform(limit[4], limit[5]); |
||||
if (model[0] == 0) |
||||
{ |
||||
x = rng.uniform(limit[0], limit[1]); |
||||
y = -d / model[1]; |
||||
} |
||||
else if (model[1] == 0) |
||||
{ |
||||
x = -d / model[0]; |
||||
y = rng.uniform(limit[2], limit[3]); |
||||
} |
||||
else |
||||
{ |
||||
x = rng.uniform(limit[0], limit[1]); |
||||
y = -(model[0] * x + d) / model[1]; |
||||
} |
||||
} |
||||
// c is not 0
|
||||
else |
||||
{ |
||||
x = rng.uniform(limit[0], limit[1]); |
||||
y = rng.uniform(limit[2], limit[3]); |
||||
z = -(model[0] * x + model[1] * y + d) / model[2]; |
||||
} |
||||
|
||||
plane_pts_ptr[3 * i] = x; |
||||
plane_pts_ptr[3 * i + 1] = y; |
||||
plane_pts_ptr[3 * i + 2] = z; |
||||
} |
||||
} |
||||
|
||||
void generateSphere(OutputArray sphere_pts, const vector<float> &model, float thr, int num, |
||||
const vector<float> &limit) |
||||
{ |
||||
if (sphere_pts.channels() == 3 && sphere_pts.isVector()) |
||||
{ |
||||
// std::vector<cv::Point3f>
|
||||
sphere_pts.create(1, num, CV_32FC3); |
||||
} |
||||
else |
||||
{ |
||||
// cv::Mat
|
||||
sphere_pts.create(num, 3, CV_32F); |
||||
} |
||||
cv::RNG rng(0); |
||||
auto *sphere_pts_ptr = (float *) sphere_pts.getMat().data; |
||||
|
||||
// Part of the points are generated for the specific model
|
||||
// The other part of the points are used to increase the thickness of the sphere
|
||||
int sphere_num = (int) (num / 1.5); |
||||
for (int i = 0; i < num; i++) |
||||
{ |
||||
// Let r change then generate thickness
|
||||
float r = i < sphere_num ? model[3] : rng.uniform(model[3] - thr, model[3] + thr); |
||||
// Generate a random vector and normalize it.
|
||||
Vec3f vec(rng.uniform(limit[0], limit[1]), rng.uniform(limit[2], limit[3]), |
||||
rng.uniform(limit[4], limit[5])); |
||||
float l = sqrt(vec.dot(vec)); |
||||
// Normalizes it to have a magnitude of r
|
||||
vec /= l / r; |
||||
|
||||
sphere_pts_ptr[3 * i] = model[0] + vec[0]; |
||||
sphere_pts_ptr[3 * i + 1] = model[1] + vec[1]; |
||||
sphere_pts_ptr[3 * i + 2] = model[2] + vec[2]; |
||||
} |
||||
} |
||||
|
||||
} // opencv_test
|
||||
@ -0,0 +1,43 @@ |
||||
// 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.
|
||||
//
|
||||
// Copyright (C) 2021, Wanli Zhong <zhongwl2018@mail.sustech.edu.cn>
|
||||
|
||||
#ifndef OPENCV_TEST_PTCLOUD_UTILS_HPP |
||||
#define OPENCV_TEST_PTCLOUD_UTILS_HPP |
||||
|
||||
#include "test_precomp.hpp" |
||||
|
||||
namespace opencv_test { |
||||
|
||||
/**
|
||||
* @brief Generate a specific plane with random points. |
||||
* |
||||
* @param[out] plane_pts Point cloud of plane, only support vector<Point3f> or Mat with Nx3 layout |
||||
* in memory. |
||||
* @param model Plane coefficient [a,b,c,d] means ax+by+cz+d=0. |
||||
* @param thr Generate the maximum distance from the point to the plane. |
||||
* @param num The number of points. |
||||
* @param limit The range of xyz coordinates of the generated plane. |
||||
* |
||||
*/ |
||||
void generatePlane(OutputArray plane_pts, const vector<float> &model, float thr, int num, |
||||
const vector<float> &limit); |
||||
|
||||
/**
|
||||
* @brief Generate a specific sphere with random points. |
||||
* |
||||
* @param[out] sphere_pts Point cloud of plane, only support vector<Point3f> or Mat with Nx3 layout |
||||
* in memory. |
||||
* @param model Plane coefficient [a,b,c,d] means x^2+y^2+z^2=r^2. |
||||
* @param thr Generate the maximum distance from the point to the surface of sphere. |
||||
* @param num The number of points. |
||||
* @param limit The range of vector to make the generated sphere incomplete. |
||||
* |
||||
*/ |
||||
void generateSphere(OutputArray sphere_pts, const vector<float> &model, float thr, int num, |
||||
const vector<float> &limit); |
||||
|
||||
} // opencv_test
|
||||
#endif //OPENCV_TEST_PTCLOUD_UTILS_HPP
|
||||
Loading…
Reference in new issue