三维点云处理04-Voxel Grid Filter代码实现

三维点云处理04-Voxel Grid Filter代码实现

基础知识点

1.首先获得输入点云在各维度上的最大，最小值
2.设定leaf_size的大小
3.根据leaf_size的大小计算每个维度上栅格的数量D_x, D_y, D_z
4.对于输入点云中的每个点计算其i_x, i_y, i_z
5.由idx = i_x + i_y * D_x + i_z * D_y * D_x获得每个点所属栅格的idx

代码实现

import os
import numpy as np
from pyntcloud import PyntCloud


def get_voxel_grid_classifier(points, leaf_size):
	#首先获得输入点云在各个维度的最大最小值
	(p_min,p_max) = (points.min(),points.max())
	#根据设定的leaf_size的大小获得各维度上体素的数量
	(D_x, D_y, D_z) = (
		np.ceil((p_max['x'] - p_min['y'])/leaf_size).astype(np.int),
		np.ceil((p_max['y'] - p_min['y'])/leaf_size).astype(np.int),
		np.ceil((p_max['z'] - p_min['z'])/leaf_size).astype(np.int))

	#创建分类函数classifier
	#输入为x,y,z坐标，输出为对应栅格的idx
	def classifier(x,y,z):
		(i_x, i_y, i_z) = (
			np.floor((x - p_min['x']) / leaf_size).astype(np.int),
			np.floor((y - p_min['y']) / leaf_size).astype(np.int),
			np.floor((z - p_min['z']) / leaf_size).astype(np.int))
		
		idx = i_x + i_y * D_x + i_z * D_x * D_y
		
		return idx
	return classifier
		
def voxel_filter(points,leaf_size, method='centroid'):
	#首先创建滤波后点云
	filtered_points = None
	#对输入点云进行深拷贝
	working_points = points.copy(deep=True)
	#调用get_voxel_grid_classifier，获得classifier函数
	classifier = get_voxel_grid_classifier(points,leaf_size)
	#获得working_points中所有点的idx
	working_points['voxel_grid_id'] = working_points.apply(
		lambda row : classifier(row['x'],row['y'],row['z']),axis=1)
	#判断采用哪种采样方法，然后对点云进行分组采样
	if method == 'centroid':
		filtered_points = working_points.groupby(['voxel_grid_id']).mean().to_numpy()
	elif method == 'random':
		filtered_points = working_points.groupby(['voxel_grid_id']).apply(
			lambda x : x[['x','y','z']].sample(1)).to_numpy()
		
		return filtered_points
	
def main(point_cloud_filename):
	#加载点云
	point_cloud_pynt = PyntCloud.from_file(point_cloud_filename)
	
	#voxel_filter参数
	leaf_size = 10.0
	
	#调用voxel_filter函数，获得滤波后的点云
	filtered_cloud = voxel_filter(point_cloud_pynt.points, leaf_size, method='random')

Voxel-Grid-Filter注意事项

1.如果检测的点云范围比较大， leaf_size的大小比较小，此时需要注意，
如果idx使用int类型存储，有可能会造成数据溢出
eg:
检测范围：x:200,y:200,z:-10-10
leaf_size:0.05
idx最大值：2.56x10^10    > int: 2^32 = 4.3x10^9

2.虽然上面的算法实现是使用python中的groupby对每个点的id进行分组，
没有使用排序操作，但是当使用C++实现时，需要对所有点的idx进行排序后，
再进行采样，因此使用排序操作时要注意使用:a.index  < b.index    
而不能使用a.index <=  b.index

Voxel-Grid Filter与哈希表

• 如果存在N个点，就需要对N个点的idx进行排序，我们知道排序算法的时间复杂度是O(N*log(N))，当输入点云的数据量非常大时，使用排序的方法时间复杂度会非常高，如何解决这个问题呢？
• 因为实际自动驾驶场景中点云的分布是非常不均匀的，大部分的voxel是空的，假设我们有10000个点，然而实际场景中非空的voxel的数量小于100个（比如只有95个），因此在这种场景中，我们可以使用一个神奇的函数（哈希表）来解决对idx排序时间复杂度较高的问题

具体做法如下：

1.计算输入点云在各个维度的最小值和最大值
2.设定每个voxel的大小-leaf_size
3.计算各个维度上voxel的数量
4.计算每个点所属voxel的i_x，i_y，i_z
5.设定一个非空voxel的数量container_size，然后建立相应数量的哈希表
6.通过哈希函数将每个点映射到对应的非空voxel中
7.迭代每个非空voxel，每个非空voxel获得一个点

哈希函数的形式有很多，这里举例一种比较常用的哈希函数

• 上面的哈希函数并不完美，存在一个很明显的问题：哈希冲突
  

• 哈希冲突：两个点的i_x，i_y，i_z并不相同，但经过哈希函数后却映射到相同的voxel中

• 如何解决哈希冲突问题：

    在通过哈希函数计算每个点的idx，将每个点送入对应的哈希表之间，
    进行冲突检测，也就是将当前点与对应哈希表中已经存在的点的i_x，
    i_y，i_z进行判断，如果不相同，则说明存在冲突	
  
    解决方法是：从该栅格已经存在的点中随机选取一个点作为降采样的
    输出，然后将该栅格中的所有点清空，再将新的点填入，最后新填入
    的点同样会选出一个点作为降采样的结果
  

• 这里仍然有疑问，点云在送入哈希函数之前并没有任何的顺序，假设有三个点经过哈希函数后，对应相同的voxel，但是实际上第一个和第三个点是属于同一个voxel的点，第二个与其不属于同一个voxel，此时就会存在同一个voxel中的第一个第三个点被重复采样的问题

• 这里我的理解是重复采样的可能性确实存在，但是相比于之前应该有95个voxel却只分出了80个voxel的情况，重复采样比漏采样更能被接受。

Voxel-Grid Filter与哈希表的代码实现后面补上