针对车载激光雷达和惯性测量单元(Inertial Measurement Unit，IMU)的坐标系标定问题，设计了一种使用角反射器作标志物的标定方法。首先通过区域分割、地面滤除和标志点提取的预处理方法来提取标志点；然后借鉴3D点云匹配的方法，将坐标系联合标定转化为点云匹配问题，使用迭代最近邻点(Iterative Closest Point，ICP)算法求得两坐标系的坐标转换矩阵。最后，将标定结果与基于最小二乘法的标定结果进行对比，结果表明使用3D点云匹配的标定方法是可行的。

实现两个三维点云的匹配，matlab实现

使用matlab实现点云匹配（ICP算法）。参数设置在代码的最前面，可以选择kd-tree或者暴力计算最近邻点。

第1部分-------------------------------------------------------- % 读取原始点云数据：X % 通过旋转平移，构造试验点云数据：P 第2部分------点云粗配准---------------------------------------- 第3部分--------ICP迭代，精配准---------------------------------

点云匹配代码，计算点云的旋转矩阵，平移矩阵，匹配精度高．

function [R1, t1] = reg(data1, data2, corr) M = data1(:,corr(:,1)); mm = mean(M,2); S = data2(:,corr(:,2)); ms = mean(S,2); Sshifted = [S(1,:)-ms(1); S(2,:)-ms(2); ]; Mshifted = [M(1,:)-mm(1); M(2,:)-mm(2); ]; b1 = Sshifted(1,:)*Mshifted(1,:)'+Sshifted(2,:)*Mshifted(2,:)'; b2 = -Sshifted(2,:)*Mshifted(1,:)'+Sshifted(1,:)*Mshifted(2,:)'; bb = (b1^2+b2^2)^0.5; c = b1/bb; s = b2/bb; R1 = [c -s s c]; t1 = mm - R1*ms;

icp点云匹配，及相关点云文件。vs2013