Skip to content
GitLab
Commit 4fe5be82 authored by David Flynn's avatar David Flynn
Browse files

attr/m43781: remove no longer necessary PCCIncrementalKdTree3 

The integration of m43781 fast lifting for lossy attributes removes the
need for PCCIncrementalKdTree3.
parent 6ef3fb6c
Hide whitespace changes
Inline Side-by-side
tmc3/PCCKdTree.h
View file @ 4fe5be82
......@@ -50,427 +50,6 @@ enum PCCAxis3
PCC_AXIS3_Z = 2
};
struct PCCPointDistInfo {
double dist2;
uint32_t index;
bool operator<(const PCCPointDistInfo& rhs) const
{
return dist2 < rhs.dist2;
}
};
struct PCCNNResult {
PCCPointDistInfo* neighbors;
size_t resultCount;
};
struct PCCRangeResult {
PCCPointDistInfo* neighbors;
size_t resultCount;
};
struct PCCNNQuery3 {
Vec3<double> point;
double radius;
size_t nearestNeighborCount;
};
struct PCCRangeQuery3 {
Vec3<double> point;
double radius;
size_t maxResultCount;
};
template<typename T>
class PCCHeap {
public:
PCCHeap(void) = default;
PCCHeap(const PCCHeap&) = delete;
PCCHeap& operator=(const PCCHeap&) = delete;
PCCHeap(T* array, size_t capacity) : array(array), cap(capacity), sz(0) {}
T& top()
{
assert(sz > 0);
return array[0];
}
const T& top() const
{
assert(sz > 0);
return array[0];
}
void push(const T& elem)
{
if (sz < cap) {
size_t i = sz;
array[i] = elem;
size_t p = parent(i);
while (i > 0 && array[p] < elem) {
std::swap(array[i], array[p]);
i = p;
p = parent(i);
}
++sz;
} else if (elem < array[0]) {
array[0] = elem;
maxHeapify(0);
}
}
void pop()
{
assert(sz > 0);
array[0] = array[--sz];
maxHeapify(0);
}
void clear() { sz = 0; }
size_t size() const { return sz; }
size_t capacity() const { return cap; }
private:
void maxHeapify(size_t i)
{
size_t l = left(i);
size_t r = right(i);
size_t g = (l < sz && array[i] < array[l]) ? l : i;
if (r < sz && array[g] < array[r]) {
g = r;
}
if (g != i) {
std::swap(array[i], array[g]);
maxHeapify(g);
}
}
size_t parent(size_t index) const { return (index + (index & 1) - 1) / 2; }
size_t left(size_t index) const { return 2 * index + 1; }
size_t right(size_t index) const { return 2 * index + 2; }
private:
T* array;
size_t cap;
size_t sz;
};
class PCCIncrementalKdTree3 {
struct PCCIncrementalKdTree3Node {
Vec3<double> pos;
uint32_t id;
uint32_t left;
uint32_t right;
PCCAxis3 axis;
};
public:
PCCIncrementalKdTree3(void) { root = PCC_UNDEFINED_INDEX; }
PCCIncrementalKdTree3(const PCCIncrementalKdTree3&) = default;
PCCIncrementalKdTree3& operator=(const PCCIncrementalKdTree3&) = default;
~PCCIncrementalKdTree3(void) = default;
void build(const PCCPointSet3& pointCloud)
{
clear();
insert(pointCloud);
}
size_t size() const { return nodes.size(); }
size_t capacity() const { return nodes.capacity(); }
void clear()
{
nodes.resize(0);
root = PCC_UNDEFINED_INDEX;
}
void reserve(const size_t pointCount) { nodes.reserve(pointCount); }
uint32_t insert(const Vec3<double> point)
{
const uint32_t id = static_cast<uint32_t>(nodes.size());
nodes.resize(id + 1);
PCCIncrementalKdTree3Node& node = nodes[id];
node.pos = point;
node.id = id;
node.left = PCC_UNDEFINED_INDEX;
node.right = PCC_UNDEFINED_INDEX;
if (root == PCC_UNDEFINED_INDEX) {
root = id;
node.axis = PCC_AXIS3_X;
} else {
insert(node, root);
}
return id;
}
void insert(const PCCPointSet3& pointCloud)
{
if (pointCloud.getPointCount()) {
const uint32_t start = static_cast<uint32_t>(nodes.size());
const uint32_t end =
start + static_cast<uint32_t>(pointCloud.getPointCount());
nodes.resize(end);
for (uint32_t index = start; index < end; ++index) {
PCCIncrementalKdTree3Node& node = nodes[index];
node.pos = pointCloud[index - start];
node.id = index;
}
root = balance(0, uint32_t(nodes.size()));
}
}
void balance()
{
if (!nodes.empty()) {
root = balance(0, uint32_t(nodes.size()));
}
}
void
findNearestNeighbors(const PCCNNQuery3& query, PCCNNResult& result) const
{
assert(result.neighbors && query.nearestNeighborCount > 0);
result.resultCount = 0;
const PCCNNQuery3 query2 = {query.point, query.radius * query.radius,
query.nearestNeighborCount};
PCCHeap<PCCPointDistInfo> neighbors(
result.neighbors, query.nearestNeighborCount);
findNearestNeighbors(root, query2, neighbors);
std::sort(result.neighbors, result.neighbors + neighbors.size());
result.resultCount = neighbors.size();
}
void
findNearestNeighbors2(const PCCNNQuery3& query2, PCCNNResult& result) const
{
assert(result.neighbors && query2.nearestNeighborCount > 0);
result.resultCount = 0;
PCCHeap<PCCPointDistInfo> neighbors(
result.neighbors, query2.nearestNeighborCount);
findNearestNeighbors(root, query2, neighbors);
std::sort(result.neighbors, result.neighbors + neighbors.size());
result.resultCount = neighbors.size();
}
void findNeighbors(const PCCRangeQuery3& query, PCCRangeResult& result) const
{
assert(result.neighbors && query.maxResultCount > 0);
result.resultCount = 0;
const PCCRangeQuery3 query2 = {query.point, query.radius * query.radius,
query.maxResultCount};
findNeighbors(root, query2, result);
}
uint32_t
hasNeighborWithinRange(const Vec3<double>& point, const double radius2) const
{
return hasNeighborWithinRange(root, point, radius2);
}
private:
static PCCAxis3 nextAxis(const PCCAxis3 axis)
{
switch (axis) {
case PCC_AXIS3_X: return PCC_AXIS3_Y;
case PCC_AXIS3_Y: return PCC_AXIS3_Z;
case PCC_AXIS3_Z: return PCC_AXIS3_X;
case PCC_AXIS3_UNDEFINED: return PCC_AXIS3_X;
default: return PCC_AXIS3_X;
}
}
void insert(PCCIncrementalKdTree3Node& node, const uint32_t parent)
{
const PCCAxis3 axis = nodes[parent].axis;
uint32_t& index = (node.pos[axis] < nodes[parent].pos[axis])
? nodes[parent].left
: nodes[parent].right;
if (index == PCC_UNDEFINED_INDEX) {
index = node.id;
node.axis = nextAxis(axis);
} else {
insert(node, index);
}
}
PCCAxis3 computeSplitAxis(const uint32_t start, const uint32_t end) const
{
Vec3<double> minBB = nodes[start].pos;
Vec3<double> maxBB = nodes[start].pos;
for (size_t i = start + 1; i < end; ++i) {
const Vec3<double>& pt = nodes[i].pos;
for (int32_t k = 0; k < 3; ++k) {
if (minBB[k] > pt[k]) {
minBB[k] = pt[k];
} else if (maxBB[k] < pt[k]) {
maxBB[k] = pt[k];
}
}
}
Vec3<double> d = maxBB - minBB;
if (d.x() > d.y() && d.x() > d.z()) {
return PCC_AXIS3_X;
} else if (d.y() > d.z()) {
return PCC_AXIS3_Y;
} else {
return PCC_AXIS3_Z;
}
}
uint32_t findMedian(uint32_t start, uint32_t end, const PCCAxis3 splitAxis)
{
assert(start < end);
if (end == start + 1) {
return start;
}
const uint32_t medianIndex = start + (end - start) / 2;
while (1) {
double pivot = nodes[medianIndex].pos[splitAxis];
std::swap(nodes[medianIndex], nodes[end - 1]);
uint32_t store, p;
for (store = p = start; p < end; p++) {
if (nodes[p].pos[splitAxis] < pivot) {
if (p != store) {
std::swap(nodes[p], nodes[store]);
}
++store;
}
}
std::swap(nodes[store], nodes[end - 1]);
while (store < medianIndex
&& nodes[store].pos[splitAxis]
== nodes[store + 1].pos[splitAxis]) {
// optimization in case of duplicated values
++store;
}
if (store == medianIndex) {
return medianIndex;
} else if (store > medianIndex) {
end = store;
} else {
start = store + 1;
}
}
}
uint32_t balance(const uint32_t start, const uint32_t end)
{
assert(start < end);
uint32_t index = start;
PCCAxis3 splitAxis = computeSplitAxis(start, end);
index = findMedian(start, end, splitAxis);
PCCIncrementalKdTree3Node& node = nodes[index];
node.axis = splitAxis;
if (start < index) {
node.left = balance(start, index);
} else {
node.left = PCC_UNDEFINED_INDEX;
}
if (index + 1 < end) {
node.right = balance(index + 1, end);
} else {
node.right = PCC_UNDEFINED_INDEX;
}
return index;
}
void findNearestNeighbors(
const uint32_t current,
const PCCNNQuery3& query2,
PCCHeap<PCCPointDistInfo>& neighbors) const
{
if (current == PCC_UNDEFINED_INDEX) {
return;
}
const PCCIncrementalKdTree3Node& node = nodes[current];
const double dist2 = (query2.point - node.pos).getNorm2();
const PCCAxis3 splitAxis = node.axis;
const double coord = node.pos[splitAxis];
const double dx = query2.point[splitAxis] - coord;
uint32_t first, second;
if (dx < 0.0) {
first = node.left;
second = node.right;
} else {
first = node.right;
second = node.left;
}
findNearestNeighbors(first, query2, neighbors);
if (dist2 < query2.radius) {
PCCPointDistInfo distInfo = {dist2, node.id};
neighbors.push(distInfo);
}
const double dx2 = dx * dx;
if (
dx2 > query2.radius
|| (neighbors.size() == query2.nearestNeighborCount
&& dx2 > neighbors.top().dist2)) {
return;
}
findNearestNeighbors(second, query2, neighbors);
}
void findNeighbors(
const uint32_t current,
const PCCRangeQuery3& query2,
PCCRangeResult& result) const
{
if (current == PCC_UNDEFINED_INDEX) {
return;
}
const PCCIncrementalKdTree3Node& node = nodes[current];
const double dist2 = (query2.point - node.pos).getNorm2();
const PCCAxis3 splitAxis = node.axis;
const double coord = node.pos[splitAxis];
const double dx = query2.point[splitAxis] - coord;
uint32_t first, second;
if (dx < 0.0) {
first = node.left;
second = node.right;
} else {
first = node.right;
second = node.left;
}
findNeighbors(first, query2, result);
if (result.resultCount == query2.maxResultCount) {
return;
}
if (dist2 < query2.radius) {
result.neighbors[result.resultCount].dist2 = dist2;
result.neighbors[result.resultCount].index = node.id;
result.resultCount++;
}
if (
(dx * dx) > query2.radius
|| result.resultCount == query2.maxResultCount) {
return;
}
findNeighbors(second, query2, result);
}
uint32_t hasNeighborWithinRange(
const uint32_t current,
const Vec3<double>& point,
const double radius2) const
{
if (current == PCC_UNDEFINED_INDEX) {
return PCC_UNDEFINED_INDEX;
}
const PCCIncrementalKdTree3Node& node = nodes[current];
const double dist2 = (point - node.pos).getNorm2();
if (dist2 < radius2) {
return node.id;
}
const PCCAxis3 splitAxis = node.axis;
const int32_t coord = node.pos[splitAxis];
const int32_t dx = point[splitAxis] - coord;
uint32_t first, second;
if (dx < 0) {
first = node.left;
second = node.right;
} else {
first = node.right;
second = node.left;
}
const uint32_t index = hasNeighborWithinRange(first, point, radius2);
if (index != PCC_UNDEFINED_INDEX) {
return index;
}
if ((dx * dx) > radius2) {
return PCC_UNDEFINED_INDEX;
}
return hasNeighborWithinRange(second, point, radius2);
}
private:
std::vector<PCCIncrementalKdTree3Node> nodes;
uint32_t root;
};
//---------------------------------------
class PCCKdTree3 {
struct PCCKdTree3Node {
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment