plot_ik_grid.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 """
00004 Visualize csv file of ik-grid in 2d heatmap
00005 """
00006 import matplotlib.pyplot as plt
00007 import sys
00008 import csv
00009 from itertools import chain
00010 import numpy as np
00011 import math
00012 
00013 csvfile = sys.argv[1]
00014 if len(sys.argv) == 3:
00015     image_file = sys.argv[2]
00016 else:
00017     image_file = None
00018 
00019 
00020 data_by_k = dict()
00021 # read data from csv file
00022 with open(csvfile) as f:
00023     reader = csv.reader(f)
00024     index = reader.next()
00025     # x, y, z, i, j, k. value
00026     x_index = index.index("x")
00027     y_index = index.index("y")
00028     z_index = index.index("z")
00029     i_index = index.index("i")
00030     j_index = index.index("j")
00031     k_index = index.index("k")
00032     value_index = index.index("value")
00033     for row in reader:
00034         (x, y, z, i, j, k, value) = [float(row[x_index]),
00035                                      float(row[y_index]),
00036                                      float(row[z_index]),
00037                                      int(row[i_index]),
00038                                      int(row[j_index]),
00039                                      int(row[k_index]),
00040                                      float(row[value_index])]
00041         data = {"x": x,
00042                 "y": y,
00043                 "z": z,
00044                 "i": i,
00045                 "j": j,
00046                 "k": k,
00047                 "value": value}
00048         if data_by_k.has_key(k):
00049             data_by_k[k].append(data)
00050         else:
00051             data_by_k[k] = [data]
00052 k_num = len(data_by_k.keys())
00053 
00054 # print "{0} k values".format(k_num)
00055 
00056 # Estimating grid size
00057 z_values = np.array(list(chain.from_iterable([[data["z"] for data in data_array]
00058                                               for data_array in data_by_k.values()])))
00059 min_z = np.amin(z_values)
00060 max_z = np.amax(z_values)
00061 # print "z value from {0} to {1}".format(min_z, max_z)
00062 step = (max_z - min_z) / (k_num - 1)
00063 # print "estimated step is", step
00064 
00065 fig, axes = plt.subplots(int(math.ceil(k_num / 3)), 3)
00066 images = dict()
00067 non_zero_counter = 0
00068 for k, counter in zip(data_by_k, range(k_num)):             # need to sort by k?
00069     data_array = data_by_k[k]
00070     i_s = sorted(set([data["i"] for data in data_array]))
00071     j_s = sorted(set([data["j"] for data in data_array]))
00072     xnum = len(i_s)
00073     ynum = len(j_s)
00074     min_i = min([data["i"] for data in data_array])
00075     min_j = min([data["j"] for data in data_array])
00076     min_value = min([data["value"] for data in data_array])
00077     max_value = max([data["value"] for data in data_array])
00078     min_x = min([data["x"] for data in data_array])
00079     max_x = max([data["x"] for data in data_array])
00080     min_y = min([data["y"] for data in data_array])
00081     max_y = max([data["y"] for data in data_array])
00082     # print "{0}-{1}".format(xnum, ynum)
00083     image = np.arange(0, xnum * ynum, 1.0).reshape((xnum, ynum))
00084     for data in data_array:
00085         i = data["i"]
00086         j = data["j"]
00087         image[i - min_i][j - min_j] = data["value"]
00088         if data["value"] != 0:
00089             non_zero_counter = non_zero_counter + 1
00090     images[k] = image
00091     ax = axes.flat[counter]
00092     # print "x: {0} - {1}".format(min_x, max_x)
00093     # print "y: {0} - {1}".format(min_y, max_y)
00094     # ax.set_xticklabels([i  * istep + min_x for i in range(xnum-1)])
00095     ax.set_title("$z = {0}$mm".format(data_array[0]["z"]))
00096     ax.set_xlabel("$x$ [mm]")
00097     ax.set_ylabel("$y$ [mm]")
00098     ax.tick_params(labelsize=8)
00099     im = ax.imshow(image, vmin=min_value, vmax=max_value, cmap="gnuplot", interpolation="none",
00100                    aspect="auto",
00101                    extent=[min_x-step/2, max_x+step/2, min_y-step/2, max_y+step/2])
00102 # cbar_ax = fig.add_axes([0.95, 0.15, 0.01, 0.7])
00103 # cb = fig.colorbar(im, cax=cbar_ax)
00104 print "{0} reached regions".format(non_zero_counter)
00105 fig.suptitle("{0} reached regions".format(non_zero_counter), fontsize=8)
00106 plt.tight_layout()
00107 
00108 if image_file:
00109     plt.savefig(image_file)
00110 else:
00111     plt.show()


jsk_ik_server
Author(s): furuta
autogenerated on Fri Apr 19 2019 03:45:39