Вычисление 3D-точки мира из 2D-точки изображения с использованием OpenCV

Я занимаюсь разработкой приложения для iOS. Я использую матрицу камеры в соответствии с книгой Мастеринг 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. Конечно, параметры соответствуют тому, что я понимаю сейчас. Если мне нужны другие параметры, я буду использовать другие.

Спасибо большое,
Илан