目录

一、概述

1.1基本思想

1.2实现步骤

二、代码实现

三、实现效果

3.1原始点云

3.2配准后点云

3.3变换矩阵

---

一、概述

        在点云配准中，SVD（Singular Value Decomposition，奇异值分解）方法是一种常用的精确计算旋转和平移变换的算法。其目标是找到一个刚体变换，使源点云和目标点云的对应点集之间的误差最小化。

1.1基本思想

1.2实现步骤

二、代码实现

import open3d as o3d
import numpy as np


def solve_transform_svd(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]
    # 2、SVD法求变换矩阵
    meanP = np.mean(P, axis=0)
    meanQ = np.mean(Q, axis=0)
    P_ = P - meanP
    Q_ = Q - meanQ
    H = np.dot(Q_.T, P_)/n
    U, S, V = np.linalg.svd(H)
    R = np.dot(U, V)
    if np.linalg.det(R) < 0:
        R[2, :] *= -1
    t = meanQ.T - np.dot(R, meanP.T)
    # 3、构建欧式变换矩阵
    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_svd(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.90151125e-09  1.03664737e-09  1.00000000e-01]
 [-6.11550849e-10  7.07106781e-01 -7.07106781e-01  5.00000000e-02]
 [-2.07759181e-09  7.07106781e-01  7.07106781e-01  1.00000000e-02]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]