graph_viewer.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import roslib; roslib.load_manifest('stereo_slam')
00003 import sys
00004 import pylab
00005 import math
00006 import numpy as np
00007 import weakref
00008 import string
00009 import random
00010 import time
00011 import ntpath
00012 import os
00013 from matplotlib import pyplot
00014 from mpl_toolkits.mplot3d import Axes3D
00015 import tf.transformations as tf
00016 from scipy.misc import imread
00017 
00018 # Global variables
00019 lock_file = ""
00020 graph_edges_file = ""
00021 legend_edited = False
00022 ax_gt = None
00023 ax_odom = None
00024 ax_vertices = None
00025 ax_edges = []
00026 edges_shown = True
00027 gt_data = []
00028 plot_dim = 3
00029 
00030 class Error(Exception):
00031   """ Base class for exceptions in this module. """
00032   pass
00033 
00034 def check_file_len(file):
00035   """ Check if the file length is > 0 """
00036   f = open(file)
00037   lines = f.readlines()
00038   f.close()
00039   return len(lines) > 0
00040 
00041 def rm_ax(ax_id):
00042   """ Remove the axes lines """
00043   if (ax_id is not None and ax_id):
00044     l = ax_id.pop(0)
00045     wl = weakref.ref(l)
00046     l.remove()
00047     del l
00048 
00049 def real_time_plot(gt_file, odom_file, graph_vertices_file):
00050   """ Function to plot the data saved into the files in real time """
00051 
00052   global lock_file, legend_edited, ax_gt, ax_odom, ax_vertices, edges_shown, gt_data, plot_dim
00053 
00054   # Remove the main axes
00055   rm_ax(ax_gt)
00056   rm_ax(ax_odom)
00057   rm_ax(ax_vertices)
00058 
00059   # Ground truth
00060   if (gt_file != "" and os.path.exists(gt_file) and check_file_len(gt_file) and gt_file != "none"):
00061 
00062     # Check gt file type
00063     f = open(gt_file)
00064     lines = f.readlines()
00065     f.close()
00066     size = lines[1].split(",")
00067     if (len(size) > 12):
00068       data = pylab.loadtxt(gt_file, delimiter=',', skiprows=1, usecols=(0,5,6,7,8,9,10,11))
00069     else:
00070       data = pylab.loadtxt(gt_file, delimiter=',', usecols=(0,1,2,3,4,5,6,7))
00071 
00072     # Plot
00073     if (len(data.shape) == 1):
00074       data = [data]
00075       data = np.array(data)
00076     if (plot_dim == 3):
00077       ax_gt = ax.plot(data[:,1], data[:,2], data[:,3],'k', label='Ground Truth')
00078     else:
00079       ax_gt = ax.plot(data[:,1], data[:,2], 'k', label='Ground Truth')
00080 
00081   # Load visual odometry data (saved with rostopic echo -p /stereo_odometer/odometry > file.txt)
00082   if (odom_file != "" and os.path.exists(odom_file) and check_file_len(odom_file)):
00083 
00084     # Read the data
00085     data = pylab.loadtxt(odom_file, delimiter=',', skiprows=1, usecols=(5,6,7,8,9,10,11))
00086     # data = pylab.loadtxt(odom_file, delimiter=',', skiprows=1, usecols=(4,5,6,7,8,9,10))
00087 
00088     # Plot
00089     if (len(data.shape) == 1):
00090       data = np.array([data])
00091     if (plot_dim == 3):
00092       ax_odom = ax.plot(data[:,0], data[:,1], data[:,2], 'c', label='Visual Odometry')
00093     else:
00094       ax_odom = ax.plot(data[:,0], data[:,1], 'c', label='Visual Odometry')
00095 
00096   # Load stereo slam vertices (file saved with node stereo_slam)
00097   if (graph_vertices_file != "" and os.path.exists(graph_vertices_file) and check_file_len(graph_vertices_file)):
00098 
00099     # Check if file is blocked
00100     while (os.path.exists(lock_file)):
00101       time.sleep(0.5)
00102 
00103     # Read the data
00104     try:
00105       data = pylab.loadtxt(graph_vertices_file, delimiter=',', skiprows=0, usecols=(2,3,4,5,6,7,8))
00106     except:
00107       return;
00108 
00109     # Plot
00110     if (len(data.shape) == 1):
00111       data = np.array([data])
00112     if (plot_dim == 3):
00113       ax_vertices = ax.plot(data[:,0], data[:,1], data[:,2], 'b', label='Stereo slam', marker='o', zorder=1)
00114     else:
00115       ax_vertices = ax.plot(data[:,0], data[:,1], 'b', label='Stereo slam', marker='o', zorder=1)
00116 
00117   # Show the edges
00118   if (edges_shown == True):
00119     draw_edges()
00120   else:
00121     remove_edges()
00122 
00123   # Update the plot
00124   pyplot.draw()
00125 
00126   # Show legend only once
00127   if (ax_vertices is not None and legend_edited is False):
00128     #ax.legend()
00129     legend_edited = True
00130 
00131 def draw_edges():
00132   """ Draw the edges """
00133   global lock_file, graph_edges_file, ax_edges, edges_shown, plot_dim
00134 
00135   # First, remove previous edges
00136   remove_edges()
00137 
00138   # Load stereo slam edges (file saved with node stereo_slam)
00139   if (graph_edges_file != "" and os.path.exists(graph_edges_file) and check_file_len(graph_edges_file)):
00140 
00141     # Check if file is blocked
00142     while (os.path.exists(lock_file)):
00143       time.sleep(0.5)
00144 
00145     # Read the data
00146     try:
00147       data = pylab.loadtxt(graph_edges_file, delimiter=',', skiprows=0, usecols=(0,1,2,3,4,5,10,11,12))
00148     except:
00149       return;
00150 
00151     # Sanity check
00152     if (len(data.shape) == 1):
00153       edges_shown = True
00154       return
00155 
00156     # Inliers column
00157     inliers = data[:,2]
00158     max_inliers = max(inliers)
00159     min_inliers = min(inliers)
00160 
00161     valid_color = True
00162     if (min_inliers - max_inliers == 0):
00163       valid_color = False
00164 
00165     if (valid_color):
00166       # red color
00167       m_red = (255) / (min_inliers - max_inliers)
00168       n_red = -m_red * max_inliers
00169 
00170       # blue color
00171       m_blue = (255) / (max_inliers - min_inliers)
00172       n_blue = -m_blue * min_inliers
00173 
00174     # Plot current
00175     if (len(data.shape) == 1):
00176       data = np.array([data])
00177     ax_edges = []
00178     for i in range(len(data)):
00179 
00180       # Get the color depending on the inlier value
00181       if (valid_color):
00182         red = hex(int(m_red * data[i,2] + n_red))
00183         blue = hex(int(m_blue * data[i,2] + n_blue))
00184         red = red[2:]
00185         blue = blue[2:]
00186         if (len(red) == 1):
00187           red += '0'
00188         if (len(blue) == 1):
00189           blue += '0'
00190         color = '#' + red + '00' + blue
00191       else:
00192         color = 'b';
00193 
00194       vect = []
00195       vect.append([data[i,3], data[i,4], data[i,5]])
00196       vect.append([data[i,6], data[i,7], data[i,8]])
00197       vect =  np.array(vect)
00198       if (plot_dim == 3):
00199         ax_edge = ax.plot(vect[:,0], vect[:,1], vect[:,2], color, linestyle='-', zorder=1)
00200       else:
00201         ax_edge = ax.plot(vect[:,0], vect[:,1], color, linestyle='-', zorder=1)
00202       ax_edges.append(ax_edge)
00203   edges_shown = True
00204   return
00205 
00206 def remove_edges():
00207   """ Remove the edges """
00208   global ax_edges, edges_shown
00209 
00210   for i in range(len(ax_edges)):
00211     l = ax_edges[i].pop(0)
00212     wl = weakref.ref(l)
00213     l.remove()
00214     del l
00215 
00216   ax_edges = []
00217   edges_shown = False
00218 
00219 def onclick(event):
00220   """ Handle the click event """
00221   global edges_shown
00222 
00223   if (event.button == 3):
00224     if (edges_shown):
00225       remove_edges()
00226     else:
00227       draw_edges()
00228     pyplot.draw()
00229 
00230 
00231 if __name__ == "__main__":
00232   import argparse
00233   parser = argparse.ArgumentParser(
00234           description='Plot 3D graphics of odometry data files in real time.',
00235           formatter_class=argparse.ArgumentDefaultsHelpFormatter)
00236   parser.add_argument('-d','--d',
00237           help='directory with the files: gt.txt (optional), odom.txt, graph_vertices.txt, graph_edges.txt',
00238           default=".")
00239   parser.add_argument('-gt','--gt',
00240           help='file with ground truth',
00241           default="")
00242   parser.add_argument('-o','--o',
00243           help='file with visual odometry',
00244           default="")
00245   parser.add_argument('-v','--v',
00246           help='file with the vertices of stereo slam',
00247           default="")
00248   parser.add_argument('-e','--e',
00249           help='file with the edges of stereo slam',
00250           default="")
00251   parser.add_argument('-dim','--dim', type=int,
00252           help='defines the plot dimensions: 2 for xy and 3 for xyz',
00253           default=3)
00254   args = parser.parse_args()
00255   plot_dim = args.dim
00256 
00257   # Some hardcode parameters
00258   # font = {'family' : 'Sans',
00259   #         'weight' : 'normal',
00260   #         'size'   : 30}
00261   # pylab.rc('font', **font)
00262 
00263   print "GRAPH VIEWER MOUSE INPUTS:"
00264   print " - Right button: activates/deactivates the visualization of graph edges."
00265 
00266   # Set parameters
00267   global_dir            = args.d
00268   ground_truth_file     = args.gt
00269   visual_odometry_file  = args.o
00270   graph_vertices_file   = args.v
00271   graph_edges_file      = args.e
00272 
00273   # Default parameters
00274   if (global_dir != ""):
00275     if (global_dir[:-1] != "/"):
00276       global_dir += "/"
00277     visual_odometry_file = global_dir + "odom.txt"
00278     graph_vertices_file = global_dir + "graph_vertices.txt"
00279     graph_edges_file = global_dir + "graph_edges.txt"
00280     ground_truth_file = global_dir + "gt.txt"
00281   if not os.path.exists(ground_truth_file):
00282     ground_truth_file = "none"
00283 
00284   # Save blocking file into global
00285   lock_file = os.path.dirname(graph_vertices_file) + "/graph.lock"
00286 
00287   # Init figure
00288   fig = pylab.figure(1)
00289   if (plot_dim == 3):
00290     ax = Axes3D(fig)
00291     ax.set_zlabel("z (m)")
00292   else:
00293     ax = fig.gca()
00294     img = imread("/home/plnegre/Downloads/mosaic1.jpg")
00295     ax.imshow(img, zorder=0, extent=[-13.5, 34.5, -17, 9])
00296     ax.set_axis_bgcolor('black')
00297 
00298   ax.grid(True)
00299   # ax.set_title("Graph Viewer")
00300   ax.set_xlabel("x (m)")
00301   ax.set_ylabel("y (m)")
00302   # ax.set_xlim([0, 28])
00303   # ax.set_ylim([-10, 23])
00304 
00305   # Handle on click callback
00306   fig.canvas.mpl_connect('button_press_event', onclick)
00307 
00308   # Start timer for real time plot
00309   timer = fig.canvas.new_timer(2500)
00310   real_time_plot(ground_truth_file, visual_odometry_file, graph_vertices_file)
00311   timer.add_callback( real_time_plot,
00312                       ground_truth_file,
00313                       visual_odometry_file,
00314                       graph_vertices_file)
00315   timer.start()
00316   pylab.show()


stereo_slam
Author(s): Pep Lluis Negre
autogenerated on Thu Jun 6 2019 21:40:57