内参标定的CERES优化重投影误差 ,C++实现

以下是使用CERES库进行相机内参标定并计算重投影误差的C++实现示例：

#include <iostream>
#include <ceres/ceres.h>
#include <ceres/rotation.h>

struct CameraIntrinsic {
    double fx, fy, cx, cy;
};

struct ReprojectionError {
    double u, v; // 2D observation
    double X, Y, Z; // 3D point
    CameraIntrinsic intrinsic;

    ReprojectionError(double u, double v, double X, double Y, double Z, CameraIntrinsic intrinsic) :
            u(u), v(v), X(X), Y(Y), Z(Z), intrinsic(intrinsic) {}

    template<typename T>
    bool operator()(const T *const camera, const T *const point, T *residuals) const {
        // camera: [0,1,2,3,4,5] -> [w,x,y,z,tx,ty]
        T p[3];
        ceres::QuaternionRotatePoint(camera, point, p);

        p[0] += camera[4];
        p[1] += camera[5];

        const T xp = p[0] / p[2];
        const T yp = p[1] / p[2];

        const T predicted_u = intrinsic.fx * xp + intrinsic.cx;
        const T predicted_v = intrinsic.fy * yp + intrinsic.cy;

        residuals[0] = predicted_u - u;
        residuals[1] = predicted_v - v;

        return true;
    }
};

int main() {
    std::vector<cv::Point3d> points3d;
    std::vector<cv::Point2d> points2d;

    // fill in 3D points and 2D observations

    double camera[6] = {1, 0, 0, 0, 0, 0}; // initial guess for the camera pose
    CameraIntrinsic intrinsic{640, 480, 320, 240}; // intrinsic parameters

    ceres::Problem problem;

    for (size_t i = 0; i < points3d.size(); ++i) {
        ceres::CostFunction *cost_function =
                new ceres::AutoDiffCostFunction<ReprojectionError, 2, 6, 3>(new ReprojectionError(
                        points2d[i].x, points2d[i].y,
                        points3d[i].x, points3d[i].y, points3d[i].z,
                        intrinsic));

        problem.AddResidualBlock(cost_function, nullptr, camera, points3d[i].ptr());
    }

    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_QR;
    options.minimizer_progress_to_stdout = true;

    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);

    std::cout << summary.FullReport() << std::endl;

    // calculate reprojection error
    double total_error = 0.0;
    for (size_t i = 0; i < points3d.size(); ++i) {
        double predicted[2];
        ReprojectionError(intrinsic.fx, intrinsic.fy, intrinsic.cx, intrinsic.cy,
                          camera, points3d[i].x, points3d[i].y, points3d[i].z, predicted);

        double error = cv::norm(cv::Point2d(predicted[0], predicted[1]) - points2d[i]);
        total_error += error;
    }
    std::cout << "Mean reprojection error: " << total_error / points3d.size() << std::endl;

    return 0;
}

在上述代码中，`points3d` 和 `points2d` 分别为三维空间点和对应的图像平面上的二维点。`CameraIntrinsic` 结构体表示相机内参，包括相机的焦距和光心位置。`ReprojectionError` 结构体表示重投影误差的计算方法，使用Ceres的AutoDiffCostFunction自动计算雅克比矩阵。

在 `main` 函数中，先构建 `ceres::Problem` 对象，并向其中添加每个三维点和对应的二维点的重投影误差项。然后使用 `ceres::Solver` 对象求解相机姿态和内参参数，并输出求解结果的概要。最后，计算所有点的重投影误差并输出平均值。