examples/slam/pose_graph_3d/plot_results.py

   1 #!/usr/bin/python
   2 #
   3 # Plots the results from the 3D pose graph optimization. It will draw a line
   4 # between consecutive vertices.  The commandline expects two optional filenames:
   5 #
   6 #   ./plot_results.py --initial_poses filename  --optimized_poses filename
   7 #
   8 # The files have the following format:
   9 #   ID x y z q_x q_y q_z q_w
  10
  11 from mpl_toolkits.mplot3d import Axes3D
  12 import matplotlib.pyplot as plot
  13 import numpy
  14 import sys
  15 from optparse import OptionParser
  16
  17 def set_axes_equal(axes):
  18     ''' Sets the axes of a 3D plot to have equal scale. '''
  19     x_limits = axes.get_xlim3d()
  20     y_limits = axes.get_ylim3d()
  21     z_limits = axes.get_zlim3d()
  22
  23     x_range = abs(x_limits[1] - x_limits[0])
  24     x_middle = numpy.mean(x_limits)
  25     y_range = abs(y_limits[1] - y_limits[0])
  26     y_middle = numpy.mean(y_limits)
  27     z_range = abs(z_limits[1] - z_limits[0])
  28     z_middle = numpy.mean(z_limits)
  29
  30     length = 0.5 * max([x_range, y_range, z_range])
  31
  32     axes.set_xlim3d([x_middle - length, x_middle + length])
  33     axes.set_ylim3d([y_middle - length, y_middle + length])
  34     axes.set_zlim3d([z_middle - length, z_middle + length])
  35
  36 parser = OptionParser()
  37 parser.add_option("--initial_poses", dest="initial_poses",
  38                   default="", help="The filename that contains the original poses.")
  39 parser.add_option("--optimized_poses", dest="optimized_poses",
  40                   default="", help="The filename that contains the optimized poses.")
  41 parser.add_option("-e", "--axes_equal", action="store_true", dest="axes_equal",
  42                   default="", help="Make the plot axes equal.")
  43 (options, args) = parser.parse_args()
  44
  45 # Read the original and optimized poses files.
  46 poses_original = None
  47 if options.initial_poses != '':
  48   poses_original = numpy.genfromtxt(options.initial_poses,
  49                                     usecols = (1, 2, 3))
  50
  51 poses_optimized = None
  52 if options.optimized_poses != '':
  53   poses_optimized = numpy.genfromtxt(options.optimized_poses,
  54                                      usecols = (1, 2, 3))
  55
  56 # Plots the results for the specified poses.
  57 figure = plot.figure()
  58
  59 if poses_original is not None:
  60   axes = plot.subplot(1, 2, 1, projection='3d')
  61   plot.plot(poses_original[:, 0], poses_original[:, 1], poses_original[:, 2],
  62             '-', alpha=0.5, color="green")
  63   plot.title('Original')
  64   if options.axes_equal:
  65     axes.set_aspect('equal')
  66     set_axes_equal(axes)
  67
  68
  69 if poses_optimized is not None:
  70   axes = plot.subplot(1, 2, 2, projection='3d')
  71   plot.plot(poses_optimized[:, 0], poses_optimized[:, 1], poses_optimized[:, 2],
  72             '-', alpha=0.5, color="blue")
  73   plot.title('Optimized')
  74   if options.axes_equal:
  75     axes.set_aspect('equal')
  76     set_axes_equal(plot.gca())
  77
  78
  79 # Show the plot and wait for the user to close.
  80 plot.show()