open3d 源码阅读kd_tree_feature_matching

基于 FPFH特征匹配和基于L32D特征匹配两个点云。

点云特征匹配算法：基于不同特征建立KDTree，并使用search_knn_vector_xd计算距离，匹配点云。

import numpy as np
import open3d as o3d

if __name__ == "__main__":

    # 1. read *.pcd file
    print("Load two aligned point clouds.")
    demo_data = o3d.data.DemoFeatureMatchingPointClouds()
    pcd0 = o3d.io.read_point_cloud(demo_data.point_cloud_paths[0])  # *.pcd
    pcd1 = o3d.io.read_point_cloud(demo_data.point_cloud_paths[1])  # *.pcd

    pcd0.paint_uniform_color([1, 0.706, 0])
    pcd1.paint_uniform_color([0, 0.651, 0.929])
    o3d.visualization.draw_geometries([pcd0, pcd1])

    # 2. FPFH feature matching. 读取两个点云各自的特征size(33, num_pt)
    print("Load their FPFH feature and evaluate.")
    print("Black : matching distance > 0.2")
    print("White : matching distance = 0")
    feature0 = o3d.io.read_feature(demo_data.fpfh_feature_paths[0])  # return open3d.pipelines.registration.Feature
    feature1 = o3d.io.read_feature(demo_data.fpfh_feature_paths[1])  #

    # 2.1. 建立f1 KDTree
    fpfh_tree = o3d.geometry.KDTreeFlann(feature1)  # 基于其中一个点云的特征size(33,num_pt)建立KDTree
    # 2.2 f0匹配f1

    # p0 match p1, 并在p0可视化距离匹配到的相应点距离。
    for i in range(len(pcd0.points)):  # 遍历p0所有点，在f1 tree中匹配f0中点
        # f0第i个点匹配f1
        [_, idx, _] = fpfh_tree.search_knn_vector_xd(query=feature0.data[:, i], knn=1)  # f1中查询与f0第i个点邻近的1个点
        # 计算p0第i个点，和p1匹配到的idx点距离。
        # np.linalg.norm用于求范数, linalg本意为linear(线性) + algebra(代数)，norm则表示范数。
        dis = np.linalg.norm(x=pcd0.points[i] - pcd1.points[idx[0]])  # 默认二范式：平方和的开根。dis是欧式距离
        # np.fmin: 数组元素的元素最小值。Compare two arrays and returns a new array containing the element-wise minima.
        # 把dis归一化到0-1之间，其中大于0.2直接置为0，等于0.2置为1， dis等于0的置为1。 以此值定为颜色值
        c = (0.2 - np.fmin(dis, 0.2)) / 0.2
        pcd0.colors[i] = [c, c, c]
    o3d.visualization.draw_geometries([pcd0])
    print("")

    # 3. L32D feature matching. 读取两个点云各自的特征size(32, num_pt)
    print("Load their L32D feature and evaluate.")
    print("Black : matching distance > 0.2")
    print("White : matching distance = 0")
    feature0 = o3d.io.read_feature(demo_data.l32d_feature_paths[0])
    feature1 = o3d.io.read_feature(demo_data.l32d_feature_paths[1])
    # 3.1 建立f1 KDTree
    fpfh_tree = o3d.geometry.KDTreeFlann(feature1)
    for i in range(len(pcd0.points)):
        [_, idx, _] = fpfh_tree.search_knn_vector_xd(feature0.data[:, i], 1)
        dis = np.linalg.norm(pcd0.points[i] - pcd1.points[idx[0]])
        c = (0.2 - np.fmin(dis, 0.2)) / 0.2
        pcd0.colors[i] = [c, c, c]
    o3d.visualization.draw_geometries([pcd0])
    print("")

                               基于FPFH特征                                                            基于L32D特征