text: """
  Does the entire mapping pipeline. Saves stuff in ONE dictionary w/parameters. Can 
  handle both the manual and automatic calibration steps. I use argparse here so that 
  I remember which settings to use and don't mistakenly use an outdated configuration.
  
  Note that with the automatic collection, I should be dealing with rotations. And
  that should _still_ use the rigid body transformation.
  
  (c) September 2017 by Daniel Seita 
  """
  
  from autolab.data_collector import DataCollector
  from collections import defaultdict
  from dvrk.robot import *
  from keras.models import Sequential
  from keras.layers import Dense, Activation
  from geometry_msgs.msg import PointStamped, Point
  from sklearn.ensemble import RandomForestRegressor
  import argparse
  import cv2
  import image_geometry
  import numpy as np
  import pickle
  import sys
  import time
  import utilities as utils
  np.set_printoptions(suppress=True)
  C_LEFT_INFO  = pickle.load(open('config/camera_info_matrices/left.p',  'r'))
  C_RIGHT_INFO = pickle.load(open('config/camera_info_matrices/right.p', 'r'))
  
  
  def train_network(X_train, Y_train, X_mean, X_std, version_out):
      """ 
      Trains a network to predict f(cx,cy,cz,yaw) = (rx,ry,rz). Here, `X_train` is 
      ALREADY normalized. X_train has shape (N,6), Y_train has shape (N,3). Thus, for
      keras, we use `input_dim = 6` since that is not including the batch size. Shuffle 
      the data beforehand, since Keras will take the last % of the input data we pass 
      as the fixed, held-out validation set.
  
      DURING PREDICTION AND TEST-TIME APPLICATIONS, DON'T FORGET TO NORMALIZE!!!!!
  
      https://keras.io/getting-started/faq/#how-can-i-save-a-keras-model
      """
      shuffle = np.random.permutation(X_train.shape[0])
      X_train = X_train[shuffle] # Already normalized!
      Y_train = Y_train[shuffle]
  
      N, input_dim = X_train.shape
      epochs = 400
      batch_size = 32
      val_split = 0.2
  
      model = Sequential()
      model.add(Dense(300, activation='relu', input_dim=input_dim))
      model.add(Dense(300, activation='relu'))
      model.add(Dense(300, activation='relu'))
      model.add(Dense(3,  activation=None))
      model.compile(optimizer='adam', loss='mse')
      model.fit(X_train, Y_train, epochs=epochs, batch_size=batch_size, validation_split=val_split)
      
      val_start = int(N*(1.-val_split))
      X_valid = X_train[val_start:, :]
      Y_valid = Y_train[val_start:, :]
      predictions = model.predict(X_valid)
      ms_errors = 0
      num_valids = X_valid.shape[0]
      head = "config/keras_results/"
      
      with open(head+"simple_{}epochs_v{}.txt".format(epochs,version_out), "w") as text_file:
          for index in range(num_valids):
              data = X_valid[index, :]
              targ = Y_valid[index, :]
              pred = predictions[index, :]
              data = (data * X_std) + X_mean   # Un-normalize the data!
              mse  = np.linalg.norm(pred-targ) ** 2
              ms_errors += mse
  
              text_file.write("[{:.3f}, {:.3f}, {:.3f}, {:.3f}, {:.3f}, {:.3f}]   ===>   [{:.3f}, {:.3f}, {:.3f}],  actual: [{:.3f}, {:.3f}, {:.3f}]  (mse: {:4f})\n".format(data[0],data[1],data[2],data[3],data[4],data[5], pred[0],pred[1],pred[2],targ[0],targ[1],targ[2], mse))
      
      print("X_mean: {}".format(X_mean))
      print("X_std: {}".format(X_std))
      print("val_start: {}".format(val_start))
      print("num_valids: {}".format(num_valids))
      print("mse as I computed it: {}".format(ms_errors / num_valids))
  
      modeldir = head+"simple_{}epochs_v{}.h5".format(epochs,version_out)
      model.save(modeldir)
      return modeldir
  
  
  def debug_1(left, right, points3d, LEFT_POINTS, RIGHT_POINTS):
      print("\nSome debug prints:")
      print("robot points in left/right camera (we'll average later):")
      for i,(pt1,pt2) in enumerate(zip(LEFT_POINTS, RIGHT_POINTS)):
          print(i,np.squeeze(pt1[0]),np.squeeze(pt2[0]))
      #print("pixel pts in left/right camera:")
      #for i,(pt1,pt2) in enumerate(zip(left, right)):
      #    print(i,pt1,pt2)
      print("camera points:")
      for i,item in enumerate(points_3d):
          print(i,np.squeeze(item))
      print("(camera) points_3d.shape: {}".format(points_3d.shape))
      print("End of debug prints.\n")
  
  
  def get_points_3d(left_points, right_points, info):
      """ 
      Assumes that corresponding (2-D pixel) points are ordered correctly 
      in `left_points` and `right_points`. Returns a numpy array of 3D camera points.
      https://github.com/BerkeleyAutomation/endoscope_calibration/blob/master/rigid_transformation.py
      """
      stereoModel = image_geometry.StereoCameraModel()
      stereoModel.fromCameraInfo(info['l'], info['r'])
      points_3d = []
  
      for i in range(len(left_points)):
          a = left_points[i]
          b = right_points[i]
          disparity = np.sqrt((a[0]-b[0]) ** 2 + (a[1]-b[1]) ** 2)
          assert disparity == np.linalg.norm(np.array(a)-np.array(b))
          (x,y,z) = stereoModel.projectPixelTo3d((a[0],a[1]), disparity)
          points_3d.append( [x,y,z] )
  
      return np.array(points_3d)
  
  
  def pixels_to_3d(LEFT_POINTS, RIGHT_POINTS):
      """ 
      Call this to start the process of getting camera points from pixels. FYI, I
      explicitly extract the pixels from `{LEFT,RIGHT}_POINTS` since they have more info.
      """
      left = []
      right = []
      info = {'l':C_LEFT_INFO, 'r':C_RIGHT_INFO}
  
      # I _think_ ... hopefully this works. There isn't specific documentation.
      for (pt1, pt2) in zip(LEFT_POINTS, RIGHT_POINTS):
          _, _, lx, ly = pt1
          _, _, rx, ry = pt2
          left.append((lx,ly))
          right.append((rx,ry))
  
      pts3d_array = get_points_3d(left, right, info)
      return (left, right, pts3d_array)
  
  
  def estimate_rf(X_train, Y_train, debug):
      """ Hopefully this will narrow down errors even further. UPDATE: this is the bad RF,
      but it's good to have a bad baseline anyway.
      
      X_train: the camera coordinates, array of (x,y,z) stuff, where z is computed via
              our method here which uses points from both cameras to get depth.
      Y_train: when we did the rigid transform, we get some error. These residuals are
              the targets, so (xp-xt,yp-yt,zp-zt) with pred-target for each component.
              These are raw values, so if we did the reverse we'd have to negate things.
              
      In real application, we need to get our camera points. Then, given those camera points,
      apply the rigid body to get the predicted robot frame point (xp,yp,zp), assuming fixed
      rotation for now. But, this has some error from that. The RF trained here can predict
      that, i.e. (xerr,yerr,zerr) = (xp-xt,yp-yt,zp-zt). Thus, we have to do:
  
          (xp,yp,zp) - (xerr,yerr,zerr)
  
      to get the new, more robust (hopefully) predictions. E.g. if xerr is 5mm, we keep over-
      shooting x by 5mm so better bring it down.
  
      Fortunately, this MIGHT not be needed, as I saw that our rigid body errors were 
      at most 2 millimeters in each of the x and y directions. Ironically, the z direction
      has more error. But hopefully we can just lower it ... I hope. The idea is that given
      the camera point, the RF should "know" how to correct for the rigid body.
      """
      assert X_train.shape[1] == Y_train.shape[1] == 3 # (N,3)
      rf = RandomForestRegressor(n_estimators=100)    
      rf.fit(X_train, Y_train)
      Y_pred = rf.predict(X_train)
      abs_errors = np.abs(Y_pred-Y_train)
      abs_mean_err = np.mean(abs_errors, axis=0)
  
      if debug:
          print("\nBegin debug prints for RFs:")
          print("X_train:\n{}".format(X_train))
          print("Y_train:\n{}".format(Y_train))
          print("Y_pred:\n{}".format(Y_pred))
          print("abs_mean_err: {}".format(abs_mean_err))
          print("End debug prints for RFs\n")
      return rf
  
  
  def solve_rigid_transform(camera_points_3d, robot_points_3d, debug=True):
      """
      Takes in two sets of corresponding points, returns the rigid transformation matrix 
      from the first to the second. This is a (3,4) matrix so we'd apply it on the original
      points in their homogeneous form with the fourth coordinate equal to one. This is
      slightly different from Brijen's code since I'm using np.array, not np.matrix.
  
      Notation: A for camera points, B for robot points, so want to find an affine mapping
      from A -> B with orthogonal rotation and a translation.
  
      UPDATE: Also returns the random forest for estimating the residual noise! This will be
      the "bad" RF baseline since the rigid body thinks it's already so good.
      """
      assert camera_points_3d.shape[1] == robot_points_3d.shape[1] == 3
      A = camera_points_3d.T # (3,N)
      B = robot_points_3d.T  # (3,N)
  
      # Look for Inge Soderkvist's solution online if confused.
      meanA = np.mean(A, axis=1, keepdims=True)
      meanB = np.mean(B, axis=1, keepdims=True)
      A = A - meanA
      B = B - meanB
      covariance = B.dot(A.T)
      U, sigma, VH = np.linalg.svd(covariance) # VH = V.T, i.e. numpy transposes it for us.
  
      V = VH.T
      D = np.eye(3)
      D[2,2] = np.linalg.det( U.dot(V.T) )
      R = U.dot(D).dot(V.T)
      t = meanB - R.dot(meanA)
      RB_matrix = np.concatenate((R, t), axis=1)
  
      #################
      # SANITY CHECKS #
      #################
  
      print("\nBegin debug prints for rigid transformation:")
      print("meanA:\n{}\nmeanB:\n{}".format(meanA, meanB))
      print("Rotation R:\n{}\nand R^TR (should be identity):\n{}".format(R, (R.T).dot(R)))
      print("translation t:\n{}".format(t))
      print("RB_matrix:\n{}".format(RB_matrix))
  
      # Get residual to inspect quality of solution. Use homogeneous coordinates for A.
      # Also, recall that we're dealing with (3,N) matrices, not (N,3).
      # In addition, we don't want to zero-mean for real applications.
      A = camera_points_3d.T # (3,N)
      B = robot_points_3d.T  # (3,N)
  
      ones_vec = np.ones((1, A.shape[1]))
      A_h = np.concatenate((A, ones_vec), axis=0)
      B_pred = RB_matrix.dot(A_h)
      assert B_pred.shape == B.shape
  
      # Careful! Use raw_errors for the RF, but it will depend on pred-targ or targ-pred.
      raw_errors = B_pred - B # Use pred-targ, of shape (3,N)
      l2_per_example = np.sum((B-B_pred)*(B-B_pred), axis=0)
      frobenius_loss = np.mean(l2_per_example)
  
      # Additional sanity checks. 
      print("\nCamera points (input), A.T:\n{}".format(A.T))
      print("Robot points (target), B.T:\n{}".format(B.T))
      print("Predicted robot points:\n{}".format(B_pred.T))
      print("Raw errors, B-B_pred:\n{}".format((B-B_pred).T))
      print("Mean abs error per dim: {}".format( (np.mean(np.abs(B-B_pred), axis=1))) )
      print("Residual (L2) for each:\n{}".format(l2_per_example.T))
      print("loss on data: {}".format(frobenius_loss))
      print("End of debug prints for rigid transformation.\n")
  
      # Now get that extra random forest. Actually we might have some liberty
      # with the input/target. For now, I use input=camera_pts, targs=abs_residual.
      X_train = camera_points_3d # (N,3)
      Y_train = raw_errors.T # (N,3)
      rf_residuals = estimate_rf(X_train, Y_train, debug)
  
      # NOW, finally, let's combine the two! Recall that A, Ah are our camera data.
      errors = rf_residuals.predict(X_train) # (N,3)
      assert errors.shape[1] == B_pred.shape[0] == 3
      assert (errors.T).shape == B_pred.shape
      B_preds_with_rf = B_pred - errors.T # (3,N)
      new_raw_errors = B_preds_with_rf - B # (3,N)
      avg_abs_error_old = np.mean(np.abs(raw_errors))
      avg_abs_error_new = np.mean(np.abs(new_raw_errors))
  
      print("\nWhat if we COMBINE the rigid body with the RF?:")
      print("Predictions, B_pred-errors transposed (Rigid+RF):\n{}".format(B_preds_with_rf.T))
      print("NEW raw errors:\n{}".format(new_raw_errors.T))
      print("avg abs err for RB:    {}".format(avg_abs_error_old))
      print("avg abs err for RB+RF: {}".format(avg_abs_error_new))
      print("Mean abs error per dim: {}".format( np.mean(np.abs(new_raw_errors), axis=1) ))
      print("End of debug prints for rigid transform PLUS RF...\n")
  
      return RB_matrix, rf_residuals
  
  
  def correspond_left_right_pixels(left, right, collection, VERSION, debug=True):
      """ This is my custom solution. To check it I should apply it on fixed
      points and just flip through the two images (literally click on the arrow
      keys) to see if they overlap.
  
      The solution is simple least squares. We can split this by column and it becomes
      minimizing L2 loss, i.e. ordinary linear regression, and solutions are known
      from the normal equations. But in case I'm paranoid I can just visualize solutions.
  
      Input: `left` and `right` are lists of (corresponding) tuples representing pixel values.
      """
      N = len(left)
      A = np.concatenate( (np.array(left),  np.ones((N,1))) , axis=1) # Left pixels
      B = np.concatenate( (np.array(right), np.ones((N,1))) , axis=1) # Right pixels
      A_nobias = np.array(left)
      B_nobias = np.array(right)
  
      # Left to Right
      col0_l2r = (np.linalg.inv((A.T).dot(A)).dot(A.T)).dot(B[:,0])
      col1_l2r = (np.linalg.inv((A.T).dot(A)).dot(A.T)).dot(B[:,1])
      theta_l2r = np.column_stack((col0_l2r, col1_l2r))
      abs_error_l2r = np.abs(A.dot(theta_l2r) - B_nobias)
  
      # Right to Left
      col0_r2l = (np.linalg.inv((B.T).dot(B)).dot(B.T)).dot(A[:,0])
      col1_r2l = (np.linalg.inv((B.T).dot(B)).dot(B.T)).dot(A[:,1])
      theta_r2l = np.column_stack((col0_r2l, col1_r2l))
      abs_error_r2l = np.abs(B.dot(theta_r2l) - A_nobias)
  
      assert theta_l2r.shape == theta_r2l.shape == (3,2)
  
      if debug:
          # Also check the mapping on an actual image.
          img_left  = cv2.imread('camera_location/calibration_blank_image_left.jpg')
          img_right = cv2.imread('camera_location/calibration_blank_image_right.jpg')
          
          # The lists `left` and `right` have corresponding tuples. If we take (x,y) from
          # `left`, which is our default camera, say it points to the third circle in a grid.
          #  We want the matrices to provide us with the pixel points in the *right* camera
          #  that would overlap with the same conceptual area, i.e. the center of that grid.
          for (cX,cY) in left:
              cv2.circle(img_left, (cX,cY), 5, (255,0,0), thickness=-1)
              cv2.putText(img=img_left, text="{},{}".format(cX,cY), org=(cX,cY), 
                          fontFace=cv2.FONT_HERSHEY_SIMPLEX,
                          fontScale=0.75, color=(255,0,0), thickness=2)
  
              pt = np.array([cX,cY,1])
              pt_right = pt.dot(theta_l2r)
              oX, oY = int(round(pt_right[0])), int(round(pt_right[1]))
  
              cv2.circle(img_right, (oX,oY), 5, (0,0,255), thickness=-1)
              cv2.putText(img=img_right, text="{},{}".format(oX,oY), org=(oX,oY), 
                          fontFace=cv2.FONT_HERSHEY_SIMPLEX,
                          fontScale=0.75, color=(0,0,255), thickness=2)
  
          if collection == 'auto':
              cv2.imwrite('images/calib_auto_left_w_left_points_v'+VERSION+'.jpg',  img_left)
              cv2.imwrite('images/calib_auto_right_w_left_points_v'+VERSION+'.jpg', img_right)
          elif collection == 'manual':
              cv2.imwrite('images/calib_manual_left_w_left_points_v'+VERSION+'.jpg',  img_left)
              cv2.imwrite('images/calib_manual_right_w_left_points_v'+VERSION+'.jpg', img_right)
  
          # Now back to traditional debugs.
          print("\nBegin debug prints:")
          print("(left pixels) A.shape: {}, and A:\n{}".format(A.shape, A))
          print("(right pixels) B.shape: {}, and B:\n{}".format(B.shape, B))
          print("theta for LEFT to RIGHT:\n{}".format(theta_l2r))
          print("theta for RIGHT to LEFT:\n{}".format(theta_r2l))
          print("abs_errors l2r (remember, these are pixels):\n{}".format(abs_error_l2r))
          print("abs_errors r2l (remember, these are pixels):\n{}".format(abs_error_r2l))
          print("End of debug prints.\n")
  
      return theta_l2r, theta_r2l
  
  
  def left_pixel_to_robot_prediction(left, params, true_points):
      """ Similar to the method in `click_and_crop.py` except that we have a full list of
      points from the left camera.
      """
      pred_points = []
  
      for left_pt in left:
          leftx, lefty = left_pt
          left_pt_hom = np.array([leftx, lefty, 1.])
          right_pt = left_pt_hom.dot(params['theta_l2r'])
  
          # Copy the code I wrote to convert these pts to camera points.
          disparity = np.linalg.norm(left_pt-right_pt)
          stereoModel = image_geometry.StereoCameraModel()
          stereoModel.fromCameraInfo(C_LEFT_INFO, C_RIGHT_INFO)
          (xx,yy,zz) = stereoModel.projectPixelTo3d( (leftx,lefty), disparity )
          camera_pt = np.array([xx, yy, zz])
  
          # Now I can apply the rigid body and RF (if desired).
          camera_pt = np.concatenate( (camera_pt, np.ones(1)) )
          robot_pt = (params['RB_matrix']).dot(camera_pt)
          target = [robot_pt[0], robot_pt[1], robot_pt[2]]
          pred_points.append(target)
  
      pred_points = np.array(pred_points)
  
      print("\nAssuming we ONLY have the left pixels, we still predict the robot points.")
      print("pred points:\n{}".format(pred_points))
      if true_points is not None:
          abs_errors = np.abs(pred_points - true_points)
          abs_mean_errors = np.mean(abs_errors, axis=0)
          print("true points:\n{}".format(true_points))
          print("mean abs err: {}".format(abs_mean_errors))
          print("mean err: {}".format(np.mean(pred_points-true_points, axis=0)))
      print("Done w/debugging.\n")
  
  
  if __name__ == "__main__":
      pp = argparse.ArgumentParser()
      pp.add_argument('--collection', type=str, help='auto or manual')
      pp.add_argument('--version_in', type=int, help='0X for gripper, 1X for scissors')
      pp.add_argument('--version_out', type=int, help='00 or 01')
      args = pp.parse_args()
      assert args.collection.lower() in ['auto', 'manual']
      assert args.version_in is not None
      assert args.version_out is not None
  
      # SAVE STUFF HERE!
      params = {}
  
      # Load left and right calibration data, since I saved them separately. That's only to 
      # maintain consistency w/the manual version (the auto one could be saved in one list).
      VERSION = str(args.version_in).zfill(2)
      if args.collection == 'auto':
          LEFT_POINTS  = pickle.load(open('config/grid_auto/auto_calib_left_'+VERSION+'.p',  'r'))
          RIGHT_POINTS = pickle.load(open('config/grid_auto/auto_calib_right_'+VERSION+'.p', 'r'))
      elif args.collection == 'manual':
          LEFT_POINTS  = pickle.load(open('config/grid/calib_circlegrid_left_v'+VERSION+'_ONELIST.p',  'r'))
          RIGHT_POINTS = pickle.load(open('config/grid/calib_circlegrid_right_v'+VERSION+'_ONELIST.p', 'r'))
          if VERSION == '00':
              assert len(LEFT_POINTS) == len(RIGHT_POINTS) == 36
          elif VERSION == '01' or VERSION == '10':
              assert len(LEFT_POINTS) == len(RIGHT_POINTS) == 35
      NUM_POINTS = len(LEFT_POINTS)
  
      # -----------------------------
      # Get the 3D **camera** points.
      # -----------------------------
      left, right, points_3d = pixels_to_3d(LEFT_POINTS, RIGHT_POINTS)
      debug_1(left, right, points_3d, LEFT_POINTS, RIGHT_POINTS)
  
      # ---------------------------------------------------------------------------
      # Gather data needed for future transformations, particulary robot points and
      # (for the auto collection) the rotations, for better calibration.
      # -----------------------------------------------------------------------------
      robot_3d = []
      rotations_3d = []
  
      for (pt1, pt2) in zip(LEFT_POINTS, RIGHT_POINTS):
          pos_l, rot_l, _, _ = pt1
          pos_r, rot_r, _, _ = pt2
          pos_l = np.squeeze(np.array(pos_l))
          pos_r = np.squeeze(np.array(pos_r))
  
          if args.collection == 'auto':
              assert np.allclose(pos_l, pos_r)
              assert np.allclose(rot_l, rot_r)
              robot_3d.append(pos_l)
              rotations_3d.append(rot_l)
          elif args.collection == 'manual':
              # We have two measurements per point.
              robot_3d.append( (pos_l+pos_r) / 2. ) 
  
      robot_3d     = np.array(robot_3d)
      rotations_3d = np.array(rotations_3d)
      print("robot_3d.shape:     {}".format(robot_3d.shape))
      print("rotations_3d.shape: {}".format(rotations_3d.shape))
      if args.collection == 'auto':
          assert robot_3d.shape[0] == rotations_3d.shape[0]      # (N,3) for each!
          assert robot_3d.shape[1] == rotations_3d.shape[1] == 3 # (N,3) for each!
          assert len(robot_3d.shape) == len(rotations_3d.shape) == 2 
  
      # -------------------------------------------------------------------------
      # For manual, get rigid body transform. For auto we have to split by cases.
      # -------------------------------------------------------------------------
  
      if args.collection == 'manual':
          rigid_body_matrix, residuals_mapping = solve_rigid_transform(
                  camera_points_3d=points_3d,
                  robot_points_3d=robot_3d,
                  debug=True)
  
      elif args.collection == 'auto':
          yaw_stuff = []
          yaws = [-90, -45, 0, 45, 90]    
  
          # For each yaw, determine indices that match it in its range.
          for yaw in yaws:
              print("\nCURRENTLY ON YAW = {}".format(yaw))
              y_min = yaw - 22.5
              y_max = yaw + 22.5
              indices = []
  
              # Extract rotation, and get the stored yaw from rot[0].
              for ii,rot in enumerate(rotations_3d):
                  if y_min <= rot[0] < y_max:
                      indices.append(ii)
  
              # Using our indices, we now extract the appropriate stuff by indexing.
              indices = np.array(indices)
              this_rb_matrix, this_internal_rf = solve_rigid_transform(
                      camera_points_3d=points_3d[indices],
                      robot_points_3d=robot_3d[indices],
                      debug=False)
              params['rigid_body_'+str(yaw)]  = this_rb_matrix
              params['internal_rf_'+str(yaw)] = this_internal_rf
              yaw_stuff.append( (indices.shape, points_3d[indices].shape, robot_3d[indices].shape) )
  
          # Debugging ...
          nums = 0
          for item in yaw_stuff:
              print(item)
              nums += float(np.squeeze(item[0]))
          print("Total # of points: {}".format(nums))
          N = robot_3d.shape[0]
          assert N == nums
  
      # ------------------------------------------------------------------------------------
      # For auto, get lots of stuff, e.g. deep networks. 
      # For the rotations, keep only the 0th column (yaw). EDIT: nope, let's use pitch/roll!
      # Just remember that the input is (cx, cy, cz, yaw, pitch, roll), IN THAT ORDER!!
      # ------------------------------------------------------------------------------------
      if args.collection == 'auto':
          X_train = np.concatenate((points_3d, rotations_3d), axis=1) # (N,6)
          X_mean = np.mean(X_train, axis=0)
          X_std = np.std(X_train, axis=0)
          y_train = robot_3d.copy() # (N,3)
          assert X_train.shape[1] == 6 and y_train.shape[1] == 3 and X_train.shape[0] == y_train.shape[0]
          assert len(X_std) == len(X_mean) == 6
          X_train = (X_train - X_mean) / X_std
          modeldir = train_network(X_train, y_train, X_mean, X_std, args.version_out)
  
      # -----------------------------------------------------------------------------------
      # Develop correspondence between left and right camera pixels. I assume in real
      # application, we'll only use the left camera at one time, but w/this correspondence,
      # we effectively "pretend" that we know what the right camera is viewing.
      # Note: better to IGNORE this if using the automatic trajectory collector, since that
      # has much noiser left/right correspondences. Just use the manual one in application.
      # -----------------------------------------------------------------------------------
      theta_l2r, theta_r2l = correspond_left_right_pixels(left, right, args.collection, VERSION)
  
      # ----------------------------------------
      # Save various matrices in one dictionary. 
      # ----------------------------------------
      params['theta_l2r'] = theta_l2r
      params['theta_r2l'] = theta_r2l
  
      if args.collection == 'auto':
          params['X_mean'] = X_mean
          params['X_std']  = X_std
          params['modeldir'] = modeldir
          pickle.dump(params, 
                  open('config/mapping_results/auto_params_matrices_v'+str(args.version_out).zfill(2)+'.p', 'w'))
          print("\nDictionary keys: {}".format(params.keys()))
  
      elif args.collection == 'manual':
          params['RB_matrix'] = rigid_body_matrix
          params['rf_residuals'] = residuals_mapping
          pickle.dump(params, 
                  open('config/mapping_results/manual_params_matrices_v'+str(args.version_out).zfill(2)+'.p', 'w'))
  
      # -----------------------------------------------------------------------------
      # Let's see what happens if we _pretend_ we ignore the right camera's pixels.
      # Given the left camera's pixels, see if we can accurately predict robot frame.
      # -----------------------------------------------------------------------------
      #left_pixel_to_robot_prediction(left, params, robot_3d)
  