Feature Description特征描述 - PFH & FPFH算法
代码链接 : github
Github链接:有关于环境感知方面的网络介绍及代码链接
1. 背景
上一章介绍了点云的特征提取ISS算法 ,对于提取每帧图片后的任务就是进行特征描述,对于同样的物体,将其进行旋转和平移其特征描述值应该不变性。 传统的方法有两种主流算法 , :
- Histogram based , eg PFH & FPFH (本文主要介绍)
- Signature based , eg SHOT
除此之外还有深度学习方法,比如3DMatch , PerfectMatch , PPFNet & PPF-FoldNet等
2. 算法简介
PFH
- 通过特征提取算法找到特征点(代码里的特征点是直接由ISS算法提取到的2个点)
- 计算特征点的radius近邻,然后计算所有点两两之间的6D pose(u , v , w , α , Φ , θ),下面是计算任意两点p1 & p2之间的6D pose算法 , 其中n1 和 n2 分别是 p1 和 p2点的法向量
- u = n1
- v = u X $p2 - p1 \over ||p2 - p1|| $
- w = u X v
d = $ ||p2 - p1||$ (受传感器远近影响,所以一般舍去)- α = v · n2
- Φ = u · p 2 − p 1 ∣ ∣ p 2 − p 1 ∣ ∣ p2 - p1 \over ||p2 - p1|| ∣∣p2−p1∣∣p2−p1
- θ = arctan(w · n2 , u · n2)
- 对于quiry的k个近邻,理论上应该有k² 个三元组[α , Φ , θ],将这k²个三元组关系放入B个直方图中(voxel grid),最终得到特征点的描述(代码里是直方图做拼接而不是做voxel grid)
FPFH
FPFH实际上是PFH的一种改进,主要是
-
通过特征提取算法找到特征点(代码里的特征点是直接由ISS算法提取到的2个点)
-
计算query点的SPFH特征(SPFH特征与PFH不同的是只计算query点与近邻点的pari,而不是所有的两两pair)
-
计算特征点的radius近邻,并且计算每个近邻点的SPFH特征
-
计算最终的FPFH特征,是一个加权的特征其公式如下图,其中权重与近邻点到query点的距离成反比
3. 代码实现
对于PFH的话核心在于计算所有两两之间的特征这个体现在了PFH.cal_group函数中:
def cal_group(self , idx_neighbors , id):
# idx_neighbors.append(id) #把quiry点加入到整个近邻里
alpha_group ,phi_group , theta_group = [] ,[] , []
for id1 in idx_neighbors:
for id2 in idx_neighbors:
if id1 == id2:#跳过自己本身
continue
afa , fai , the = self.cal_pair(id1, id2)
alpha_group.append(afa)
phi_group.append(fai)
theta_group.append(the)
# print(len(alpha_group) , len(phi_group) , len(theta_group))
alpha_hist , _ = np.histogram(alpha_group , self.B )
phi_hist , _ = np.histogram(phi_group , self.B )
theta_hist , _ = np.histogram(theta_group , self.B )
feature = np.hstack((alpha_hist , phi_hist , theta_hist)) / len(alpha_group) #由于每个query的点的近邻个数不一样所以需要归一化
return feature
而对于FPFH,核心是计算SPFH特征,其在FPFH.cal_SPFH函数中:
def cal_SPFH(self , id:int):
[num_neighbors, idx_neighbors , dis_neighbors] = self.tree.search_radius_vector_3d(self.array[id],self.radius)
query = self.array[id]
neigbhors = self.array[idx_neighbors[1:]]
#计算n1 , n2 ,以及dis
n1 = self.normal[id]
n2 = self.normal[idx_neighbors[1:]]
dis = (neigbhors - query) / np.linalg.norm((neigbhors - query) , ord = 2)
#计算u , v , w
u = self.normal[id]
v = np.cross(u , dis)
w = np.cross(u , v)
#计算其3个夹角alpha , phi , theta
afa = (v * n2).sum(axis = 1)
fai = (u * dis).sum(axis = 1)
the = np.arctan2((w * n2).sum(axis = 1),(u * n2).sum(axis = 1))
#计算alpah , phi , theta 的直方图
alpha_hist , _ = np.histogram(afa , self.B)
phi_hist , _ = np.histogram(fai , self.B)
theta_hist , _ = np.histogram(the , self.B)
spfh = np.hstack((alpha_hist , phi_hist , theta_hist)) / (num_neighbors - 1) #由于每个query的点的近邻个数不一样所以需要归一化
return spfh
以及计算加权的FPFH特征:
for id in feature_id:
#计算query点的SPFH特征
query_SPFH = self.cal_SPFH(id)
#计算query近邻的SPFH特征
[k, idx_neighbors , dis_neighbors] = self.tree.search_radius_vector_3d(self.array[id],self.radius) #计算query点的近邻
weight = 1.0 / np.linalg.norm(self.array[id] - self.array[idx_neighbors[1:]] , ord = 2 , axis = 1) # 权重
neighbors_SPFH = np.array([self.cal_SPFH(i) for i in idx_neighbors[1:]]) # 近邻的SPFH
#计算query点的FPFH,并归一化
query_FPFH = query_SPFH + np.dot(weight.reshape((1,-1)) ,neighbors_SPFH).reshape(-1) / (k - 1)
query_FPFH = query_FPFH / np.linalg.norm(query_FPFH)
self.feature_list.append(query_FPFH)
4. 效果
最后的效果由matplotlib表示,这里的B = 10,所以最后维度都是30维,可以看到无论是PFH还是FPFH对于同样类型的特征点的描述还是很类似的。