【ROS】【Python3】实现计算点云每一帧基于特征值的6维特征向量-CFANZ编程社区

在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()　　# 一直循环（好像是这个意思）