标签：pp 基本上 shape error np array ICP data 解法
四元法
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
标签：pp,基本上,shape,error,np,array,ICP,data,解法 来源： https://www.cnblogs.com/qiuyeruolan/p/15110407.html
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。