ICP点云配准算法,基于Python实现ICP点云配准算法,基于Python实现

  • 虫虫123456
    了解作者
  • matlab
    开发工具
  • 9.7MB
    文件大小
  • rar
    文件格式
  • 0
    收藏次数
  • 5 积分
    下载积分
  • 1
    下载次数
  • 2022-07-19 12:24
    上传日期
ICP点云配准算法,基于Python实现ICP点云配准算法,基于Python实现
ICP.rar
  • ICP
  • .idea
  • inspectionProfiles
  • Project_Default.xml
    571B
  • misc.xml
    225B
  • workspace.xml
    15.7KB
  • ICP.iml
    398B
  • modules.xml
    258B
  • __pycache__
  • dataio.cpython-36.pyc
    750B
  • myICP.cpython-36.pyc
    2.6KB
  • 1_calib.asc
    8.2MB
  • main.py
    255B
  • 2_icp.asc
    8.2MB
  • myICP.py
    2.5KB
  • 2_calib.asc
    6.4MB
  • dataio.py
    477B
内容介绍
import numpy as np import random import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D 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
评论
    相关推荐