private ulong onePointKnn(
DenseColumnMajorMatrixStorage <float> query,
int i,
DenseColumnMajorMatrixStorage <int> indices,
DenseColumnMajorMatrixStorage <float> dists2,
Vector <float> maxRadii,
int k,
float epsilon,
SearchOptionFlags optionFlags)
{
var results = new ListPriorityQueue <int>(maxSize: k);
var queryMatrix = new MathNet.Numerics.LinearAlgebra.Single.DenseMatrix(query);
var q = queryMatrix.Column(i);
for (int j = 0; j < cloud.ColumnCount; j++)
{
var c = cloud.Column(j);
var diff = (c - q);
float l2 = diff * diff;
results.Enqueue(j, l2);
}
int kIdx = 0;
foreach (var j in results.Items)
{
indices.At(kIdx, i, j);
kIdx++;
}
kIdx = 0;
foreach (var d2 in results.Priorities)
{
dists2.At(kIdx, i, d2);
}
return(0);
}
ulong onePointKnn(
DenseColumnMajorMatrixStorage <float> query,
DenseColumnMajorMatrixStorage <float> indices, //int
DenseColumnMajorMatrixStorage <float> dists2,
int i,
IPriorityQueue <BucketEntry> heap,
float[] off,
float maxError2,
float maxRadius2,
bool allowSelfMatch)
{
Array.Clear(off, 0, off.Length); // reset the maximum off values to 0
//heap.reset();
ulong leafTouchedCount = 0;
leafTouchedCount += recurseKnn(query, i, 0, 0, heap, off, maxError2, maxRadius2, allowSelfMatch);
/*if (sortResults)
* heap.sort();*/
int j = 0;
foreach (var idx in heap.Items)
{
indices.At(j, i, idx.index);
j++;
}
j = 0;
foreach (var dist in heap.Priorities)
{
dists2.At(j, i, dist);
j++;
}
return(leafTouchedCount);
}
public KdTreeNearestNeighborSearch(DenseColumnMajorMatrixStorage <float> cloud, int bucketSize = 8)
{
this.bucketSize = bucketSize;
if (bucketSize < 2)
{
throw new ArgumentException(string.Format("Requested bucket size {0}, but must be larger than 2", bucketSize), "bucketSize");
}
this.dimensions = (uint)cloud.RowCount;
this.dimBitCount = GetStorageBitCount(dimensions);
this.dimMask = (uint)((1 << dimBitCount) - 1);
this.cloud = cloud;
if (cloud.ColumnCount <= bucketSize)
{
// make a single-bucket tree
for (int i = 0; i < cloud.ColumnCount; i++)
{
buckets.Add(new BucketEntry(i));
}
nodes.Add(Node.ConstuctLeafNode(CreateDimChildBucketSize((uint)this.dimensions, (uint)cloud.ColumnCount), 0));
return;
}
int maxNodeCount = (0x1 << (int)(32 - dimBitCount)) - 1;
int estimatedNodeCount = cloud.ColumnCount / (bucketSize / 2);
if (estimatedNodeCount > maxNodeCount)
{
throw new ArgumentException(
string.Format(
"Cloud has a risk to have more nodes ({0}) than the kd-tree allows ({1}). The kd-tree has {2} bits for dimensions and {3} bits for node indices",
estimatedNodeCount,
maxNodeCount,
dimBitCount,
32 - dimBitCount));
}
// build point vector and compute bounds
List <int> buildPoints = new List <int>(cloud.ColumnCount);
for (int i = 0; i < cloud.ColumnCount; ++i)
{
buildPoints.Add(i);
}
Vector <float> minBound = new DenseVector(cloud.RowCount);
Vector <float> maxBound = new DenseVector(cloud.RowCount);
for (int i = 0; i < cloud.ColumnCount; i++)
{
for (int j = 0; j < cloud.RowCount; j++)
{
float v = cloud.At(j, i);
minBound[j] = Math.Min(minBound[j], v);
maxBound[j] = Math.Max(maxBound[j], v);
}
}
// create nodes
this.BuildNodes(buildPoints, 0, buildPoints.Count, minBound, maxBound);
}
