/// <summary>
/// returns arbNumber many planes, sliced height wise, which have been averaged, subsampled and clock sorted.
/// </summary>
/// <param name="pc">PointCloud</param>
/// <param name="arbNumber">int</param>
/// <returns>Tuple(List(List(Point3D)),double)</returns>
public static Tuple <List <List <Point3D> >, double> pullAll(PointCloud pc, int arbNumber)
{
double xmin = pc.getxMin();
double xmax = pc.getxMax();
double zmin = pc.getzMin();
double zmax = pc.getzMax();
double[] limits = { xmin, zmin, xmax, zmax };
double ymin = pc.getyMin();
double ymax = pc.getyMax();
double increment = (ymax - ymin) / arbNumber;
List <List <Point3D> > output = new List <List <Point3D> >();
for (double i = ymin + (increment / 2); i <= ymax - (increment / 2); i = i + increment)
{
List <Point3D> plane = pc.getKDTree().getAllPointsAt(i, increment / 2, limits);
plane = SubSampler.averageSubSample(plane, sampleNumber);
plane = PointSorter.clockSort(plane);
output.Add(plane);
}
return(Tuple.Create(output, increment));
}
/// <summary>
/// returns arbNumber many planes, sliced height wise, which have been averaged, subsampled and clock sorted.
/// </summary>
/// <param name="pc">PointCloud</param>
/// <param name="arbNumber">int</param>
/// <returns>Tuple(List(List(Point3D)),double)</returns>
public static Tuple<List<List<Point3D>>, double> pullAll(PointCloud pc, int arbNumber)
{
double xmin = pc.getxMin();
double xmax = pc.getxMax();
double zmin = pc.getzMin();
double zmax = pc.getzMax();
double[] limits = { xmin, zmin, xmax, zmax };
double ymin = pc.getyMin();
double ymax = pc.getyMax();
double increment = (ymax - ymin) / arbNumber;
List<List<Point3D>> output = new List<List<Point3D>>();
for (double i = ymin + (increment / 2); i <= ymax - (increment / 2); i = i + increment)
{
List<Point3D> plane = pc.getKDTree().getAllPointsAt(i, increment / 2, limits);
plane = SubSampler.averageSubSample(plane, sampleNumber);
plane = PointSorter.clockSort(plane);
output.Add(plane);
}
return Tuple.Create(output,increment);
}
/// <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>
/// 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");
}
}
