使用Python写简单的点云SUSAN关键点检测

一、代码

Python

import numpy as np
import open3d as o3d
from scipy.spatial import cKDTree
from joblib import Parallel, delayed


def calculate_response(idx, points, radius, t, kdtree):
    # 使用KD树找到半径内的所有点
    neighbor_indices = kdtree.query_ball_point(points[idx], radius)
    neighbor_points = points[neighbor_indices]

    # 计算距离
    distances = np.linalg.norm(neighbor_points - points[idx], axis=1)

    # 计算响应值
    response = np.sum(distances < t) / len(neighbor_indices)
    return response if response > 0 else 0, idx


def susan_keypoint_detection_optimized(pcd, radius=0.05, t=0.01, response_threshold=0.5, n_jobs=-1):
    points = np.asarray(pcd.points)
    kdtree = cKDTree(points)

    # 并行计算每个点的响应值
    results = Parallel(n_jobs=n_jobs)(
        delayed(calculate_response)(i, points, radius, t, kdtree) for i in range(len(points))
    )

    # 分离响应值和索引
    responses, indices &