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

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

一、PnP 问题优化实现

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

1. 核心步骤

• 代价函数设计
  重投影误差（Reprojection Error）是最小化目标：
  $$\text{error}=\Vert \text{observed\_pixel}-\pi (K\cdot (R\cdot \text{object\_point}+t)){\Vert }_2$$
  其中 $\pi$ 是投影函数（将 3D 点转为 2D 像素坐标），$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