estimate_pose_for_tag_homography

`estimate_pose_for_tag_homography` 是一个用于估计相机位姿的函数，它基于 Apriltag 检测结果中的 Homography 矩阵来计算相机的旋转和平移。具体来说，该函数使用 OpenCV 中的 `solvePnP` 函数来求解相机位姿。

以下是 `estimate_pose_for_tag_homography` 的主要实现步骤：

1. 根据 Apriltag 检测结果中的 Homography 矩阵，计算相机的内参矩阵和畸变系数。

cv::Mat cameraMatrix, distCoeffs;
cv::Mat H = cv::Mat::zeros(3, 3, CV_64F);
for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
        H.at<double>(i, j) = det->H[i*3+j];
    }
}
cv::Mat K_inv = cv::Mat::eye(3, 3, CV_64F) / H.at<double>(2, 2);
cameraMatrix = K_inv.inv();
distCoeffs = cv::Mat::zeros(1, 5, CV_64F);

2. 根据 Apriltag 检测结果中的码的实际大小和标定板的大小，计算相机的外参矩阵。

std::vector<cv::Point3f> objPoints;
for (int i = 0; i < 4; i++) {
    objPoints.push_back(cv::Point3f(i==0||i==3 ? 0 : tag_size, i==0||i==1 ? 0 : tag_size, 0));
}
std::vector<cv::Point2f> imgPoints;
for (int i = 0; i < 4; i++) {
    imgPoints.push_back(cv::Point2f(det->p[i][0], det->p[i][1]));
}
cv::Mat rvec, tvec;
cv::solvePnP(objPoints, imgPoints, cameraMatrix, distCoeffs, rvec, tvec);
cv::Mat R;
cv::Rodrigues(rvec, R);

3. 将旋转矩阵和平移向量合并为一个 4x4 的变换矩阵，得到相机的位姿。

cv::Mat T = cv::Mat::eye(4, 4, CV_64F);
R.copyTo(T(cv::Rect(0, 0, 3, 3)));
tvec.copyTo(T(cv::Rect(3, 0, 1, 3)));

需要注意的是，该函数只能处理单个 Apriltag 的情况，如果需要处理多个 Apriltag 的位姿估计，需要对每个 Apriltag 分别调用该函数进行计算。此外，由于相机位姿的计算过程涉及到多个参数，因此在实际使用中需要进行适当的参数调整和误差处理。