/// <summary>
/// Returns all points from the kd tree
/// </summary>
/// <returns>The points from the kd tree</returns>
public PARSE.ICP.PointRGB[] getAllPoints()
{
//max and min values
double[] minVal = new double[3] {
double.MinValue, double.MinValue, double.MinValue
};
double[] maxVal = new double[3] {
double.MaxValue, double.MaxValue, double.MaxValue
};
//pull objects from kd tree
Object[] objects = points.range(minVal, maxVal);
//create somewhere to jam all the points
PARSE.ICP.PointRGB[] retPoints = new PARSE.ICP.PointRGB[objects.Length];
//filthy: typecast everything in turn
int i = 0;
foreach (Object pt in objects)
{
retPoints[i] = (PARSE.ICP.PointRGB)pt;
i++;
}
return(retPoints);
}
/// <summary>
/// Adds an existing point cloud into this point cloud
/// </summary>
/// <param name="pc">The point cloud to add</param>
public void addPointCloud(PointCloud pc)
{
//retrieve the kd tree
KdTree.KDTree kd = pc.getKDTree();
//define a max and min point
//TODO: set these to proper max+min vals from the point cloud object
double[] minPoint = new double[3] {
-100, -100, -100
};
double[] maxPoint = new double[3] {
100, 100, 100
};
//retrieve a list of all item in the tree
Object[] points2 = kd.range(minPoint, maxPoint);
//iterate over every point and jam it in this point cloud
foreach (Object element in points2)
{
//create k,v pair from data extracted
PARSE.ICP.PointRGB value = (PARSE.ICP.PointRGB)element;
double[] key = new double[3] {
value.point.X, value.point.Y, value.point.Z
};
//jam the data into the existing kd-tree
int duplicates = 0;
try {
this.points.insert(key, value);
}
catch (KeyDuplicateException) {
//ignore duplicates
duplicates++;
}
//Console.WriteLine("There were " + duplicates + " duplicate keys in the tree");
}
}