使用Python写简单的点云harris 3D关键点检测

一、代码

Python

import numpy as np
import open3d as o3d


def compute_harris_keypoints_optimized(point_cloud, rad, thresh):
    if not point_cloud.has_normals():
        point_cloud.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))

    # 创建KD树
    kdtree = o3d.geometry.KDTreeFlann(point_cloud)

    points = np.asarray(point_cloud.points)
    num_points = points.shape[0]
    harris_responses = np.zeros(num_points)
    keypoints_idx = []

    # 使用向量化计算协方差矩阵和Harris响应
    for i in range(num_points):
        [_, idx, _] = kdtree.search_radius_vector_3d(point_cloud.points[i], rad)
        if len(idx) >= 3:
            neighbors = points[idx, :]
            # 减去均值以进行去中心化
            neighbor_shifted = neighbors - neighbors.mean(axis=0)
            # 计算去中心化之后的协方差矩阵
            cov = np.dot(neighbor_shifted.T, neighbor_shifted) / len(neighbor_sh