Выровняйте камеру по цели

Я работаю над небольшим проектом, чтобы выровнять камеру по фиксированной цели (размер в миллиметрах известен). Цель должна быть выровнена по горизонтали и вертикали по центру изображения с камеры, без вращения. Для этого камера установлена ​​на устройстве с 6 DoF, чтобы камера могла переводить в x, y, z и вращаться по горизонтали, крену, рысканию. Цель на изображении определяется с помощью четырех маркеров по углам. Я попробовал следующее, но выравнивание не правильно.

// Focal lenght in pixel for x and y is needed for the camera matrix
double fx = img.getHeight() * img.getMetaDouble(Exif::FOCAL_LENGTH_35MM) / 24.;

// Principle point of the camera
double center_x = 1931.3;
double center_y = 1336.9;

// Creation of the camera matrix
cv::Mat camMatrix = (cv::Mat_<double>(3, 3) <<
fx,  0,  center_x,
0,  fx,  center_y,
0,  0,     1);

// worldPoints are in arbitrary coordinate system.
std::vector<cv::Point3d> worldPoints;
worldPoints.push_back(cv::Point3d(-target.width_mm / 2., -target.heigth_mm / 2., 0));
worldPoints.push_back(cv::Point3d(target.width_mm / 2., -target.heigth_mm / 2., 0));
worldPoints.push_back(cv::Point3d(target.width_mm / 2., target.heigth_mm / 2., 0));
worldPoints.push_back(cv::Point3d(-target.width_mm / 2., target.heigth_mm / 2., 0));

// imagePoints are the detected marker in the image
std::vector<cv::Point2d> imagePoints;
imagePoints.push_back(cv::Point2d(targetInImage.top_left.getX(), chartInImage.top_left.getY()));
imagePoints.push_back(cv::Point2d(targetInImage.top_right.getX(), chartInImage.top_right.getY()));
imagePoints.push_back(cv::Point2d(targetInImage.bottom_right.getX(), chartInImage.bottom_right.getY()));
imagePoints.push_back(cv::Point2d(targetInImage.bottom_left.getX(), chartInImage.bottom_left.getY()));

cv::Mat rVec, tVec;

// Solve for pose of object in image
cv::solvePnP(worldPoints, imagePoints, camMatrix, cv::noArray(), rVec, tVec);

cv::Mat rotationMatrix;
cv::Rodrigues(rVec, rotationMatrix);
rotationMatrix = rotationMatrix.t();
tVec = -rotationMatrix * tVec;

double* _r = rotationMatrix.ptr<double>();
double projMatrix[12] = {_r[0], _r[1], _r[2], 0,
_r[3], _r[4], _r[5], 0,
_r[6], _r[7], _r[8], 0
};

cv::decomposeProjectionMatrix(cv::Mat(3, 4, CV_64FC1, projMatrix),
cameraMatrix,
rotMatrix,
transVect,
rotMatrixX,
rotMatrixY,
rotMatrixZ,
eulerAngles);

// Allocate single results
double x = tVec.at<double>(2, 0);
double y = tVec.at<double>(0, 0);
double z = tVec.at<double>(1, 0);

double roll = eulerAngles[2];
double pitch = eulerAngles[0];
double yaw = eulerAngles[1];

Полученные значения не являются правильными для перемещения камеры таким образом, чтобы цель была выровнена, как на картинке. Что я делаю неправильно? Я смешал разные системы координат?