（源代码）CloudComPy+open3d实现点云配准项目（手动选点配准+ICP点云配准+误差显示）

目录

CloudComPy介绍

---

CloudComPy介绍

简单来说，CloudComPy通过Python接口，调用点云操作软件CloudCompare的一些功能。

官方文档：CloudComPy官方文档https://www.simulation.openfields.fr/documentation/CloudComPy/html/

环境配置

必须在pt310及以上版本使用，最好部署在anaconda环境下。具体可以参考这篇CSDN博客：windows平台cloudcompy安装https://blog.csdn.net/sljsxy/article/details/146494374

注意win7是不支持pt310版本的，如果非要使用，网上有适用于win7版本的pt310包，至于环境和包会不会起冲突则未加实验过。

项目代码

项目包含了测试样例生成、手动选择三点作为粗配准、ICP细配准，以及配准可视化的模块，每个模块都可以单独使用。

环境配置与依赖检查

基于上一章环境配置所述环境，另外安装适合版本的open3d即可。如果需要图形化界面，则需要安装PyQt5（后续会更新图形化界面版本的软件）。

import open3d as o3d
import numpy as np
import copy
import os
import sys

# ================== 环境配置与依赖检查 ==================
SCIPY_AVAILABLE = False
CLOUDCOMPY_AVAILABLE = False

try:
    from scipy.spatial import cKDTree

    SCIPY_AVAILABLE = True
except ImportError:
    pass

try:
    import cloudComPy as cc

    CLOUDCOMPY_AVAILABLE = True
except ImportError:
    pass

基础工具

# ================== 基础工具 ==================

def load_ply(file_path):
    """读取点云，支持PLY/Mesh自动转换"""
    if not os.path.exists(file_path):
        print(f"[错误] 文件不存在: {file_path}")
        return None

    pcd = o3d.io.read_point_cloud(file_path)
    if len(pcd.points) == 0:
        mesh = o3d.io.read_triangle_mesh(file_path)
        pcd = o3d.geometry.PointCloud()
        pcd.points = mesh.vertices
        if mesh.has_vertex_normals():
            pcd.normals = mesh.vertex_normals
    pcd.remove_non_finite_points()
    return pcd


def save_ply(pcd, file_path):
    o3d.io.write_point_cloud(file_path, pcd, write_ascii=True)


def generate_random_transform():
    """生成随机变换矩阵及其逆矩阵(GT)"""
    angle = 0.25 * np.pi
    r_xyz = np.random.uniform(-angle, angle, 3)
    t_xyz = np.random.uniform(-100, 100, 3)

    R = o3d.geometry.get_rotation_matrix_from_xyz(r_xyz)
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = t_xyz
    return T, np.linalg.inv(T)  # 返回: 扰动矩阵, 还原矩阵(GT)


def get_downsampled_pcd(pcd, target_count=10000):
    """自适应降采样"""
    if len(pcd.points) <= target_count:
        return copy.deepcopy(pcd)

    bbox = pcd.get_axis_aligned_bounding_box()
    voxel_size = max(bbox.get_extent()) / 50.0
    pcd_down = pcd.voxel_down_sample(voxel_size)

    # 简单迭代确保点数适中
    while len(pcd_down.points) > target_count * 1.5:
        voxel_size *= 1.5
        pcd_down = pcd.voxel_down_sample(voxel_size)
    return pcd_down

矩阵误差分析

# ================== 矩阵误差分析 ==================

def calculate_matrix_error(T_est, T_gt):
    """
    比较估计矩阵与参考矩阵的平移和旋转误差
    :param T_est: 算法计算出的最终变换矩阵 (4x4)
    :param T_gt:  真值矩阵 (Ground Truth) (4x4)
    """
    print("\n" + "=" * 20 + " 矩阵精度分析 " + "=" * 20)

    # 1. 平移误差 (欧氏距离)
    t_est = T_est[:3, 3]
    t_gt = T_gt[:3, 3]
    trans_error = np.linalg.norm(t_est - t_gt)

    # 2. 旋转误差 (Geodesic distance on SO(3))
    R_est = T_est[:3, :3]
    R_gt = T_gt[:3, :3]

    # R_diff = R_est * R_gt^T
    R_diff = np.dot(R_est, R_gt.T)
    trace = np.trace(R_diff)

    # 限制数值范围在 [-1, 1] 避免数值噪音导致的 NaN
    val = (trace - 1) / 2.0
    val = np.clip(val, -1.0, 1.0)

    rot_error_rad = np.arccos(val)
    rot_error_deg = np.degrees(rot_error_rad)

    print(f"平移误差 (Translation Error): {trans_error:.6f}")
    print(f"旋转误差 (Rotation Error):    {rot_error_deg:.6f} 度")

    print("\n--- 矩阵详情 ---")
    print("Estimated Matrix:\n", T_est)
    print("Ground Truth Matrix:\n", T_gt)
    print("Diff (Est - GT):\n", T_est - T_gt)

    return trans_error, rot_error_deg

配准流程

# ================== 配准流程 ==================

def manual_registration(source, target):
    """手动粗配准：左右分屏选点"""
    print("\n>>> 开始手动粗配准 (请在两窗口选至少3个对应点，Shift+左键)")
    src_vis = get_downsampled_pcd(source)
    tgt_vis = get_downsampled_pcd(target)

    src_vis.paint_uniform_color([1, 0.706, 0])  # 橙 (Source)
    tgt_vis.paint_uniform_color([0, 0.651, 0.929])  # 蓝 (Target)

    # 偏移显示 Source
    tgt_bbox = tgt_vis.get_axis_aligned_bounding_box()
    offset = tgt_bbox.get_extent()[0] * 1.5 + 50.0
    shift_vec = (tgt_bbox.get_center() - src_vis.get_center()) + [offset, 0, 0]
    src_vis.translate(shift_vec)

    vis = o3d.visualization.VisualizerWithEditing()
    vis.create_window(window_name='选点: 左目标(蓝) - 右源(橙)', width=1280, height=720)
    vis.add_geometry(src_vis + tgt_vis)
    vis.run()
    vis.destroy_window()

    indices = vis.get_picked_points()
    pts_s, pts_t = [], []
    points_all = np.asarray((src_vis + tgt_vis).points)
    len_tgt = len(tgt_vis.points)

    for idx in indices:
        p = points_all[idx]
        if idx < len_tgt:
            pts_t.append(p)
        else:
            pts_s.append(p - shift_vec)  # 还原坐标

    min_k = min(len(pts_s), len(pts_t))
    if min_k < 3:
        print("[警告] 选点不足3对，跳过粗配准。")
        return np.eye(4)

    print(f"使用 {min_k} 对点进行计算...")
    pcd_s = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(pts_s[:min_k]))
    pcd_t = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(pts_t[:min_k]))
    corr = o3d.utility.Vector2iVector([[i, i] for i in range(min_k)])

    reg = o3d.pipelines.registration.TransformationEstimationPointToPoint()
    return reg.compute_transformation(pcd_s, pcd_t, corr)


def run_icp(source_path, target_path):
    """调用 CloudComPy 进行 ICP"""
    if not CLOUDCOMPY_AVAILABLE:
        print("[跳过] 未检测到 cloudComPy，仅使用粗配准结果。")
        return np.eye(4)

    print("\n>>> 执行 CloudComPy ICP 精配准...")
    try:
        cc_src = cc.loadPointCloud(source_path)
        cc_tgt = cc.loadPointCloud(target_path)
        # 参数: overlap=0.8, threads=auto, etc.
        res = cc.ICP(cc_src, cc_tgt, 1.0e-6, 50, 50000, True,
                     cc.CONVERGENCE_TYPE.MAX_ERROR_CONVERGENCE, False, 0.8)

        mat = []
        for line in res.transMat.toString().strip().split('\n'):
            if line.strip():
                mat.append([float(x) for x in line.split()])
        return np.array(mat)
    except Exception as e:
        print(f"[ICP 错误] {e}")
        return np.eye(4)

结果可视化

# ================== 结果可视化 (基于 Target 的色谱) ==================

def visualize_heatmap_on_target(source, target, limit=5.0):
    print("\n>>> 生成误差色谱 (基准: 目标点云)")

    if not target.has_normals():
        target.estimate_normals()
        target.orient_normals_consistent_tangent_plane(50)

    src_pts = np.asarray(source.points)
    tgt_pts = np.asarray(target.points)
    tgt_nrm = np.asarray(target.normals)

    dists, signed_dists = np.zeros(len(tgt_pts)), np.zeros(len(tgt_pts))

    if SCIPY_AVAILABLE:
        tree = cKDTree(src_pts)
        dists, idxs = tree.query(tgt_pts, k=1, workers=-1)
        nearest_pts = src_pts[idxs]
        diff_vec = nearest_pts - tgt_pts
        # 投影距离：(源-目标) 点乘 目标法线
        signed_dists = np.einsum('ij,ij->i', diff_vec, tgt_nrm)
    else:
        print("未安装 Scipy，仅显示绝对距离。")
        dists = np.asarray(target.compute_point_cloud_distance(source))
        signed_dists = dists

    # 渲染颜色: 红(+), 蓝(-), 绿(0)
    colors = np.zeros((len(tgt_pts), 3))
    colors[:, 1] = 1.0  # 默认绿

    # 正偏差 (Source 在 Target 法线方向外侧) -> 红
    mask_pos = signed_dists > 0
    if np.any(mask_pos):
        norm_val = np.clip(signed_dists[mask_pos] / limit, 0, 1)
        colors[mask_pos, 0] = norm_val
        colors[mask_pos, 1] = 1.0 - norm_val
        colors[mask_pos, 2] = 0

    # 负偏差 (Source 在 Target 法线方向内侧) -> 蓝
    mask_neg = signed_dists < 0
    if np.any(mask_neg):
        norm_val = np.clip(np.abs(signed_dists[mask_neg]) / limit, 0, 1)
        colors[mask_neg, 0] = 0
        colors[mask_neg, 1] = 1.0 - norm_val
        colors[mask_neg, 2] = norm_val

    vis_pcd = copy.deepcopy(target)
    vis_pcd.colors = o3d.utility.Vector3dVector(colors)

    stat_msg = f"Abs Mean: {np.mean(np.abs(signed_dists)):.4f}, Max: {np.max(signed_dists):.4f}"
    print(stat_msg)

    o3d.visualization.draw_geometries([vis_pcd],
                                      window_name=f"偏差分析 (阈值 {limit})",
                                      width=1024, height=768, left=50, top=50)

主程序

# ================== 主程序 ==================

def main():
    # 路径配置
    work_dir = r""
    target_ply = os.path.join(work_dir, ".ply")  # 目标 (Standard)
    source_ply = os.path.join(work_dir, ".ply")  # 源 (待配准)
    out_dir = os.path.join(work_dir, "results")

    os.makedirs(out_dir, exist_ok=True)

    # 1. 准备数据
    print(">>> 初始化数据...")
    pcd_raw_source = load_ply(source_ply)
    pcd_target = load_ply(target_ply)

    if not pcd_raw_source or not pcd_target:
        return

    # 生成测试扰动: pcd_disturbed = T_random * pcd_raw
    # 我们的目标是找到变换矩阵将 pcd_disturbed 变回 pcd_raw (即与 pcd_target 对齐)
    # 真实矩阵(Ground Truth)应为 T_random 的逆矩阵
    T_rand, T_gt = generate_random_transform()

    pcd_disturbed = copy.deepcopy(pcd_raw_source)
    pcd_disturbed.transform(T_rand)

    temp_dist_path = os.path.join(out_dir, "disturbed_input.ply")
    save_ply(pcd_disturbed, temp_dist_path)

    # 2. 粗配准
    T_coarse = manual_registration(pcd_disturbed, pcd_target)

    # 保存粗配准中间结果供ICP读取
    pcd_coarse = copy.deepcopy(pcd_disturbed)
    pcd_coarse.transform(T_coarse)
    path_coarse = os.path.join(out_dir, "temp_coarse.ply")
    save_ply(pcd_coarse, path_coarse)

    # 3. 精配准
    T_icp = run_icp(path_coarse, target_ply)

    # 4. 计算最终变换矩阵: T_final = T_icp * T_coarse
    T_final = np.dot(T_icp, T_coarse)

    # 5. 误差分析 (核心需求)
    calculate_matrix_error(T_final, T_gt)

    # 6. 结果可视化
    pcd_final = copy.deepcopy(pcd_disturbed)
    pcd_final.transform(T_final)

    # 几何叠加
    pcd_final.paint_uniform_color([0, 1, 0])
    temp_target = copy.deepcopy(pcd_target)
    temp_target.paint_uniform_color([0, 0, 1])
    o3d.visualization.draw_geometries([pcd_final, temp_target], window_name="最终对齐效果")

    # 色谱分析
    visualize_heatmap_on_target(pcd_final, pcd_target, limit=2.0)


if __name__ == "__main__":
    main()

可能的报错与解决方案

使用PyInstaller打包该项目，报错：找不到cloudComPy

参考我自己写的解决方案

CloudComPy使用PyInstaller打包后报错解决方案https://blog.csdn.net/CCCP_lus/article/details/149609890