[英]Calculating 3D world point from 2D image point using OpenCV
我正在為iOS開發應用程序。 我根據Mastering OpenCV一書使用相機矩陣。 在我的場景中,我有一個眾所周知的盒子。 我知道它的真實尺寸,我確切地知道它的角落的像素。 使用此信息,我計算相機旋轉和平移向量。 從這些參數我可以計算攝像機位置。 我通過將3D世界坐標投影回圖像來檢查我的計算,並得到非常准確的結果。
在我的情況下,世界起源是盒子底線的中間位置。 盒子從一側打開。 圖像是在那個方向拍攝的,所以我可以看到盒子的內容。
現在,我在框中有對象。 我非常清楚這個物體角落的圖像坐標(2D)。 我知道角落的真正高度(真正的Y和Y <> 0)。 如何計算對象角落的世界X和Z.
這是我的代碼:
#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <iostream>
#include <ctype.h>
using namespace cv;
using namespace std;
Point2f point;
vector<vector<Point2f>> objectPoints(1);
vector<vector<Point2f>> boxPoints(1);
Point3f calc3DPointOutOf2DwithYknown(double u, double v, float worldY, double fx, double fy, double cx, double cy, Mat tvec, Mat rotMat)
{
Point3f tmpPoint;
// This fiunction I need to complete
return tmpPoint;
}
int main( int argc, char** argv )
{
///////// Loading image
Mat sourceImage = imread("/Users/Ilan/Xcode/LK Test/LK Test/images/box_center640X480.jpg");
namedWindow( "Source", 1 );
///// Setting box corners /////
point = Point2f((float)102,(float)367.5); //640X480
boxPoints[0].push_back(point);
circle( sourceImage, boxPoints[0][0], 3, Scalar(0,255,0), -1, 8);
point = Point2f((float)83,(float)90.5); //640X480
boxPoints[0].push_back(point);
circle( sourceImage, boxPoints[0][1], 3, Scalar(0,255,0), -1, 8);
point = Point2f((float)520,(float)82.5); //640X480
boxPoints[0].push_back(point);
circle( sourceImage, boxPoints[0][2], 3, Scalar(0,255,0), -1, 8);
point = Point2f((float)510.5,(float)361); //640X480
boxPoints[0].push_back(point);
circle( sourceImage, boxPoints[0][3], 3, Scalar(0,255,0), -1, 8);
///// Setting object corners /////
point = Point2f((float)403.5,(float)250); //640X480
objectPoints[0].push_back(point);
circle( sourceImage, objectPoints[0][0], 3, Scalar(0,255,0), -1, 8);
point = Point2f((float)426.5,(float)251.5); //640X480
objectPoints[0].push_back(point);
circle( sourceImage, objectPoints[0][1], 3, Scalar(0,255,0), -1, 8);
imshow("Source", sourceImage);
vector<vector<Point3f>> worldBoxPoints(1);
Point3f tmpPoint;
tmpPoint = Point3f((float)-100,(float)0,(float)0);
worldBoxPoints[0].push_back(tmpPoint);
tmpPoint = Point3f((float)-100,(float)-150,(float)0);
worldBoxPoints[0].push_back(tmpPoint);
tmpPoint = Point3f((float)100,(float)-150,(float)0);
worldBoxPoints[0].push_back(tmpPoint);
tmpPoint = Point3f((float)100,(float)0,(float)0);
worldBoxPoints[0].push_back(tmpPoint);
std::cout << "There are " << boxPoints[0].size() << " roomPoints and " << worldBoxPoints[0].size() << " worldRoomPoints." << std::endl;
cv::Mat cameraMatrix1(3,3,cv::DataType<double>::type);
cv::setIdentity(cameraMatrix1);
cv::Mat distCoeffs1(4,1,cv::DataType<double>::type);
distCoeffs1.at<double>(0) = 0;
distCoeffs1.at<double>(1) = 0;
distCoeffs1.at<double>(2) = 0;
distCoeffs1.at<double>(3) = 0;
//Taken from Mastring OpenCV
double fx = 6.24860291e+02 * ((float)(sourceImage.cols)/352.);
double fy = 6.24860291e+02 * ((float)(sourceImage.rows)/288.);
double cx = (float)(sourceImage.cols)/2.;
double cy = (float)(sourceImage.rows)/2.;
cameraMatrix1.at<double>(0, 0) = fx;
cameraMatrix1.at<double>(1, 1) = fy;
cameraMatrix1.at<double>(0, 2) = cx;
cameraMatrix1.at<double>(1, 2) = cy;
std::cout << "After calib cameraMatrix --- 1: " << cameraMatrix1 << std::endl;
std::cout << "After calib distCoeffs: --- 1" << distCoeffs1 << std::endl;
cv::Mat rvec1(3,1,cv::DataType<double>::type);
cv::Mat tvec1(3,1,cv::DataType<double>::type);
cv::solvePnP(worldBoxPoints[0], boxPoints[0], cameraMatrix1, distCoeffs1, rvec1, tvec1);
std::cout << "rvec --- 1: " << rvec1 << std::endl;
std::cout << "tvec --- 1: " << tvec1 << std::endl;
cv::Mat rvecM1(3,3,cv::DataType<double>::type);
cv::Rodrigues(rvec1,rvecM1);
std::cout << "cameraRotation --- 1 : " << rvecM1 << std::endl;
std::cout << "cameraPosition --- 1 : " << (rvecM1.t())*((-1.0)*tvec1) << std::endl;
std::vector<cv::Point2f> projectedPoints1;
cv::projectPoints(worldBoxPoints[0], rvec1, tvec1, cameraMatrix1, distCoeffs1, projectedPoints1);
for(unsigned int i = 0; i < projectedPoints1.size(); ++i)
{
std::cout << "box point --- 1: " << boxPoints[0][i] << " Projected to --- 1: " << projectedPoints1[i] << std::endl;
}
vector<vector<Point3f>> worldObjectPoints(1);
tmpPoint = calc3DPointOutOf2DwithYknown(objectPoints[0][0].x, objectPoints[0][0].y, /*the real Y of the object*/ -40.0, fx, fy, cx, cy, tvec1, rvecM1);
worldObjectPoints[0].push_back(tmpPoint);
tmpPoint = calc3DPointOutOf2DwithYknown(objectPoints[0][1].x, objectPoints[0][1].y, /*the real Y of the object*/ -40.0, fx, fy, cx, cy, tvec1, rvecM1);
worldObjectPoints[0].push_back(tmpPoint);
cv::projectPoints(worldObjectPoints[0], rvec1, tvec1, cameraMatrix1, distCoeffs1, projectedPoints1);
for(unsigned int i = 0; i < projectedPoints1.size(); ++i)
{
std::cout << "object point --- 1: " << objectPoints[0][i] << " Projected to --- 1: " << projectedPoints1[i] << std::endl;
}
waitKey(0);
return 0;
}
所以,我想實現calc3DPointOutOf2DwithYknown函數。 當然參數是根據我現在理解的。 如果我需要其他參數,我會使用其他參數。
非常感謝你,Ilan
我成功地自己解決了這個問題。 如果它對任何人都有幫助,那么下面是代碼:
Point3f calc3DPointOutOf2DwithYknown(double u, double v, float worldY, double fx, double fy, double cx, double cy, Mat tvec, Mat rotMat)
{
Point3f tmpPoint;
float r1 = rotMat.at<double>(0,0);
float r2 = rotMat.at<double>(0,1);
float r3 = rotMat.at<double>(0,2);
float r4 = rotMat.at<double>(1,0);
float r5 = rotMat.at<double>(1,1);
float r6 = rotMat.at<double>(1,2);
float r7 = rotMat.at<double>(2,0);
float r8 = rotMat.at<double>(2,1);
float r9 = rotMat.at<double>(2,2);
float t1 = tvec.at<double>(0,0);
float t2 = tvec.at<double>(1,0);
float t3 = tvec.at<double>(2,0);
float xt = (u/fx) - (cx/fx);
float yt = (v/fy) - (cy/fy);
float K1 = xt*r8*worldY + xt*t3 - r2*worldY - t1;
float K2 = xt*r9 - r3;
float K3 = r1 - xt*r7;
float worldZ = (yt*r7*K1 + yt*K3*r8*worldY + yt*K3*t3 - r4*K1 - K3*r5*worldY - K3*t2)/
(r4*K2 + K3*r6 - yt*r7*K2 - yt*K3*r9);
float worldX = (K1 + worldZ*K2)/K3;
tmpPoint = Point3f(worldX, worldY, worldZ);
return tmpPoint;
}
聲明:本站的技術帖子網頁,遵循CC BY-SA 4.0協議,如果您需要轉載,請注明本站網址或者原文地址。任何問題請咨詢:yoyou2525@163.com.