ceres 世界坐标系转图像坐标系 C++

在C++中，可以使用ceres库来进行世界坐标系到图像坐标系的转换。下面是一个简单的示例：

首先，我们定义一个误差模型类，用于计算图像坐标系中的点与世界坐标系中的点之间的误差：

struct ReprojectionError {
    ReprojectionError(double observed_x, double observed_y, double x, double y, double z)
        : observed_x(observed_x), observed_y(observed_y), x(x), y(y), z(z) {}

    template<typename T>
    bool operator()(const T* const camera, const T* const point, T* residuals) const {
        // 将相机参数和世界坐标系中的点转换为齐次坐标
        T p[3];
        ceres::AngleAxisRotatePoint(camera, point, p);
        p[0] += camera[3];
        p[1] += camera[4];
        p[2] += camera[5];

        // 将齐次坐标转换为图像坐标系中的坐标
        T xp = p[0] / p[2];
        T yp = p[1] / p[2];

        // 计算误差
        residuals[0] = xp - T(observed_x);
        residuals[1] = yp - T(observed_y);
        return true;
    }

    double observed_x;
    double observed_y;
    double x;
    double y;
    double z;
};

然后，我们定义一个优化问题，并将相机参数和世界坐标系中的点作为优化变量：

ceres::Problem problem;

// 添加观测数据
for (int i = 0; i < num_observations; ++i) {
    ceres::CostFunction* cost_function =
        new ceres::AutoDiffCostFunction<ReprojectionError, 2, 6, 3>(new ReprojectionError(
            observed_x[i], observed_y[i], world_x[i], world_y[i], world_z[i]));
    problem.AddResidualBlock(cost_function, NULL, camera, points + 3 * i);
}

// 设置优化选项并运行优化
ceres::Solver::Options options;
options.max_num_iterations = 1000;
options.linear_solver_type = ceres::DENSE_SCHUR;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);

最后，我们可以将优化后的相机参数和世界坐标系中的点转换为图像坐标系中的坐标：

// 获取优化后的相机参数和世界坐标系中的点
double optimized_camera[6];
double optimized_points[num_points * 3];
std::memcpy(optimized_camera, camera, sizeof(double) * 6);
std::memcpy(optimized_points, points, sizeof(double) * num_points * 3);

// 将优化后的相机参数和世界坐标系中的点转换为图像坐标系中的坐标
for (int i = 0; i < num_points; ++i) {
    double p[3] = { optimized_points[3 * i], optimized_points[3 * i + 1], optimized_points[3 * i + 2] };
    ceres::AngleAxisRotatePoint(optimized_camera, p, p);
    p[0] += optimized_camera[3];
    p[1] += optimized_camera[4];
    p[2] += optimized_camera[5];
    double xp = p[0] / p[2];
    double yp = p[1] / p[2];
    std::cout << "Point " << i << " in image coordinates: (" << xp << ", " << yp << ")" << std::endl;
}

这样，我们就完成了世界坐标系到图像坐标系的转换。