Python中使用kitti数据集实现自动驾驶(绘制出所有物体的行驶轨迹)
作者:秃头小苏 发布时间:2023-06-27 17:02:54
标签:python,kitti,数据集,自动驾驶,行驶轨迹
本次内容主要是上周内容的延续,主要画出kitti车的行驶的轨迹
同样的,我们先来看看最终实现的效果:
视频
接下来就进入一步步的编码环节。。。
1、利用IMU、GPS计算汽车移动距离和旋转角度
计算移动距离
通过GPS计算
#定义计算GPS距离方法
def computer_great_circle_distance(lat1,lon1,lat2,lon2):
delta_sigma = float(np.sin(lat1*np.pi/180)*np.sin(lat2*np.pi/180)+\
np.cos(lat1*np.pi/180)*np.cos(lat2*np.pi/180)*np.cos(lon1*np.pi/180-lon2*np.pi/180))
return 6371000.0*np.arccos(np.clip(delta_sigma,-1,1))
#使用GPS计算距离
gps_distance += [computer_great_circle_distance(imu_data.lat,imu_data.lon,prev_imu_data.lat,prev_imu_data.lon)]
通过IMU计算
IMU_COLUMN_NAMES = ['lat','lon','alt','roll','pitch','yaw','vn','ve','vf','vl','vu','ax','ay','az','af',
'al','au','wx','wy','wz','wf','wl','wu','posacc','velacc','navstat','numsats','posmode',
'velmode','orimode']
#获取IMU数据
imu_data = read_imu('/home/wsj/data/kitty/RawData/2011_09_26/2011_09_26_drive_0005_sync/oxts/data/%010d.txt'%frame)
#使用IMU计算距离
imu_distance += [0.1*np.linalg.norm(imu_data[['vf','vl']])]
比较两种方式计算出的距离(GPS/IMU)
import matplotlib.pyplot as plt
plt.figure(figsize=(20,10))
plt.plot(gps_distance, label='gps_distance')
plt.plot(imu_distance, label='imu_distance')
plt.legend()
plt.show()
显然,IMU计算的距离较为平滑。
计算旋转角度 旋转角度的计算较为简单,我们只需要根据IMU获取到的yaw值就可以计算(前后两帧图像的yaw值相减)
2、画出kitti车的行驶轨迹
prev_imu_data = None
locations = []
for frame in range(150):
imu_data = read_imu('/home/wsj/data/kitty/RawData/2011_09_26/2011_09_26_drive_0005_sync/oxts/data/%010d.txt'%frame)
if prev_imu_data is not None:
displacement = 0.1*np.linalg.norm(imu_data[['vf','vl']])
yaw_change = float(imu_data.yaw-prev_imu_data.yaw)
for i in range(len(locations)):
x0, y0 = locations[i]
x1 = x0 * np.cos(yaw_change) + y0 * np.sin(yaw_change) - displacement
y1 = -x0 * np.sin(yaw_change) + y0 * np.cos(yaw_change)
locations[i] = np.array([x1,y1])
locations += [np.array([0,0])]
prev_imu_data =imu_data
plt.figure(figsize=(20,10))
plt.plot(np.array(locations)[:, 0],np.array(locations)[:, 1])
3、画出所有车辆的轨迹
class Object():
def __init__(self, center):
self.locations = deque(maxlen=20)
self.locations.appendleft(center)
def update(self, center, displacement, yaw):
for i in range(len(self.locations)):
x0, y0 = self.locations[i]
x1 = x0 * np.cos(yaw_change) + y0 * np.sin(yaw_change) - displacement
y1 = -x0 * np.sin(yaw_change) + y0 * np.cos(yaw_change)
self.locations[i] = np.array([x1,y1])
if center is not None:
self.locations.appendleft(center)
def reset(self):
self.locations = deque(maxlen=20)
#创建发布者
loc_pub = rospy.Publisher('kitti_loc', MarkerArray, queue_size=10)
#获取距离和旋转角度
imu_data = read_imu('/home/wsj/data/kitty/RawData/2011_09_26/2011_09_26_drive_0005_sync/oxts/data/%010d.txt'%frame)
if prev_imu_data is None:
for track_id in centers:
tracker[track_id] = Object(centers[track_id])
else:
displacement = 0.1*np.linalg.norm(imu_data[['vf','vl']])
yaw_change = float(imu_data.yaw - prev_imu_data.yaw)
for track_id in centers: # for one frame id
if track_id in tracker:
tracker[track_id].update(centers[track_id], displacement, yaw_change)
else:
tracker[track_id] = Object(centers[track_id])
for track_id in tracker:# for whole ids tracked by prev frame,but current frame did not
if track_id not in centers: # dont know its center pos
tracker[track_id].update(None, displacement, yaw_change)
prev_imu_data = imu_data
def publish_loc(loc_pub, tracker, centers):
marker_array = MarkerArray()
for track_id in centers:
marker = Marker()
marker.header.frame_id = FRAME_ID
marker.header.stamp = rospy.Time.now()
marker.action = marker.ADD
marker.lifetime = rospy.Duration(LIFETIME)
marker.type = Marker.LINE_STRIP
marker.id = track_id
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 1.0
marker.scale.x = 0.2
marker.points = []
for p in tracker[track_id].locations:
marker.points.append(Point(p[0], p[1], 0))
marker_array.markers.append(marker)
loc_pub.publish(marker_array)
来源:https://juejin.cn/post/7106412494125531173
0
投稿
猜你喜欢
- 1.如果每页都增加打印时间,又如何设置?打印时间的,你可以参考 for(var i=0;i<page.length;i++)
- curses 库 ( ncurses ) 提供了控制字符屏幕的独立于终端的方法。curses 是大多数类似于 UNIX 的系统(包括 Lin
- 我需要查询从现在算起五天前的日期。按照商业习惯,这五天应该不包含星期六和星期天。专家回答:对于许多跟商业日期有关的情况,最好的解决方案是使用
- Google Talk是一个功能很简洁的即时通讯工具,尤其是它的文字输入区域,不同于其他IM,除了一个文字输入区域外没有任何其他操作。但是用
- 幸运草又名四叶草,一般指四叶的苜蓿、或车轴草。在十万株苜蓿草中,你可能只会发现一株是四叶草,机会率大约是十万分之一。因此四叶草是国际公认的幸
- 我们可以利用Session对象来进行注册验证。Session对象会帮我们把某一用户的信息保留下来,让后续的网页读取。我们就可以在用户注册成功
- 本文介绍如何建立基于Web的日历,同时为不熟悉Active Server Pages(ASP)、SQL和ADO的开发者提供建立Web站点的过
- 一. 介绍fire是python中用于生成命令行界面(Command Line Interfaces, CLIs)的工具,不需要做任何额外的
- 当子类继承父类后,需要调用父类的方法和属性时,需要调用父类的初始化函数。class A(object): def __init_
- 计是一个输入-输出的过程,因为首先有用户的需求,客户的项目才有设计的产生,设计是带有目的性和市场行为的,当然也有一部分的创造性设计,仅仅为了
- 一种类似Flask开发的WebSocket-Server服务端框架,适用python3.X1、安装模块Pywsspip install py
- 1.0 为什么要做这个博客站?在工作学习中,经常要搜索查找各种各样的资料,每次找到相关资料后都会顺手添加到浏览器书签中,时间一长,书签也就满
- 在SQL Server中,为了查询性能的优化,有时我们就需要对数据表通过建立索引的方式,目的主要是根据查询要求,迅速缩小查询范围,避免全表扫
- 本文研究的主要是python实现Decorator模式,具体介绍如下。一般来说,装饰器是一个函数,接受一个函数(或者类)作为参数,返回值也是
- 最近做了一个微信小程序的项目,关于后端给我传递日期的时候,我拿到的是一串数字如:createDate: 1552117531000 ,这是一
- 1、引言小丝:鱼哥,2023年了, 你有啥愿望啊?小鱼:这, 我可以选择不告诉你吗?小丝:可以选择不告诉我,但是,你自己憋着,不难受吗?小鱼
- 如下所示:import torchfrom torch.autograd import Variableimport matplotlib.
- 前言提示:这里可以添加本文要记录的大概内容:将一个EXCEL等份拆成多个EXCEL将多个小EXCEL合并成一个大EXCEL并标记来源提示:以
- 不论什么时候,只要系统带有多个设备,而这些设备的性能又各不相同,就存在从慢速设备到快速设备不断更换工作地点以改善系统性能的可能性,这就是缓存
- 没人愿意等待。所以,没有访问者真的能够忍受一个打开速度极慢的网站。但是,网页打开速度到底对用户行为有什么影响,恐怕没几个人能够说清楚吧。前几