PclSharp--ICP迭代最近点算法

1、ICP算法对待拼接的2片点云，首先根据一定的准则确立对应点集P与Q，其中对应点对的个数为n。然后通过最小二乘法迭代计算最优的坐标变化，即旋转矩阵R和平移矢量t，使得误差函数最小。ICP算法计算简便直观且可使拼接具有较好的精度，但是算法的运行速度以及向全局最优的收敛性却在很大程度上依赖于给定的初始变换估计以及在迭代过程中对应关系的确立。各种粗拼接技术可以为ICP算法提供较好的初始位置，所以迭代过程中确立正确对应点集以避免迭代陷入局部极值成为各种改进算法的关键，决定了算法的收敛速度与最终的拼接精度。ICP处理流程分为4个主要步骤：

    1）对原始点云数据进行采样；

    2）确定初始对应点集；

    3）去除错误对应点对；

    4）坐标变换矩阵的求解。
 

2、代码

using PclSharp;
using PclSharp.Eigen;
using PclSharp.Filters;
using PclSharp.Helpers;
using PclSharp.IO;
using PclSharp.Registration;
using PclSharp.Std;
using PclSharp.Struct;
using PclSharp.Surface;
using System;

namespace PclSharpTest
{
    class Program
    {
        static void Main(string[] args)
        {
            Console.WriteLine($"C#--PclSharp算法库测试：");
            //读取兔子点云对象
            var src_cloud = new PointCloudOfXYZ();
            using (var reader = new PCDReader())
                reader.Read(AppDomain.CurrentDomain.BaseDirectory + $"//pcd//rabbit.pcd", src_cloud);
            //实现一个简单的点云刚体变换，以构造目标点云
            Matrix4f matrix4 = Matrix4f.Identity();//创建4x4单位矩阵
            matrix4[0, 3] = 10f;
            //matrix4[1, 1] = 0.5f;
            //matrix4[2, 2] = 0.5f;
            //输出变换矩阵
            PrintfMatrix4x4(matrix4);
            var trt_cloud = new PointCloudOfXYZ();
            //根据变换矩阵转换点云
            PclHelper.transformPointCloud(src_cloud, trt_cloud, matrix4);
            Console.WriteLine($"进行ICP配准...");
            //ICP配准
            var matrix_icp = new Matrix4f();
            //定义一个点云，以便将源点云进行刚性变换，和目标点云进行匹配
            PointCloudOfXYZ final_cloud = new PointCloudOfXYZ();
            using (IterativeClosestPointOfPointXYZ_PointXYZ icp = new IterativeClosestPointOfPointXYZ_PointXYZ())
            {

                // 1. 设置输入点云
                icp.InputSource = src_cloud;
                icp.InputTarget = trt_cloud;
                //2.配置ICP，可设置停止迭代的条件，算法参数等等
                //迭代次数并不是设置多少就真的要迭代多少，程序检测到收敛会自动停止，因此此设置足够大即可
                icp.MaximumIterations = 100;// 最大迭代次数
                icp.TransformationEpsilon = 1e-10;//设置终止条件的最小转换差异
                icp.EuclideanFitnessEpsilon = 0.2;//设置收敛的条件是均方误差和小于这个数值，则认为匹配完成
                icp.MaxCorrespondenceDistance = 20;//40mm
                // 3. 运行ICP，注意被放入align函数的点云会变成变换后的Source点云
                //因此将src_cloud放在这里会改变这个点云的值，若不希望改变src_cloud应新建一个点云作为参数
                icp.Align(final_cloud);
                //PCDIO.Write(final_cloud);
                // 5.获取结果：是否收敛的标志位、匹配度（匹配点距离的平均方差）、变换矩阵
                if (icp.hasConverged)
                {
                    //源点云到目标点云的欧式距离之和
                    //double match_score = icp.FitnessScore;
                    //获取变换矩阵
                    icp.FinalTransformation(matrix_icp);

                }

            }
            Console.WriteLine($"ICP配准矩阵：");
            //输出配准矩阵
            PrintfMatrix4x4(matrix_icp);

            using (var visualizer = new PclSharp.Vis.Visualizer(" visual window"))
            {
                //创建两个观察视点
                int v1 = 0;
                int v2 = 2;
                visualizer.CreateViewPort(0.0, 0.0, 0.5, 1.0, v1);
                visualizer.CreateViewPort(0.5, 0.0, 1.0, 1.0, v2);

                visualizer.SetBackgroundColor_ViewPort(0f, 0f, 0f, v1);
                visualizer.SetBackgroundColor_ViewPort(0.05f, 0f, 0f, v2);
                //添加点云
                visualizer.AddPointCloud(src_cloud, "src_v1", v1);
                visualizer.AddPointCloud(trt_cloud, "tat_cloud_v1", v1);
                //转换后的点云
                visualizer.AddPointCloud(trt_cloud, "tat_cloud_v2", v2);
                visualizer.AddPointCloud(final_cloud, "icp_cloud_v2", v2);

                visualizer.SetPointCloudColor(1, 0.5, 1, "tat_cloud_v2");//旋转后目标点云附粉色
                visualizer.SetPointCloudColor(1, 1, 0.5, "icp_cloud_v2");//配准后点云附黄色
               

                while (!visualizer.WasStopped)
                    visualizer.SpinOnce(100);
            }

            Console.ReadKey();
        }

        /// <summary>
        /// 打印输出变换矩阵
        /// </summary>
        /// <param name="transformation"></param>
        public static void PrintfMatrix4x4(Matrix4f transformation)
        {
            //输出变换矩阵信息  
            Console.WriteLine("\r\n");
            Console.WriteLine($"4x4矩阵： ");
            Console.WriteLine($"   {transformation[0, 0]}, {transformation[0, 1]}, {transformation[0, 2]}, {transformation[0, 3]}");
            Console.WriteLine($"R= {transformation[1, 0]}, {transformation[1, 1]}, {transformation[1, 2]}, {transformation[1, 3]}");
            Console.WriteLine($"   {transformation[2, 0]}, {transformation[2, 1]}, {transformation[2, 2]}, {transformation[2, 3]}");
            Console.WriteLine($"   {transformation[3, 0]}, {transformation[3, 1]}, {transformation[3, 2]}, {transformation[3, 3]}");
        }

    }
}

  

3、编译结果