【ROS】【Python3】实现计算点云每一帧基于特征值的6维特征向量

在ros中实现计算每一帧点云的特征向量

---

思路流程

订阅点云数据

创建一个类class eigen_feature:

class eigen_feature:
	def __init__(self):
	def callback(self,lidar):
	def get_6features(self,eigenvalues):	
	def PCA(self,k_neighbor,sort=True):

其中的构造函数如下，订阅点云话题/rslidar_points，消息类型位PointCloud2

def __init__(self):
	# self.feature_pub = rospy.Publisher('/features',eigen_features,queue_size=1)
	self.pt_sub=rospy.Subscriber("/rslidar_points", PointCloud2, self.callback)

回调函数

当订阅到点云时，执行回调函数，回调函数中使用了open3d库。并且调用了类函数self.PCA、self.get_6features。其中，self.PCA对每一个点进行主成分分析，计算特征值和特征向量；self.get_6features函数根据PCA计算出的特征值计算基于特征值的6个特征。

def callback(self,lidar):
	
		# convert pointcloud2 to array
		lidar = pc2.read_points(lidar)
		points = np.array(list(lidar))
		
		# get a PointCloud
		point_cloud = PointCloud()
		point_cloud.points = Vector3dVector(points[:,0:3].reshape(-1,3))
    	
		# downsample
		point_cloud= point_cloud.voxel_down_sample(voxel_size=0.6)
		points=point_cloud.points
		points=np.asarray(points)
    	
		# build a kdtree
		pcd_tree = o3d.geometry.KDTreeFlann(point_cloud)
    	
		eigenfeatures = []
		K=19
		
		save_filename='/home/s/Dataset/syz.txt'
		
		for i in range(points.shape[0]):
			[k,index,_]=pcd_tree.search_knn_vector_3d(point_cloud.points[i],K+1)
			
			eigenvalues,eigenvectors=self.PCA(points[index[:],:])
			
			tmp=self.get_6features(eigenvalues=eigenvalues)
			
			eigenfeatures.append(tmp)
			
		# print("eigenfeatures size:{}".format(len(eigenfeatures)))
		
		# convert list[] to array
		eigenfeatures=np.array(eigenfeatures)
		print('eigenfeatures shape:{}'.format(eigenfeatures.shape))
			
		np.savetxt(save_filename,eigenfeatures)
		print(save_filename+" save!")

		# 退出
		try:
			os._exit(0)
		except:
			print('Program is dead')

---

self.PCA

def PCA(self,k_neighbor,sort=True):
	k_neighbor_T=k_neighbor.T
	C=np.matmul(k_neighbor_T,k_neighbor) # 3x3
	eigenvalues,eigenvectors=np.linalg.eig(C)

	if sort:
		sort=eigenvalues.argsort()[::-1]
		eigenvalues = eigenvalues[sort]
		eigenvectors = eigenvectors[:, sort]
	return eigenvalues, eigenvectors

---

self.get_6features

	
def get_6features(self,eigenvalues):
	
	v1=np.float(eigenvalues[0]) 
	v2=np.float(eigenvalues[1])
	v3=np.float(eigenvalues[2])
	
	sum=v1+v2+v3
	if sum==0:
		sum=0.000000001
	e1=v1/sum
	e2=v2/sum
	e3=v3/sum

	# print('v1:{},v2:{},v3:{}'.format(v1,v2,v3))
	if v1==0:
		v1=0.000000001
	L=(v1-v2)/v1   # 线性
	P=(v2-v3)/v1   # 平面性
	S=v3/v1        # 散度性
	O=3*np.power(e1*e2*e3,1/3)   # 全方差

	if e1<=0:
		e1=0.000000001
	if e2<=0:
		e2=0.000000001
	if e3<=0:
		e3=0.000000001

	E=e1*np.log(e1)+e2*np.log(e2)+e3*np.log(e3)  # 特征熵
	E=E*(-1)
	C=3*v3/sum       # 曲率变化

	return [L,P,S,O,E,C]

初始化节点

if __name__ == '__main__':
	print('------------------start-----------------')
	print("open3d:{}".format(o3d.__version__))
	eigen=eigen_feature()  # 初始化类，会调用类构造函数
	rospy.init_node('eigenfeature')  #　初始化节点
	rospy.spin()　　# 一直循环（好像是这个意思）

完整代码

https://github.com/suyunzzz/aiimooc_lesson/tree/master/aiimooc_syz3

要注意代码运行时需要在对应的conda环境中运行，如果不知道怎么做可以参考我的上一篇博客

参考

https://github.com/suyunzzz/aiimooc_lesson/tree/master/aiimooc_syz3
点云生成鸟瞰图（Python）