Open3D 四元数法实现对应点集配准

目录

一、概述

1.1 基本思想

​1.2详细步骤

二、代码实现

三、实现效果

3.1原始点云

3.2配准后点云

3.3变换矩阵

---

一、概述

        在点云配准中，四元数法是一种精确计算旋转和平移变换的方法。其目标是找到一个刚体变换，使源点云和目标点云的对应点集之间的误差最小化。四元数法通过利用四元数的性质来优化旋转矩阵的计算，避免了传统方法中的奇异性问题。

1.1 基本思想

1.2详细步骤

二、代码实现

import open3d as o3d
import numpy as np


def solve_transform_quaternion(s, t):
    P = np.asarray(s.points)
    Q = np.asarray(t.points)
    # 判断两个点集中点的个数是否一致
    if P.shape[0] != Q.shape[0]:
        raise Exception("两个点集不匹配")
    else:
        n = P.shape[0]
    # 1、分别求质心
    meanP = np.mean(P, axis=0)
    meanQ = np.mean(Q, axis=0)
    # 2、去质心
    P_ = P - meanP
    Q_ = Q - meanQ
    # 3、构建协方差矩阵D
    D = np.dot(P_.T, Q_)/n
    # 4、计算4 X 4 矩阵中所需元素
    Dt = np.transpose(D)  # D的转置
    tr_D = np.trace(D)    # D的迹
    A_ij = D - Dt
    v_A = np.array([[A_ij[1][2], A_ij[2][0], A_ij[0][1]]])
    M = D + Dt - tr_D * np.eye(3)
    # 5、构建 4 X 4 矩阵
    Ql = np.vstack((np.array([[tr_D]]), np.transpose(v_A)))
    Qr = np.vstack((v_A, M))
    Q = np.hstack((Ql, Qr))
    # 6、求 4 X4 矩阵的特征值与特征向量
    [eig_v, eig_u] = np.linalg.eig(Q)
    i = np.argmax(eig_v)  # 最大特征值的位置
    q = eig_u[:, i]       # 最大特征值对应的特征向量
    # 7、由四元数求旋转矩阵
    R = o3d.geometry.get_rotation_matrix_from_quaternion(q)
    t = meanQ - np.dot(R, meanP)      # 计算平移向量
    # 8、构建欧式变换矩阵
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = t
    T[3, 3] = 1.0
    return T


# ---------------加载点云数据--------------------
source = o3d.io.read_point_cloud("Horse.pcd")
target = o3d.io.read_point_cloud("Horse_trans.pcd")
# ------对初始位置的点云进行颜色渲染--------------
source.paint_uniform_color([0, 1, 0])  # 绿色
target.paint_uniform_color([0, 0, 1])  # 蓝色
o3d.visualization.draw_geometries([source, target], width=800, height=800)

Tran = solve_transform_quaternion(source, target)
print('变换矩阵为:\n', Tran)
align = source.transform(Tran)
align.paint_uniform_color([1, 0, 0])  # 红色
o3d.visualization.draw_geometries([align, target], width=800, height=800)

三、实现效果

3.1原始点云

3.2配准后点云

3.3变换矩阵

 [[ 1.00000000e+00  1.90151073e-09  1.03664745e-09  1.00000000e-01]
 [-6.11550683e-10  7.07106781e-01 -7.07106781e-01  5.00000000e-02]
 [-2.07759157e-09  7.07106781e-01  7.07106781e-01  1.00000000e-02]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]