cartographer定位结果的数据分析

最近忙着做实车定位，所以好久没写博客了，鉴于有博友问我一些关于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轴的变化，变换矩阵考虑的也是二维坐标系的变换}