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