ICP 基本上有两种解法

１．四元法

image.png

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