Files
opencv-MIRROR/modules/geometry/src/ptcloud/normal_estimation.cpp
Vincent Rabaud ec8d521e11 Merge pull request #30286 from vrabaud:fuzztest_fixes
Fix code according to fuzzer findings - #30286

### Pull Request Readiness Checklist

See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request

- [x] I agree to contribute to the project under Apache 2 License.
- [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV
- [x] The PR is proposed to the proper branch
- [x] There is a reference to the original bug report and related work
- [x] There is accuracy test, performance test and test data in opencv_extra repository, if applicable
      Patch to opencv_extra has the same branch name.
- [x] The feature is well documented and sample code can be built with the project CMake
2026-10-19 09:39:08 +03:00

149 lines
5.6 KiB
C++

// 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"
#include "opencv2/flann.hpp" // internal kNN when nn_idx is not supplied
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;
// Empty input yields empty outputs (no error), matching the pre-internal-kNN behavior.
if (pts_size == 0)
{
normals.release();
curvatures.release();
return;
}
// When the caller does not provide neighbor indices, build them here with a kd-tree
// (max_neighbor_num is then the number of neighbors k, and must be >= 2). The first
// neighbor of each point is itself, exactly as the pre-computed nn_idx contract expects.
std::vector<Mat> nn_idx;
Mat builtIdx;
const bool buildNN = nn_idx_.empty();
if (buildNN)
{
CV_Check(max_neighbor_num_, max_neighbor_num_ >= 2,
"When nn_idx is empty, max_neighbor_num (the number of neighbors k) must be >= 2.");
const int kk = std::min(max_neighbor_num_, pts_size);
flann::Index index(ori_pts, flann::KDTreeIndexParams(1)); // single tree for exact search
flann::SearchParams exact(cvflann::FLANN_CHECKS_UNLIMITED);
builtIdx.create(pts_size, kk, CV_32S);
parallel_for_(Range(0, pts_size), [&](const Range &range) {
Mat rowDist; // discarded; only the neighbor indices are used
for (int i = range.start; i < range.end; ++i)
index.knnSearch(ori_pts.row(i), builtIdx.row(i), rowDist, kk, exact);
});
}
else
{
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.");
for (int i = 0; i < pts_size; ++i)
{
CV_CheckTypeEQ(nn_idx[i].type(), CV_32SC1, "Each matrix in nn_idx must be CV_32SC1");
CV_Assert(!nn_idx[i].empty() && (nn_idx[i].rows == 1 || nn_idx[i].cols == 1));
}
}
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)
{
// Neighbor indices for this point come from either the built kd-tree or the caller.
const int *nn_idx_ptr_base = buildNN ? builtIdx.ptr<int>(i) : nullptr;
int row_cols = buildNN ? builtIdx.cols : (int)nn_idx[i].total();
// The maximum size that may be used for this row
int bound = max_neighbor_num > row_cols ? row_cols : max_neighbor_num;
// 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 = buildNN ? nn_idx_ptr_base[j] : nn_idx[i].at<int>(j);
// If the index is less than 0 or out of range,
// the nn_idx of this row will no longer have any information.
if (cur_nei_idx < 0 || cur_nei_idx >= pts_size) break;
pt_set.push_back(ori_pts.row(cur_nei_idx));
}
float *normals_ptr_base = normals_ptr + 3 * i;
if (pt_set.rows < 3)
{
normals_ptr_base[0] = 0.f;
normals_ptr_base[1] = 0.f;
normals_ptr_base[2] = 0.f;
curvatures_ptr[i] = 0.f;
continue;
}
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;
if (pca.eigenvectors.rows >= 3)
{
normals_ptr_base[0] = eigenvectors_ptr[6];
normals_ptr_base[1] = eigenvectors_ptr[7];
normals_ptr_base[2] = eigenvectors_ptr[8];
}
else
{
normals_ptr_base[0] = 0.f;
normals_ptr_base[1] = 0.f;
normals_ptr_base[2] = 0.f;
}
const float *eigenvalues_ptr = (float *) pca.eigenvalues.data;
if (pca.eigenvalues.rows >= 3)
{
curvatures_ptr[i] = eigenvalues_ptr[2] /
(eigenvalues_ptr[0] + eigenvalues_ptr[1] + eigenvalues_ptr[2]);
}
else
{
curvatures_ptr[i] = 0.f;
}
}
});
}
//! @} _3d
} //end namespace cv