ethzasl_icp_mapper_sync_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 Odometry
9 from geometry_msgs.msg import Point
10 
11 def callback(data):
12  global slamPoses
13  global gtPoses
14  global stamps
15  global listener
16  global rmse
17  global lastTime
18 
19  if rospy.get_time() - lastTime < 1:
20  return
21  lastTime = rospy.get_time()
22 
23  t = rospy.Time(data.header.stamp.secs, data.header.stamp.nsecs)
24  try:
25  listener.waitForTransform(fixedFrame, baseFrame, t, rospy.Duration(0.2))
26  (trans,rot) = listener.lookupTransform(fixedFrame, baseFrame, t)
27  gtPoses.append(Point(trans[0], trans[1], 0))
28  stamps.append(t.to_sec())
29  slamPoses.append(data.pose.pose.position)
30  first_xyz = numpy.empty([0,3])
31  second_xyz = numpy.empty([0,3])
32  for g in gtPoses:
33  newrow = [g.x,g.y,g.z]
34  first_xyz = numpy.vstack([first_xyz, newrow])
35  for p in slamPoses:
36  newrow = [p.x,p.y,p.z]
37  second_xyz = numpy.vstack([second_xyz, newrow])
38  first_xyz = numpy.matrix(first_xyz).transpose()
39  second_xyz = numpy.matrix(second_xyz).transpose()
40  rot,trans,trans_error = evaluate_ate.align(second_xyz, first_xyz)
41  rmse_v = numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))
42  rmse.append(rmse_v)
43  print " added=" + str(len(slamPoses)) + " rmse=" + str(rmse_v)
45  print str(e)
46 
47 if __name__ == '__main__':
48  rospy.init_node('sync_odom_gt', anonymous=True)
49  listener = tf.TransformListener()
50  fixedFrame = rospy.get_param('~fixed_frame_id', 'world')
51  baseFrame = rospy.get_param('~frame_id', 'base_link_gt')
52  rospy.Subscriber("icp_odom", Odometry, callback, queue_size=1)
53  slamPoses = []
54  gtPoses = []
55  stamps = []
56  rmse = []
57  lastTime = rospy.get_time()
58  rospy.spin()
59  fileSlam = open('slam_poses.txt','w')
60  fileGt = open('gt_poses.txt','w')
61  fileRMSE = open('rmse.txt','w')
62  print "slam= " + str(len(slamPoses))
63  print "gt= " + str(len(gtPoses))
64  print "stamps= " + str(len(stamps))
65  for c, g, t, r in zip(slamPoses, gtPoses, stamps, rmse):
66  fileSlam.write('%f %f %f 0 0 0 0 1\n' % (t, c.x, c.y))
67  fileGt.write('%f %f %f 0 0 0 0 1\n' % (t, g.x, g.y))
68  fileRMSE.write('%f %f\n' % (t, r))
69  fileSlam.close()
70  fileGt.close()
71  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