Files
opencv-MIRROR/modules/ptcloud/src/ball_pivoting.cpp

405 lines
16 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) 2026, BigVision LLC, all rights reserved.
// Third party copyrights are property of their respective owners.
// Ball-Pivoting surface reconstruction (Bernardini et al., 1999), multi-radius variant.
#include "precomp.hpp"
#include "ptcloud_utils.hpp" // toPointVec
#include "opencv2/flann.hpp" // cv::flann::Index (neighbor indices)
#include <list>
#include <deque>
#include <set>
#include <unordered_map>
namespace cv {
namespace {
// Per-query neighbor cap for the local (radius ~2r) searches; generous but finite.
static const int kMaxNeighbors = 256;
// A directed edge on the advancing front, bounding the triangle whose third vertex is `opp`.
struct FrontEdge
{
int i, j, opp;
Point3f center; // ball center of the triangle this edge belongs to
bool active; // false once frozen as a boundary edge (may be retried with a larger ball)
};
// Median nearest-neighbor distance over a bounded sample (shared by both spacing entry points).
static float medianNN(flann::Index& index, const Mat& ptsMat)
{
const int N = ptsMat.rows;
const int sample = std::min(N, 1000);
// Gather all sampled queries and search them in a single batched FLANN call.
Mat q(sample, 3, CV_32F);
for (int s = 0; s < sample; s++)
ptsMat.row((int)((int64_t)s * N / sample)).copyTo(q.row(s));
Mat qi, qd;
index.knnSearch(q, qi, qd, 2, flann::SearchParams()); // col 0 self, col 1 nearest other
std::vector<float> nn; nn.reserve(sample);
if (qd.cols >= 2)
for (int s = 0; s < sample; s++)
nn.push_back(std::sqrt(qd.at<float>(s, 1)));
if (nn.empty()) return 0.f;
std::nth_element(nn.begin(), nn.begin() + nn.size() / 2, nn.end());
return nn[nn.size() / 2];
}
class BallPivoter
{
public:
BallPivoter(const std::vector<Point3f>& points, const std::vector<Vec3f>& normals)
: pts(points), nrm(normals), N((int)points.size()), r(0.f),
exact(cvflann::FLANN_CHECKS_UNLIMITED), // BPA predicates need EXACT neighbor sets
used(N, 0)
{
ptsMat = Mat(pts).reshape(1, N); // N x 3, CV_32F
index = makePtr<flann::Index>(ptsMat, flann::KDTreeIndexParams(1)); // single tree for exact search
}
// Mean point spacing from this pivoter's own index (avoids building a second kd-tree).
float medianSpacing() { return medianNN(*index, ptsMat); }
// Roll each radius in turn (ascending): smaller balls first, larger balls fill the leftover gaps.
void run(std::vector<Vec3i>& tris, const std::vector<float>& radii)
{
const int64_t maxIters = (int64_t)100 * N + 1000; // safety bound against pathological loops
for (float rad : radii)
{
r = rad;
// Re-arm boundary edges so the larger ball gets a chance to pivot across them.
for (auto it = front.begin(); it != front.end(); ++it)
if (!it->active) { it->active = true; work.push_back(ekeyDir(it->i, it->j)); }
int seedCursor = 0;
int64_t iters = 0;
while (iters++ < maxIters)
{
std::list<FrontEdge>::iterator e = popActive();
if (e == front.end())
{
if (!findSeed(seedCursor, tris)) break; // no more seeds for this radius
continue;
}
int k; Point3f c;
if (pivot(*e, k, c) && canAdd(e->j, e->i, k) && !isDuplicate(e->j, e->i, k))
{
int i = e->i, j = e->j;
addTriangle(tris, j, i, k); // winding consistent with the seed
removeEdge(e);
insertEdge(i, k, j, c);
insertEdge(k, j, i, c);
}
else
{
e->active = false; // boundary edge (retried at a larger radius)
}
}
if (iters >= maxIters)
CV_LOG_WARNING(NULL, "BPA: iteration cap hit at radius " << r
<< "; the mesh may be incomplete");
}
}
private:
const std::vector<Point3f>& pts;
const std::vector<Vec3f>& nrm;
int N;
float r;
Mat ptsMat;
Ptr<flann::Index> index;
flann::SearchParams exact; // FLANN_CHECKS_UNLIMITED
std::vector<char> used; // point belongs to >=1 triangle
std::list<FrontEdge> front;
std::unordered_map<int64_t, std::list<FrontEdge>::iterator> emap; // live directed edges
std::deque<int64_t> work; // directed-edge keys awaiting a pivot
std::unordered_map<int64_t, int> ecount; // undirected edge use count (packed key)
std::set<std::tuple<int,int,int>> triSet; // built triangles (sorted)
mutable bool truncWarned = false; // one-shot neighbor-cap warning
int64_t ekeyDir(int a, int b) const { return (int64_t)a * N + b; }
int64_t ekeyUndir(int a, int b) const { return (int64_t)std::min(a,b) * N + std::max(a,b); }
static std::tuple<int,int,int> sorted3(int a, int b, int c)
{
if (a > b) std::swap(a, b);
if (b > c) std::swap(b, c);
if (a > b) std::swap(a, b);
return std::make_tuple(a, b, c);
}
// --- neighbor queries -------------------------------------------------------------------------
void neighbors(const Point3f& p, float rad, int maxN, std::vector<int>& out) const
{
float buf[3] = { p.x, p.y, p.z }; // stack query (no per-call alloc)
Mat q(1, 3, CV_32F, buf), idx, dist;
int found = index->radiusSearch(q, idx, dist, (double)(rad * rad), maxN, exact);
if (found >= maxN && !truncWarned)
{
truncWarned = true;
CV_LOG_WARNING(NULL, "BPA: neighbor cap (" << maxN << ") reached; the mesh may be incomplete");
}
out.clear();
int cnt = std::min(found, idx.cols);
for (int t = 0; t < cnt; t++)
{
int id = idx.at<int>(t);
if (id >= 0) out.push_back(id);
}
}
// Ball of radius r resting on a, b, c; center pushed to the +avgNormal side. False if it can't fit.
static bool ballCenter(const Point3f& a, const Point3f& b, const Point3f& c,
float r, const Vec3f& avgNormal, Point3f& out)
{
Vec3f u(b.x-a.x, b.y-a.y, b.z-a.z);
Vec3f v(c.x-a.x, c.y-a.y, c.z-a.z);
Vec3f n = u.cross(v);
float n2 = n.dot(n);
if (n2 < 1e-16f) return false; // degenerate triangle
Vec3f o = (u.dot(u) * v - v.dot(v) * u).cross(n) * (1.0f / (2.0f * n2));
float circ2 = o.dot(o);
float h2 = r * r - circ2;
if (h2 < 0.f) return false; // circumradius > r
Vec3f cc(a.x + o[0], a.y + o[1], a.z + o[2]);
Vec3f nhat = n * (1.0f / std::sqrt(n2));
if (nhat.dot(avgNormal) < 0.f) nhat = -nhat;
float h = std::sqrt(std::max(0.f, h2));
out = Point3f(cc[0] + nhat[0]*h, cc[1] + nhat[1]*h, cc[2] + nhat[2]*h);
return true;
}
// No point (other than the three it rests on) lies strictly inside the ball.
bool ballEmpty(const Point3f& center, int e0, int e1, int e2) const
{
// 3 supports lie on the ball; an interior point is closer, so k=4 catches any intruder.
// Clamped to N: FLANN asserts if asked for more neighbors than points in the index.
float buf[3] = { center.x, center.y, center.z };
Mat q(1, 3, CV_32F, buf), beIdx, beDist;
index->knnSearch(q, beIdx, beDist, std::min(4, N), exact);
const float rq2 = (r * (1.f - 1e-3f)) * (r * (1.f - 1e-3f)); // FLANN dist is squared L2
for (int t = 0; t < beIdx.cols; t++)
{
int id = beIdx.at<int>(t);
if (id < 0 || beDist.at<float>(t) > rq2) continue;
if (id != e0 && id != e1 && id != e2) return false;
}
return true;
}
bool canAdd(int a, int b, int c) const
{
auto it = ecount.find(ekeyUndir(a,b)); if (it != ecount.end() && it->second >= 2) return false;
it = ecount.find(ekeyUndir(b,c)); if (it != ecount.end() && it->second >= 2) return false;
it = ecount.find(ekeyUndir(c,a)); if (it != ecount.end() && it->second >= 2) return false;
return true;
}
bool isDuplicate(int a, int b, int c) const { return triSet.count(sorted3(a,b,c)) > 0; }
void addTriangle(std::vector<Vec3i>& tris, int a, int b, int c)
{
tris.push_back(Vec3i(a, b, c));
triSet.insert(sorted3(a, b, c));
used[a] = used[b] = used[c] = 1;
ecount[ekeyUndir(a,b)]++;
ecount[ekeyUndir(b,c)]++;
ecount[ekeyUndir(c,a)]++;
}
// --- front management -------------------------------------------------------------------------
void insertEdge(int i, int j, int opp, const Point3f& center)
{
auto rev = emap.find(ekeyDir(j, i));
if (rev != emap.end()) // reverse present -> now an interior edge (glue)
{
front.erase(rev->second);
emap.erase(rev);
return;
}
if (emap.count(ekeyDir(i, j))) return; // already on the front
front.push_back(FrontEdge{i, j, opp, center, true});
auto lit = std::prev(front.end());
emap[ekeyDir(i, j)] = lit;
work.push_back(ekeyDir(i, j));
}
void removeEdge(std::list<FrontEdge>::iterator lit)
{
emap.erase(ekeyDir(lit->i, lit->j));
front.erase(lit);
}
std::list<FrontEdge>::iterator popActive()
{
while (!work.empty())
{
int64_t key = work.front();
work.pop_front();
// look up by key: the edge may have been erased while its key sat in the queue
auto mit = emap.find(key);
if (mit != emap.end() && mit->second->active)
return mit->second;
}
return front.end();
}
// --- pivoting ---------------------------------------------------------------------------------
bool pivot(const FrontEdge& e, int& outK, Point3f& outCenter)
{
Point3f pi = pts[e.i], pj = pts[e.j];
Point3f mid((pi.x+pj.x)*0.5f, (pi.y+pj.y)*0.5f, (pi.z+pj.z)*0.5f);
Vec3f axis = normalize(Vec3f(pj.x-pi.x, pj.y-pi.y, pj.z-pi.z));
Vec3f a0(e.center.x-mid.x, e.center.y-mid.y, e.center.z-mid.z);
a0 -= axis * a0.dot(axis);
if (norm(a0) < 1e-12) return false;
a0 = normalize(a0);
Vec3f wdir = axis.cross(a0);
std::vector<int> nb;
neighbors(mid, 2.f * r, kMaxNeighbors, nb);
float best = FLT_MAX; int bestK = -1; Point3f bestC;
for (int k : nb)
{
if (k == e.i || k == e.j || k == e.opp) continue;
Vec3f an = normalize(nrm[e.i] + nrm[e.j] + nrm[k]);
Point3f c;
if (!ballCenter(pi, pj, pts[k], r, an, c)) continue;
Vec3f a1(c.x-mid.x, c.y-mid.y, c.z-mid.z);
a1 -= axis * a1.dot(axis);
if (norm(a1) < 1e-12) continue;
a1 = normalize(a1);
float x = a1.dot(a0), y = a1.dot(wdir);
float ang = std::atan2(y, x);
if (ang < 0.f) ang += 2.f * (float)CV_PI; // roll angle in [0, 2pi)
if (ang < best) { best = ang; bestK = k; bestC = c; }
}
if (bestK < 0) return false;
if (!ballEmpty(bestC, e.i, e.j, bestK)) return false;
outK = bestK; outCenter = bestC;
return true;
}
// --- seeding ----------------------------------------------------------------------------------
bool findSeed(int& cursor, std::vector<Vec3i>& tris)
{
for (; cursor < N; cursor++)
{
int i = cursor;
if (used[i]) continue;
std::vector<int> nb;
neighbors(pts[i], 2.f * r, kMaxNeighbors, nb);
for (size_t x = 0; x < nb.size(); x++)
{
int a = nb[x];
if (a == i || used[a]) continue;
for (size_t y = x + 1; y < nb.size(); y++)
{
int b = nb[y];
if (b == i || used[b]) continue;
Vec3f an = normalize(nrm[i] + nrm[a] + nrm[b]);
int aa = a, bb = b;
// orient the seed so its geometric normal agrees with the point normals
Vec3f gn = Vec3f(pts[aa].x-pts[i].x, pts[aa].y-pts[i].y, pts[aa].z-pts[i].z)
.cross(Vec3f(pts[bb].x-pts[i].x, pts[bb].y-pts[i].y, pts[bb].z-pts[i].z));
if (gn.dot(an) < 0.f) std::swap(aa, bb);
Point3f c;
if (!ballCenter(pts[i], pts[aa], pts[bb], r, an, c)) continue;
if (!ballEmpty(c, i, aa, bb)) continue;
if (!canAdd(i, aa, bb) || isDuplicate(i, aa, bb)) continue;
addTriangle(tris, i, aa, bb);
insertEdge(i, aa, bb, c);
insertEdge(aa, bb, i, c);
insertEdge(bb, i, aa, c);
return true;
}
}
}
return false;
}
};
} // namespace
float estimateMedianSpacing(InputArray inputCloud)
{
CV_TRACE_FUNCTION();
std::vector<Point3f> pts;
toPointVec(inputCloud, pts);
const int N = (int)pts.size();
if (N < 2) return 0.f;
Mat ptsMat = Mat(pts).reshape(1, N); // N x 3, CV_32F
flann::Index index(ptsMat, flann::KDTreeIndexParams(4));
return medianNN(index, ptsMat);
}
void createMeshBPA(InputArray inputCloud, InputArray normals_, OutputArray vertices,
OutputArray triangles, InputArray radii_)
{
CV_TRACE_FUNCTION();
std::vector<Point3f> pts;
toPointVec(inputCloud, pts);
const int N = (int)pts.size();
if (N < 3) { vertices.release(); triangles.release(); return; }
Mat nm = normals_.getMat();
CV_Assert(!nm.empty() && nm.channels() * (int)nm.total() == 3 * N);
CV_CheckDepthEQ(nm.depth(), CV_32F, "Normals with only depth CV_32F are supported");
Mat nmw = nm.isContinuous() ? nm : nm.clone(); // read-only; reshape needs contiguity
Mat nmf = nmw.reshape(3, N);
std::vector<Vec3f> nrm(N);
for (int i = 0; i < N; i++)
nrm[i] = safeNormalize(nmf.at<Vec3f>(i));
BallPivoter bp(pts, nrm); // builds the kd-tree once; reused for spacing and pivoting
// Radii: caller-supplied, else {1x, 2x, 4x} the median spacing.
std::vector<float> radii;
if (radii_.empty())
{
float s = bp.medianSpacing();
if (s <= 0.f) { vertices.release(); triangles.release(); return; }
radii = { 1.0f * s, 2.0f * s, 4.0f * s };
}
else
{
Mat r; radii_.getMat().convertTo(r, CV_64F);
const double* rp = r.ptr<double>();
for (int i = 0; i < (int)r.total(); i++)
if (rp[i] > 0.0) radii.push_back((float)rp[i]);
std::sort(radii.begin(), radii.end());
}
if (radii.empty()) { vertices.release(); triangles.release(); return; }
std::vector<Vec3i> tris;
bp.run(tris, radii);
// BPA is interpolating: the mesh vertices are the input points.
Mat(pts).copyTo(vertices); // N x 1, CV_32FC3
if (tris.empty()) { triangles.release(); return; }
Mat(tris).reshape(1, (int)tris.size()).copyTo(triangles); // M x 3, CV_32S
}
} // namespace cv