三维点云学习（8）5-3D Feature Description 实现FPFH

三维点云学习（8）5-3D Feature Description 实现FPFH

本次python实现FPFH代码，大部分参考：大神的GitHub
参考博客：【PCL学习笔记】之快速点特征直方图FPFH - pcl::FPFHSignature33
本次使用的数据集是：modelNet40 的 airplane_0001.txt

modelNet40 数据集下载

为40种物体的三维点云数据集
链接：https://pan.baidu.com/s/1LX9xeiXJ0t-Fne8BCGSjlQ
提取码：es14

效果图：

使用 ISS 特征点提取出的 keypoints：

所有特征点的 FPFH：

相似的特征点 FPFH比较：

代码流程：

iss 代码参考 三维点云学习（7）5-Feature 实现ISS

step1 使用iss 找出所有keypoint,并以label的形式输出
step2 求出所有点的法向量,在modelNet40数据集中,每个数据点的后三位为该点的法向量
step3 构建radius nn tree
step4 求解每个keypoint的 fpfh:
	step5 寻找每个keypoint 的nearest points
	step6 计算该关键点群 权重 weights
	step7 计算该关键点的邻近点的 spfh
		step8 计算 u,v,w
		step9 计算直方图 histogram
		step10 拼接直方图 histogram
	step11 该关键点的邻近点的 spfh 和
	step12 计算该关键点的 spfh
	step12 计算该关键点FPFH = spfh(keypoint) + spfh(nearest)

测试FPFH：

测试方法：通过比较相同特征点的FPFH分布情况，判断FPFH算法是否合适，如下颜色相同为经过测试得出的相近特征点，若相似关键点的FPFH分布大致相等，则可验证该FPFH算法效果良好：

测试模块：

    #visualization test similar feature description ,相近的点 7847-3843 ； 6336-8605 ； 5508-7644
    test_keypoint_idx = [7847,3843] # [7847,3843] , [6336,8605] , [5508,7644]
    test_FPFH = np.asarray(
        [describe(point_cloud_raw, nearest_idx, keypoint_id, radius, B) for keypoint_id in test_keypoint_idx]
    )
    visual_feature_description(test_FPFH, test_keypoint_idx)

测试结果：

部分代码模块：

FPFH折线图可视化：

def visual_feature_description(fpfh,keypoint_idx):
    for i in range(len(fpfh)):
        x = [i for i in range(len(fpfh[i]))]
        y = fpfh[i]
        plt.plot(x,y,label=keypoint_idx[i])
    #添加显示图例
    plt.title('Description Visualization for Keypoints')
    plt.legend(bbox_to_anchor=(1, 1),  # 图例边界框起始位置
               loc="upper right",  # 图例的位置
               ncol=1,  # 列数
               mode="None",  # 当值设置为“expend”时，图例会水平扩展至整个坐标轴区域
               borderaxespad=0,  # 坐标轴和图例边界之间的间距
               title="keypoints",  # 图例标题
               shadow=False,  # 是否为线框添加阴影
               fancybox=True)  # 线框圆角处理参数
    plt.xlabel("label")
    plt.ylabel("fpfh")
    plt.show()

求解 sfph

def get_spfh(point_cloud, nearest_idx, keypoint_id, radius, B):   # single pfh
    points = np.asarray(point_cloud)
    keypoint = np.asarray(point_cloud)[keypoint_id]
    #remove query point 去除关键点  :
    key_nearest_idx = nearest_idx[keypoint_id]
    key_nearest_idx = list(set(nearest_idx[keypoint_id]) - set([keypoint_id]))
    key_nearest_idx = np.asarray(key_nearest_idx)
    ##step8 计算 u,v,w
    #向量 p2_p1
    diff = points[key_nearest_idx] - keypoint  # p2 - p1,shape: (k,3)  k为该点有多少个nearest points
    diff /= np.linalg.norm(diff,ord=2,axis=1)[:,None] #[:,None]的效果就是将二维数组按每行分割，最后形成一个三维数组 ,eg shape : (k,1)
    #compute n1 n2
    n1 = np.asarray(point_cloud_normals[keypoint_id])       #keypoint 邻近点的法向量
    n2 = np.asarray(point_cloud_normals[key_nearest_idx])   #keypoint 邻近点的邻近点的法向量
    #compute u v w
    u = n1
    v = np.cross(u,diff)
    w = np.cross(u,v)
    #compute alpha  phi theta 三元组
    alpha = (v*n2).sum(axis=1)
    phi = (u * diff).sum(axis=1)
    theta = np.arctan2((w * n2).sum(axis=1), (u * n2).sum(axis=1))
    ##step9 计算直方图 histogram
    # get alpha histogram:
    alpha_histogram = np.histogram(alpha, bins=B, range=(-1.0, +1.0))[0]
    alpha_histogram = alpha_histogram / alpha_histogram.sum()
    # get phi histogram:
    phi_histogram = np.histogram(phi, bins=B, range=(-1.0, +1.0))[0]
    phi_histogram = phi_histogram / phi_histogram.sum()
    # get theta histogram:
    theta_histogram = np.histogram(theta, bins=B, range=(-np.pi, +np.pi))[0]
    theta_histogram = theta_histogram / theta_histogram.sum()
    ##step10 拼接直方图 histogram
    # build signature:
    signature = np.hstack(
        (
            # alpha:
            alpha_histogram,
            # phi:
            phi_histogram,
            # theta:
            phi_histogram
        )
    )
    return signature

describe feature

def describe(point_cloud, nearest_idx, keypoint_id, radius, B):   # single pfh
    ##step5 寻找每个keypoint 的nearest points
    points = np.asarray(point_cloud)
    keypoint = np.asarray(point_cloud)[keypoint_id]
    #remove query point 去除关键点  :
    key_nearest_idx = nearest_idx[keypoint_id]
    key_nearest_idx = list(set(nearest_idx[keypoint_id]) - set([keypoint_id]))
    key_nearest_idx = np.asarray(key_nearest_idx)
    k = len(key_nearest_idx)             #keypoint的临近点个数
    ##step6 计算该关键点 群 权重 weights:
    W = 1.0 / np.linalg.norm(points[key_nearest_idx] - keypoint , ord=2, axis=1)
    ##step7 计算nearest points 的spfh
    X = np.asarray(
        [get_spfh(point_cloud,nearest_idx,i,radius,B) for i in key_nearest_idx]
    )
    ##step11 neighbor 的 spfh 权重和
    spfh_neighborhood = 1.0 / (k) * np.dot(W, X)
    ##step12 keypoints 的 spfh
    spfh_query = get_spfh(point_cloud,nearest_idx,keypoint_id,radius,B)
    ##step13 finally
    spfh = spfh_query + spfh_neighborhood
    # normalize again:
    spfh = spfh / np.linalg.norm(spfh)

    return spfh

全部代码：

FPFH.py

#FPFH.py
import open3d as o3d
import os
import numpy as np
from pyntcloud import PyntCloud
import matplotlib.pyplot as plt
import pandas as pd
import seaborn as sns
from sklearn.neighbors import KDTree # KDTree 进行搜索
import random
from ISS import  iss,Point_Cloud_Show          #  iss feature detection
from pandas import DataFrame

def visual_feature_description(fpfh,keypoint_idx):
    for i in range(len(fpfh)):
        x = [i for i in range(len(fpfh[i]))]
        y = fpfh[i]
        plt.plot(x,y,label=keypoint_idx[i])
    #添加显示图例
    plt.title('Description Visualization for Keypoints')
    plt.legend(bbox_to_anchor=(1, 1),  # 图例边界框起始位置
               loc="upper right",  # 图例的位置
               ncol=1,  # 列数
               mode="None",  # 当值设置为“expend”时，图例会水平扩展至整个坐标轴区域
               borderaxespad=0,  # 坐标轴和图例边界之间的间距
               title="keypoints",  # 图例标题
               shadow=False,  # 是否为线框添加阴影
               fancybox=True)  # 线框圆角处理参数
    plt.xlabel("label")
    plt.ylabel("fpfh")
    plt.show()


def get_spfh(point_cloud, nearest_idx, keypoint_id, radius, B):   # single pfh
    points = np.asarray(point_cloud)
    keypoint = np.asarray(point_cloud)[keypoint_id]
    #remove query point 去除关键点  :
    key_nearest_idx = nearest_idx[keypoint_id]
    key_nearest_idx = list(set(nearest_idx[keypoint_id]) - set([keypoint_id]))
    key_nearest_idx = np.asarray(key_nearest_idx)
    ##step8 计算 u,v,w
    #向量 p2_p1
    diff = points[key_nearest_idx] - keypoint  # p2 - p1,shape: (k,3)  k为该点有多少个nearest points
    diff /= np.linalg.norm(diff,ord=2,axis=1)[:,None] #[:,None]的效果就是将二维数组按每行分割，最后形成一个三维数组 ,eg shape : (k,1)
    #compute n1 n2
    n1 = np.asarray(point_cloud_normals[keypoint_id])       #keypoint 邻近点的法向量
    n2 = np.asarray(point_cloud_normals[key_nearest_idx])   #keypoint 邻近点的邻近点的法向量
    #compute u v w
    u = n1
    v = np.cross(u,diff)
    w = np.cross(u,v)
    #compute alpha  phi theta 三元组
    alpha = np.multiply(v,n2).sum(axis=1)#alpha = (v*n2).sum(axis=1)
    phi = np.multiply(u,diff).sum(axis=1)#phi = (u * diff).sum(axis=1)
    theta = np.arctan2(np.multiply(w,n2).sum(axis=1), (u * n2).sum(axis=1))#theta = np.arctan2((w * n2).sum(axis=1), (u * n2).sum(axis=1))
    ##step9 计算直方图 histogram
    # get alpha histogram:
    alpha_histogram = np.histogram(alpha, bins=B, range=(-1.0, +1.0))[0]
    alpha_histogram = alpha_histogram / alpha_histogram.sum()
    # get phi histogram:
    phi_histogram = np.histogram(phi, bins=B, range=(-1.0, +1.0))[0]
    phi_histogram = phi_histogram / phi_histogram.sum()
    # get theta histogram:
    theta_histogram = np.histogram(theta, bins=B, range=(-np.pi, +np.pi))[0]
    theta_histogram = theta_histogram / theta_histogram.sum()
    ##step10 拼接直方图 histogram
    # build signature:
    signature = np.hstack(
        (
            # alpha:
            alpha_histogram,
            # phi:
            phi_histogram,
            # theta:
            theta_histogram
        )
    )
    return signature



def describe(point_cloud, nearest_idx, keypoint_id, radius, B):   # single pfh
    ##step5 寻找每个keypoint 的nearest points
    points = np.asarray(point_cloud)
    keypoint = np.asarray(point_cloud)[keypoint_id]
    #remove query point 去除关键点  :
    key_nearest_idx = nearest_idx[keypoint_id]
    key_nearest_idx = list(set(nearest_idx[keypoint_id]) - set([keypoint_id]))
    key_nearest_idx = np.asarray(key_nearest_idx)
    k = len(key_nearest_idx)             #keypoint的临近点个数
    ##step6 计算该关键点 群 权重 weights:
    W = 1.0 / np.linalg.norm(points[key_nearest_idx] - keypoint , ord=2, axis=1)
    ##step7 计算nearest points 的spfh
    X = np.asarray(
        [get_spfh(point_cloud,nearest_idx,i,radius,B) for i in key_nearest_idx]
    )
    ##step11 neighbor 的 spfh 权重和
    spfh_neighborhood = 1.0 / (k) * np.dot(W, X)
    ##step12 keypoints 的 spfh
    spfh_query = get_spfh(point_cloud,nearest_idx,keypoint_id,radius,B)
    ##step13 finally
    spfh = spfh_query + spfh_neighborhood
    # normalize again:
    spfh = spfh / np.linalg.norm(spfh)

    return spfh

if __name__ == '__main__':
    point_cloud = np.genfromtxt(r"airplane_0001.txt", delimiter=",")
    point_cloud_raw = point_cloud[:, 0:3]  # 为 xyz的 N*3矩阵
    ##step1 使用iss 找出所有关键点,并以label的形式输出
    keypoint_idx = iss(point_cloud_raw)
    print(keypoint_idx)
    feature_point = point_cloud[keypoint_idx]
    #visualization feature points
    #Point_Cloud_Show(point_cloud,feature_point)
    ##step2 求出所有点的法向量,在modelNet40数据集中,每个数据点的后三位为该点的法向量
    point_cloud_normals = point_cloud[:, 3:6]  # 为 point normal的法向量
    ##step3 构建radius nn tree
    leaf_size = 4
    radius = 0.05
    search_tree = KDTree(point_cloud_raw,leaf_size)  #构建 kd_tree
    ##step4 求解每个关键点的 spfh
    B = 5  # 每个 直方图 bin的个数
    nearest_idx = search_tree.query_radius(point_cloud_raw, radius)   #求解每个点的最邻近点
    #description the keypoints
    FPFH = np.asarray(
        [describe(point_cloud_raw, nearest_idx, keypoint_id, radius, B) for keypoint_id in keypoint_idx]
    )
    #visualization all feature description
    visual_feature_description(FPFH,keypoint_idx)
    #visualization test similar feature description ,相近的点 7847-3843 ； 6336-8605 ； 5508-7644
    test_keypoint_idx = [7847,3843] # [7847,3843] , [6336,8605] , [5508,7644]
    test_FPFH = np.asarray(
        [describe(point_cloud_raw, nearest_idx, keypoint_id, radius, B) for keypoint_id in test_keypoint_idx]
    )
    visual_feature_description(test_FPFH, test_keypoint_idx)
    # describe(point_cloud_raw, nearest_idx, keypoint_idx[0], radius, B)

ISS.py

#ISS.py
import open3d as o3d
import os
import numpy as np
from pyntcloud import PyntCloud
import matplotlib.pyplot as plt
from sklearn.neighbors import KDTree # KDTree 进行搜索
import random
from pandas import DataFrame

# matplotlib显示点云函数
def Point_Cloud_Show(point_cloud,feature_point):
    fig = plt.figure(dpi=150)
    ax = fig.add_subplot(111, projection='3d')
    ax.scatter(point_cloud[:, 0], point_cloud[:, 1], point_cloud[:, 2], cmap='spectral', s=2, linewidths=0, alpha=1, marker=".")
    ax.scatter(feature_point[:, 0], feature_point[:, 1], feature_point[:, 2], cmap='spectral', s=2, linewidths=5, alpha=1,marker=".",color='red')
    # ax.scatter(feature_point[0], feature_point[1], feature_point[2], cmap='spectral', s=2, linewidths=5,
    #            alpha=1, marker=".", color='red')
    plt.title('Point Cloud')
    ax.set_xlabel('x')
    ax.set_ylabel('y')
    ax.set_zlabel('z')
    plt.show()

def compute_cov_eigval(point_cloud):
    x = np.asarray(point_cloud[:,0])
    y = np.asarray(point_cloud[:,1])
    z = np.asarray(point_cloud[:,2])
    M = np.vstack((x, y, z))  # 每行表示一个属性， 每列代表一个点
    cov = np.cov(M)  # 使用每个点的坐标求解cov
    eigval, eigvec = np.linalg.eigh(cov)  # 求解三个特征值，升序排列 linda1 < linda2 < linda3
    eigval = eigval[np.argsort(-eigval)]  # 改为降序排列  linda1 > linda2 > linda3
    return  eigval      #返回特征值

def iss(data):
    #parameters
    eigvals = []
    feature = []
    T = set()  #T 关键点的集合
    linda3_threshold = None  #阈值，初步筛选 ,各文件参数  airplane_0001:0.001; chair_0001:0.0001
    #构建 kd_tree
    leaf_size = 4
    radius = 0.1              # 各文件参数  airplane_00001:0.1; chair_0001:0.1
    tree = KDTree(data,leaf_size)
    #step1 使用radius NN 得到n个初始关键点, threshold 阈值 ：每个radius内的linda大于某个数值
    nearest_idx = tree.query_radius(data,radius)
    for i in range(len(nearest_idx)):
        eigvals.append(compute_cov_eigval(data[nearest_idx[i]]))
    eigvals = np.asarray(eigvals)  # 求解每个点在各自的 radius 范围内的linda
    # print(eigvals)     #打印所有的 特征值，供调试用
    # 根据linda3的数值 确定linda3_threshold(linda的阈值)
    linda3_threshold = np.median(eigvals,axis=0)[2]*5   #阈值取大约 是所有linda3的 中值得5倍，  eg 为什么取5倍是个人调试决定，也可取1倍
    # print(linda3_threshold)
    for i in range(len(nearest_idx)):
        if eigvals[i,2] > linda3_threshold:       # compute_cov_eigval(data[nearest_idx[i]])[2] -> 每个radius 里的最小的特征值 linda3
            T.add(i)   #获得初始关键点的索引
    # print(T)           #输出 初始关键点
    #step2   有 重叠(IOU)的 关键点群
    unvisited = T   #未访问集合
    while len(T):
        unvisited_old = unvisited #更新访问集合
        core = list(T)[np.random.randint(0,len(T))]   #从 关键点集T 中随机选取一个 关键点core
        unvisited = unvisited - set([core])           #把核心点标记为 visited,从 unvisited 集合中剔除
        visited = []
        visited.append(core)

        while len(visited):   #遍历所有初始关键点
            new_core = visited[0]
            if new_core in T:
                S = unvisited & set(nearest_idx[new_core])  #S : 当前 关键点(core) 的范围内所包含的其他关键点
                # print(T)
                # print(S)
                visited += (list(S))
                unvisited = unvisited - S
            visited.remove(new_core)   #new core 已做检测，去掉new core
        cluster = unvisited_old - unvisited    #cluster, 有 重叠(IOU)的 关键点群
        T = T - cluster                        #去掉该类对象里面包含的核心对象,差集
    #step3  NMS 非极大抑制，求解 一个关键点群的linda3最大 为  关键点
        cluster_linda3 = []
        for i in list(cluster):
            cluster_linda3.append(eigvals[i][2])    #获取 每个关键点 的 linda3
        cluster_linda3 = np.asarray(cluster_linda3)
        NMS_OUTPUT = np.argmax(cluster_linda3)
        feature.append(list(cluster)[NMS_OUTPUT])   #添加到 feature 特征点数组中
    #output
    return  feature



if __name__ == '__main__':
    point_cloud = np.genfromtxt(r"airplane_0001.txt", delimiter=",")
    point_cloud = point_cloud[:, 0:3]  # 为 xyz的 N*3矩阵
    feature_idx = iss(point_cloud)
    feature_point = point_cloud[feature_idx]
    print(feature_point)
    Point_Cloud_Show(point_cloud,feature_point)