%?程序說明：輸入data_source和data_target兩個點云，找尋將data_source映射到data_targe的旋轉(zhuǎn)和平移參數(shù)
clear;
close?all;
clc;
%%?參數(shù)配置
kd?=?1;
inlier_ratio?=?0.9;
Tolerance?=?0.001;
step_Tolerance?=?0.0001;
max_iteration?=?200;
show?=?1;

%%?生成數(shù)據(jù)
data_source=load（‘satellite.txt‘）;
data_source=data_source‘;

theta_x?=?50;??%x旋轉(zhuǎn)角度
theta_y?=?30;??%y旋轉(zhuǎn)角度
theta_z?=?20;??%z旋轉(zhuǎn)角度

t=[0-100200];???%平移向量

[data_targetT0]=rotate（data_sourcetheta_xtheta_ytheta_zt）;

%?只取其中一部分點，打亂點的順序，添加噪聲，添加離群點
data_source?=?data_source（:1:300）;
data_source?=?data_source（:randperm（size（data_source2）））;
data_source?=?data_source?+?（rand（size（data_source））-0.5）*10;
data_source（:end+1）?=?[1000;2000;500];

%%?繪制原始點與旋轉(zhuǎn)后的點圖像
figure;
scatter3（data_source（1:）data_source（2:）data_source（3:）‘b.‘）;
hold?on;
scatter3（data_target（1:）data_target（2:）data_target（3:）‘r.‘）;
hold?off;
daspect（[1?1?1]）;

%%?開始ICP
T_final=eye（44）;???%旋轉(zhuǎn)矩陣初始值
iteration=0;
Rf=T_final（1:31:3）;
Tf=T_final（1:34）;
data_source=Rf*data_source+Tf*ones（1size（data_source2））;????%初次更新點集（代表粗配準(zhǔn)結(jié)果）
err=1;
data_source_old?=?data_source;
%%?迭代優(yōu)化
while（1）
????iteration=iteration+1;
????if?kd?==?1
????????%利用Kd-tree找出對應(yīng)點集
????????kd_tree?=?KDTreeSearcher（data_target‘‘BucketSize‘10）;
????????[index?dist]?=?knnsearch（kd_tree?data_source‘）;
????else
????????%利用歐式距離找出對應(yīng)點集
????????k=size（data_source2）;
????????for?i?=?1:k
????????????data_q1（1:）?=?data_target（1:）?-?data_source（1i）;????%?兩個點集中的點x坐標(biāo)之差
????????????data_q1（2:）?=?data_target（2:）?-?data_source（2i）;????%?兩個點集中的點y坐標(biāo)之差
????????????data_q1（3:）?=?data_target（3:）?-?data_source（3i）;????%?兩個點集中的點z坐標(biāo)之差
????????????distance?=?sqrt（data_q1（1:）.^2?+?data_q1（2:）.^2?+?data_q1（3:）.^2）;??%?歐氏距離
????????????[dist（i）?index（i）]?=?min（distance）;???%?找到距離最小的那個點
????????end
????end
????
????disp（[‘誤差err=‘num2str（mean（dist））]）;
????disp（[‘迭代次數(shù)ieration=‘num2str（iteration）]）;
????err_rec（iteration）?=?mean（dist）;
????
????%?按距離排序，只取前面占比為inlierratio內(nèi)的點以應(yīng)對外點
????[~?idx]?=?sort（dist）;
????inlier_num?=?round（size（data_source2）*inlier_ratio）;
????idx?=?idx（1:inlier_num）;
????data_source_temp?=?data_source（:idx）;
????dist?=?dist（idx）;
????index?=?index（idx）;
????data_mid?=?data_target（:index）;
????
????%?去中心化后SVD分解求解旋轉(zhuǎn)矩陣與平移向量
????[R_new?t_new]?=?rigidTransform3D（data_source_temp‘?data_mid‘）;
????
????%?計算累計的旋轉(zhuǎn)矩陣與平移向量
????Rf?=?R_new?*?Rf;
????Tf?=?R_new?*?Tf?+?t_new;
????
????%?????更新點集
????%?????data_source=R_new*data_source+t_new*ones（1size（data_source2））;
????data_source=Rf*data_source_old+Tf*ones（1size（data_source_old2））;
????
????%?顯示中間結(jié)果
????if?show?==?1
????????h?=?figure（2）;
????????scatter3（data_source（1:）data_source（2:）data_source（3:）‘b.‘）;
????????hold?on;
????????scatter3（data_target（1:）data_target（2:）data_target（3:）‘r.‘）;
????????hold?off;
????????daspect（[1?1?1]）;
????????pause（0.1）;
????????drawnow
????end
????
????if?err?????????disp（‘———————

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件???????36652??2020-07-21?10:37??satellite.txt
?????文件????????6073??2020-07-22?17:18??my_ICP_final.m