最近忙着做实车定位,所以好久没写博客了,鉴于有博友问我一些关于carto的数据分析的问题,这里抽空写些简短的东西给大家,顺便分享一下自己写的(辣鸡)脚本,希望对各位有帮助。
首先,我们需要做好自己的定位,并录下自己定位数据的bag,因为我们的项目目前是考虑到了GPS信号不好时切换至slam 定位,所以,我这里只做一些SLAM和gps 的数据分析对比。
那么,明确自己的bag中gps和SLAM结果的话题是刚需,比我的bag中,就是/trajectory_node_list和/gpsposition这两个话题。那么,如下xxx.py文件,我只做1s保存一个slam定位数据至csv,文件中。(gps同理,自己发挥吧!!!)
#!/usr/bin/env python
# _*_ coding:utf-8 _*_
#JiaoWoLiXianSheng
import rospy
import csv
import time
from visualization_msgs.msg import MarkerArray
fileName_SlamPoints = open('slamPoints.csv', 'wb')
SlamPoints_writer = csv.DictWriter(fileName_SlamPoints, fieldnames=['time', 'x', 'y', 'z'])
SlamPoints_writer.writeheader()
t = [1590668634,]#包第一帧数据的第一个时间戳
def callback(msg):
start = msg.markers[5].header.stamp.secs
if start - t[-1]>0:
t.append(start)
SlamPoints_writer.writerow({'time': str(msg.markers[5].header.stamp.secs),'x': str(msg.markers[5].points[-1].x),'y': str(msg.markers[5].points[-1].y),'z': str(msg.markers[5].points[-1].z)})
def run():
rospy.init_node("saveSlamPoints",anonymous=True)
rospy.Subscriber("/trajectory_node_list",MarkerArray,callback)
rospy.spin()
if __name__ == '__main__':
try:
run()
except rospy.ROSInterruptException:
pass
roscore
rosbag play xxx.bag
python xxx.py
等bag播完,就可看到slamPoints.csv文件中 内容,打开后连接轨迹点,如图所示:
这是建图数据,数据并未闭环,不过,此篇重在解释数据分析的方法。同理,我们来看gps.csv文件中轨迹图:
即,录的建图轨迹与GPS在此路段的轨迹图。我们需要将两张图放在一个坐标系进行比较,就需要算两个坐标系中,(x,y)在相同时间戳下的转换矩阵(这一步不是我做的,网上也有很多教程,大家自行发挥吧)。
gps:高斯坐标系。SLAM:SLAM坐标系,以起点为原点。
统一至同一个坐标系后如下图:
蓝色的是gps轨迹,绿色是slam 轨迹!直观 可以看出此次建图的效果还是很不错的。那么,车的行驶,除了位置,还需要一个东西,就是航向角,GPS的航向角可以通过传感器读取,SLAM的我们就要自己算了。
首先,将slam.csv和gps.csv文件分别读取并计算保存航向角(GPS这个传感器有航向角,所以之计算slam.csv文件中的航向角就可以)
#!/usr/bin/python2.7
# _*_ coding:utf-8 _*_
#JiaoWoLiXianSheng
import rospy
from visualization_msgs.msg import MarkerArray
from util.msg import GpsPosition
from math import *
import matplotlib.pyplot as plt
import numpy as np
import csv
fileName_sig = open("slamHeading.csv","wb")
sig_writer = csv.DictWriter(fileName_sig,fieldnames=['time','dis','slam','gps'])
sig_writer.writeheader()
global s
s =[[0,0,1]]
global ori
ori = [[0,0,1]] #坐标原点
global o
o = [[1589012,366181,1]]
global og
og = [[1589012,366181,1]]#GPS坐标原点
global t
t =[0]
#高斯坐标系下的旋转矩阵
Trans = np.array([[ 8.90950224e-01, 4.54100978e-01, 3.66203777e+05],
[ -4.54100978e-01, 8.90950224e-01, 1.58913979e+06],
[ 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])
def callback(msg):
#global s
p1 = np.array([msg.markers[2].points[-1].x,msg.markers[2].points[-1].y,1])
Y = p1[1]-s[-1][1]
X = p1[0]-s[-1][0]
dis = sqrt(X**2 +Y**2)
my = p1[1]-ori[0][1]
nx = p1[0]-ori[0][0]
dis0 = sqrt(my**2+nx**2)#跟原点的距离
g1 = np.dot(Trans,p1.T)#变换到高斯坐标系下
g2 = np.dot(Trans,np.array(s[-1]).T)
start = msg.markers[2].header.stamp.secs
if start - t[-1] > 0:
t.append(start)
if dis >0.2:
s.append(p1)
if len(s) >10:
s.pop(0)
Y0 = g1[1] - g2[1]
X0 = g1[0] - g2[0]
brng = degrees(atan2(Y0,X0))
brng = (brng + 360)%360
#print brng
sig_writer.writerow({'time':str(start),'dis':str(dis0),'slam':str(round(brng,2)*100)})
print len(s),s[-1]
pass
def run():
rospy.init_node("slamToGpsPosition",anonymous=True)
rospy.Subscriber("/trajectory_node_list",MarkerArray,callback)
rospy.spin()
if __name__ == "__main__":
try:
run()
except rospy.ROSInterruptException:
pass
获得slamheading.csv文件,假设你也获得了gpsheading.csv文件,那么我们用plt画出这两个文件的样子。
红色是slam,蓝色是gps。额,画图脚本在这里
#!/usr/bin/python2.7
# _*_ coding:utf-8 _*_
#JiaoWoLiXianSheng
import matplotlib.pyplot as plt
import csv
gpsFile = open("./gpsheading.csv",'r')
slamFile = open("./slamHeading.csv",'r')
gpsRead = csv.DictReader(gpsFile)
slamRead = csv.DictReader(slamFile)
tg = []
ts = []
g = []
s = []
#print type(gpsRead)
for row in gpsRead:
#print row
tg.append(float(row['time']))
g.append(float(row['gps']))
for r in slamRead:
ts.append(float(r['time']))
s.append(float(r['slam']))
#print len(s),len(ts),len(g),len(tg)
plt.scatter(tg,g,color="b")
plt.scatter(ts,s,color='r')
plt.show()
gpsFile.close()
slamFile.close()
这就是我们目前做定位时车辆需要的位姿信息了,(x,y,z)和航向角heading。{z轴由于是车体上下运动的标杆,而我们的场景里只有水平,所以忽略了z轴的变化,变换矩阵考虑的也是二维坐标系的变换}