基于ICP算法的三维点云模型的自动配准算法matlab仿真

目录

​​一、理论基础​​

​​二、核心MATLAB程序​​

​​三、MATLAB仿真测试结果​​

一、理论基础

        点云配准（Point Cloud Registration）指的是求得一个变换 T，对于两幅点云 Ps (source) 和 Pt (target) ，使得 T(Ps) 和 Pt 的重合程度尽可能高。本文 T 只考虑刚性变换的情况，即变换只包括旋转、平移。点云配准可以分为粗配准（Coarse Registration）和精配准（Fine Registration）两步。粗配准指的是在两幅点云之间的变换完全未知的情况下进行较为粗糙的配准，目的主要是为精配准提供较好的变换初值（可以用FPFH+SAC来做）；精配准则是给定一个初始变换，进一步优化得到更精确的变换（可以用NDT和ICP来做）。目前应用最广泛的点云精配准算法是迭代最近点算法（Iterative Closest Point, ICP）及各种变种 ICP 算法。

      点云匹配用公式来写就是：

ICP 算法的思想如下：

• 如果我们知道两幅点云上点的对应关系，那么我们可以用 Least Squares 来求解刚性变换 T 中的 R , t 参数；
• 怎么知道点的对应关系呢？如果我们已经知道了一个大概靠谱的R , t参数，那么我们可以通过贪心的方式找两幅点云上点的对应关系（直接找距离最近的点作为对应点）。

ICP 算法实际上就是交替进行上述两个步骤，迭代进行计算，直到收敛。

ICP 一般算法流程为：

1. 点云预处理

- 滤波（去除一定范围之外的点，去除地面）、清理数据（去除nan值）等

2. 匹配

- 应用上一步求解出的变换，找最近点（之前最好先做好粗配准，ICP对初值的要求比较高）

3. 加权

- 调整一些对应点对的权重

4. 剔除不合理的对应点对

5. 计算 loss

6. 最小化 loss，求解当前最优变换

7. 回到步骤 2. 进行迭代，直到收敛

整体上来看，ICP 把点云配准问题拆分成了两个子问题：

• 找最近点
• 找最优变换

二、核心MATLAB程序

clc;
clear;
close all;
warning off;

%调用几种测试数据
SEL = 2;
if  SEL == 1
    load EXAMPLE 
    G1    = source;
    fai   = 0.15;
    X     = G1(:,1).*cos(2*pi*fai)-G1(:,3).*sin(2*pi*fai);
    Y     = G1(:,2);
    Z     = G1(:,1).*sin(2*pi*fai)+G1(:,3).*cos(2*pi*fai);
    T     = [X,Y,Z];
    source(:,1)  = T(:,1)+1200;
    source(:,2)  = T(:,2)+200;
    source(:,3)  = T(:,3)+400;
end


if  SEL == 2
    load model_25_partial.mat 
    %旋转，方便观察是否配准
    G1    = node_xyz';
    fai   = 0.15;
    X     = G1(:,1).*cos(2*pi*fai)-G1(:,3).*sin(2*pi*fai);
    Y     = G1(:,2);
    Z     = G1(:,1).*sin(2*pi*fai)+G1(:,3).*cos(2*pi*fai);
    T     = [X,Y,Z];

    source(:,1)  = T(:,1)+1200;
    source(:,2)  = T(:,2)+200;
    source(:,3)  = T(:,3)+400;
    fsource = face_node';

    load model_25.mat 
    target  = node_xyz';
    ftarget = face_node';
end


if  SEL == 3
    load model_5_partial.mat 
    %旋转，方便观察是否配准
    G1    = node_xyz';
    fai   = 0.15;
    X     = G1(:,1).*cos(2*pi*fai)-G1(:,3).*sin(2*pi*fai);
    Y     = G1(:,2);
    Z     = G1(:,1).*sin(2*pi*fai)+G1(:,3).*cos(2*pi*fai);
    T     = [X,Y,Z];
    source(:,1)  = T(:,1)+1200;
    source(:,2)  = T(:,2)+200;
    source(:,3)  = T(:,3)+400;
    fsource = face_node';

    load model_5.mat 
    target  = node_xyz';
    ftarget = face_node';
end



 

figure;
subplot(1,2,1);
trisurf(ftarget,target(:,1),target(:,2),target(:,3),'facecolor','y','Edgecolor','none');
hold on
light
lighting phong;
set(gca,'DataAspectRatio',[1 1 1],'PlotBoxAspectRatio',[1 1 1]);
trisurf(fsource,source(:,1),source(:,2),source(:,3),'facecolor','g','Edgecolor','none');
 
[error,Reallignedsource,transform,Derr]=rigidICP(target,source);
 
subplot(1,2,2);
trisurf(ftarget,target(:,1),target(:,2),target(:,3),'facecolor','y','Edgecolor','none');
hold on
light
set(gca,'DataAspectRatio',[1 1 1],'PlotBoxAspectRatio',[1 1 1]);
trisurf(fsource,Reallignedsource(:,1),Reallignedsource(:,2),Reallignedsource(:,3),'facecolor','g','Edgecolor','none');
light
title('ICP算法配准结果');


figure;
plot(Derr,'b-o');
xlabel('迭代次数');
ylabel('迭代误差');
grid on
      

function [error,Reallignedsource]=ICPmanu_allign2(target,source)

[IDX1(:,1),IDX1(:,2)]=knnsearch(target,source);
[IDX2(:,1),IDX2(:,2)]=knnsearch(source,target);
IDX1(:,3)=1:length(source(:,1));
IDX2(:,3)=1:length(target(:,1));

SES = [1:0.05:2];
ERR = [];
for i = 1:length(SES)
    K = SES(i);

    m1=mean(IDX1(:,2));
    s1=std(IDX1(:,2));
    IDX1=IDX1(IDX1(:,2)<(m1+K*s1),:);

    m2=mean(IDX2(:,2));
    s2=std(IDX2(:,2));
    IDX2=IDX2(IDX2(:,2)<(m2+K*s2),:);

    Datasetsource=vertcat(source(IDX1(:,3),:),source(IDX2(:,1),:));
    Datasettarget=vertcat(target(IDX1(:,1),:),target(IDX2(:,3),:));

    [error,Reallignedsource,transform] = procrustes(Datasettarget,Datasetsource);
    ERR = [ERR,error];
    Reallignedsource=transform.b*source*transform.T+repmat(transform.c(1,1:3),size(source,1),1);
end

[V,I] = min(ERR);
Kbest = SES(I);
clear IDX1 IDX2 m1 s1 m2 s2 Datasetsource Datasettarget error Reallignedsource transform Reallignedsource

[IDX1(:,1),IDX1(:,2)]=knnsearch(target,source);
[IDX2(:,1),IDX2(:,2)]=knnsearch(source,target);
IDX1(:,3)=1:length(source(:,1));
IDX2(:,3)=1:length(target(:,1));
m1=mean(IDX1(:,2));
s1=std(IDX1(:,2));
IDX1=IDX1(IDX1(:,2)<(m1+Kbest*s1),:);

m2=mean(IDX2(:,2));
s2=std(IDX2(:,2));
IDX2=IDX2(IDX2(:,2)<(m2+Kbest*s2),:);

Datasetsource=vertcat(source(IDX1(:,3),:),source(IDX2(:,1),:));
Datasettarget=vertcat(target(IDX1(:,1),:),target(IDX2(:,3),:));

[error,Reallignedsource,transform] = procrustes(Datasettarget,Datasetsource);
 
Reallignedsource=transform.b*source*transform.T+repmat(transform.c(1,1:3),size(source,1),1);      

三、MATLAB仿真测试结果