draw-graph-from-rosbag.py
Go to the documentation of this file.
00001 import rosbag
00002 import argparse
00003 import matplotlib.pyplot, numpy, mpl_toolkits.mplot3d.axes3d
00004 from BayesianCurveFitting import *
00005 from math import *
00006 
00007 def parse_args():
00008     parser = argparse.ArgumentParser(description='plot data from rosbag')
00009     parser.add_argument('inbag', type=str, help='input bag file', metavar='bagfile')
00010     parser.add_argument('-t', type=int, help='1 or 2', metavar='graph type', choices=[1, 2], default=2)
00011     parser.add_argument('-u', type=int, help='step in u', default=80)
00012     parser.add_argument('-v', type=int, help='step in v', default=80)
00013     parser.add_argument('-d', type=float, help='step in depth', default=0.1)
00014     parser.add_argument('--max_depth', type=float, help='maximum depth', default=1.5)
00015     parser.add_argument('-D', type=int, help='D degree polynomial', default=2)
00016     parser.add_argument('-a', type=float, help='ALPHA', default=0.005)
00017     parser.add_argument('-b', type=float, help='BETA', default=11.1)
00018     return parser.parse_args()
00019 
00020 def setup_data_list(v_num, u_num):
00021     data_list = []
00022     for v_i in range(0, v_num):
00023         data_list.append([])
00024         for u_i in range(0, u_num):
00025             data_list[v_i].append([])
00026     return data_list
00027 
00028 def process(args):
00029     width = 640
00030     height = 480
00031     step_u = args.u
00032     step_v = args.v
00033     data_list = setup_data_list(height/step_v, width/step_u)
00034     f, (axes_array) = matplotlib.pyplot.subplots(len(data_list), len(data_list[0]), sharex='col', sharey='row')
00035     with rosbag.Bag(args.inbag) as b:
00036         for topic, msg, t in b.read_messages():
00037             u_i = msg.u / step_u
00038             v_i = msg.v / step_v
00039             if abs((msg.observed_depth - msg.true_depth) * 1000) < 1000: # for outlier
00040                 data_list[v_i][u_i].append([msg.observed_depth, # x, [m]
00041                                             (msg.observed_depth - msg.true_depth) * 1000]) # y, [mm]
00042     for x in range(0, len(axes_array[0])):
00043         for y in range(0, len(axes_array)):
00044             xlist = [tmp[0] for tmp in data_list[y][x]]
00045             ylist = [tmp[1] for tmp in data_list[y][x]]
00046             bcf = BayesianCurveFitting(args.D, args.a, args.b, xlist, ylist).main()
00047             axes_array[y][x].plot(xlist, ylist, '.')
00048             axes_array[y][x].plot(bcf[0], bcf[1], 'r-')
00049             axes_array[y][x].plot(bcf[0], bcf[2], 'g--')
00050             axes_array[y][x].plot(bcf[0], bcf[3], 'g--')
00051             axes_array[y][x].set_title('u : %d~%d, v : %d~%d' % (x*step_u, (x+1)*step_u, y*step_v, (y+1)*step_v))
00052     matplotlib.pyplot.show()
00053 
00054 
00055 def setup_data_list2(d_num):
00056     data_list = []
00057     for d_i in range(d_num):
00058         data_list.append([])
00059     return data_list
00060 
00061 def process2(args):
00062     max_depth = args.max_depth
00063     step_d = args.d
00064     data_list = setup_data_list2(int(max_depth / step_d))
00065     fig = matplotlib.pyplot.figure(figsize=matplotlib.pyplot.figaspect(0.5))
00066     axes_array = []
00067     for i in range(len(data_list)):
00068         axes_array.append(fig.add_subplot(int(sqrt(len(data_list))) + 1, int(sqrt(len(data_list))) + 1, i+1, projection='3d'))
00069     with rosbag.Bag(args.inbag) as b:
00070         for topic, msg, t in b.read_messages():
00071             if msg.true_depth < step_d * len(data_list):
00072                 d_i = int(msg.true_depth / step_d)
00073                 if abs((msg.observed_depth - msg.true_depth) * 1000) < 1000: # for outlier
00074                     data_list[d_i].append([msg.u, msg.v,
00075                                            (msg.observed_depth - msg.true_depth) * 1000])
00076     for d in range(len(axes_array)):
00077         xlist = numpy.array([tmp[0] for tmp in data_list[d]])
00078         ylist = numpy.array([tmp[1] for tmp in data_list[d]])
00079         X, Y = numpy.meshgrid(xlist, ylist)
00080         zlist = numpy.array([tmp[2] for tmp in data_list[d]])
00081         axes_array[d].scatter3D(xlist, ylist, zlist, c='b', marker='o')
00082         # axes_array[d].plot_surface(X, Y, zlist)
00083         # axes_array[d].plot_wireframe(xlist, ylist, zlist)
00084         axes_array[d].set_title('d : %.2f~%.2f[m]' % (d*step_d, (d+1)*step_d))
00085     matplotlib.pyplot.show()
00086 
00087 
00088 
00089 def main():
00090     args = parse_args()
00091     if args.t == 1:
00092         process(args)
00093     else:
00094         process2(args)
00095 
00096 if __name__ == '__main__':
00097     main()


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Tue Jul 2 2019 19:41:42