PnP相机位姿估计：从最小二乘原理到Ceres实战优化

1. 项目概述：从三维到二维的几何“破译”

在计算机视觉和机器人领域，我们常常面临一个核心挑战：如何从一张或多张二维图像中，反推出相机在三维世界中的“姿态”——也就是它的位置和朝向。这个问题，就是经典的“相机位姿估计”。而 PnP（Perspective-n-Point） 问题，正是解决这一挑战的基石算法之一。简单来说，PnP就是已知一组三维空间点的坐标，以及它们在二维图像平面上的投影点坐标，求解出相机的旋转矩阵（R）和平移向量（t）。这个过程，就像是给你一张照片，照片上有几个你知道真实世界位置的路标，让你反过来推算拍摄这张照片时，摄影师站在哪里、相机朝哪个方向。

PnP的应用场景无处不在。在增强现实（AR）中，手机需要实时计算出自己相对于一个识别图或现实场景的位置，才能将虚拟物体准确地“贴”在屏幕上；在机器人导航中，机器人通过摄像头观察环境中的已知特征点，从而确定自身在地图中的精确位置；在三维重建中，它也是多视图几何中不可或缺的一环。可以说，凡是需要将虚拟信息与现实世界对齐，或者让机器“看懂”自己身处何处的场景，PnP都是背后的关键数学工具。

然而，求解PnP并非易事。它是一个典型的非线性优化问题，因为三维点投影到二维图像的过程（透视投影模型）本身是非线性的。直接求解解析解往往只在特定点数（如n=3, 4, 5）下存在，且对噪声敏感。因此，在实际工程中，我们更常将其构建为一个 最小二乘问题 ：寻找一组相机位姿参数，使得所有三维点投影到图像上的理论位置，与实际观测到的图像点位置之间的误差平方和最小。这个“误差平方和最小”的思想，就是最小二乘法的核心。本文将深入拆解如何将PnP问题形式化为最小二乘问题，并详解几种主流求解方法的原理、实现细节与避坑指南。

2. 核心原理：透视投影与最小二乘建模

要理解PnP的求解，首先必须透彻掌握其背后的几何与数学模型。整个过程始于一个简单的针孔相机模型。

2.1 透视投影模型：从3D到2D的映射

假设我们有一个三维世界点 ( P_w = [X_w, Y_w, Z_w, 1]^T )（齐次坐标），以及相机的位姿，由旋转矩阵 ( R )（3x3）和平移向量 ( t )（3x1）定义。首先，我们将世界点转换到相机坐标系下： [ P_c = [X_c, Y_c, Z_c]^T = R \cdot [X_w, Y_w, Z_w]^T + t ] 或者用齐次坐标形式的变换矩阵 ( T = [R | t] ) 表示。

然后，通过针孔模型，我们将相机坐标系下的三维点投影到归一化图像平面（Z=1的平面）上，得到归一化坐标： [ x_n = \frac{X_c}{Z_c}, \quad y_n = \frac{Y_c}{Z_c} ]

最后，考虑到相机镜头可能存在的畸变以及图像传感器像素的缩放和偏移，我们应用相机内参矩阵 ( K ) 将其转换到像素坐标系： [ \begin{bmatrix} u \ v \ 1 \end{bmatrix} = K \cdot \begin{bmatrix} x_n \ y_n \ 1 \end{bmatrix}

\begin{bmatrix} f_x & 0 & c_x \ 0 & f_y & c_y \ 0 & 0 & 1 \end{bmatrix} \cdot \begin{bmatrix} x_n \ y_n \ 1 \end{bmatrix} ] 其中，( (u, v) ) 就是三维点 ( P_w ) 在图像上对应的二维像素坐标。( f_x, f_y ) 是焦距（像素单位），( (c_x, c_y) ) 是主点坐标。

注意 ：这里我们暂时忽略了镜头畸变（如径向畸变、切向畸变）。在实际高精度应用中，必须在投影过程中加入畸变模型进行校正，否则会引入系统性误差，导致优化无法收敛或结果不准。通常的做法是，先将观测到的像素坐标通过内参和畸变系数反投影到归一化平面，在归一化平面上构建重投影误差。

2.2 构建最小二乘问题

PnP问题的输入是：n对匹配点 ( { P_{w,i} \leftrightarrow p_{i} } )，其中 ( P_{w,i} ) 是世界坐标系下的三维点，( p_i = (u_i, v_i) ) 是对应的图像像素坐标。相机内参 ( K ) 已知。

我们的目标是求解未知的相机位姿 ( R, t )。根据最小二乘原理，我们定义 重投影误差 。对于第i对点，其重投影误差 ( e_i ) 定义为观测到的像素坐标与根据当前估计的 ( R, t ) 将 ( P_{w,i} ) 投影计算得到的像素坐标之间的差值。

通常使用欧氏距离的平方作为误差项： [ e_i(R, t) = | p_i - \pi(K, R, t, P_{w,i}) |_2^2 ] 其中，( \pi(\cdot) ) 代表上述完整的透视投影函数。

那么，整个PnP问题就转化为一个非线性最小二乘优化问题： [ \min_{R, t} \sum_{i=1}^{n} | p_i - \pi(K, R, t, P_{w,i}) | 2^2 ] 或者，更一般地，写作： [ \min {\mathbf{x}} \sum_{i=1}^{n} | f_i(\mathbf{x}) |^2 ] 这里，待优化变量 ( \mathbf{x} ) 就是代表 ( R, t ) 的参数（例如，用旋转向量和平移向量共6个参数表示）。

这个问题的核心难点在于：

1. 非线性 ：投影函数 ( \pi ) 是非线性的，因为存在除法运算（( Z_c ) 在分母）。
2. 旋转矩阵的约束 ：( R ) 必须是一个正交矩阵（( R^T R = I )）且行列式为1。这构成了一个流形优化问题，直接在欧氏空间对 ( R ) 的9个元素进行优化会破坏约束。
3. 可能存在多解或退化配置 ：当点共面或数量很少时，解可能不唯一。

3. 主流求解方法：从直接线性变换到非线性优化

针对上述最小二乘问题，衍生出了多种求解策略，大致可分为直接法（求闭式解）和迭代优化法两大类。

3.1 直接线性变换（DLT）

DLT是一种经典的直接求解方法。它的核心思想是 暂时忽略旋转矩阵的正交约束 ，将问题线性化。

我们从投影方程出发（忽略畸变）： [ \lambda_i \begin{bmatrix} u_i \ v_i \ 1 \end{bmatrix} = K [R | t] \begin{bmatrix} X_{w,i} \ Y_{w,i} \ Z_{w,i} \ 1 \end{bmatrix} ] 其中 ( \lambda_i = Z_{c,i} ) 是深度尺度因子。令 ( P = K[R|t] ) 为一个3x4的投影矩阵。上式可以写为： [ \lambda_i p_i = P P_{w,i} ] 利用叉乘性质 ( p_i \times (\lambda_i p_i) = 0 )，我们可以消去未知的 ( \lambda_i )，得到两个线性方程： [ \begin{cases} u_i (P^{3\top} P_{w,i}) - (P^{1\top} P_{w,i}) = 0 \ v_i (P^{3\top} P_{w,i}) - (P^{2\top} P_{w,i}) = 0 \ \end{cases} ] 这里 ( P^{k\top} ) 是矩阵 ( P ) 的第k行。对于每一对3D-2D匹配点，我们可以得到两个关于 ( P ) 的12个未知元素的线性方程。当有n>=6对非共面点时，我们可以构建一个齐次线性方程组 ( A \mathbf{p} = 0 )，其中 ( \mathbf{p} ) 是将 ( P ) 按行展开成的12维向量。

通过求解 ( A ) 的 最小奇异值对应的右奇异向量 （在SVD分解 ( A = U \Sigma V^T ) 中取 ( V ) 的最后一列），我们可以得到 ( \mathbf{p} ) 的一个解（在相差一个尺度因子的意义下）。然后，我们需要从这12个值中恢复出 ( K, R, t )。由于我们已知 ( K )，可以通过 ( P ) 的左3x3部分 ( KR ) 进行RQ分解（类似于QR分解，但因子顺序相反），来分离出 ( K ) 和 ( R )。最后，根据 ( K ) 和求得的 ( R ) 可以解出 ( t )。

实操要点与避坑 ：

• 需要至少6个点 ：因为每个点提供两个独立方程，P有12个未知数（实际是11个自由度，因为尺度模糊），所以理论上需要至少6个点。如果点共面，则需要至少4个点并采用专门的平面DLT算法。
• 结果需正交化 ：DLT求解出的 ( R ) 通常不严格满足正交约束。 必须 对其进行正交化处理，例如通过SVD：令 ( M = KR )，对 ( M ) 进行SVD分解 ( M = U \Sigma V^T )，则最优的正交矩阵 ( R' ) 为 ( R' = U V^T )，然后重新计算 ( t )。
• 对噪声敏感 ：DLT是一种代数误差最小化方法，而非几何的重投影误差最小化。因此，在噪声较大时，其精度不如迭代优化方法。 它通常用作后续非线性优化的一个高质量的初始值 。

3.2 P3P与EPnP：高效的多解选择

对于点数量较少的情况（n=3, 4），存在一些高效的解析或半解析方法。

P3P ：仅使用3个点，通过余弦定理构建方程组，理论上可以得到最多4个解。它需要额外一个点来进行解的选择。P3P速度快，常用于RANSAC框架内进行模型假设生成。

EPnP（Efficient PnP） ：这是一种非常流行且高效的方法，适用于n>=4的情况。它的核心思想是，将所有的3D参考点表示为4个虚拟控制点的加权和。这样，相机坐标系下的3D点坐标也可以表示为这4个控制点在相机坐标系下坐标的相同加权和。于是，问题转化为求解这4个控制点在相机坐标系下的坐标（共12个未知数）。通过巧妙构建方程组，可以线性地求解出这些坐标，然后通过相似变换（SVD）恢复出 ( R, t )。

EPnP的优势 在于它将问题规模从与点数相关降低到固定规模（12维线性系统），计算效率高，且精度通常优于DLT。OpenCV中的 solvePnP 函数默认就使用了EPnP的改进版本（如果点数>4）。

3.3 非线性优化：Bundle Adjustment的精髓

为了获得最高精度的解，最终往往需要诉诸非线性优化，也就是 光束法平差（Bundle Adjustment, BA） 在PnP问题上的特例——只优化相机位姿，不优化三维点。

我们回到最小二乘问题： [ \min_{\mathbf{x}} \frac{1}{2} \sum_{i=1}^{n} | f_i(\mathbf{x}) |^2 ] 其中 ( \mathbf{x} \in \mathbb{R}^6 )，通常用李代数 ( \mathfrak{se}(3) ) 或 ( \mathfrak{se}(3) ) 的平移部分加 ( \mathfrak{so}(3) ) 的旋转向量（轴角）来表示位姿。这样可以在欧氏空间进行优化，同时通过指数映射自动满足旋转矩阵的约束。

求解此类问题最常用的方法是 高斯-牛顿法 和 列文伯格-马夸尔特法 。

高斯-牛顿法 的迭代步骤为：

1. 给定初始估计 ( \mathbf{x}_0 )。
2. 对于第k次迭代，计算当前残差 ( f(\mathbf{x}_k) ) 和雅可比矩阵 ( J(\mathbf{x} k) = \frac{\partial f}{\partial \mathbf{x}} | {\mathbf{x}_k} )。
3. 求解增量方程：( (J^T J) \Delta \mathbf{x}_k = -J^T f )。
4. 更新状态：( \mathbf{x}_{k+1} = \mathbf{x}_k + \Delta \mathbf{x}_k )。
5. 重复2-4步，直到收敛（如 ( |\Delta \mathbf{x}_k| ) 小于阈值）。

列文伯格-马夸尔特法 是高斯-牛顿法的改进，通过引入阻尼因子 ( \mu ) 来调整增量方程：( (J^T J + \mu I) \Delta \mathbf{x}_k = -J^T f )。当 ( \mu ) 大时，接近最速下降法，保证收敛；当 ( \mu ) 小时，接近高斯-牛顿法，收敛快。LM算法更鲁棒。

这里的核心在于雅可比矩阵 ( J ) 的计算 。对于重投影误差 ( e = p_{obs} - p_{pred} )，我们需要求误差关于位姿李代数 ( \xi ) 的导数。根据链式法则： [ \frac{\partial e}{\partial \xi} = -\frac{\partial p_{pred}}{\partial P_c} \cdot \frac{\partial P_c}{\partial \xi} ] 其中：

• ( \frac{\partial p_{pred}}{\partial P_c} ) 是像素坐标对相机坐标系下三维点的导数，涉及内参和投影模型的雅可比。
• ( \frac{\partial P_c}{\partial \xi} ) 是相机坐标系下点坐标关于位姿李代数的导数，这由李群李代数的扰动模型给出。

在Ceres Solver或g2o等优化库中，我们通常只需要定义误差函数 f(x) ，并为其提供 解析导数 或使用 自动微分 ，库会自动处理迭代优化过程。

4. 实战：使用Ceres Solver实现PnP优化

理论说得再多，不如一行代码。下面我们以Ceres Solver为例，展示如何将PnP问题构建为一个非线性最小二乘问题并求解。

首先，定义重投影误差结构体。这里我们使用李代数 Sophus::SE3d 表示位姿，但Ceres优化需要普通数组，所以我们用数组 pose_se3 存储一个7维向量 [qx, qy, qz, qw, tx, ty, tz] （前四元数，后平移）。

struct ReprojectionError {
  ReprojectionError(double observed_x, double observed_y, const Eigen::Vector3d& point3d)
      : observed_x_(observed_x), observed_y_(observed_y), point3d_w_(point3d) {}

  template <typename T>
  bool operator()(const T* const pose_se3, T* residuals) const {
    // 1. 从数组读取位姿（四元数+平移向量）
    Eigen::Quaternion<T> q(pose_se3[3], pose_se3[0], pose_se3[1], pose_se3[2]); // w, x, y, z
    Eigen::Matrix<T, 3, 1> t(pose_se3[4], pose_se3[5], pose_se3[6]);

    // 2. 将世界点转换到相机坐标系
    Eigen::Matrix<T, 3, 1> point_c = q * point3d_w_.cast<T>() + t;

    // 3. 透视投影（假设内参已知且已归一化，即处理到归一化平面）
    // 如果使用像素坐标，这里需要乘以内参矩阵K
    T xp = point_c[0] / point_c[2];
    T yp = point_c[1] / point_c[2];

    // 4. 计算残差（观测值 - 预测值）
    // 假设观测坐标已经去除了内参和畸变，是归一化平面坐标
    residuals[0] = T(observed_x_) - xp;
    residuals[1] = T(observed_y_) - yp;

    return true;
  }

  static ceres::CostFunction* Create(double observed_x, double observed_y, const Eigen::Vector3d& point3d) {
    // 残差维度2，优化变量维度7（四元数+平移）
    return new ceres::AutoDiffCostFunction<ReprojectionError, 2, 7>(
        new ReprojectionError(observed_x, observed_y, point3d));
  }

 private:
  double observed_x_;
  double observed_y_;
  Eigen::Vector3d point3d_w_; // 世界坐标系下的3D点
};

然后，构建问题并求解：

void solvePnPWithCeres(const std::vector<Eigen::Vector3d>& points_3d,
                       const std::vector<Eigen::Vector2d>& points_2d_normalized,
                       Sophus::SE3d& init_pose) {
  ceres::Problem problem;

  // 将初始位姿转换为数组形式 [qx, qy, qz, qw, tx, ty, tz]
  double pose_se3[7];
  Eigen::Quaterniond q = init_pose.unit_quaternion();
  pose_se3[0] = q.x(); pose_se3[1] = q.y(); pose_se3[2] = q.z(); pose_se3[3] = q.w();
  Eigen::Vector3d t = init_pose.translation();
  pose_se3[4] = t.x(); pose_se3[5] = t.y(); pose_se3[6] = t.z();

  // 添加残差块
  for (size_t i = 0; i < points_3d.size(); ++i) {
    ceres::CostFunction* cost_function =
        ReprojectionError::Create(points_2d_normalized[i].x(),
                                  points_2d_normalized[i].y(),
                                  points_3d[i]);
    problem.AddResidualBlock(cost_function, nullptr /* loss function */, pose_se3);
  }

  // 由于使用四元数，需要添加局部参数化以保证更新后仍是单位四元数
  ceres::LocalParameterization* quaternion_local_parameterization =
      new ceres::EigenQuaternionParameterization;
  problem.SetParameterization(pose_se3, quaternion_local_parameterization);

  // 配置并运行求解器
  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_SCHUR; // 对于BA问题，SCHUR消元更高效
  options.minimizer_progress_to_stdout = true;
  options.max_num_iterations = 100;
  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);

  std::cout << summary.BriefReport() << "\n";

  // 将优化结果转换回Sophus::SE3d
  Eigen::Quaterniond q_opt(pose_se3[3], pose_se3[0], pose_se3[1], pose_se3[2]);
  Eigen::Vector3d t_opt(pose_se3[4], pose_se3[5], pose_se3[6]);
  init_pose = Sophus::SE3d(q_opt, t_opt);
}

关键操作解析 ：

1. 误差定义 ： ReprojectionError 的 operator() 是核心，它根据当前位姿估计计算三维点的投影位置，并与观测位置作差得到残差。这里假设输入的2D点 points_2d_normalized 是已经用内参和畸变系数校正后的归一化平面坐标 (x_n, y_n) 。 这是最佳实践 ，将畸变校正从优化循环中剥离，简化雅可比计算，提高数值稳定性。
2. 自动微分 ：我们使用了 ceres::AutoDiffCostFunction 。只需提供误差计算函数，Ceres能自动计算导数，极大降低了实现难度。对于极致性能，可以手动提供解析导数。
3. 四元数参数化 ：旋转用四元数表示时，必须添加 EigenQuaternionParameterization ，确保优化过程中四元数更新后能被重新归一化，保持其单位长度性质。
4. 初始值 ：非线性优化极度依赖初始值。通常我们会用EPnP或DLT求出一个粗略解，作为 init_pose 传入。
5. 鲁棒核函数 ：上述代码没有使用损失函数（ nullptr ）。在实际中，如果数据存在误匹配（外点），应该使用鲁棒核函数（如Huber损失、Cauchy损失）来降低外点的影响。这是工程中提升鲁棒性的关键一步。

5. 工程实践中的关键细节与避坑指南

将理论算法投入实际应用，会碰到一系列纸上谈兵时遇不到的问题。下面分享一些从实战中积累的经验。

5.1 坐标系的统一与内参处理

坑点 ：三维点坐标、相机位姿、内参矩阵的坐标系不统一，是导致结果完全错误的最常见原因。

• 世界坐标系 ：你的3D点云是在哪个坐标系下定义的？是SLAM地图坐标系，还是物体自身坐标系？必须明确，并保持一致性。
• 相机坐标系 ：通常定义为Z轴向前，X轴向右，Y轴向下（符合图像坐标系）。OpenCV等库常用此约定。但有些文献或库可能使用Y轴向上，务必核对。
• 内参矩阵K ： K 的定义必须与你的投影公式严格匹配。常用的形式是 [fx, 0, cx; 0, fy, cy; 0, 0, 1] 。确保你使用的焦距 fx, fy 是以像素为单位的。
• 归一化平面 ：强烈建议在优化前，将所有观测到的像素坐标 (u, v) 通过内参 K 和畸变系数转换到归一化平面 (x_n, y_n) 。这样，优化时的投影模型简化为 [X_c/Z_c, Y_c/Z_c] ，雅可比矩阵更简洁，且避免了在优化循环中重复进行畸变校正（非线性更强）。

5.2 外点处理与鲁棒优化

PnP的输入匹配点对中，几乎必然存在误匹配（外点）。直接使用所有点进行最小二乘优化，即使只有一个外点，也可能将解拉偏到错误的地方。

标准流程 ：

1. RANSAC + P3P/EPnP ：首先使用RANSAC框架。在每次迭代中，随机选取最小样本集（如4个点）用P3P或EPnP计算一个位姿假设，然后用这个位姿检验所有点，统计内点数量（重投影误差小于某个阈值）。迭代结束后，选择内点最多的那个模型。
2. 内点集优化 ：使用RANSAC筛选出的所有内点，用EPnP或DLT计算一个更精确的初始位姿。
3. 带核函数的非线性优化 ：将内点集和上一步得到的初始位姿，送入非线性优化器（如Ceres、g2o）。 此时，必须给误差项加上鲁棒核函数 。例如使用Huber损失：

   ceres::LossFunction* loss_function = new ceres::HuberLoss(1.0); // 阈值参数可调
   problem.AddResidualBlock(cost_function, loss_function, pose_se3);
   

   核函数会降低大残差（可能仍是隐藏的外点）对总代价函数的影响，使优化结果对少量外点不敏感。

5.3 旋转的参数化与优化技巧

在非线性优化中，如何参数化旋转至关重要。

• 李代数/旋转向量 ：最自然的方式，3个参数，无冗余。但更新时涉及指数映射和对数映射，雅可比计算稍复杂。Ceres和g2o都内置了 AngleAxis 或 SO3 的参数化支持。
• 四元数 ：4个参数，有单位约束。如上例所示，需要设置局部参数化。优点是插值、更新平滑，不易出现万向节锁。
• 欧拉角 ： 绝对不要直接用于优化 ！因为存在万向节锁，且其参数空间存在奇异性，优化极易失败。

实操心得 ：对于BA类问题，使用四元数加平移向量（共7维）作为参数块，并为其设置 EigenQuaternionParameterization ，是Ceres中非常稳定和方便的做法。在g2o中，则可以直接使用 VertexSE3Expmap 这类已经定义好的李代数顶点。

5.4 退化场景与尺度问题

• 共面点 ：当所有3D点共面时，平移向量在法线方向上的分量是不可观的。此时，DLT或P3P可能有多个解，非线性优化也可能收敛到局部极小值或出现数值不稳定。解决方法是在场景中引入非共面的点，或者使用专门针对平面目标的算法（如Homography分解）。
• 纯旋转 ：如果相机只进行旋转而没有平移，则无法三角化出深度，PnP问题也病态。通常需要足够的平移量。
• 尺度模糊 ：在单目视觉中，从图像恢复的位姿和三维结构存在一个尺度因子不确定性。PnP求解的平移向量 t 的尺度取决于你提供的3D点云的尺度。如果你的3D点云是以米为单位重建的，那么 t 的单位也是米。 确保你的3D点云具有真实的物理尺度 ，或者明确你得到的位姿是一个相似变换（包含尺度因子s）。

6. 性能调优与高级话题

当点数很多（>1000）或需要在嵌入式设备上运行时，性能成为关键。

• 稀疏性利用 ：在BA中，每个残差只依赖于一个相机位姿和一个3D点，这导致海塞矩阵 ( J^T J ) 具有特殊的稀疏块结构。使用 ceres::SPARSE_SCHUR 或 g2o 的稀疏求解器，可以极大提升求解速度。
• 雅可比计算 ：自动微分方便但比手动提供解析导数慢。对于性能瓶颈模块，可以考虑手动推导并编码雅可比矩阵，使用 ceres::SizedCostFunction 。
• 先验信息 ：如果你有IMU等其他传感器提供的位姿先验，可以将其作为先验约束添加到优化问题中，这能显著提高解的精度和鲁棒性，尤其是在视觉信息不足的时段。
• 不确定性建模 ：每个特征点的检测精度不同。可以为每个残差项赋予一个权重（信息矩阵），这个权重可以来源于特征点尺度的倒数（尺度越大，定位越不准）或描述子匹配的置信度。在Ceres中，可以通过设置残差项的协方差矩阵来实现。

7. 调试与验证：如何知道你的PnP解是对的？

写完代码，跑出结果，如何验证其正确性？

1. 重投影误差可视化 ：将优化后的位姿用于所有点（包括内点和外点），计算重投影误差，并在图像上画出连线。肉眼观察误差向量是否基本指向特征点真实位置。这是最直观的方法。
2. 收敛曲线 ：观察优化器输出的每次迭代的代价函数值。它应该单调下降并最终趋于平稳。如果震荡或上升，说明学习率、核函数参数可能设置不当。
3. 与Ground Truth对比 ：如果有真实位姿数据（如运动捕捉系统），直接计算估计位姿与真实位姿的旋转误差（如角度差）和平移误差（欧氏距离）。
4. 合成数据测试 ：在一个完全可控的环境下测试。用已知的位姿 ( R_{gt}, t_{gt} ) 和内参 ( K ) 将一组3D点投影生成2D点，并加入高斯噪声。然后用你的PnP求解器去算，对比结果与真实值的差异。这是验证算法实现正确性的黄金标准。
5. 尺度一致性检查 ：对于单目序列，虽然绝对尺度未知，但相邻帧间的相对平移尺度应该是稳定的。可以检查连续帧间估计出的平移向量的模长比例是否合理。

我个人的体会是，PnP作为视觉里程计和SLAM的底层模块，其稳定性和精度是上层应用可靠性的基石。花时间深入理解其原理，精心处理数据（去畸变、特征匹配筛选），合理设置优化参数（特别是鲁棒核函数），比盲目尝试更复杂的算法往往更有效。在实际项目中，一个由EPnP提供初值、经过LM算法优化、并辅以RANSAC和Huber核函数的PnP流程，足以应对绝大多数场景。当遇到极端情况时，再回过头来分析是数据问题、参数问题还是模型本身（如共面）的局限性，这样才能有的放矢地解决问题。