Open3d 大型场景点云关键点ICP算法，人工提点后的一个简单的ICP

Open3d 大型场景点云半自动关键点快速ICP算法，人工提点后的一个简单的ICP（无深度学习，半自动）

前言

大型点云场景，直接ICP会耗费很大的内存和时间，这里人工提取关键点，将关键点位移配准到关键点，即可得到旋转矩阵，然后作为大点云的旋转矩阵。

ICP算法有很多；Open3d自带的全局快速注册execute_fast_global_registration可以计算FPFH特征，快速对齐点云，这里不做介绍，当你已知N个点，和另外一个点云的N个点是完全一直的，采用点对点遍历距离来配准

---

一、思路

条件1：已有两个点云的对应关键点，而且最好完全一致
可以采用CC的点云提取或者o3d的点云提取功能，提取关键点

条件2：旋转关系已经确定，不确定请手动旋转对齐
# 计算旋转矩阵，使得源点云相对于目标点云旋转90度 rotation_matrix = np.array([[0, 1, 0], [-1, 0, 0], [0, 0, 1]]) source.rotate(rotation_matrix, center=target_center)

二、使用步骤

1.引入库

代码如下（示例）：

import copy
import open3d as o3d
import numpy as np
import  geomeas

2.核心代码

代码如下（示例）：

    # 获取源点云和目标点云的中心
    source_center = source.get_center()
    target_center = target.get_center()

    # 将源点云移动到目标点云的中心
    source.translate(target_center - source_center)
    source_model_pcd.translate(target_center - source_center)

---

3.完整代码

已经有两个点云的对应关键点——质心平移到质心——所有点遍历移动到其他点，计算所有点到最近的距离总和，选择最小的作为最终的精配准——手动旋转平移|——调用O3d的icp精配准

代码如下（示例）：

# 简单的ICP，遍历所有点距离最近点的平移后所有点和最近点的距离总和
def icp(source, target, num_iterations=100):
    min_translation = np.zeros(3)
    min_distance_sum = float('inf')

    # 建立kdTree，找到距离pt最近的target点pt2
    target_tree = o3d.geometry.KDTreeFlann(target)

    for iteration in range(num_iterations):
        distance_sum = 0.0

        # 迭代遍历每个点
        for idx, pt in enumerate(np.asarray(source.points)):
            # 寻找最近邻点
            [k, ind, _] = target_tree.search_knn_vector_3d(pt, 1)
            pt2 = np.asarray(target.points)[ind][0]

            # 拷贝一份source点云
            source_copy = copy.deepcopy(source)

            # 将拷贝点云里的点按照pt移动到pt2
            source_copy.points[idx] = pt2

            # 计算所有点距离最近点的距离总和
            distance_sum += np.linalg.norm(pt - pt2)

        # 更新最小距离总和和对应的平移
        if distance_sum < min_distance_sum:
            min_distance_sum = distance_sum
            min_translation = pt2 - pt

    # 将原始点云拷贝一份，按照保留最小距离总和对应的平移平移
    # result =copy.deepcopy(source)
    # result.translate(min_translation)

    return min_translation

得到的关键点的旋转矩阵，即可作为两个点云的旋转矩阵

ICP

min_translation = geomeas.icp(source, target)
print(min_translation)
source.translate(min_translation)
source_model_pcd.translate(min_translation)

---

然后在手动观察，直接调整其他方向的位移，最后，在用o3d的精细配准

# 提取几何特征
# 对点云进行下采样,估计法线,最后计算每个点的FPFH特征。FPFH特征是一个33维向量，描述了一个点的局部几何特性。在33维空间中的最近邻查询可以返回具有相似局部几何结构的点。详细细节请查看 [Rasu2009].
def preprocess_point_cloud(pcd, voxel_size):
    print(":: 使用大小为为{}的体素下采样点云.".format(voxel_size))
    pcd_down = pcd.voxel_down_sample(voxel_size)

    radius_normal = voxel_size * 2
    print(":: 使用搜索半径为{}估计法线".format(radius_normal))
    pcd_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))

    radius_feature = voxel_size * 5
    print(":: 使用搜索半径为{}计算FPFH特征".format(radius_feature))
    pcd_fpfh = o3d.pipelines.registration.compute_fpfh_feature(pcd_down, o3d.geometry.KDTreeSearchParamHybrid(
        radius=radius_feature, max_nn=100))
    return pcd_down, pcd_fpfh

# 使用搜索半径为{}计算FPFH特征
def preprocess_point_cloud_NoVoxel(pcd):
    pcd.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=10, max_nn=30))
    radius_feature = 25
    print(":: 使用搜索半径为{}计算FPFH特征".format(radius_feature))
    pcd_fpfh = o3d.pipelines.registration.compute_fpfh_feature(pcd, o3d.geometry.KDTreeSearchParamHybrid(
        radius=radius_feature, max_nn=100))
    return pcd, pcd_fpfh

# 全局快速注册
# 通过修剪步骤的匹配来计算变换，该变换在整个点云上进行验证。核心功能是. 该函数最重要的超参数是。它定义了 RANSAC 迭代的最大次数和置信概率。这两个数字越大，结果越准确，但算法花费的时间也越长。
def execute_global_registration(source_down, target_down, source_fpfh,
                                target_fpfh, voxel_size):
    distance_threshold = voxel_size * 1.5
    print(":: RANSAC registration on downsampled point clouds.")
    print("   Since the downsampling voxel size is %.3f," % voxel_size)
    print("   we use a liberal distance threshold %.3f." % distance_threshold)
    result = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
        source_down, target_down, source_fpfh, target_fpfh, True,
        distance_threshold,
        o3d.pipelines.registration.TransformationEstimationPointToPoint(False),
        3, [
            o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(
                0.9),
            o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(
                distance_threshold)
        ], o3d.pipelines.registration.RANSACConvergenceCriteria(100000, 0.999))
    return result