matlab練習程序（NDT）

NDT全稱Normal Distributions Transform（正態分布變換），用來計算不同點雲之間的旋轉平移關系，和ICP功能類似，並且該算法能夠寫出多線程版本，因此速度可以比較快。

算法步驟如下，以二維點雲為例：

1. 比如我們有兩組點雲A和B，A是固定點雲，我們要把B轉換到和A對齊，就要計算出B到A的旋轉矩陣R和平移矩陣T，對應的就是三個參數（x,y,theta）。

2. 首先對A進行柵格化，計算每個柵格中的點雲的均值和方差，記為和u和∑。

3. 設定損失函數，其中x為待匹配點雲（就是上面的B點雲），n為x點雲總個數，損失函數記為：

4. 要計算損失函數S達到最小時的x,y和theta，用牛頓迭代求解。

5. 計算S對x,y,theta的一階偏導，其中p就代表（x,y,theta）：

6. 計算S對x,y,theta的二階偏導，即黑塞矩陣：

7. 設定迭代次數或者迭代閾值，計算delta=-inv(H)*g，得到迭代步長。

8. 更新參數p = p+delta，最后達到設定閾值或迭代次數即可。

matlab代碼如下：

clear all;close all;clc;

%生成原始點集
X=[];Y=[];
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)];
    end
end
P=[X(1:3000)' Y(1:3000)'];

%生成變換后點集
theta=0.5;
R=[cos(theta) -sin(theta);sin(theta) cos(theta)];
X=(R*P')' + [2.4,3.5];

plot(P(:,1),P(:,2),'b.');
hold on;
plot(X(:,1),X(:,2),'r.');

meanP = mean(P);
meanX = mean(X);

P = P - meanP;          %統一起始點，否則兩組點雲間可能沒有交集，算法會失效
X = X - meanX; 

minx = min(X(:,1));
miny = min(X(:,2));
maxx = max(X(:,1));
maxy = max(X(:,2));

cellsize = 0.3;         %網格大小

M = floor((maxx - minx)/cellsize+1);
N = floor((maxy - miny)/cellsize+1);

grid = cell(M,N);
meanGrid = zeros(2,M,N);
convGrid = zeros(2,2,M,N);

for i = 1:length(X)             %划分網格並填入數據
    m = floor((X(i,1) - minx)/cellsize + 1);
    n = floor((X(i,2) - miny)/cellsize + 1);
    grid{m,n} = [grid{m,n};X(i,:)];
end

%計算每個網格中的均值和方差
for i=1:M
    for j=1:N
        if(size(grid{i,j},1)>=2)
            meanGrid(:,i,j) = mean(grid{i,j});
            convGrid(:,:,i,j) = cov(grid{i,j});
        end
    end
end

pre =zeros(3,1);
for i=1:40          %迭代40次
    g = zeros(3,1);
    H = zeros(3,3);
    
    tx = pre(1);
    ty = pre(2);
    theta = pre(3);
    for j=1:length(P)
        x = P(j,1);
        y = P(j,2);
        
        p_trans = [x*cos(theta)-y*sin(theta)+tx;x*sin(theta)+y*cos(theta)+ty];
        
        m = floor((p_trans(1) - minx)/cellsize + 1);
        n = floor((p_trans(2) - miny)/cellsize + 1);
        
        if m>=1 && n>=1 && m<=M && n<=N         %只計算投影到網格中的點雲數據
            if (size(grid{m,n},1)>=2)
                
                q = meanGrid(:,m,n);
                sigma = convGrid(:,:,m,n);
                
                if(cond(sigma)>50)              %根據矩陣條件數判斷是否是病態矩陣
                    continue;
                end
                invsigma = inv(sigma);
                
                xk = p_trans - q;
                
                dx = [1;0];
                dy = [0;1];
                dt = [-x*sin(theta)-y*cos(theta);x*cos(theta)-y*sin(theta)];
                ddt = [-x*cos(theta)+y*sin(theta);-x*sin(theta)-y*cos(theta)];
                
                g(1) = g(1) + (xk'*invsigma* dx *exp(-0.5*xk'*invsigma*xk));    %計算損失函數對x的偏導
                g(2) = g(2) + (xk'*invsigma* dy *exp(-0.5*xk'*invsigma*xk));    %計算損失函數對y的偏導
                g(3) = g(3) + (xk'*invsigma* dt *exp(-0.5*xk'*invsigma*xk));    %計算損失函數對theta的偏導
                
                %計算黑塞矩陣，分別計算損失函數對x,y,theta的二階偏導
                H(1,1) = H(1,1) + exp(-0.5*xk'*invsigma*xk)*(-(xk'*invsigma*dx)*(xk'*invsigma*dx)+(dx'*invsigma*dx));
                H(1,2) = H(1,2) + exp(-0.5*xk'*invsigma*xk)*(-(xk'*invsigma*dx)*(xk'*invsigma*dy)+(dx'*invsigma*dy));
                H(1,3) = H(1,3) + exp(-0.5*xk'*invsigma*xk)*(-(xk'*invsigma*dx)*(xk'*invsigma*dt)+(dx'*invsigma*dt));
                
                H(2,1) = H(2,1) + exp(-0.5*xk'*invsigma*xk)*(-(xk'*invsigma*dy)*(xk'*invsigma*dx)+(dy'*invsigma*dx));
                H(2,2) = H(2,2) + exp(-0.5*xk'*invsigma*xk)*(-(xk'*invsigma*dy)*(xk'*invsigma*dy)+(dy'*invsigma*dy));
                H(2,3) = H(2,3) + exp(-0.5*xk'*invsigma*xk)*(-(xk'*invsigma*dy)*(xk'*invsigma*dt)+(dy'*invsigma*dt));
                
                H(3,1) = H(3,1) + exp(-0.5*xk'*invsigma*xk)*(-(xk'*invsigma*dt)*(xk'*invsigma*dx)+(dt'*invsigma*dx));
                H(3,2) = H(3,2) + exp(-0.5*xk'*invsigma*xk)*(-(xk'*invsigma*dt)*(xk'*invsigma*dy)+(dt'*invsigma*dy));
                H(3,3) = H(3,3) + exp(-0.5*xk'*invsigma*xk)*(-(xk'*invsigma*dt)*(xk'*invsigma*dt)+(dt'*invsigma*dt) + xk'*invsigma*ddt);
                
            end
        end
    end
    
    %牛頓迭代求解
    delta = -H\g;
    pre = pre + delta;
end

pre
theta=pre(3);
R=[cos(theta) -sin(theta);sin(theta) cos(theta)];       %畫出變換后的點雲
XX=(R*P')' + [pre(1),pre(2)] + meanX;
plot(XX(:,1),XX(:,2),'g.');
axis equal;
legend('原始點雲','變換后點雲','配准點雲')

下面給一個用matlab自帶函數計算的例子：

clear all;close all;clc;

%生成原始點集
X=[];Y=[];
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)];
    end
end
P=[X(1:3000)' Y(1:3000)'];

%生成變換后點集
theta=-0.5;
R=[cos(theta) -sin(theta);sin(theta) cos(theta)];
X=P*R + [2.4,3.5];

plot(P(:,1),P(:,2),'b.');
hold on;
plot(X(:,1),X(:,2),'r.');

P = [P zeros(length(P),1)];
X = [X zeros(length(X),1)];

moving = pointCloud(P);
fixed = pointCloud(X);

gridStep = 0.3;
tform = pcregisterndt(moving,fixed,gridStep);

R = tform.Rotation;
T = tform.Translation;

XX=P*R + T;
plot(XX(:,1),XX(:,2),'g.');
axis equal
legend('原始點雲','變換后點雲','配准點雲')

結果如下：