예제 #1
0
        static void Main(string[] args)
        {
            List<double[]> head_pts = PointCloudStorage.deserialize("model_pts");
            List<double[]> scene_pts = PointCloudStorage.deserialize("scene_pts");

            double[,] M = new double[head_pts.Count, 3];
            for (int i = 0; i < head_pts.Count; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    M[i, j] = head_pts[i][j];
                }
            }

            double[,] T = new double[scene_pts.Count, 3];
            for (int i = 0; i < scene_pts.Count; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    T[i, j] = scene_pts[i][j];
                }
            }

            ManagedICP icp = new ManagedICP(M, head_pts.Count, 3);

            double[,] Rot = new double[3, 3];
            /*Rot[0, 0] = 0.707;
            Rot[1, 1] = 0.707;
            Rot[0, 1] = -0.707;
            Rot[1, 0] = 0.707;
            Rot[2, 2] = 1.0;
             * */
            Rot[0, 0] = 0;
            Rot[1, 1] = 0;
            Rot[0, 1] = -1;
            Rot[1, 0] = 1;
            Rot[2, 2] = 1.0;
            double[] trans = new double[3];
            icp.fit(T, scene_pts.Count, Rot, trans, -1);
            Console.WriteLine("Rotation:");
            for (int i = 0; i < 3; i++)
            {
                Console.WriteLine(Rot[i, 0] + " " + Rot[i, 1] + " " + Rot[i, 2]);
            }
            int b = 5;
        }
예제 #2
0
파일: Program.cs 프로젝트: whigg/libicp-net
        static void Main(string[] args)
        {
            int dim   = 3;
            int M_num = 10000;
            int T_num = 10000;

            double[,] M = new double[M_num, 3];
            double[,] T = new double[T_num, 3];
            int k = 0;

            for (double x = -2; x < 2; x += 0.04)
            {
                for (double y = -2; y < 2; y += 0.04)
                {
                    double z = 5 * x * Math.Exp(-x * x - y * y);
                    M[k, 0] = x;
                    M[k, 1] = y;
                    M[k, 2] = z;
                    T[k, 0] = x - 1;
                    T[k, 1] = y - 1;
                    T[k, 2] = z - 1;
                    k++;
                }
            }
            icp_net.ManagedICP m = new ManagedICP(M, M_num, dim);

            double[,] R = new double[3, 3];
            R[0, 0]     = 1.0;
            R[1, 1]     = 1.0;
            R[2, 2]     = 1.0;

            double[] t = new double[3];

            m.fit(T, T_num, R, t, -1);

            Console.WriteLine("Transformation Results:");
            Console.WriteLine("R:" + R);
            Console.WriteLine("T:" + t[0] + " " + t[1] + " " + t[2]);
            Console.Read();
        }
예제 #3
0
        static void Main(string[] args)
        {
            int dim = 3;
            int M_num = 10000;
            int T_num = 10000;

            double[,] M = new double[M_num, 3];
            double[,] T = new double[T_num, 3];
            int k = 0;
            for (double x = -2; x < 2; x += 0.04)
            {
                for (double y = -2; y < 2; y += 0.04)
                {
                    double z = 5 * x * Math.Exp(-x * x - y * y);
                    M[k, 0] = x;
                    M[k, 1] = y;
                    M[k, 2] = z;
                    T[k, 0] = x - 1;
                    T[k, 1] = y - 1;
                    T[k, 2] = z - 1;
                    k++;
                }

            }
            icp_net.ManagedICP m = new ManagedICP(M, M_num, dim);

            double[,] R = new double[3, 3];
            R[0, 0] = 1.0;
            R[1, 1] = 1.0;
            R[2, 2] = 1.0;

            double[] t = new double[3];

            m.fit(T, T_num, R, t, -1);

            Console.WriteLine("Transformation Results:");
            Console.WriteLine("R:" + R);
            Console.WriteLine("T:" + t[0] + " " + t[1] + " " + t[2]);
            Console.Read();
        }
예제 #4
0
        /// <summary>
        /// aligns two point clouds using a transformation matrix. Additionally an ICP implementation can be used
        /// </summary>
        /// <param name="pReferencePointCloud">the reference cloud to align the adding cloud to</param>
        /// <param name="pAddingPointCloud">the cloud that is supposed to be aligned</param>
        /// <param name="pTransMat">the transformation matrix: 4x4 with [column,row] where [0,0] to [2,2] is the rotation and [3,0] to [3,2] the translation. The other fields are unused</param>
        /// <param name="pUseICP">boolean to check if ICP is supposed to be used</param>
        /// <param name="pIndis">inlier distance for ICP</param>
        /// <returns>a modified point cloud with the valies [i,{x,y,z}]</returns>
        public static double[,] alignPointClouds(PointCloud pReferencePointCloud, PointCloud pAddingPointCloud, double[,] pTransMat, bool pUseICP, int pIndis)
        {
            //log messages
            Log.LogManager.updateAlgorithmStatus("Point Cloud Alignment");
            Log.LogManager.writeLogDebug("[PointCloudAlignment] Use ICP: " + pUseICP + ", (ICP) In distance inliers : " + pIndis + ", Transformation Matrix: " +
                                         pTransMat[0, 0] + "," + pTransMat[1, 0] + "," + pTransMat[2, 0] + "," + pTransMat[3, 0] + ";" +
                                         pTransMat[0, 1] + "," + pTransMat[1, 1] + "," + pTransMat[2, 1] + "," + pTransMat[3, 1] + ";" +
                                         pTransMat[0, 2] + "," + pTransMat[1, 2] + "," + pTransMat[2, 2] + "," + pTransMat[3, 2] + ";" +
                                         pTransMat[0, 3] + "," + pTransMat[1, 3] + "," + pTransMat[2, 3] + "," + pTransMat[3, 3]);

            //transform point clouds into format
            double[,] referenceCloudDouble = new double[pReferencePointCloud.count, 3];
            double[,] addingCloudDouble    = new double[pAddingPointCloud.count, 3];

            Task t1 = Task.Factory.StartNew(() =>
            {
                int counterRef = 0;
                foreach (PointCloud.Point p in pReferencePointCloud.pointcloud_hs)
                {
                    referenceCloudDouble[counterRef, 0] = p.point.X;
                    referenceCloudDouble[counterRef, 1] = p.point.Y;
                    referenceCloudDouble[counterRef, 2] = p.point.Z;
                    counterRef++;
                }
            });

            Task t2 = Task.Factory.StartNew(() =>
            {
                int counterRef = 0;
                foreach (PointCloud.Point p in pAddingPointCloud.pointcloud_hs)
                {
                    addingCloudDouble[counterRef, 0] = p.point.X;
                    addingCloudDouble[counterRef, 1] = p.point.Y;
                    addingCloudDouble[counterRef, 2] = p.point.Z;
                    counterRef++;
                }
            });

            t1.Wait(); t2.Wait();

            //perform translation
            Parallel.For(0, addingCloudDouble.GetLength(0), i =>
            {
                //fetch point
                double[] point = new double[3];
                for (int a = 0; a < 3; a++)
                {
                    point[a] = addingCloudDouble[i, a];
                }

                //transform with matrix
                double[] pointNEW = transformPointWithMatrix(point, pTransMat);

                //write point
                for (int a = 0; a < 3; a++)
                {
                    addingCloudDouble[i, a] = pointNEW[a];
                }
            });

            ///*****************************
            ///***ITERATIVE CLOSEST POINT***
            ///*****************************
            if (pUseICP)
            {
                //perform icp
                //create reference icp object
                ManagedICP icpManager = new ManagedICP(referenceCloudDouble, pReferencePointCloud.count, 3);

                //fit
                double[,] R = new double[3, 3]; //rotation matrix
                R[0, 0]     = 1.0;
                R[1, 1]     = 1.0;
                R[2, 2]     = 1.0;

                double[] t = new double[3]; //translation vector

                icpManager.fit(addingCloudDouble, pAddingPointCloud.count, R, t, pIndis);

                //add to matrix4
                double[,] rem = new double[4, 4];
                rem[0, 0]     = 1; rem[1, 1] = 2; rem[2, 2] = 1; rem[3, 3] = 1;

                //add transformation matrix;
                rem[0, 0] = (float)R[0, 0]; rem[0, 1] = (float)R[0, 1]; rem[0, 2] = (float)R[0, 2];
                rem[1, 0] = (float)R[1, 0]; rem[1, 1] = (float)R[1, 1]; rem[1, 2] = (float)R[1, 2];
                rem[2, 0] = (float)R[2, 0]; rem[2, 1] = (float)R[2, 1]; rem[2, 2] = (float)R[2, 2];

                //add translation vector
                rem[3, 0] = (float)t[0]; rem[3, 1] = (float)t[1]; rem[3, 2] = (float)t[2];

                //set remaining fields
                rem[0, 3] = 0; rem[1, 3] = 0; rem[2, 3] = 0;

                //perform translation
                Parallel.For(0, addingCloudDouble.GetLength(0), i =>
                {
                    //fetch point
                    double[] point = new double[3];
                    for (int a = 0; a < 3; a++)
                    {
                        point[a] = addingCloudDouble[i, a];
                    }

                    //transform with matrix
                    double[] pointNEW = transformPointWithMatrix(point, rem);

                    //write point
                    for (int a = 0; a < 3; a++)
                    {
                        addingCloudDouble[i, a] = pointNEW[a];
                    }
                });
            }
            ///******************************

            //updates the status
            Log.LogManager.updateAlgorithmStatus("Done");

            return(addingCloudDouble);
        }