网站首页 seo,电子商务平台经营者不能是,网业怎么保存到桌面,制作网页实训报告点云数据获取和处理的代码如下#xff1a;
一、用DBSCAN聚类的方法处理点云数据 通过设置点云坐标的最大聚类对点云坐标进行归类#xff0c;再将相同类的坐标求均值#xff08;中心点坐标#xff09;#xff0c;这些均值坐标通过手眼标定的转换矩阵转换为二维的相机坐标
一、用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, radius0.2):points: pointcloudradius: max cluster rangeitems []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], axis0)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], axis0)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_nansFalse, field_names(x, y, z, intensity,ring))np_p_2 np.array(list(pcl_msg), dtypenp.float32)print(,np_p_2.shape)ssnp.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])hhnp_p_2[ss]print(hh.shape)return hhdef velo_callback(msg):pcl_msg pc2.read_points(msg, skip_nansFalse, field_names(x, y, z, intensity,ring))print(type(pcl_msg))global max_marker_size_,frequencefrequence1if frequence % 2 0:q.put(msg)msg_1s q.get()else:q.put(msg)msg_1s q.get()ans recon_barriers(msg,msg_1s)itemcluster(ans, radius0.2)m_item[]for items in item:print(..............,items.shape)#x,y,zint(items[:,:1].sum().mean())x,y,z,ritems[:,: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], s1)#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_size100)pub_arr_bbox rospy.Publisher(visualization_marker, MarkerArray, queue_size100)print(ros_node has started!)rospy.spin()
二、通过雷达的不同颜色对点云进行处理 将相同颜色的点云坐标归为一类并求每个类的坐标的平均值中心点坐标。当环境比较单一雷达反射的点云颜色类型较少时可以用这种方法。点云的返回坐标是x,y,z,r)其中r是颜色所以我们可以将颜色的数据切取后setset是将重复的元素去掉再遍历set对返回的点云np.where即可。
