diff --git a/modules/3d/doc/3d.bib b/modules/3d/doc/3d.bib index 8cd120ceab..5a7f653766 100644 --- a/modules/3d/doc/3d.bib +++ b/modules/3d/doc/3d.bib @@ -95,4 +95,15 @@ pages={381--395}, year={1981}, publisher={ACM New York, NY, USA} +} + +@inproceedings{Rabbani2006SegmentationOP, + title={Segmentation of point clouds using smoothness constraints}, + keywords={ADLIB-ART-1373, EOS}, + author={Tahir Rabbani and Frank van den Heuvel and George Vosselman}, + year={2006}, + volume={35}, + pages={248--253}, + editor={H.G. Maas and D. Schneider}, + booktitle={ISPRS 2006 : Proceedings of the ISPRS commission V symposium Vol. 35, part 6 : image engineering and vision metrology, Dresden, Germany 25-27 September 2006} } \ No newline at end of file diff --git a/modules/3d/include/opencv2/3d/ptcloud.hpp b/modules/3d/include/opencv2/3d/ptcloud.hpp index e11b689074..ba25395115 100644 --- a/modules/3d/include/opencv2/3d/ptcloud.hpp +++ b/modules/3d/include/opencv2/3d/ptcloud.hpp @@ -259,6 +259,153 @@ CV_EXPORTS int farthestPointSampling(OutputArray sampled_point_flags, InputArray CV_EXPORTS int farthestPointSampling(OutputArray sampled_point_flags, InputArray input_pts, float sampled_scale, float dist_lower_limit = 0, RNG *rng = nullptr); +/** + * @brief Estimate the normal and curvature of each point in point cloud from NN results. + * + * Normal estimation by PCA: + * + Input: Nearest neighbor points of a specific point: \f$ pt\_set \f$ + * + Step: + * 1. Calculate the \f$ mean(\bar{x},\bar{y},\bar{z}) \f$ of \f$ pt\_set \f$; + * 2. A 3x3 covariance matrix \f$ cov \f$ is obtained by \f$ mean^T \cdot mean \f$; + * 3. Calculate the eigenvalues(\f$ λ_2 \ge λ_1 \ge λ_0 \f$) and corresponding + * eigenvectors(\f$ v_2, v_1, v_0 \f$) of \f$ cov \f$; + * 4. \f$ v0 \f$ is the normal of the specific point, + * \f$ \frac{λ_0}{λ_0 + λ_1 + λ_2} \f$ is the curvature of the specific point; + * + Output: Normal and curvature of the specific point. + * + * @param[out] normals Normal of each point, support vector and Mat of size Nx3. + * @param[out] curvatures Curvature of each point, support vector and Mat. + * @param input_pts Original point cloud, support vector and Mat of size Nx3/3xN. + * @param nn_idx Index information of nearest neighbors of all points. The first nearest neighbor of + * each point is itself. Support vector>, vector and Mat of size NxK. + * If the information in a row is [0, 2, 1, -5, -1, 4, 7 ... negative number], it will + * use only non-negative indexes until it meets a negative number or bound of this row + * i.e. [0, 2, 1]. + * @param max_neighbor_num The maximum number of neighbors want to use including itself. Setting to + * a non-positive number or default will use the information from nn_idx. + */ + +CV_EXPORTS void normalEstimate(OutputArray normals, OutputArray curvatures, InputArray input_pts, + InputArrayOfArrays nn_idx, int max_neighbor_num = 0); + +/** + * @brief Region Growing algorithm in 3D point cloud. + * + * The key idea of region growing is to merge the nearest neighbor points that satisfy a certain + * angle threshold into the same region according to the normal between the two points, so as to + * achieve the purpose of segmentation. For more details, please refer to @cite Rabbani2006SegmentationOP. + */ +class CV_EXPORTS RegionGrowing3D +{ +public: + //-------------------------- CREATE ----------------------- + + static Ptr create(); + + // -------------------------- CONSTRUCTOR, DESTRUCTOR -------------------------- + + RegionGrowing3D() = default; + + virtual ~RegionGrowing3D() = default; + + //-------------------------- SEGMENT ----------------------- + + /** + * @brief Execute segmentation using the Region Growing algorithm. + * + * @param[out] regions_idx Index information of all points in each region, support + * vector>, vector. + * @param[out] labels The label corresponds to the model number, 0 means it does not belong to + * any model, range [0, Number of final resultant models obtained]. Support + * vector and Mat. + * @param input_pts Original point cloud, support vector and Mat of size Nx3/3xN. + * @param normals Normal of each point, support vector and Mat of size Nx3. + * @param nn_idx Index information of nearest neighbors of all points. The first nearest + * neighbor of each point is itself. Support vector>, vector and + * Mat of size NxK. If the information in a row is + * [0, 2, 1, -5, -1, 4, 7 ... negative number] + * it will use only non-negative indexes until it meets a negative number or bound + * of this row i.e. [0, 2, 1]. + * @return Number of final resultant regions obtained by segmentation. + */ + virtual int + segment(OutputArrayOfArrays regions_idx, OutputArray labels, InputArray input_pts, + InputArray normals, InputArrayOfArrays nn_idx) = 0; + + //-------------------------- Getter and Setter ----------------------- + + //! Set the minimum size of region. + //！Setting to a non-positive number or default will be unlimited. + virtual void setMinSize(int min_size) = 0; + + //! Get the minimum size of region. + virtual int getMinSize() const = 0; + + //! Set the maximum size of region. + //！Setting to a non-positive number or default will be unlimited. + virtual void setMaxSize(int max_size) = 0; + + //! Get the maximum size of region. + virtual int getMaxSize() const = 0; + + //! Set whether to use the smoothness mode. Default will be true. + //! If true it will check the angle between the normal of the current point and the normal of its neighbor. + //! Otherwise, it will check the angle between the normal of the seed point and the normal of current neighbor. + virtual void setSmoothModeFlag(bool smooth_mode) = 0; + + //! Get whether to use the smoothness mode. + virtual bool getSmoothModeFlag() const = 0; + + //! Set threshold value of the angle between normals, the input value is in radian. + //！Default will be 30(degree)*PI/180. + virtual void setSmoothnessThreshold(double smoothness_thr) = 0; + + //! Get threshold value of the angle between normals. + virtual double getSmoothnessThreshold() const = 0; + + //! Set threshold value of curvature. Default will be 0.05. + //! Only points with curvature less than the threshold will be considered to belong to the same region. + //! If the curvature of each point is not set, this option will not work. + virtual void setCurvatureThreshold(double curvature_thr) = 0; + + //! Get threshold value of curvature. + virtual double getCurvatureThreshold() const = 0; + + //! Set the maximum number of neighbors want to use including itself. + //! Setting to a non-positive number or default will use the information from nn_idx. + virtual void setMaxNumberOfNeighbors(int max_neighbor_num) = 0; + + //! Get the maximum number of neighbors including itself. + virtual int getMaxNumberOfNeighbors() const = 0; + + //! Set the maximum number of regions you want. + //！Setting to a non-positive number or default will be unlimited. + virtual void setNumberOfRegions(int region_num) = 0; + + //! Get the maximum number of regions you want. + virtual int getNumberOfRegions() const = 0; + + //! Set whether the results need to be sorted in descending order by the number of points. + virtual void setNeedSort(bool need_sort) = 0; + + //! Get whether the results need to be sorted you have set. + virtual bool getNeedSort() const = 0; + + //! Set the seed points, it will grow according to the seeds. + //! If noArray() is set, the default method will be used: + //! 1. If the curvature of each point is set, the seeds will be sorted in ascending order of curvatures. + //! 2. Otherwise, the natural order of the point cloud will be used. + virtual void setSeeds(InputArray seeds) = 0; + + //! Get the seed points. + virtual void getSeeds(OutputArray seeds) const = 0; + + //! Set the curvature of each point, support vector and Mat. If not, you can set it to noArray(). + virtual void setCurvatures(InputArray curvatures) = 0; + + //! Get the curvature of each point if you have set. + virtual void getCurvatures(OutputArray curvatures) const = 0; +}; //! @} _3d } //end namespace cv #endif //OPENCV_3D_PTCLOUD_HPP diff --git a/modules/3d/src/ptcloud/normal_estimation.cpp b/modules/3d/src/ptcloud/normal_estimation.cpp new file mode 100644 index 0000000000..c08df1e029 --- /dev/null +++ b/modules/3d/src/ptcloud/normal_estimation.cpp @@ -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 + +#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 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 + 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 \ No newline at end of file diff --git a/modules/3d/src/ptcloud/region_growing_3d.cpp b/modules/3d/src/ptcloud/region_growing_3d.cpp new file mode 100644 index 0000000000..1446ed2340 --- /dev/null +++ b/modules/3d/src/ptcloud/region_growing_3d.cpp @@ -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 + +#include "../precomp.hpp" +#include "region_growing_3d.hpp" + +#include +#include + +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 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 *natural_order_seeds = new std::vector(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 has_grown(pts_size, false); + // Store the indexes of the points in each region + std::vector> 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 region = {cur_seed}; + std::queue 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 &r1, + const std::vector &r2) -> bool + { + return r1.size() > r2.size(); + }; + sort(regions_idx.begin(), regions_idx.end(), compareRegionSize); + } + + std::vector> &_regions_idx = *(std::vector> *) 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:: \ No newline at end of file diff --git a/modules/3d/src/ptcloud/region_growing_3d.hpp b/modules/3d/src/ptcloud/region_growing_3d.hpp new file mode 100644 index 0000000000..d35455e1d2 --- /dev/null +++ b/modules/3d/src/ptcloud/region_growing_3d.hpp @@ -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 + +#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::create() +{ + return makePtr(); +} + +} //end namespace cv + +#endif //OPENCV_REGION_ROWING_3D_HPP diff --git a/modules/3d/test/test_normal_estimation.cpp b/modules/3d/test/test_normal_estimation.cpp new file mode 100644 index 0000000000..ed1248b187 --- /dev/null +++ b/modules/3d/test/test_normal_estimation.cpp @@ -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 + +#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 model({1, 2, 3, 4}); + float thr = 0.f; + int num = 1000; + vector 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 normals; + vector 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 diff --git a/modules/3d/test/test_ptcloud_utils.cpp b/modules/3d/test/test_ptcloud_utils.cpp new file mode 100644 index 0000000000..c3c7451e57 --- /dev/null +++ b/modules/3d/test/test_ptcloud_utils.cpp @@ -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 + +#include "test_ptcloud_utils.hpp" + +namespace opencv_test { + +void generatePlane(OutputArray plane_pts, const vector &model, float thr, int num, + const vector &limit) +{ + if (plane_pts.channels() == 3 && plane_pts.isVector()) + { + // std::vector + 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 &model, float thr, int num, + const vector &limit) +{ + if (sphere_pts.channels() == 3 && sphere_pts.isVector()) + { + // std::vector + 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 diff --git a/modules/3d/test/test_ptcloud_utils.hpp b/modules/3d/test/test_ptcloud_utils.hpp new file mode 100644 index 0000000000..05b7c88559 --- /dev/null +++ b/modules/3d/test/test_ptcloud_utils.hpp @@ -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 + +#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 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 &model, float thr, int num, + const vector &limit); + +/** + * @brief Generate a specific sphere with random points. + * + * @param[out] sphere_pts Point cloud of plane, only support vector 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 &model, float thr, int num, + const vector &limit); + +} // opencv_test +#endif //OPENCV_TEST_PTCLOUD_UTILS_HPP diff --git a/modules/3d/test/test_sac_segmentation.cpp b/modules/3d/test/test_sac_segmentation.cpp index 6ed7f1f597..994d175219 100644 --- a/modules/3d/test/test_sac_segmentation.cpp +++ b/modules/3d/test/test_sac_segmentation.cpp @@ -5,6 +5,7 @@ // Copyright (C) 2021, Wanli Zhong #include "test_precomp.hpp" +#include "test_ptcloud_utils.hpp" namespace opencv_test { namespace { @@ -171,63 +172,6 @@ TEST_F(SacSegmentationTest, PlaneSacSegmentation) }; models_num = (int) models.size(); - /** - * Used to generate a specific plane with random points - * model: plane coefficient [a,b,c,d] means ax+by+cz+d=0 - * thr: generate the maximum distance from the point to the plane - * limit: the range of xyz coordinates of the generated plane - **/ - auto generatePlane = [](Mat &plane_pts, const vector &model, float thr, int num, - const vector &limit) { - plane_pts = Mat(num, 3, CV_32F); - cv::RNG rng(0); - auto *plane_pts_ptr = (float *) plane_pts.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; - } - }; - // 1 * 3.1415926f / 180 float vector_radian_tolerance = 0.0174533f, ratio_tolerance = 0.1f; CheckDiffFunction planeCheckDiff = [vector_radian_tolerance, ratio_tolerance](const Mat &a, @@ -316,38 +260,6 @@ TEST_F(SacSegmentationTest, SphereSacSegmentation) }; models_num = (int) models.size(); - /** - * Used to generate a specific sphere with random points - * model: sphere coefficient [x,y,z,r] means x^2+y^2+z^2=r^2 - * thr: generate the maximum distance from the point to the surface of sphere - * limit: the range of vector to make the generated sphere incomplete - **/ - auto generateSphere = [](Mat &sphere_pts, const vector &model, float thr, int num, - const vector &limit) { - sphere_pts = cv::Mat(num, 3, CV_32F); - cv::RNG rng(0); - auto *sphere_pts_ptr = (float *) sphere_pts.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]; - } - }; - float distance_tolerance = 0.1f, radius_tolerance = 0.1f; CheckDiffFunction sphereCheckDiff = [distance_tolerance, radius_tolerance](const Mat &a, const Mat &b) -> bool {