SLAM:在 Ceres 中实现 PnP 和 ICP 的优化

在 Ceres Solver 中实现 PnP（Perspective-n-Point） 和 ICP（Iterative Closest Point） 的优化，需结合非线性最小二乘问题的建模技巧、参数化方法及鲁棒性设计。以下从核心步骤、代码实现、优化策略三方面系统阐述，并辅以关键代码片段说明。

一、PnP 问题优化实现

PnP 的目标是求解相机位姿（旋转矩阵 R 和平移向量 t），使 3D 点投影到图像平面的位置与观测的 2D 点匹配。

1. 核心步骤

代价函数设计
重投影误差（Reprojection Error）是最小化目标：
error = ∥ observed_pixel − π ( K ⋅ ( R ⋅ object_point + t ) ) ∥ 2 ext{error} = | ext{observed\_pixel} – pi(K cdot (R cdot ext{object\_point} + t)) |_2 error=∥observed_pixel−π(K⋅(R⋅object_point+t))∥2​
其中 π pi π 是投影函数（将 3D 点转为 2D 像素坐标）， K K K 为相机内参矩阵。

参数化旋转
旋转矩阵需用李代数（angle-axis）或四元数表示，避免过参数化。Ceres 提供 EigenQuaternionParameterization 或自定义局部参数化（如 ceres::LocalParameterization）。

代价函数实现
使用仿函数（Functor）定义误差计算：

struct PnPCostFunctor {
              
    cv::Point3f obj_pt;
    cv::Point2f img_pt;
    cv::Mat K; // 相机内参

    template <typename T>
    bool operator()(const T* const rotation, const T* const translation, T* residual) const {
              
        T point[3] = {
               T(obj_pt.x), T(obj_pt.y), T(obj_pt.z) };
        T rotated_point[3];
        ceres::AngleAxisRotatePoint(rotation, point, rotated_point); // 旋转
        rotated_point[0] += translation[0];
        rotated_point[1] += translation[1];
        rotated_point[2] += translation[2];

        // 投影到归一化平面
        T xp = rotated_point[0] / rotated_point[2];
        T yp = rotated_point[1] / rotated_point[2];

        // 转换为像素坐标
        T u = K.at<T>(0,0)*xp + K.at<T>(0,2);
        T v = K.at<T>(1,1)*yp + K.at<T>(1,2);

        residual[0] = u - T(img_pt.x);
        residual[1] = v - T(img_pt.y);
        return true;
    }
};

2. 优化问题构建

ceres::Problem problem;
Eigen::Vector3d rotation_axis_angle; // 旋转（angle-axis）
Eigen::Vector3d translation;         // 平移

for (int i = 0; i < points_3d.size(); ++i) {
            
    ceres::CostFunction* cost_function = 
        new ceres::AutoDiffCostFunction<PnPCostFunctor, 2, 3, 3>(new PnPCostFunctor(obj_pts[i], img_pts[i], K));
    problem.AddResidualBlock(cost_function, nullptr, rotation_axis_angle.data(), translation.data());
}

// 设置旋转参数化（使用 angle-axis）
problem.SetParameterization(rotation_axis_angle.data(), new ceres::EigenQuaternionParameterization);

ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);

3. 关键优化技巧

解析雅可比：自动微分（AutoDiffCostFunction）易用，但解析雅可比（NumericDiffCostFunction 或手动实现）可加速计算。
鲁棒损失函数：添加 ceres::CauchyLoss 抑制误匹配点影响。
初始值：使用 OpenCV 的 solvePnP 或 SVD 方法提供初值，避免局部最优。

二、ICP 问题优化实现

ICP 用于对齐两组点云，最小化对应点之间的距离。

1. 核心步骤

代价函数设计
点对点误差（Point-to-Point）：
error = ∥ target_point − ( R ⋅ source_point + t ) ∥ 2 ext{error} = | ext{target\_point} – (R cdot ext{source\_point} + t) |_2 error=∥target_point−(R⋅source_point+t)∥2​
点对面误差（Point-to-Plane）更鲁棒：
error = ∥ ( target_point − ( R ⋅ source_point + t ) ) ⋅ normal ∥ 2 ext{error} = | ( ext{target\_point} – (R cdot ext{source\_point} + t)) cdot ext{normal} |_2 error=∥(target_point−(R⋅source_point+t))⋅normal∥2​

参数化
位姿参数化同 PnP（angle-axis 或四元数）。

对应点匹配
使用 KD-Tree 加速最近邻搜索（如 nanoflann），每轮迭代更新对应关系。

2. 代码实现（点对点误差）

struct ICPCostFunctor {
            
    Eigen::Vector3d source_pt;
    Eigen::Vector3d target_pt;

    template <typename T>
    bool operator()(const T* const rotation, const T* const translation, T* residual) const {
            
        T src_pt[3] = {
             T(source_pt.x()), T(source_pt.y()), T(source_pt.z()) };
        T transformed_pt[3];
        ceres::AngleAxisRotatePoint(rotation, src_pt, transformed_pt);
        transformed_pt[0] += translation[0];
        transformed_pt[1] += translation[1];
        transformed_pt[2] += translation[2];

        residual[0] = transformed_pt[0] - T(target_pt.x());
        residual[1] = transformed_pt[1] - T(target_pt.y());
        residual[2] = transformed_pt[2] - T(target_pt.z());
        return true;
    }
};

// 构建问题
for (size_t i = 0; i < correspondences.size(); ++i) {
            
    ceres::CostFunction* cost_function = 
        new ceres::AutoDiffCostFunction<ICPCostFunctor, 3, 3, 3>(new ICPCostFunctor(source_pts[i], target_pts[i]));
    problem.AddResidualBlock(cost_function, new ceres::HuberLoss(0.5), rotation_aa.data(), translation.data());
}

3. 性能优化策略

三、通用优化技巧

雅可比计算

优先使用解析雅可比（手动推导或 ceres::CostFunctionToFunctor）。
避免数值微分（NumericDiff），因其计算开销大。

鲁棒性提升

problem.AddResidualBlock(cost_function, new ceres::CauchyLoss(0.5), params); // 使用 Cauchy 核函数

抑制误匹配点（如 PnP 中的误检测、ICP 中的非重叠区域）。

局部参数化（Manifold）
对旋转使用 EigenQuaternionParameterization，对平移直接优化：

problem.SetParameterization(rotation_ptr, new ceres::EigenQuaternionParameterization);

求解器配置

options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; // 大规模问题用稀疏求解
options.minimizer_progress_to_stdout = true;                // 输出迭代信息
options.max_num_iterations = 100;                           // 控制迭代次数

性能分析

输出 Solver::Summary::FullReport() 分析时间瓶颈。
使用 CUDA 或 OpenMP 并行化代价函数评估（需自定义多线程回调）。

四、实例源码参考

PnP 完整实现
知乎：Ceres|手写PnP问题求解代码 提供完整 OpenCV+Ceres 的 PnP 实现。
ICP 优化示例
知乎：Ceres|手写ICP问题求解代码 包含位姿优化与误差评估代码。
高级应用

基于 SE(3) 的位姿优化 ：使用 Sophus 处理李群。
多帧 ICP + 图优化 ：融合闭环检测的全局优化。

总结

在 Ceres 中实现 PnP/ICP 的核心是：
① 正确建模误差函数（重投影/点距）；
② 合理参数化位姿（避免过参数化）；
③ 引入鲁棒核与加速策略（KD-Tree、稀疏求解）。
结合初始值估计与多阶段优化（粗配准+精配准），可显著提升算法收敛性与精度。进一步研究可扩展到 BA（Bundle Adjustment） 或 SLAM 后端优化 。