如何确定相机的世界坐标？

Question

我在墙上有一个已知尺寸和位置的矩形目标，机器人上有一个移动摄像头。当机器人在房间里行驶时，我需要定位目标并计算相机的位置及其姿势。作为进一步的转折，可以使用伺服系统改变相机的仰角和方位角。我可以使用 OpenCV 定位目标，但在计算相机的位置时我仍然很模糊（实际上，上周我的头撞到墙上，我的额头上有一个平坦的点）。这是我正在做的事情：

1. 读入之前计算的相机内在函数文件
2. 从轮廓中获取目标矩形4个点的像素坐标
3. 用矩形的世界坐标、像素坐标、相机矩阵和畸变矩阵调用solvePnP
4. 使用旋转和平移向量调用 projectPoints
5. ???

我读过 OpenCV 的书，但我想我只是错过了一些关于如何使用投影点、旋转和平移向量来计算相机的世界坐标及其姿势的内容（我不是数学专家) :-(

2013-04-02 根据“morynicz”的建议，我编写了这个简单的独立程序。

#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'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<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));
}

我在测试中使用的像素坐标来自真实图像，该图像是在目标矩形（62 英寸宽和 20 英寸高）左侧约 27 英尺处以大约 45 度角拍摄的。输出不是我所期望的。我做错了什么？

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

如果我的世界坐标的 Y 轴与 OpenCV 的屏幕 Y 轴相反，会有问题吗？ （我的坐标系的原点在目标左侧的地板上，而 OpenCV 的原点在屏幕的左上角）。

姿势的单位是什么？

Answer 1

您从solvePnP 获得平移和旋转向量，它们说明了对象在相机坐标中的位置。您需要进行逆变换。

变换相机 -> 对象可以写成矩阵[R T;0 1] 用于齐次坐标。这个矩阵的逆矩阵是，使用它的特殊属性，[R^t -R^t*T;0 1]，其中 R^t 是 R 转置的。您可以从Rodrigues 变换中获取 R 矩阵。这样你就得到了变换对象->相机坐标的平移向量和旋转矩阵。

如果您知道对象在世界坐标中的位置，您可以使用 world->object transform * object->camera transform 矩阵来提取相机平移和姿势。

姿势由单个向量或 R 矩阵描述，你一定会在你的书中找到它。如果是“学习 OpenCV”，您可以在第 401 - 402 页找到它：)

查看你的代码，你需要做这样的事情

    cv::Mat R;
    cv::Rodrigues(rotation_vector, R);

    cv::Mat cameraRotationVector;

    cv::Rodrigues(R.t(),cameraRotationVector);

    cv::Mat cameraTranslationVector = -R.t()*translation_vector;

cameraTranslationVector 包含相机坐标。 cameraRotationVector 包含相机姿势。

Answer 2

我花了很长时间才理解它，但姿势的含义是每个轴上的旋转 - x,y,z。 它以弧度表示。值介于 Pie 到减 Pie (-3.14 - 3.14) 之间

编辑： 我可能弄错了。我读到pose是表示相机方向的向量，向量的长度表示相机围绕该向量旋转多少。