四元法
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中一个点的ｘ轴坐标
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
参考博客１：https://zhuanlan.zhihu.com/p/35893884
参考博客２：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