/// <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>
/// 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);
}
}
/// <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");
}
}
//this is not fully implemented as I don't know how colours are represented!
//TODO: throw an exception if the rawdepth is not the same length as rgb
public void setPoints(int[] rawDepth, int[] r, int[] g, int[] b)
{
//instantiate the matrix object, now that we have the data points
m = new DenseMatrix(width * height, 3);
for (int iy = 0; iy < 480; iy++)
{
for (int ix = 0; ix < 640; ix++)
{
int i = (iy * 640) + ix;
if (rawDepth[i] == unknownDepth || rawDepth[i] < tooCloseDepth || rawDepth[i] > tooFarDepth)
{
rawDepth[i] = -1;
//at the moment we seem to be deleting points that are too far away, this will need changing at some point
//this.depthFramePoints[i] = new Point3D();
}
else
{
double zz = rawDepth[i] * scale;
double x = (cx - ix) * zz * fxinv;
double y = (cy - iy) * zz * fyinv;
double z = zz;
//check min values
if (x < minx)
{
minx = x;
}
if (y < miny)
{
miny = y;
}
if (z < minz)
{
minz = z;
}
//check max values
if (x > maxx)
{
maxx = x;
}
if (y > maxy)
{
maxy = y;
}
if (z > maxz)
{
maxz = z;
}
//create a new point key
double[] pointKey = new double[3];
//set key
pointKey[0] = x;
pointKey[1] = y;
pointKey[2] = z;
Point3D poLoc = new Point3D(x, y, z);
PARSE.ICP.PointRGB po = new PARSE.ICP.PointRGB(poLoc, r[i], g[i], b[i]);
this.points.insert(pointKey, po);
}
}
}
}
public void setPoints(int[] rawDepth, int r, int g, int b)
{
for (int iy = 0; iy < 480; iy++)
{
for (int ix = 0; ix < 640; ix++)
{
int i = (iy * 640) + ix;
if (rawDepth[i] == unknownDepth || rawDepth[i] < tooCloseDepth || rawDepth[i] > tooFarDepth)
{
rawDepth[i] = -1;
//at the moment we seem to be deleting points that are too far away, this will need changing at some point
//this.depthFramePoints[i] = new Point3D();
}
else
{
double zz = rawDepth[i] * scale;
double x = (cx - ix) * zz * fxinv;
double y = (cy - iy) * zz * fyinv;
double z = zz;
//TODO: this can probably be made more efficient at a later date, perhaps through threading...
//check min values
if (x < minx)
{
minx = x;
}
if (y < miny)
{
miny = y;
}
if (z < minz)
{
minz = z;
}
//check max values
if (x > maxx)
{
maxx = x;
}
if (y > maxy)
{
maxy = y;
}
if (z > maxz)
{
maxz = z;
}
//create a new point key
double[] pointKey = new double[3];
//set key
pointKey[0] = x;
pointKey[1] = y;
pointKey[2] = z;
//shove point into the kd-tree
Point3D poLoc = new Point3D(x, y, z);
PARSE.ICP.PointRGB po = new PARSE.ICP.PointRGB(poLoc, r, g, b);
this.points.insert(pointKey, po);
//calculate the matrix row
int row = height * iy + ix;
//dump the data into the row
m.At(row, 1, x);
m.At(row, 2, y);
m.At(row, 3, z);
}
}
}
}
//this is not fully implemented as I don't know how colours are represented!
//TODO: throw an exception if the rawdepth is not the same length as rgb
public void setPoints(int[] rawDepth, int[] r, int[] g, int[] b)
{
//instantiate the matrix object, now that we have the data points
m = new DenseMatrix(width * height, 3);
for (int iy = 0; iy < 480; iy++)
{
for (int ix = 0; ix < 640; ix++)
{
int i = (iy * 640) + ix;
if (rawDepth[i] == unknownDepth || rawDepth[i] < tooCloseDepth || rawDepth[i] > tooFarDepth)
{
rawDepth[i] = -1;
//at the moment we seem to be deleting points that are too far away, this will need changing at some point
//this.depthFramePoints[i] = new Point3D();
}
else
{
double zz = rawDepth[i] * scale;
double x = (cx - ix) * zz * fxinv;
double y = (cy - iy) * zz * fyinv;
double z = zz;
//check min values
if (x < minx) { minx = x; }
if (y < miny) { miny = y; }
if (z < minz) { minz = z; }
//check max values
if (x > maxx) { maxx = x; }
if (y > maxy) { maxy = y; }
if (z > maxz) { maxz = z; }
//create a new point key
double[] pointKey = new double[3];
//set key
pointKey[0] = x;
pointKey[1] = y;
pointKey[2] = z;
Point3D poLoc = new Point3D(x, y, z);
PARSE.ICP.PointRGB po = new PARSE.ICP.PointRGB(poLoc, r[i], g[i], b[i]);
this.points.insert(pointKey, po);
}
}
}
}
public void setPoints(int[] rawDepth, int r, int g, int b)
{
for (int iy = 0; iy < 480; iy++) {
for (int ix = 0; ix < 640; ix++) {
int i = (iy * 640) + ix;
if (rawDepth[i] == unknownDepth || rawDepth[i] < tooCloseDepth || rawDepth[i] > tooFarDepth) {
rawDepth[i] = -1;
//at the moment we seem to be deleting points that are too far away, this will need changing at some point
//this.depthFramePoints[i] = new Point3D();
}
else {
double zz = rawDepth[i] * scale;
double x = (cx - ix) * zz * fxinv;
double y = (cy - iy) * zz * fyinv;
double z = zz;
//TODO: this can probably be made more efficient at a later date, perhaps through threading...
//check min values
if (x < minx) { minx = x; }
if (y < miny) { miny = y; }
if (z < minz) { minz = z; }
//check max values
if (x > maxx) { maxx = x; }
if (y > maxy) { maxy = y; }
if (z > maxz) { maxz = z; }
//create a new point key
double[] pointKey = new double[3];
//set key
pointKey[0] = x;
pointKey[1] = y;
pointKey[2] = z;
//shove point into the kd-tree
Point3D poLoc = new Point3D(x, y, z);
PARSE.ICP.PointRGB po = new PARSE.ICP.PointRGB(poLoc, r, g, b);
this.points.insert(pointKey, po);
//calculate the matrix row
int row = height * iy + ix;
//dump the data into the row
m.At(row, 1, x);
m.At(row, 2, y);
m.At(row, 3, z);
}
}
}
}
/// <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>
/// 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);
}
}
