// adds the whole cloud to a tree, then calculate normals on the querypoints
private void CalculateNormals(IEnumerable <CloudPoint> cloudPoints)
{
// somewhat slow, but faster than a brute-force search
KdTree <CloudPoint> tree = KdTree <CloudPoint> .Construct(3, PointList, x => x.location);
foreach (CloudPoint cp in cloudPoints)
{
// find nearest 4 neighbors and do the thing here: http://pointclouds.org/documentation/tutorials/normal_estimation.php
C5.IPriorityQueue <CloudPoint> neighbors = tree.FindNearestNNeighbors(cp.location, 4);
cp.CalculateNormal(neighbors);
}
}
/// <summary>
/// Sets the host used by the network database
/// </summary>
/// <param name="host">The host to use</param>
private void _setHost(Host host)
{
_refreshQueue = new C5.IntervalHeap <IRefreshTask>(new RefreshTaskComparer());
_refreshQueueSem = new SemaphoreSlim(0);
_disposeHostEvent = new ManualResetEventSlim(false);
_host = host;
_client = new Client(host);
_unconfirmedRequestSubscription = _host.Subscribe(this);
_fillRefreshQueueTimer = new Timer(_fillRefreshQueue, null, TimeSpan.Zero, FillRefreshQueueInterval);
_refreshThreads = new Thread[ReadThreadCount];
for (int i = 0; i < ReadThreadCount; i++)
{
_refreshThreads[i] = new Thread(_refreshThreadStart);
_refreshThreads[i].Start();
}
}
internal void CalculateNormal(C5.IPriorityQueue <CloudPoint> neighbors)
{
Vector avg = neighbors.Aggregate(new Vector(3), (sum, val) => sum + val.location) / (double)neighbors.Count;
Matrix cov = ((double)1 / neighbors.Count) *
neighbors.Aggregate(new Matrix(3, 3),
(sum, val) => sum +
(val.location - avg).ToColumnMatrix() *
(val.location - avg).ToRowMatrix());
// that dude says the smallest eigenvalue is the normal
// first column of eigenvectors is the smallest eigenvalued one
normal = cov.EigenVectors.GetColumnVector(0);
// orient normal toward viewpoint
// to do this we have to push the origin into the world frame
//v = ZigInput.ConvertImageToWorldSpace(v);
var viewp = ZigInput.ConvertImageToWorldSpace(Vector3.zero);
normal = ((normal * (viewp.ToVector() - this.location)) > 0 ? normal : -normal);
normal.Normalize();
}
private void FindNearestNNeighbors(Vector <TField> location, KdTreeNode <TValue> node,
ref TValue maxBestValue, ref TField maxBestDistance, int numNeighbors, C5.IPriorityQueue <TValue> valuesList,
int depth)
{
if (node == null)
{
return;
}
var dimension = depth % this.dimensionality;
var nodeLocation = this.locationGetter(node.Value);
var distance = (nodeLocation - location).Norm(this.dimensionality);
// Check if current node is better than maximum best node, and replace maximum node in list with it.
// Current node cannot be same as search location.
if (!fieldArithmetic.AlmostEqual(distance, fieldArithmetic.Zero) &&
fieldComparer.Compare(distance, maxBestDistance) < 0)
{
TValue maxValue;
if (valuesList.Count == numNeighbors)
{
maxValue = valuesList.DeleteMax();
}
valuesList.Add(node.Value);
if (valuesList.Count == numNeighbors)
{
maxBestValue = valuesList.FindMax();
maxBestDistance = (this.locationGetter(maxBestValue) - location).Norm(this.dimensionality);
}
}
// Check for best node in sub-tree of near child.
var nearChildNode = fieldComparer.Compare(location[dimension], nodeLocation[dimension]) < 0 ?
node.LeftChild : node.RightChild;
if (nearChildNode != null)
{
FindNearestNNeighbors(location, nearChildNode, ref maxBestValue, ref maxBestDistance, numNeighbors,
valuesList, depth + 1);
}
// Check whether splitting hyperplane given by current node intersects with hypersphere of current smallest
// distance around given location.
if (fieldComparer.Compare(maxBestDistance, fieldArithmetic.Abs(fieldArithmetic.Subtract(
nodeLocation[dimension], location[dimension]))) > 0)
{
// Check for best node in sub-tree of far child.
var farChildValue = nearChildNode == node.LeftChild ? node.RightChild : node.LeftChild;
if (farChildValue != null)
{
FindNearestNNeighbors(location, farChildValue, ref maxBestValue, ref maxBestDistance, numNeighbors,
valuesList, depth + 1);
}
}
}
