c++ - 将相机对准目标

Question

我正在做一个小项目，将相机对准固定目标（以毫米为单位的尺寸是已知的）。目标必须在相机图像中水平和垂直居中对齐，不能旋转。为了实现这一点，相机安装在具有 6DoF 的设备上，因此相机可以在 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];

结果值不正确，无法像图片中那样以目标对齐的方式移动相机。我究竟做错了什么？我混合了不同的坐标系吗？