SOLVEPNP_PERSPECTIVE_FISHEYE 使用例程 C++

以下是使用SOLVEPNP_PERSPECTIVE_FISHEYE的简单例程。

#include <iostream>
#include <opencv2/opencv.hpp>

int main(int argc, char** argv)
{
    // 读取图像
    cv::Mat img = cv::imread("test.jpg");
    if (img.empty()) {
        std::cerr << "Failed to read image file." << std::endl;
        return -1;
    }

    // 检测角点
    cv::Size patternSize(8, 6);
    std::vector<cv::Point2f> corners;
    bool found = cv::findChessboardCorners(img, patternSize, corners, cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_FAST_CHECK | cv::CALIB_CB_NORMALIZE_IMAGE);
    if (!found) {
        std::cerr << "Failed to find chessboard corners." << std::endl;
        return -1;
    }

    // 亚像素级别精度优化
    cv::Mat gray;
    cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
    cv::cornerSubPix(gray, corners, cv::Size(11, 11), cv::Size(-1, -1), cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 30, 0.1));

    // 相机内参矩阵
    cv::Mat K = cv::Mat::eye(3, 3, CV_64F);
    K.at<double>(0, 0) = 1000.0; // fx
    K.at<double>(1, 1) = 1000.0; // fy
    K.at<double>(0, 2) = img.cols / 2.0; // cx
    K.at<double>(1, 2) = img.rows / 2.0; // cy

    // 畸变参数
    cv::Mat D = cv::Mat::zeros(4, 1, CV_64F);

    // 估计相机位姿
    cv::Mat rvec, tvec;
    cv::solvePnP(corners, corners3D, K, D, rvec, tvec, false, cv::SOLVEPNP_PERSPECTIVE_FISHEYE);

    // 输出结果
    std::cout << "rvec: " << rvec << std::endl;
    std::cout << "tvec: " << tvec << std::endl;

    return 0;
}

说明：

1. 读取图像并检测棋盘格角点。
2. 对角点进行亚像素级别精度优化。
3. 定义相机内参矩阵和畸变参数，这里使用固定值。
4. 调用 `cv::solvePnP` 函数估计相机位姿。注意最后一个参数设置为 `SOLVEPNP_PERSPECTIVE_FISHEYE`。
5. 输出旋转向量和平移向量。

需要注意的是，在使用 `cv::solvePnP` 函数时，输入的角点是经过亚像素级别精度优化的，输出的旋转向量和平移向量是相机坐标系下的，需要经过变换才能得到世界坐标系下的坐标。