將 2D 圖像坐標轉換為 z = 0 的 3D 世界坐標

Question

• OpenCV => 3.2
• 操作系統/平台 => Windows 64 位
• 編譯器 => Visual Studio 2015

我目前正在開展我的項目，該項目涉及車輛檢測和跟蹤以及估計和優化車輛周圍的長方體。 為此，我已經完成了車輛的檢測和跟蹤，我需要找到車輛邊界框邊緣的圖像點的 3D 世界坐標，然后估計長方體和項目邊緣的世界坐標它返回圖像以顯示它。

所以，我是計算機視覺和 OpenCV 的新手，但據我所知，我只需要圖像上的 4 個點，需要知道這 4 個點的世界坐標，並在 OpenCV 中使用 solvePNP 來獲得旋轉和平移向量（我已經有相機矩陣和失真系數）。 然后，我需要使用 Rodrigues 將旋轉向量轉換為旋轉矩陣，然后將其與平移向量連接以獲得我的外在矩陣，然后將外在矩陣與相機矩陣相乘以獲得我的投影矩陣。 由於我的 z 坐標為零，因此我需要從投影矩陣中刪除第三列，該矩陣給出了用於將 2D 圖像點轉換為 3D 世界點的單應矩陣。 現在，我找到了單應矩陣的逆矩陣，它給出了 3D 世界點到 2D 圖像點之間的單應性。 之后，我將圖像點 [x, y, 1]t 與逆單應矩陣相乘得到 [wX, wY, w]t 並將整個向量除以標量 w 得到 [X, Y, 1]，其中給我世界坐標的 X 和 Y 值。

我的代碼如下所示：

#include "opencv2/opencv.hpp"
#include <stdio.h>
#include <iostream>
#include <sstream>
#include <math.h> 
#include <conio.h>

using namespace cv;
using namespace std;

Mat cameraMatrix, distCoeffs, rotationVector, rotationMatrix, 
translationVector,extrinsicMatrix, projectionMatrix, homographyMatrix, 
inverseHomographyMatrix;


Point point;
vector<Point2d> image_points;
vector<Point3d> world_points;

int main()
{
FileStorage fs1("intrinsics.yml", FileStorage::READ);

fs1["camera_matrix"] >> cameraMatrix;
cout << "Camera Matrix: " << cameraMatrix << endl << endl;

fs1["distortion_coefficients"] >> distCoeffs;
cout << "Distortion Coefficients: " << distCoeffs << endl << endl;



image_points.push_back(Point2d(275, 204));
image_points.push_back(Point2d(331, 204));
image_points.push_back(Point2d(331, 308));
image_points.push_back(Point2d(275, 308));

cout << "Image Points: " << image_points << endl << endl;

world_points.push_back(Point3d(0.0, 0.0, 0.0));
world_points.push_back(Point3d(1.775, 0.0, 0.0));
world_points.push_back(Point3d(1.775, 4.620, 0.0));
world_points.push_back(Point3d(0.0, 4.620, 0.0));

cout << "World Points: " << world_points << endl << endl;

solvePnP(world_points, image_points, cameraMatrix, distCoeffs, rotationVector, translationVector);
cout << "Rotation Vector: " << endl << rotationVector << endl << endl;
cout << "Translation Vector: " << endl << translationVector << endl << endl;

Rodrigues(rotationVector, rotationMatrix);
cout << "Rotation Matrix: " << endl << rotationMatrix << endl << endl;

hconcat(rotationMatrix, translationVector, extrinsicMatrix);
cout << "Extrinsic Matrix: " << endl << extrinsicMatrix << endl << endl;

projectionMatrix = cameraMatrix * extrinsicMatrix;
cout << "Projection Matrix: " << endl << projectionMatrix << endl << endl;

double p11 = projectionMatrix.at<double>(0, 0),
    p12 = projectionMatrix.at<double>(0, 1),
    p14 = projectionMatrix.at<double>(0, 3),
    p21 = projectionMatrix.at<double>(1, 0),
    p22 = projectionMatrix.at<double>(1, 1),
    p24 = projectionMatrix.at<double>(1, 3),
    p31 = projectionMatrix.at<double>(2, 0),
    p32 = projectionMatrix.at<double>(2, 1),
    p34 = projectionMatrix.at<double>(2, 3);


homographyMatrix = (Mat_<double>(3, 3) << p11, p12, p14, p21, p22, p24, p31, p32, p34);
cout << "Homography Matrix: " << endl << homographyMatrix << endl << endl;

inverseHomographyMatrix = homographyMatrix.inv();
cout << "Inverse Homography Matrix: " << endl << inverseHomographyMatrix << endl << endl;

Mat point2D = (Mat_<double>(3, 1) << image_points[0].x, image_points[0].y, 1);
cout << "First Image Point" << point2D << endl << endl;

Mat point3Dw = inverseHomographyMatrix*point2D;
cout << "Point 3D-W : " << point3Dw << endl << endl;

double w = point3Dw.at<double>(2, 0);
cout << "W: " << w << endl << endl;

Mat matPoint3D;
divide(w, point3Dw, matPoint3D);

cout << "Point 3D: " << matPoint3D << endl << endl;

_getch();
return 0;

我已經獲得了四個已知世界點的圖像坐標並對其進行了硬編碼以進行簡化。 image_points包含四個點的圖像坐標， world_points包含四個點的世界坐標。 我將第一個世界點視為世界軸上的原點 (0, 0, 0)，並使用已知距離計算其他四個點的坐標。 現在在計算逆單應矩陣后，我將其與與世界坐標 (0, 0, 0) 相關的 [image_points[0].x, image_points[0].y, 1]t 相乘。 然后我將結果除以第三個分量 w 得到 [X, Y, 1]。 但是在打印出 X 和 Y 的值后，它們分別不是 0、0。 做錯了什么？

我的代碼的輸出是這樣的：

Camera Matrix: [517.0036881709533, 0, 320;
0, 517.0036881709533, 212;
0, 0, 1]

Distortion Coefficients: [0.1128663679798094;
-1.487790079922432;
0;
0;
2.300571896761067]

Image Points: [275, 204;
331, 204;
331, 308;
275, 308]

World Points: [0, 0, 0;
1.775, 0, 0;
1.775, 4.62, 0;
0, 4.62, 0]

Rotation Vector:
[0.661476468596541;
-0.02794460022559267;
0.01206996342819649]

Translation Vector:
[-1.394495345140898;
-0.2454153722672731;
15.47126945512652]

Rotation Matrix:
[0.9995533907649279, -0.02011656447351923, -0.02209848058392758;
 0.002297501163799448, 0.7890323093017149, -0.6143474069013439;
 0.02979497438726573, 0.6140222623910194, 0.7887261380159]

Extrinsic Matrix:
[0.9995533907649279, -0.02011656447351923, -0.02209848058392758, 
-1.394495345140898;
 0.002297501163799448, 0.7890323093017149, -0.6143474069013439, 
-0.2454153722672731;
 0.02979497438726573, 0.6140222623910194, 0.7887261380159, 
15.47126945512652]

Projection Matrix:
[526.3071813531748, 186.086785938988, 240.9673682002232, 4229.846989065414;
7.504351145361707, 538.1053336219271, -150.4099339268854, 3153.028471890794;
0.02979497438726573, 0.6140222623910194, 0.7887261380159, 15.47126945512652]

Homography Matrix:
[526.3071813531748, 186.086785938988, 4229.846989065414;
7.504351145361707, 538.1053336219271, 3153.028471890794;
0.02979497438726573, 0.6140222623910194, 15.47126945512652]

Inverse Homography Matrix:
[0.001930136511648154, -8.512427241879318e-05, -0.5103513244724983;
-6.693679705844383e-06, 0.00242178892313387, -0.4917279870709287
-3.451449134581896e-06, -9.595179260534558e-05, 0.08513443835773901]

First Image Point[275;
204;
1]

Point 3D-W : [0.003070864657310213;
0.0004761913292736786;
0.06461112415423849]

W: 0.0646111
Point 3D: [21.04004290792539;
135.683117651025;
1]

Answer 1

您的推理是合理的，但是您在最后一個分區中犯了一些錯誤……還是我遺漏了什么？

W除法前的結果是：

Point 3D-W : 
[0.003070864657310213;
0.0004761913292736786;
0.06461112415423849]

現在我們需要通過將所有坐標除以 W（數組的第三個元素）來規范化，如您在問題中所述。 所以：

Point 3D-W Normalized = 
[0.003070864657310213 / 0.06461112415423849;
0.0004761913292736786 / 0.06461112415423849;
0.06461112415423849 / 0.06461112415423849]

結果是：

Point 3D-W Normalized = 
[0.047528420183179314;
 0.007370113668614144;
 1.0]

這該死的接近 [0,0]。