sync_path_gt.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 import roslib
3 import rospy
4 import os
5 import tf
6 import numpy
7 import evaluate_ate
8 from nav_msgs.msg import Path
9 from geometry_msgs.msg import Pose
10 from geometry_msgs.msg import Point
11 
12 def callback(data):
13  global slamPoses
14  global gtPoses
15  global stamps
16  global listener
17  global lastSize
18  global slamPosesInd
19  global rmse
20  if lastSize != len(data.poses):
21  t = rospy.Time(data.poses[len(data.poses)-1].header.stamp.secs, data.poses[len(data.poses)-1].header.stamp.nsecs)
22  try:
23  listener.waitForTransform(fixedFrame, baseFrame, t, rospy.Duration(0.2))
24  (trans,rot) = listener.lookupTransform(fixedFrame, baseFrame, t)
25  gtPoses.append(Pose(trans, rot))
26  stamps.append(t.to_sec())
27  slamPoses.append(data.poses[len(data.poses)-1].pose)
28  slamPosesInd.append(len(data.poses)-1)
29  first_xyz = numpy.empty([0,3])
30  second_xyz = numpy.empty([0,3])
31  for g in gtPoses:
32  newrow = [g.position[0],g.position[1],g.position[2]]
33  first_xyz = numpy.vstack([first_xyz, newrow])
34  for p in slamPoses:
35  newrow = [p.position.x,p.position.y,p.position.z]
36  second_xyz = numpy.vstack([second_xyz, newrow])
37  first_xyz = numpy.matrix(first_xyz).transpose()
38  second_xyz = numpy.matrix(second_xyz).transpose()
39  rot,trans,trans_error = evaluate_ate.align(second_xyz, first_xyz)
40  rmse_v = numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))
41  rmse.append(rmse_v)
42  print "points= " + str(len(data.poses)) + " added=" + str(len(slamPoses)) + " rmse=" + str(rmse_v)
44  print str(e)
45  if len(data.poses) > 0 and len(slamPosesInd) > 0:
46  j=0
47  for i in slamPosesInd:
48  slamPoses[j] = data.poses[i].pose
49  j+=1
50  lastSize = len(data.poses)
51 
52 if __name__ == '__main__':
53  rospy.init_node('sync_path_gt', anonymous=True)
54  listener = tf.TransformListener()
55  fixedFrame = rospy.get_param('~fixed_frame_id', 'world')
56  baseFrame = rospy.get_param('~frame_id', 'base_link_gt')
57  rospy.Subscriber("mapPath", Path, callback, queue_size=1)
58  slamPoses = []
59  gtPoses = []
60  stamps = []
61  slamPosesInd = []
62  rmse = []
63  lastSize = 0
64  rospy.spin()
65  fileSlam = open('slam_poses.txt','w')
66  fileGt = open('gt_poses.txt','w')
67  fileRMSE = open('rmse.txt','w')
68  print "slam= " + str(len(slamPoses))
69  print "gt= " + str(len(gtPoses))
70  print "stamps= " + str(len(stamps))
71  for c, g, t, r in zip(slamPoses, gtPoses, stamps, rmse):
72  fileSlam.write('%f %f %f 0 0 0 0 1\n' % (t, c.position.x, c.position.y))
73  fileGt.write('%f %f %f 0 0 0 0 1\n' % (t, g.position[0], g.position[1]))
74  fileRMSE.write('%f %f\n' % (t, r))
75  fileSlam.close()
76  fileGt.close()
77  fileRMSE.close()
def align(model, data)
Definition: evaluate_ate.py:47


rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Tue Jan 24 2023 04:04:40