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