コード例 #1
0
ファイル: PointCloud.cs プロジェクト: robinj/parse-client
        /// <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);
        }
コード例 #2
0
ファイル: PointCloud.cs プロジェクト: robinj/parse-client
        /// <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);
            }
        }
コード例 #3
0
ファイル: PointCloud.cs プロジェクト: robinj/parse-client
        /// <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");
            }
        }
コード例 #4
0
ファイル: PointCloud.cs プロジェクト: robinj/parse-client
        //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);
                    }
                }
            }
        }
コード例 #5
0
ファイル: PointCloud.cs プロジェクト: robinj/parse-client
        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);
                    }
                }
            }
        }
コード例 #6
0
ファイル: PointCloud.cs プロジェクト: robinj/parse-client
        //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);
                    }
                }
            }
        }
コード例 #7
0
ファイル: PointCloud.cs プロジェクト: robinj/parse-client
        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);
                    }
                }
            }
        }
コード例 #8
0
ファイル: PointCloud.cs プロジェクト: robinj/parse-client
        /// <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;
        }
コード例 #9
0
ファイル: PointCloud.cs プロジェクト: robinj/parse-client
        /// <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);
            }
        }