00001
00002 import roslib; roslib.load_manifest('stereo_slam')
00003 import pylab
00004 import numpy as np
00005 from matplotlib import pyplot
00006 from mpl_toolkits.mplot3d import Axes3D
00007
00008 class Error(Exception):
00009 """ Base class for exceptions in this module. """
00010 pass
00011
00012 def get_xyz(gps_point):
00013 lat = gps_point[3]*np.pi/180
00014 lon = gps_point[4]*np.pi/180
00015 alt = gps_point[5]*np.pi/180
00016 a = 6378137.0
00017 f = 1/298.257223563
00018 e2 = 2*f - np.power(f,2)
00019
00020 chi = np.sqrt(1-e2 * np.power(np.sin(lat),2));
00021 x = (a/chi +alt) * np.cos(lat) * np.cos(lon);
00022 y = (a/chi +alt) * np.cos(lat) * np.sin(lon);
00023 z = (a*(1-e2)/chi + alt) * np.sin(lat);
00024 return x, y, z
00025
00026 if __name__ == "__main__":
00027 rospy.init_node('gps_to_std_gt')
00028 import argparse
00029 parser = argparse.ArgumentParser(
00030 description='Convert gps/fix topic to standard ground truth file',
00031 formatter_class=argparse.ArgumentDefaultsHelpFormatter)
00032 parser.add_argument('gps_topic_file',
00033 help='file with the gps/fix topic. Saved with "rostopic echo -p /gps/fix > data/anselm_turmeda/gt_gps.txt"')
00034 parser.add_argument('output_file',
00035 help='output file where the standard ground truth values will be saved.')
00036 args = parser.parse_args()
00037
00038
00039 gps_topic = pylab.loadtxt(args.gps_topic_file, delimiter=',', skiprows=1, usecols=(0,1,2,6,7,8))
00040
00041
00042 with open(args.output_file, 'w') as outfile:
00043 outfile.write("%time,field.header.seq,field.header.stamp,field.header.frame_id,field.child_frame_id,x,y,z,qx,qy,qz,qw\n")
00044 x0, y0, z0 = get_xyz(gps_topic[0])
00045 gt = []
00046 gt.append([0.0, 0.0, 0.0])
00047 for i in range(len(gps_topic)-1):
00048 x, y, z = get_xyz(gps_topic[i+1])
00049 x = x-x0
00050 y = y-y0
00051 z = z-z0
00052 gt.append([x, y, z])
00053 outfile.write("%.9F," % gps_topic[i+1, 0])
00054 outfile.write(str(gps_topic[i+1, 1]) + "," + str(gps_topic[i+1, 2]) + ",/gps,,")
00055 outfile.write("%.9F," % x)
00056 outfile.write("%.9F," % y)
00057 outfile.write("%.9F," % z)
00058 outfile.write("0.0,0.0,0.0,0.0")
00059 outfile.write("\n")
00060 gt = np.array(gt)
00061
00062
00063 fig = pylab.figure(1)
00064 ax = Axes3D(fig)
00065 ax.grid(True)
00066 ax.set_title("GT Viewer")
00067 ax.set_xlabel("X")
00068 ax.set_ylabel("Y")
00069 ax.set_zlabel("Z")
00070
00071 ax.plot(gt[:,0], gt[:,1], gt[:,2], 'g', label='Ground Truth')
00072 ax.legend()
00073
00074 pyplot.draw()
00075 pylab.show()