Some mild refactoring and commenting... Looks about right to start te…

…sting on the trig-00 data.

fit_mapping.py

@@ -17,24 +17,14 @@
 import sys
 import itertools
 
+from sim_stereo import distance_from_disparity
 
 import mpl_toolkits.mplot3d.axes3d as p3
 
+from color_block import gucci_dict
 
 import pdb
 
-## '#51c373', '#ea6949', '#a370ff' -> original 'G. color block rip-off' palette
-gucci_dict = {'red': ((0.0, 0x51/255., 0x51/255.,),
-                      (0.5, 0xea/255., 0xea/255.,),
-                      (1.0, 0xa3/255., 0xa3/255.,),),
-              'green': ((0.0, 0xc3/255., 0xc3/255.,),
-                        (0.5, 0xc3/255., 0xc3/255.,),
-                        (1.0, 0x70/255., 0x70/255.,),),
-              'blue': ((0.0, 0x73/255., 0x73/255.,),
-                       (0.5, 0xb5/255., 0xb5/255.,),
-                       (1.0, 0xff/255., 0xff/255.,),),
-             }
-
 class IntrinsicParameters:
   def __init__(self, f, center):
     self.f = f
@@ -43,15 +33,7 @@ def __init__(self, f, center):
   ## The magical formula that gives distance form the disparity. This is the
   ## theoretical perfect model, a x**-1 expression.
   def distance_from_disparity(self, d):
-    ## "identity" version
-    #return 1/(d/1e3)
-    # return 3e2-1./(d/5e1) ## for cone-00
-    # return 2-1./(d/5e3) ## for trig-00
-    return 2-1./(d/5e1) ## for trig-00
-    # return 1000-1/(d/1e5)
-    ## Correct version, inverse of the function from http://mathnathan.com/2011/02/03/depthvsdistance/
-    ## return 348.0 / (1091.5 - d)
-
+    return distance_from_disparity(d)
 
   def coordinates_from_disparity(self, disparity):
     ## Calculate the world coordinates of each pixel.
@@ -82,71 +64,75 @@ def generate_xyz_mesh(self):
 
 if __name__ == '__main__':
 
-  ion()
+  ion() ## Turn on real-time plotting
+
+  ## Plot stuff or not?
+  do_plot = True
 
   register_cmap(name='guc', data=gucci_dict)
-  # rc('image', cmap='RdBu')
   rc('image', cmap='guc')
+  # rc('image', cmap='RdBu')
 
   ## Check number of parameters
   if len(sys.argv)<2:
     raise Exception('''Incorrect number of parameters.
 
 Usage: %s <data_path>'''%(sys.argv[0]))
 
-  ## Gete the name of directory that contains the data.
+  ## Get the name of directory that contains the data. It should contain two
+  ## files named 'params.txt' and 'disparity.txt'.
   data_path = '%s/'%(sys.argv[1])
 
-  print data_path+'params.txt'
-  print data_path+'disparity.txt'
-
-  # [f, p[0], p[1], p[2], theta, phi, psi, k]
-  params_file = loadtxt(data_path+'params.txt')
+  ## Load the image with the disparity values. E.g., the range data produced by Kinect.
   disparity = loadtxt(data_path+'disparity.txt')
-
-
-
-
-  fig = plt.figure(figsize=plt.figaspect(.5))
-  fig.suptitle('Calculation of 3D coordinates from range data (with quantization)', fontsize=20, fontweight='bold')
-
-  ax = fig.add_subplot(1,2,1)
-  #p3.Axes3D(fig, rect = [.05, .2, .4, .6])
-
-  title('Kinect data (disparity)', fontsize=16)
-
-  cax = ax.imshow(disparity, interpolation='nearest')
-  #colorbar(cax)
-
-  #mypar = IntrinsicParameters(300, array([200,200]))
-  mypar = IntrinsicParameters(params_file[0], .5*(1+array([disparity.shape[1], disparity.shape[0]])))
+  ## Load the file with the camera parameters used to render the scene
+  ## The values are: [f, p[0], p[1], p[2], theta, phi, psi, k]
+  params_file = loadtxt(data_path+'params.txt')
+  ## The optical center is another important intrinsic parameter, but the
+  ## current simulator just pretend this is not an issue. So the optical center
+  ## is just the middle of the image, and there is also no radial lens
+  ## distortion.
+  optical_center = .5*(1+array([disparity.shape[1], disparity.shape[0]]))
+
+  ## Instantiate intrinsic parameters object.
+  mypar = IntrinsicParameters(params_file[0], optical_center)
+  ## Instantiate mesh object, and calculate grid parameters in 3D from the
+  ## disparity array and intrinsic parameters.
   sqmesh = SquareMesh(disparity, mypar)
   sqmesh.generate_xyz_mesh()
 
-
-  ax = p3.Axes3D(fig, rect = [.55, .2, .4, .6], aspect='equal')
-  title('Square mesh on 3D space', fontsize=16)
   x,y,z = sqmesh.xyz.T
   x = x.reshape(disparity.shape)
   y = y.reshape(disparity.shape)
   z = z.reshape(disparity.shape)
 
-  ## For debugging, just use image coordinates "r and s" for x and y, and the disparity for z.
-  #x,y = mgrid[:disparity.shape[0],:disparity.shape[1]]
-  #z = disparity
-
-  P = 12
+  P = 6 ## 6 for trig-00
   x = x[::P,::P]
   y = y[::P,::P]
   z = z[::P,::P]
 
-  ax.axis('equal')
-  ax.plot_wireframe(x,y,z)
-
-  mrang = max([x.max()-x.min(), y.max()-y.min(), z.max()-z.min()])/2
-  midx = (x.max()+x.min())/2
-  midy = (y.max()+y.min())/2
-  midz = (z.max()+z.min())/2
-  ax.set_xlim3d(midx-mrang, midx+mrang)
-  ax.set_ylim3d(midy-mrang, midy+mrang)
-  ax.set_zlim3d(midz-mrang, midz+mrang)
+  #############################################################################
+  ## Plot the disparity as an image
+  if do_plot:
+    fig = plt.figure(figsize=plt.figaspect(.5))
+    fig.suptitle('Calculation of 3D coordinates from range data (with quantization)', fontsize=20, fontweight='bold')
+    ax = fig.add_subplot(1,2,1)
+    title('Kinect data (disparity)', fontsize=16)
+    cax = ax.imshow(disparity, interpolation='nearest')
+    colorbar(cax, shrink=.5)
+
+    ax = p3.Axes3D(fig, rect = [.55, .2, .4, .6], aspect='equal')
+    title('Square mesh on 3D space', fontsize=16)
+
+    ## Plot the disparity as an image
+
+    ax.axis('equal')
+    ax.plot_wireframe(x,y,z)
+
+    mrang = max([x.max()-x.min(), y.max()-y.min(), z.max()-z.min()])/2
+    midx = (x.max()+x.min())/2
+    midy = (y.max()+y.min())/2
+    midz = (z.max()+z.min())/2
+    ax.set_xlim3d(midx-mrang, midx+mrang)
+    ax.set_ylim3d(midy-mrang, midy+mrang)
+    ax.set_zlim3d(midz-mrang, midz+mrang)

sim_stereo.py

@@ -174,20 +174,28 @@ def trig_get_texture_coordinates(verticesW,k):
 
 
 def disparity_from_range(z):
-  #d = zeros(z.shape, dtype=uint16)
-  d = zeros(z.shape, dtype=float)
+  d = zeros(z.shape, dtype=uint16)
+  # d = zeros(z.shape, dtype=float)
   ## "identity" function, for testing. Scaling is necessary because output is not floating-point.
   # d[:] = 1e4 * z
   # d[:] = 5e3* (1./(3e2-z)) #for cone-00
   # d[:] = 5e3* (1./(2-z)) #for trig-00
-  d[:] = 5e1* (1./(2-z)) #for trig-00
+  # d[:] = 5e1* (1./(2-z)) #for trig-00
 
   ## from http://mathnathan.com/2011/02/03/depthvsdistance/
-  #d[:] = floor(0.5+ 1091.5 - 348.0/z)
-
+  d[:] = floor(0.5+ 1091.5 - 348.0/z)
   return d
 
 
+def distance_from_disparity(d):
+  z = zeros(d.shape, dtype=float)
+  ## "identity" version
+  #return 1/(d/1e3)
+  # return 3e2-1./(d/5e1) ## for cone-00
+  # return 2-1./(d/5e3) ## for trig-00
+  # return 1000-1/(d/1e5)
+  ## Correct version, inverse of the function from http://mathnathan.com/2011/02/03/depthvsdistance/
+  return 348.0 / (1091.5 - d)