上一篇博客中我們使用了四元數法計算點集配准。
本篇我們使用SVD計算點集配准。
下面是《視覺slam十四講》中的計算方法：
計算步驟如下：
我們看到，只要求出了兩組點之間的旋轉，平移是非常容易得到的，所以我們重點關注R的計算。展開關於R的誤差項，得：
注意到第一項和R無關，第二項由於R'R=I，亦與R無關。因此，實際上優化目標函數變為：
接下來，我們介紹怎樣通過SVD解出上述問題中最優的R，但關於最優性的證明較為復雜，感興趣的讀者請參考【50,51】，為了解R，先定義矩陣：
W是一個3*3的矩陣，對W進行SVD分解，得：
其中，
為奇異值組成的對角矩陣，對角線元素從大到小排列，而U和V為正交矩陣，當W滿秩時，R為：
解得R后，按式7.53求解t即可。
具體證明可以參考：
代碼如下：
clear all; close all; clc; %生成原始點集 X=[];Y=[];Z=[]; for i=-180:2:180 for j=-90:2:90 x = i * pi / 180.0; y = j * pi / 180.0; X =[X,cos(y) * cos(x)]; Y =[Y,sin(y) * cos(x)]; Z =[Z,sin(x)]; end end P=[X(1:3000)' Y(1:3000)' Z(1:3000)']; %生成變換后點集 i=0.5;j=0.3;k=0.7; Rx=[1 0 0;0 cos(i) -sin(i); 0 sin(i) cos(i)]; Ry=[cos(j) 0 sin(j);0 1 0;-sin(j) 0 cos(j)]; Rz=[cos(k) -sin(k) 0;sin(k) cos(k) 0;0 0 1]; R=Rx*Ry*Rz; X=P*R + [0.2,0.3,0.4]; plot3(P(:,1),P(:,2),P(:,3),'b.'); hold on; plot3(X(:,1),X(:,2),X(:,3),'r.'); %計算點集均值 up = mean(P); ux = mean(X); P1=P-up; X1=X-ux; %計算點集協方差 sigma=P1'*X1/(length(X1)); [u s v] = svd(sigma); RR=u*v'; %計算平移向量 qr=ux-up*RR; %驗證旋轉矩陣與平移向量正確性 Pre = P*RR+qr; figure; plot3(P(:,1),P(:,2),P(:,3),'b.'); hold on; plot3(X(:,1),X(:,2),X(:,3),'r.'); plot3(Pre(:,1),Pre(:,2),Pre(:,3),'go');
處理效果和四元數法一致：
原始點集：
其中藍點為原始點集，紅點為旋轉平移后的點集。
配准后點集：
計算得到的旋轉平移矩陣，通過對藍點集進行轉換得到綠點集，比較紅點集與綠點集是否基本一致。