点云数据获取和处理的代码如下:
一、用DBSCAN聚类的方法处理点云数据
通过设置点云坐标的最大聚类对点云坐标进行归类,再将相同类的坐标求均值(中心点坐标),这些均值坐标通过手眼标定的转换矩阵转换为二维的相机坐标,再和相机拍到的目标的中心点坐标拟合,找到与目标坐标最适合的点云坐标,从而获得目标物的距离。 相机和雷达的手眼标定代码本人已经写完,可以参考微博1.激光雷达与相机的融合标定(附python代码)_雷达坐标系转相机坐标系-CSDN博客
这里我们只是通过聚类获得了点云的均值坐标。
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from sensor_msgs.msg import PointCloud2
import sensor_msgs.point_cloud2 as pc2
from std_msgs.msg import Header
from visualization_msgs.msg import Marker, MarkerArray
from geometry_msgs.msg import Point#import torch
import numpy as np
import sys
import time
print(sys.version)
#from recon_barriers_model import recon_barriers
#from pclpy import pcl
from queue import Queueimport matplotlib
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
#%matplotlib#聚类的数据处理
def cluster(points, radius=0.2):"""points: pointcloudradius: max cluster range"""items = []while len(points)>1:item = np.array([points[0]])base = points[0]points = np.delete(points, 0, 0)distance = (points[:,0]-base[0])**2+(points[:,1]-base[1])**2+(points[:,2]-base[2])**2infected_points = np.where(distance <= radius**2)item = np.append(item, points[infected_points], axis=0)border_points = points[infected_points]points = np.delete(points, infected_points, 0)while len(border_points) > 0:border_base = border_points[0]border_points = np.delete(border_points, 0, 0)border_distance = (points[:,0]-border_base[0])**2+(points[:,1]-border_base[1])**2border_infected_points = np.where(border_distance <= radius**2)item = np.append(item, points[border_infected_points], axis=0)border_points = points[border_infected_points]points = np.delete(points, border_infected_points, 0)items.append(item)return items#点云的获取的部分数据的过滤
def recon_barriers(filename,msg_1s):pcl_msg = pc2.read_points(filename, skip_nans=False, field_names=("x", "y", "z", "intensity","ring"))np_p_2 = np.array(list(pcl_msg), dtype=np.float32)print("===>",np_p_2.shape)ss=np.where([s[0]>2 and s[1]<3 and s[-1]>-3 and s[2]>-0.5 for s in np_p_2])#print(len(ss[0]))#print(ss[0])hh=np_p_2[ss]print(hh.shape)return hhdef velo_callback(msg):pcl_msg = pc2.read_points(msg, skip_nans=False, field_names=("x", "y", "z", "intensity","ring"))print(type(pcl_msg))global max_marker_size_,frequencefrequence=1if frequence % 2 == 0:q.put(msg)msg_1s = q.get()else:q.put(msg)msg_1s = q.get()ans = recon_barriers(msg,msg_1s)item=cluster(ans, radius=0.2)m_item=[]for items in item:print("..............",items.shape)#x,y,z=int(items[:,:1].sum().mean())x,y,z,r=items[:,:1].mean(),items[:,1:2].mean(),items[:,2:3].mean(),items[:,3:4].mean()m_item.append([x,y,z])print("=====+++++>>>>",len(item))print(len(item[0]))print(m_item)fig = plt.figure()ax = Axes3D(fig)fig = plt.figure()ax = Axes3D(fig)#ax.scatter(item[:,0], item[:,1], item[:,2], s=1)#fig.show()if __name__ == '__main__':# code added for using ROSglobal max_marker_size_,frequenceq = Queue()q.put(None)rospy.init_node('lidar_node')sub_ = rospy.Subscriber("livox/lidar", PointCloud2,velo_callback, queue_size=100)pub_arr_bbox = rospy.Publisher("visualization_marker", MarkerArray, queue_size=100)print("ros_node has started!")rospy.spin()
二、通过雷达的不同颜色对点云进行处理
将相同颜色的点云坐标归为一类,并求每个类的坐标的平均值(中心点坐标)。当环境比较单一,雷达反射的点云颜色类型较少时可以用这种方法。点云的返回坐标是(x,y,z,r),其中r是颜色,所以我们可以将颜色的数据切取后set,set是将重复的元素去掉,再遍历set对返回的点云np.where即可。