基于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仿真測試結果