How to determine the world coordinates of the camera?

Question

I have a rectangular target of known size and location on the wall and a mobile camera on the robot. When the robot moves around the room, I need to find the target and calculate the location of the camera and its position. As an additional rotation, the height and azimuth of the camera can be changed using servos. I can find the target using OpenCV, but I'm still fuzzy when calculating the position of the camera (in fact, I got a flat spot on my forehead, hitting my head against the wall last week). That's what I'm doing:

• Reading in a previously computed embedded camera file
• Get the pixel coordinates of the four points of the target rectangle from the path
• Call solvePnP with the world coordinates of the rectangle, pixel coordinates, camera matrix and distortion matrix.
• Calling up projection points with rotation and translation vectors
• ???

I read the OpenCV book, but I think I just missed something, how to use the predicted points, rotation and translation vectors to calculate the world coordinates of the camera and its position (I'm not a mathematical wiz): - (

2013-04-02 Following the advice of "morynicz", I wrote this simple stand-alone program.

#include <Windows.h> #include "opencv\cv.h" using namespace cv; int main (int argc, char** argv) { const char *calibration_filename = argc >= 2 ? argv [1] : "M1011_camera.xml"; FileStorage camera_data (calibration_filename, FileStorage::READ); Mat camera_intrinsics, distortion; vector<Point3d> world_coords; vector<Point2d> 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"] >> camera_intrinsics; camera_data ["distortion_coefficients"] >> 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->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->camera transform (camera->world) to calculate // the camera 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<double>(0), camera_translation_vector.at<double>(1), camera_translation_vector.at<double>(2)); printf ("Camera pose %f, %f, %f\n", camera_rotation_vector.at<double>(0), camera_rotation_vector.at<double>(1), camera_rotation_vector.at<double>(2)); } 

The pixel coordinates I used in my test is a real image that was taken at a distance of about 27 feet from the target rectangle (62 inches wide and 20 inches high) at an angle of 45 degrees. Exit is not what I expect. What am I doing wrong?

 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 

Will there be a problem if my world coordinates have an inverted Y axis from the Y axis of OpenCV? (the origin of my coordinate system is on the floor to the left of the target, and OpenCV orgin is in the upper left corner of the screen).

What units is the pose in?

Answer 1

You get translation and rotation vectors from solvePnP , which tell you where the object is in camera coordinates. You need to get the inverse transform.

The transform camera → object can be written as the [RT;0 1] matrix for uniform coordinates. The inverse of this matrix will be, using its special properties, [R^t -R^t*T;0 1] , where R ^ t is transposed R. You can get the matrix R from Rodrigues . Thus, you get a translation vector and a rotation matrix for the coordinators of object objects → cameras.

If you know where the object lies in world coordinates, you can use the object transformation matrix world-> object transform * object-> to extract and position the camera broadcast.

The pose is described either by a single vector or by the matrix R, you will definitely find it in your book. If this is "OpenCV Training", you will find it on pages 401-402 :)

Looking at your code you need to do something like this

  cv::Mat R; cv::Rodrigues(rotation_vector, R); cv::Mat cameraRotationVector; cv::Rodrigues(Rt(),cameraRotationVector); cv::Mat cameraTranslationVector = -Rt()*translation_vector; 

cameraTranslationVector contains the coordinates of the camera. cameraRotationVector contains camera poses.

Answer 2

It took me a while to figure this out, but the point of the pose is to rotate on each axis - x, y, z. It is in radians. Values ​​range from Pie to minus Pie (-3.14 - 3.14)

Edit: I could be wrong. I read that a pose is a vector indicating the direction of the camera, and the length of the vector indicates how much to rotate the camera around this vector.