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); }