Python-PCL点云边缘检测源码造轮子 --- AngleCriterion （附Python代码）

前言与参考文章

最近在使用python-pcl库来做一个边界检测的相关项目，但python-pcl这个库吧……确实实现了很多功能，但找来找去就是没有boundary-estimation。
不仅没有边界检测，还没有空间直线的交点求解……

• 所以啊，写一个项目，都在疯狂造python的轮子，很吐血……

好吧，没有边界检测，那我就自己写吧……
在此特别感谢McQueen_LT的文章：

1. PCL点云边缘检测源码解析（过程记录）—AngleCriterion （附C++代码）
2. PCL点云边界特征检测 （附完整代码 C++）

这两篇文章思路清晰，介绍算法和代码详解也很不错
【但最终1文章代码的72行有一个小错误（🤫）size应该是size-1】

代码详解

Dependency

• python-pcl
  How to install python-pcl in Ubuntu1804
  适配好 vtk6.3 的源码
• numpy

pip3 install numpy

Code

Talk is cheap，show me the code.

#!/usr/bin/env python
# -*- coding: utf-8 -*-

'''
Author  : Huang Chenrui
Email   : hcr2077@outlook.com
Version : 1.0.0
Date    : Aug 02, 2021
Reference: https://blog.csdn.net/McQueen_LT/article/details/118298329
'''

import pcl 
import pcl.pcl_visualization
import numpy as np
FLT_EPSILON = 1e-1 # 这个数我瞎设的，他说ismuchsmaller我也不知道具体应该多少

'''
Inputs:
    points_array: all points array
    point_array: single point
    neighbor_idx: kdtree nearest indice
    u,v: getCoordinateSystemOnPlane
    angle_threshold: rad
Outputs:
    True or False
Reference: https://pointclouds.org/documentation/boundary_8hpp_source.html
'''
def isBoundaryPoint(points_array, point_array, neighbor_idx, u, v, angle_threshold):
    if neighbor_idx.size <= 3: return False
    max_dif = 0
    cp = 0
    angle_array = np.zeros(neighbor_idx.size)
    for i in range(neighbor_idx.size):
        delta = points_array[neighbor_idx[0][i]] - point_array # 1 * 3 XYZ
        if all(delta == np.zeros(3)): continue
        angle = np.arctan2(np.dot(v, delta), np.dot(u, delta)) # (rad)the angles are fine between -PI and PI too
        angle_array[cp] = angle
        cp = cp + 1
    if cp == 0: return False
    angle_array = np.resize(angle_array, cp)
    angle_array = np.sort(angle_array) # 升序
    # Compute the maximal angle difference between two consecutive angles
    for i in range(angle_array.size - 1):
        dif = angle_array[i+1] - angle_array[i]
        if max_dif < dif: max_dif = dif
    # Get the angle difference between the last and the first
    dif = 2 * np.pi - angle_array[angle_array.size - 1] + angle_array[0]
    if max_dif < dif: max_dif = dif

    return (max_dif > angle_threshold)

'''
Inputs: tuple
Outputs: array( np.array([[]]) )
i.e: np.array([[1,2,3]])
# L109
# https://pointclouds.org/documentation/cuda_2common_2include_2pcl_2cuda_2common_2eigen_8h_source.html
'''
def unitOrthogonal(tuple):
    def isMuchSmallerThan(x, y):
        global FLT_EPSILON
        prec_sqr = FLT_EPSILON * FLT_EPSILON
        if x*x <= prec_sqr*y*y:
            return True
        else:
            return False
    if (not isMuchSmallerThan(tuple[0], tuple[1])) or (not isMuchSmallerThan(tuple[1], tuple[2])):
        invum = 1.0 / np.sqrt(tuple[0]**2 + tuple[1]**2)
        perp_x = -tuple[1] * invum
        perp_y = tuple[0] * invum
        perp_z = 0.0
    else:
        invum = 1.0 / np.sqrt(tuple[2]**2 + tuple[1]**2)
        perp_x = 0.0
        perp_y = -tuple[2] * invum
        perp_z = tuple[1] * invum
    perp_array = np.array([[perp_x, perp_y, perp_z]])
    return perp_array

'''
Inputs: point_normal(pcl.PointCloud_Normal)注意这里的normal已经是单位向量了而且是个tuple
pcl::Normal::Normal	(	
float 	n_x,
float 	n_y,
float 	n_z,
float 	_curvature
)		
注意哦：他们四个数的平方和是1，在之前算法线的时候已经算完了
Outputs: u and v
'''
def getCoordinateSystemOnPlane(point_normal):
    normal_tuple = point_normal
    # v = p_coeff_v.unitOrthogonal();
	# u = p_coeff_v.cross3(v);
    # v是normal的正交向量
    v = unitOrthogonal(normal_tuple)
    u = np.cross(point_normal[:3], v)
    return u, v

'''
Inputs: pointCloud(pcl.pointCloud)
Outputs: normals(pcl.PointCloud_Normal)
Reference by demo NormalEstimation.py
'''
def compute_normals(pointCloud):
    ne = pointCloud.make_NormalEstimation()
    tree = pointCloud.make_kdtree()
    ne.set_SearchMethod(tree)
    ne.set_KSearch(30)
    normals = ne.compute()
    return normals

'''
Inputs: 3D-pointcloud(points array) (n*3)XYZ
Outputs: 3D-pointcloud(points array) (n*3)XYZ
'''
def boundary_detection(points):
    # Create a PointCloud
    points[np.isnan(points)] = 0 # Delete NAN
    xyz_points = points[:, :3] # Change n*5 to n*3
    pc = pcl.PointCloud(xyz_points.astype(np.float32))
    # Calc Normals 计算法线
    pc_normals = compute_normals(pc)
    # Build a kdtree
    pc_kdtree = pc.make_kdtree_flann()

    boundary_array = np.zeros((1,3))
    for i in range(len(xyz_points)):
        # Find nearestKSearch points
        K = 40
        searchPoint = pcl.PointCloud()
        searchPoint.from_array(np.array([xyz_points[i]]))
        [neighbor_idx, neighbor_dist] = pc_kdtree.nearest_k_search_for_cloud(searchPoint, K)
        # pc[neighbor_idx[0][i]]
        # getCoordinateSystemOnPlane, Obtain a coordinate system on the least-squares plane
        u, v = getCoordinateSystemOnPlane(pc_normals[i])
        # isBoundaryPoint
        if isBoundaryPoint(xyz_points, xyz_points[i], neighbor_idx, u, v, angle_threshold=1.5):
            boundary_array = np.vstack([boundary_array, xyz_points[i]])
    boundary_array = np.delete(boundary_array, 0, axis=0)

    return boundary_array



'''
Test Code use bunny.pcd
调整那个threshold哈
就是isBoundaryPoints()的那个angle_threshold
kdtree的K我也是瞎设的，可以都调调看
'''
pc = pcl.load("bunny.pcd")
pc_array = pc.to_array()
boundary_array = boundary_detection(pc_array)

boundary_pc = pcl.PointCloud()
boundary_pc.from_array(boundary_array.astype(np.float32))

visual = pcl.pcl_visualization.CloudViewing()
visual.ShowMonochromeCloud(boundary_pc)
flag = True
while flag:
    if visual.WasStopped() == True:
        flag = False
        break

Function

整个流程就是遍历每个点，对每个点找近邻，并调用getCoordinateSystemOnPlane()对每个点计算 u 和 v 这两个向量，然后调用isBoundaryPoint()函数，判断每个点是否是边缘点

boundary_detection()

调用的主函数，主要包括了算法线、KDTree、找近邻、算uv、判定边缘点这五大块。

• 参考：boundary.h #116

compute_normals()

• 参考：python-pcl test demo

getCoordinateSystemOnPlane()

• getCoordinateSystemOnPlane()函数的意思是在当前点建立坐标系，分别法线n、法线与z轴（或x轴）叉乘得到的向量v，以及法线n与u的叉乘得到的向量u建立局部正交坐标系（n, u, v均已经过单位化处理）。
• 关于unitOthogonal()这个函数的具体实现见这里#L109
• cross()就是两个3维向量的叉乘，用numpy操作即可。

isBoundaryPoints()

• 参考：PCL具体实现#L62
• 注意这里的for (std::size_t i = 0; i < angles.size () - 1; ++i)

测试

• 下载 “著名的斯坦福桥” Bunny 兔子到你所创建的文件夹下
• 把上面的代码复制到你最喜欢的编辑器中，保存成boundary_estimation.py
• 接着：

python3 boundary_estimation.py

效果：

• 原图
  
• 边缘检测后
  
  其实效果还是可以的，如果再加上体素滤波等操作，整体的点会更加有特征一些，读者可以自己试着做一下。

致谢

本文主要参考了McQueen_LT的文章，相当于把PCL的C++库用python-pcl重新造了一下轮子。
有时候适当的造轮子还是必要的，写完代码也让我对于AngleCriterion这个算法的理解更深了一层。
在此附上论文地址：Detecting Holes in Point Set Surfaces

请各位读者不吝批评指正。