ICP 基本上有兩種解法
阿新 • • 發佈:2021-08-06
四元法
function ret = normal_gravity( data ) % 資料在重心上正則化 [m, n] = size(data); data_mean = mean(data);%座標在x,y,z上的平均值 ret = data - ones(m, 1) * data_mean; end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [data_g, data_p] = icp_process_xgtu(data_g, data_p) [ data_g, data_p, error, data_pp, R ] = icp_process(data_g, data_p); log_info(strcat('點雲之間當前差值:', num2str(error))); log_info('當前變換矩陣:'); disp(R); cnt = 1; last_error = 0; last_R = R; % 進行迭代處理點雲 while abs(error - last_error) > 0.01 cnt = cnt + 1; last_error = error; last_R = R; [data_g, data_p, error, data_pp, R] = icp_process(data_g, data_p); R = last_R * R; log_info(strcat('當前迴圈次數', num2str(cnt), '點雲之間的差值', num2str(error))); log_info('當前變換矩陣:'); disp(R); end end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function [ data_g, data_p, err, data_pp, R ] = icp_process( data_g, data_p ) [k1, n] = size(data_g); [k2, m] = size(data_p); data_p1 = zeros(k2, 3); % 用來儲存兩個點集之間的臨時誤差 data_pp = zeros(k1, 3); % 用來儲存data_g在data_p中對應的最小點 distance = zeros(k1, 1); % data_g(i)與data_p中的距離 error = zeros(k1, 1); % 用來儲存data_g的臨時最小誤差 % 座標資料重心正則化 data_g = normal_gravity(data_g); data_p = normal_gravity(data_p); % 求對應的最近點 for i = 1:k1 data_p1(:, 1) = data_p(:, 1) - data_g(i, 1); % data_p所有點的x座標都減去data_g中一個點的x軸座標 data_p1(:, 2) = data_p(:, 2) - data_g(i, 2); % data_p所有點的y座標都減去data_g中一個點的y軸座標 data_p1(:, 3) = data_p(:, 3) - data_g(i, 3); % data_p所有點的z座標都減去data_g中一個點的z軸座標 distance = data_p1(:, 1).^2 + data_p1(:, 2).^2 + data_p1(:, 3).^2; % data_p與data_g(i)點的距離 [min_dis, min_index] = min(distance); % data_p與data_g(i)點的距離最小的點 data_pp(i, :) = data_p(min_index, :); % data_pp中儲存data_g的對應最小點 error(i) = min_dis; % 儲存對應的誤差 end % 四元數法求解 V = (data_g' * data_pp) ./ k1; % 對稱矩陣Q用於求解四元 matrix_Q = [V(1,1)+V(2,2)+V(3,3), V(2,3)-V(3,2), V(3,1)-V(1,3), V(1,2)-V(2,1); V(2,3)-V(3,2), V(1,1)-V(2,2)-V(3,3), V(1,2)+V(2,1), V(1,3)+V(3,1); V(3,1)-V(1,3), V(1,2)+V(2,1), V(2,2)-V(1,1)-V(3,3), V(2,3)+V(3,2); V(1,2)-V(2,1), V(1,3)+V(3,1), V(2,3)+V(3,2), V(3,3)-V(1,1)-V(2,2)]; [V2, D2] = eig(matrix_Q); % 返回特徵的對角矩陣D2和矩陣v2 lambdas = [D2(1, 1), D2(2, 2), D2(3, 3), D2(4, 4)]; % 特徵值 [lambda, ind] = max(lambdas); % 最大的特徵值以及索引 Q = V2(:, ind); % Q所對應的值便是四元 % 變換矩陣 R=[Q(1,1)^2+Q(2,1)^2-Q(3,1)^2-Q(4,1)^2, 2*(Q(2,1)*Q(3,1)-Q(1,1)*Q(4,1)), 2*(Q(2,1)*Q(4,1)+Q(1,1)*Q(3,1)); 2*(Q(2,1)*Q(3,1)+Q(1,1)*Q(4,1)), Q(1,1)^2-Q(2,1)^2+Q(3,1)^2-Q(4,1)^2, 2*(Q(3,1)*Q(4,1)-Q(1,1)*Q(2,1)); 2*(Q(2,1)*Q(4,1)-Q(1,1)*Q(3,1)), 2*(Q(3,1)*Q(4,1)+Q(1,1)*Q(2,1)), Q(1,1)^2-Q(2,1)^2-Q(3,1)^2-Q(4,1)^2; ]; % 更新data_p,其實這應該也是對應論文中平移矩陣的過程,至於是否等價我覺得應該是不等價的,在不同情況下效果應該不同 data_p = data_p * R; data_pp = data_pp * R; data_p = normal_gravity(data_p); data_pp = normal_gravity(data_pp); err = mean(error); end % 參考文獻:A method for registration of 3-D shapes % 程式碼來源:[https://github.com/XgTu/2DASL](https://github.com/XgTu/2DASL),我只是添加了下注釋啦
SVD
參考部落格1:https://zhuanlan.zhihu.com/p/35893884
參考部落格2:https://blog.csdn.net/jsgaobiao/article/details/78873718
import numpy as np import random import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D # https://zhuanlan.zhihu.com/p/35893884 def nearest_point(P, Q): P = np.array(P) Q = np.array(Q) dis = np.zeros(P.shape[0]) index = np.zeros(Q.shape[0], dtype = np.int) for i in range(P.shape[0]): minDis = np.inf for j in range(Q.shape[0]): tmp = np.linalg.norm(P[i] - Q[j], ord = 2) if minDis > tmp: minDis = tmp index[i] = j dis[i] = minDis return dis, index def find_optimal_transform(P, Q): meanP = np.mean(P, axis = 0) meanQ = np.mean(Q, axis = 0) P_ = P - meanP Q_ = Q - meanQ W = np.dot(Q_.T, P_) U, S, VT = np.linalg.svd(W) R = np.dot(U, VT) if np.linalg.det(R) < 0: R[2, :] *= -1 T = meanQ.T - np.dot(R, meanP.T) return R, T def icp(src, dst, maxIteration=50, tolerance=0.001, controlPoints=100): A = np.array(src) B = np.array(dst) lastErr = 0 if (A.shape[0] != B.shape[0]): length = min(A.shape[0], B.shape[0]) length = min(length, controlPoints) sampleA = random.sample(range(A.shape[0]), length) sampleB = random.sample(range(B.shape[0]), length) P = np.array([A[i] for i in sampleA]) Q = np.array([B[i] for i in sampleB]) else: length = A.shape[0] if (length > controlPoints): sampleA = random.sample(range(A.shape[0]), length) sampleB = random.sample(range(B.shape[0]), length) P = np.array([A[i] for i in sampleA]) Q = np.array([B[i] for i in sampleB]) else : P = A Q = B for i in range(maxIteration): print("Iteration : " + str(i) + " with Err : " + str(lastErr)) dis, index = nearest_point(P, Q) R, T = find_optimal_transform(P, Q[index,:]) A = np.dot(R, A.T).T + np.array([T for j in range(A.shape[0])]) P = np.dot(R, P.T).T + np.array([T for j in range(P.shape[0])]) meanErr = np.sum(dis) / dis.shape[0] if abs(lastErr - meanErr) < tolerance: break lastErr = meanErr # visualization # ax = plt.subplot(1, 1, 1, projection='3d') # ax.scatter(P[:, 0], P[:, 1], P[:, 2], c='r') # ax.scatter(Q[:, 0], Q[:, 1], Q[:, 2], c='g') # plt.show(block = False) R, T = find_optimal_transform(A, np.array(src)) return R, T, A