/// <summary> /// Suboptimal constructor, used for list of 3d points /// </summary> /// <param name="input"></param> public PointCloud(List <Point3D> input) { this.points = new KdTree.KDTree(3); for (int i = 0; i < input.Count; i++) { Point3D poLoc = input[i]; //check min values if (poLoc.X < minx) { minx = poLoc.X; } if (poLoc.Y < miny) { miny = poLoc.Y; } if (poLoc.Z < minz) { minz = poLoc.Z; } //check max values if (poLoc.X > maxx) { maxx = poLoc.X; } if (poLoc.Y > maxy) { maxy = poLoc.Y; } if (poLoc.Z > maxz) { maxz = poLoc.Z; } //this is a very dark kd hole PARSE.ICP.PointRGB po = new PARSE.ICP.PointRGB(poLoc, 0, 0, 0); double[] key = { poLoc.X, poLoc.Y, poLoc.Z }; this.points.insert(key, po); } }
//the following variables may or may not be defined, depending on future need //sensor_orientation //sensor_origin public PointCloud(BitmapSource bs, int[] rawDepth) { this.bs = bs; this.rawDepth = rawDepth; textureCoordinates = new System.Windows.Point[depthFrameHeight * depthFrameWidth]; depthFramePoints = new Point3D[depthFrameHeight * depthFrameWidth]; this.points = new KdTree.KDTree(3); //convert bitmap stream into a format that is supported by the kd-tree method if (bs != null) { Bitmap b = convertToBitmap(bs); setPoints(rawDepth, b); } else { setPoints(rawDepth); } }
/// <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"); } }
/// <summary> /// Rotates the point cloud by a given angle /// </summary> /// <param name="axis">The axis of rotation</param> /// <param name="angle">The angle to which te point cloud is to be rotated</param> public void rotate(double[] axis, double angle) { if (!(axis.Length != 3)) { //centre of rotation Point3D centre = new Point3D(axis[0], axis[1], axis[2]); //pull out the entire tree PARSE.ICP.PointRGB[] pts = this.getAllPoints(); //create a new kd tree KdTree.KDTree newPoints = new KdTree.KDTree(3); //iterate over every point and translate + jam in new tree foreach (PARSE.ICP.PointRGB point in pts) { //create rot matrix Matrix3D mtx = new Matrix3D(); Quaternion q = new Quaternion(new Vector3D(0, 1, 0), angle); mtx.RotateAt(q, centre); //complete rotation Point3D newPoint = mtx.Transform(point.point); //check if the x, y and z max and min coords need updating //check min values if (newPoint.X < minx) { minx = newPoint.X; } if (newPoint.Y < miny) { miny = newPoint.Y; } if (newPoint.Z < minz) { minz = newPoint.Z; } //check max values if (newPoint.X > maxx) { maxx = newPoint.X; } if (newPoint.Y > maxy) { maxy = newPoint.Y; } if (newPoint.Z > maxz) { maxz = newPoint.Z; } //jam into the tree hole double[] key = new double[3] { newPoint.X, newPoint.Y, newPoint.Z }; newPoints.insert(key, new PARSE.ICP.PointRGB(newPoint, point.r, point.g, point.b)); } //replace the old kd tree with the new one this.points = newPoints; } else { //throw an exception and annoy Bernie in the process ;) } }
/// <summary> /// Translate the point cloud by a given value /// </summary> /// <param name="tx">Up to three co-ords</param> public void translate(double[] tx) { if (tx.Length == 3) { //turn the transformation vector into and object Console.WriteLine("Translating"); TranslateTransform3D translation = new TranslateTransform3D(tx[0], tx[1], tx[2]); //pull out the entire tree PARSE.ICP.PointRGB[] pts = this.getAllPoints(); //create a new kd tree KdTree.KDTree newPoints = new KdTree.KDTree(3); //iterate over every point and translate + jam in new tree foreach (PARSE.ICP.PointRGB point in pts) { //perform the new translation which does appear to work. Matrix3D mtx = new Matrix3D(); mtx.Translate(new Vector3D(tx[0], tx[1], tx[2])); //complete translation Point3D newPoint = mtx.Transform(point.point); //check if the x, y and z max and min coords need updating //check min values if (newPoint.X < minx) { minx = newPoint.X; } if (newPoint.Y < miny) { miny = newPoint.Y; } if (newPoint.Z < minz) { minz = newPoint.Z; } //check max values if (newPoint.X > maxx) { maxx = newPoint.X; } if (newPoint.Y > maxy) { maxy = newPoint.Y; } if (newPoint.Z > maxz) { maxz = newPoint.Z; } //jam into the tree double[] key = new double[3] { newPoint.X, newPoint.Y, newPoint.Z }; newPoints.insert(key, new PARSE.ICP.PointRGB(newPoint, point.r, point.g, point.b)); //perform the old translation method which doesn't appear to work. //point.point.Offset(tx[0], tx[1], tx[2]); //double[] key = new double[3]{point.point.X, point.point.Y, point.point.Z}; //newPoints.insert(key, point); } //replace the old kd tree with the new one this.points = newPoints; } else { //probably want to throw an exception here } }
/// <summary> /// Translate the point cloud by a given value /// </summary> /// <param name="tx">Up to three co-ords</param> public void translate(double[] tx) { if (tx.Length == 3) { //turn the transformation vector into and object Console.WriteLine("Translating"); TranslateTransform3D translation = new TranslateTransform3D(tx[0], tx[1], tx[2]); //pull out the entire tree PARSE.ICP.PointRGB[] pts = this.getAllPoints(); //create a new kd tree KdTree.KDTree newPoints = new KdTree.KDTree(3); //iterate over every point and translate + jam in new tree foreach(PARSE.ICP.PointRGB point in pts) { //perform the new translation which does appear to work. Matrix3D mtx = new Matrix3D(); mtx.Translate(new Vector3D(tx[0], tx[1], tx[2])); //complete translation Point3D newPoint = mtx.Transform(point.point); //check if the x, y and z max and min coords need updating //check min values if (newPoint.X < minx) { minx = newPoint.X; } if (newPoint.Y < miny) { miny = newPoint.Y; } if (newPoint.Z < minz) { minz = newPoint.Z; } //check max values if (newPoint.X > maxx) { maxx = newPoint.X; } if (newPoint.Y > maxy) { maxy = newPoint.Y; } if (newPoint.Z > maxz) { maxz = newPoint.Z; } //jam into the tree double[] key = new double[3] { newPoint.X, newPoint.Y, newPoint.Z }; newPoints.insert(key, new PARSE.ICP.PointRGB(newPoint, point.r, point.g, point.b)); //perform the old translation method which doesn't appear to work. //point.point.Offset(tx[0], tx[1], tx[2]); //double[] key = new double[3]{point.point.X, point.point.Y, point.point.Z}; //newPoints.insert(key, point); } //replace the old kd tree with the new one this.points = newPoints; } else { //probably want to throw an exception here } }
/// <summary> /// Rotates the point cloud by a given angle /// </summary> /// <param name="axis">The axis of rotation</param> /// <param name="angle">The angle to which te point cloud is to be rotated</param> public void rotate(double[] axis, double angle) { if (!(axis.Length != 3)) { //centre of rotation Point3D centre = new Point3D(axis[0], axis[1], axis[2]); //pull out the entire tree PARSE.ICP.PointRGB[] pts = this.getAllPoints(); //create a new kd tree KdTree.KDTree newPoints = new KdTree.KDTree(3); //iterate over every point and translate + jam in new tree foreach (PARSE.ICP.PointRGB point in pts) { //create rot matrix Matrix3D mtx = new Matrix3D(); Quaternion q = new Quaternion(new Vector3D(0, 1, 0), angle); mtx.RotateAt(q, centre); //complete rotation Point3D newPoint = mtx.Transform(point.point); //check if the x, y and z max and min coords need updating //check min values if (newPoint.X < minx) { minx = newPoint.X; } if (newPoint.Y < miny) { miny = newPoint.Y; } if (newPoint.Z < minz) { minz = newPoint.Z; } //check max values if (newPoint.X > maxx) { maxx = newPoint.X; } if (newPoint.Y > maxy) { maxy = newPoint.Y; } if (newPoint.Z > maxz) { maxz = newPoint.Z; } //jam into the tree hole double[] key = new double[3] { newPoint.X, newPoint.Y, newPoint.Z }; newPoints.insert(key, new PARSE.ICP.PointRGB(newPoint, point.r, point.g, point.b)); } //replace the old kd tree with the new one this.points = newPoints; } else{ //throw an exception and annoy Bernie in the process ;) } }
/// <summary> /// Suboptimal constructor, used for list of 3d points /// </summary> /// <param name="input"></param> public PointCloud(List<Point3D> input) { this.points = new KdTree.KDTree(3); for (int i = 0; i < input.Count; i++) { Point3D poLoc = input[i]; //check min values if (poLoc.X < minx) { minx = poLoc.X; } if (poLoc.Y < miny) { miny = poLoc.Y; } if (poLoc.Z < minz) { minz = poLoc.Z; } //check max values if (poLoc.X > maxx) { maxx = poLoc.X; } if (poLoc.Y > maxy) { maxy = poLoc.Y; } if (poLoc.Z > maxz) { maxz = poLoc.Z; } //this is a very dark kd hole PARSE.ICP.PointRGB po = new PARSE.ICP.PointRGB(poLoc, 0, 0, 0); double[] key = { poLoc.X, poLoc.Y, poLoc.Z }; this.points.insert(key, po); } }