title: From camera calibration to picking up colored cubes with robot arm
body: <p>I want to pick up colored cubes with a robot arm that I detect with a camera and OpenCV, in Python. I managed to detect the different colored cubes and I calibrated the camera with the checkerboard process.</p>
  
  <p><strong>The setup:</strong></p>
  
  <p><a href="https://i.stack.imgur.com/dt0rH.jpg" rel="nofollow noreferrer"><img src="https://i.stack.imgur.com/dt0rH.jpg" alt="Setup front"></a>
  <a href="https://i.stack.imgur.com/rpWsl.jpg" rel="nofollow noreferrer"><img src="https://i.stack.imgur.com/rpWsl.jpg" alt="Setup top"></a></p>
  
  <p><strong>Cube detection:</strong>
  <a href="https://i.stack.imgur.com/wYvNL.jpg" rel="nofollow noreferrer"><img src="https://i.stack.imgur.com/wYvNL.jpg" alt="Cubes detected"></a></p>
  
  <p>The problem is that I don't understand the rest of the process of getting the coordinates from the object with the camera and translating them to the robot arm for pick up.</p>
  
  <p><strong>The following steps have been completed:</strong></p>
  
  <ol>
  <li><p>Separating color boundaries with HSV and drawing bounding boxes. So I have the pixel x, y of the object.</p></li>
  <li><p>Calibrating the camera resulting in the following camera matrix and distortion coefficients: </p>
  
  <p>Camera Matrix: [[1.42715609e+03 0.00000000e+00 9.13700651e+02]  [0.00000000e+00 1.43275509e+03 5.58917609e+02]  [0.00000000e+00
  0.00000000e+00 1.00000000e+00]]</p>
  
  <p>Distortion:[[ 0.03924722 -0.30622971  0.00124042 -0.00303094  0.49458539]]</p></li>
  <li><p>Trying to figure out the next steps in the documentation of OpenCV</p></li>
  </ol>
  
  <p><strong>The result</strong></p>
  
  <p>I read through the API documentation on this page: <a href="https://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html" rel="nofollow noreferrer">https://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html</a></p>
  
  <p>But with my skill level I can't seem the extract the practical steps to take, in order to attain my goal.</p>
  
  <p>My questions:</p>
  
  <ol>
  <li>How do I use the camera matrix and distortion coefficients to get coordinates of the object in the image frame?</li>
  <li>How do I translate the coordinates on the image frame to the robot end-effector coordinates?</li>
  <li>If I keep the camera on a fixed position. Does this mean I only have to do the calibration ones?</li>
  </ol>
  
  <p>******* <strong>edit:</strong> *******</p>
  
  <p>I went for a different approach. I managed to solve the rotation and translation between two coordinate systems through SVD. But I made an error in thinking I could use pixel coordinates to translate from the camera coordinate system to that of the robot. I think you need to translate the u, v values first. </p>
  
  <p><strong>How can I translate the pixel uv to world coordinates, so that I can use the code below to get translation and rotation to robot arm?</strong></p>
  
  <p>Here's my code:</p>
  
  <pre><code>#######################################################################
  # Step 1: Input camera and world coordinates, and calculate centroids #
  #######################################################################
  print("")
  
  # Camera and robot to world coordinates
  Pc = np.matrix([[604,119,0],[473,351,0], [730,329,0]])
  print("Camera points matrix: ")
  print(Pc)
  print("")
  
  Pr = np.matrix([[177,-38,0],[264,-93,0], [258,4.7,0]])
  print("Robot points matrix: ")
  print(Pr)
  print("")
  
  # Calculate centroids
  Cc = Pc.mean(axis=0)
  Cr = Pr.mean(axis=0)
  
  print("Centroid camera: ")
  print(Cc)
  print("")
  print("Centroid robot: ")
  print(Cr)
  print("")
  
  # Pc and Pr - centroids of Pc and Pr
  Pc_minus_Cc = np.subtract(Pc, Cc)
  print("Pc - centroidC: ")
  print(Pc_minus_Cc)
  print("")
  
  Pr_minus_Cr = np.subtract(Pr, Cr)
  print("Pr - centroidR: ")
  print(Pr_minus_Cr)
  print("")
  
  ############################################################################
  # Step 2: Calculate H, perform SVD and get rotation and translation matrix #
  ############################################################################
  
  # Get H
  print("(Pr - centroidR) transposed: ")
  print(Pr_minus_Cr.T)
  print("")
  H = np.matmul(Pc_minus_Cc, Pr_minus_Cr.T)
  print("H: ")
  print(H)
  print("")
  
  # Perform SVD
  u, s, v = np.linalg.svd(H)
  print("SVD result: ")
  print("u: ")
  print("")
  print(u)
  print("")
  print("s: ")
  print(s)
  print("")
  print("v: ")
  print(v)
  print("")
  
  # Calculate rotation matrix
  R = np.matmul(v,u.T)
  print("Rotation matrix: ")
  print(R)
  
  # Calculate t
  t = -R * Cc.T + Cr.T
  print("t: ")
  print(t)
  </code></pre>