title: How to determine world coordinates of a camera?
body: <p>I have a rectangular target of known dimensions and location on a wall, and a mobile camera on a robot. As the robot is driving around the room, I need to locate the target and compute the location of the camera and its pose. As a further twist, the camera's elevation and azimuth can be changed using servos. I am able to locate the target using OpenCV, but I am still fuzzy on calculating the camera's position (actually, I've gotten a flat spot on my forehead from banging my head against a wall for the last week). Here is what I am doing:</p>
  
  <ol>
  <li>Read in previously computed camera intrinsics file</li>
  <li>Get the pixel coordinates of the 4 points of the target rectangle from the contour</li>
  <li>Call solvePnP with the world coordinates of the rectangle, the pixel coordinates, the camera matrix and the distortion matrix</li>
  <li>Call projectPoints with the rotation and translation vectors</li>
  <li>???</li>
  </ol>
  
  <p>I have read the OpenCV book, but I guess I'm just missing something on how to use the projected points, rotation and translation vectors to compute the world coordinates of the camera and its pose (I'm not a math wiz) :-(</p>
  
  <p>2013-04-02
  Following the advice from "morynicz", I have written this simple standalone program.</p>
  
  <pre><code>#include &lt;Windows.h&gt;
  #include "opencv\cv.h"
  
  using namespace cv;
  
  int main (int argc, char** argv)
  {
  const char          *calibration_filename = argc &gt;= 2 ? argv [1] : "M1011_camera.xml";
  FileStorage         camera_data (calibration_filename, FileStorage::READ);
  Mat                 camera_intrinsics, distortion;
  vector&lt;Point3d&gt;     world_coords;
  vector&lt;Point2d&gt;     pixel_coords;
  Mat                 rotation_vector, translation_vector, rotation_matrix, inverted_rotation_matrix, cw_translate;
  Mat                 cw_transform = cv::Mat::eye (4, 4, CV_64FC1);
  
  
  // Read camera data
  camera_data ["camera_matrix"] &gt;&gt; camera_intrinsics;
  camera_data ["distortion_coefficients"] &gt;&gt; distortion;
  camera_data.release ();
  
  // Target rectangle coordinates in feet
  world_coords.push_back (Point3d (10.91666666666667, 10.01041666666667, 0));
  world_coords.push_back (Point3d (10.91666666666667, 8.34375, 0));
  world_coords.push_back (Point3d (16.08333333333334, 8.34375, 0));
  world_coords.push_back (Point3d (16.08333333333334, 10.01041666666667, 0));
  
  // Coordinates of rectangle in camera
  pixel_coords.push_back (Point2d (284, 204));
  pixel_coords.push_back (Point2d (286, 249));
  pixel_coords.push_back (Point2d (421, 259));
  pixel_coords.push_back (Point2d (416, 216));
  
  // Get vectors for world-&gt;camera transform
  solvePnP (world_coords, pixel_coords, camera_intrinsics, distortion, rotation_vector, translation_vector, false, 0);
  dump_matrix (rotation_vector, String ("Rotation vector"));
  dump_matrix (translation_vector, String ("Translation vector"));
  
  // We need inverse of the world-&gt;camera transform (camera-&gt;world) to calculate
  // the camera's location
  Rodrigues (rotation_vector, rotation_matrix);
  Rodrigues (rotation_matrix.t (), camera_rotation_vector);
  Mat t = translation_vector.t ();
  camera_translation_vector = -camera_rotation_vector * t;
  
  printf ("Camera position %f, %f, %f\n", camera_translation_vector.at&lt;double&gt;(0), camera_translation_vector.at&lt;double&gt;(1), camera_translation_vector.at&lt;double&gt;(2));
  printf ("Camera pose %f, %f, %f\n", camera_rotation_vector.at&lt;double&gt;(0), camera_rotation_vector.at&lt;double&gt;(1), camera_rotation_vector.at&lt;double&gt;(2));
  }
  </code></pre>
  
  <p>The pixel coordinates I used in my test are from a real image that was taken about 27 feet left of the target rectangle (which is 62 inches wide and 20 inches high), at about a 45 degree angle. The output is not what I'm expecting. What am I doing wrong?</p>
  
  <pre><code>Rotation vector
  2.7005
  0.0328
  0.4590
  
  Translation vector
  -10.4774
  8.1194
  13.9423
  
  Camera position -28.293855, 21.926176, 37.650714
  Camera pose -2.700470, -0.032770, -0.459009
  </code></pre>
  
  <p>Will it be a problem if my world coordinates have the Y axis inverted from that of OpenCV's screen Y axis? (the origin of my coordinate system is on the floor to the left of the target, while OpenCV's orgin is the top left of the screen).</p>
  
  <p>What units is the pose in?</p>