1. 从PnP到李代数为什么我们需要另一种优化视角在计算机视觉和机器人领域PnPPerspective-n-Point问题是一个经典且基础的任务。简单来说就是已知一组3D空间点及其在2D图像上的投影点求解相机的位姿旋转和平移。这听起来像是SLAM、AR、三维重建等应用中的标准步骤。传统的解法比如直接线性变换DLT、EPnP、UPnP等已经非常成熟能快速给出一个不错的初始解。那么为什么我们还要大费周章地引入“李代数”和“基于李代数的优化”呢问题的核心在于“优化”二字。那些直接解法给出的往往是一个解析解或最小二乘解但相机成像模型本身是非线性的透视投影而且我们获取的2D观测点像素坐标不可避免地带有噪声。直接解法通常通过代数技巧或近似来绕过非线性得到一个解但这个解可能并不是在最大似然意义下最优的。换句话说它没有充分利用所有观测信息也没有对噪声模型进行最优的估计。这就引出了非线性优化。我们想找到一个相机位姿使得所有3D点投影到图像上的位置与观测到的2D点之间的“重投影误差”总和最小。这是一个典型的非线性最小二乘问题。而描述相机位姿的旋转矩阵R属于特殊正交群SO(3)平移向量t属于三维欧氏空间。在优化过程中我们需要不断调整R和t。但麻烦来了旋转矩阵R有9个参数却只有3个自由度并且必须满足正交且行列式为1的约束。直接在9维空间中对R做加法更新比如R_new R_old ΔR会破坏其正交性导致更新后的矩阵不再是合法的旋转矩阵。这就是李代数大显身手的地方。李代数这里特指so(3)可以看作是旋转矩阵在单位元处的切空间它只有3个自由度正好对应旋转的三个轴角并且其上的加法运算通过指数映射可以对应到旋转矩阵的乘法运算。这意味着我们可以用一个三维向量φ即李代数来表示一个旋转的微小扰动。在优化迭代中我们不再直接更新旋转矩阵R而是更新其对应的李代数φ。通过指数映射exp(φ^)将李代数扰动加到当前估计上得到的新旋转矩阵自动满足SO(3)的约束。平移向量t本身没有约束可以直接在其三维空间中进行更新。因此“基于李代数的PnP优化”的本质是将带有旋转矩阵约束的非线性最小二乘问题转化为在无约束的李代数空间和欧氏空间中进行数值优化的问题。这种方法通常结合高斯-牛顿法或列文伯格-马夸尔特法能迭代地找到使重投影误差最小的相机位姿其结果通常比直接解法更精确、更鲁棒尤其是在数据有噪声或存在外点的情况下。接下来我们将深入这个过程的每一个细节。2. 数学基础SO(3)、SE(3)与它们的李代数要理解基于李代数的优化必须先打好数学基础。这部分内容可能有些抽象但我会尽量用几何直观和实际意义来解释这是后续所有推导和实操的基石。2.1 旋转矩阵SO(3)与李代数so(3)旋转矩阵R的集合构成了一个李群称为特殊正交群SO(3)。“群”意味着它满足封闭性、结合律、有单位元、有逆元等性质。“特殊正交”意味着R^T R I 且 det(R) 1。李代数so(3)定义为所有三维反对称矩阵的集合so(3) { φ^ ∈ R^{3×3} | φ^ -φ^T }其中符号^表示将一个三维向量φ [φ1, φ2, φ3]^T 映射为一个反对称矩阵φ^ [ 0, -φ3, φ2; φ3, 0, -φ1; -φ2, φ1, 0 ]这个反对称矩阵φ^就是李代数so(3)的一个元素。向量φ本身可以看作so(3)空间中的一个坐标。关键的联系在于指数映射对于任意φ∈R^3有 R exp(φ^)。这个指数映射将李代数so(3)中的元素映射到李群SO(3)上。反之对数映射log(R)可以将旋转矩阵映射回李代数注意对于旋转角为π的旋转对数映射不唯一。指数映射的物理意义向量φ的模长||φ||表示旋转的角度其方向表示旋转轴遵循右手法则。exp(φ^)计算出的正是绕轴φ/||φ||旋转||φ||角度的旋转矩阵。当φ是一个微小量时指数映射有一阶近似exp(φ^) ≈ I φ^。这个近似在优化中至关重要它允许我们用简单的加法来处理旋转的局部更新。2.2 变换矩阵SE(3)与李代数se(3)相机位姿通常由旋转和平移共同构成表示为变换矩阵TT [ R t; 0^T 1 ] 其中 R ∈ SO(3) t ∈ R^3所有这样的变换矩阵T构成了特殊欧氏群SE(3)。对应的李代数se(3)是一个六维空间。一个se(3)元素可以记作ξ [ρ, φ]^T其中ρ∈R^3φ∈R^3。它对应的矩阵形式为ξ^ [ φ^ ρ; 0^T 0 ]同样存在指数映射将se(3)映射到SE(3)T exp(ξ^)。这个映射的物理意义更丰富φ部分仍然代表旋转而ρ部分在旋转的基础上还包含了与平移相关的信息。当旋转为小量时也有近似exp(ξ^) ≈ I ξ^。为什么用se(3)而不用so(3)t在优化中我们有时会将位姿的旋转和平移分开参数化即用so(3)的φ和R^3的t。这被称为“旋转-平移参数化”。而使用se(3)的ξ进行参数化被称为“李代数参数化”或“指数坐标参数化”。两者在理论上是等价的局部同胚但在实际优化中se(3)参数化有时能提供更统一的处理方式和更好的数值性质尤其是在使用某些优化库时。本文后续将主要采用更直观的“旋转-平移参数化”但原理完全相通。2.3 李代数的求导扰动模型优化算法的核心是求导。我们需要计算重投影误差关于位姿参数的雅可比矩阵导数。直接对旋转矩阵R求导非常不便。李代数提供了优雅的解决方案扰动模型。思路是给当前估计的旋转R施加一个微小的左扰动或右扰动ΔR这个扰动由李代数φ对应的微小旋转矩阵exp(φ^) ≈ I φ^来表示。然后计算误差关于这个扰动李代数φ的导数。因为φ是三维无约束向量这个导数很容易计算。具体来说假设我们有一个三维点P_w在世界坐标系下当前相机位姿为T包含R, t。点投影到相机归一化平面为u π( R * P_w t ) 其中π是投影函数[X, Y, Z]^T - [X/Z, Y/Z]^T重投影误差e u_observed - u。现在给旋转R施加一个左扰动ΔR exp(φ^)。扰动后的投影为u π( (ΔR * R) * P_w t ) ≈ π( (I φ^) * R * P_w t ) π( R*P_w t φ^ * (R*P_w) )注意φ^ * (RP_w) - (RP_w)^ × φ根据反对称矩阵的性质a^ * b - b^ * a。令P_c R * P_w t 为点在相机坐标系下的坐标则u ≈ π( P_c - (P_c)^ × φ )这里(P_c)^是P_c的反对称矩阵。接下来利用归一化坐标u [u_x, u_y]^T [X_c/Z_c, Y_c/Z_c]^T我们可以计算u关于扰动φ的雅可比矩阵。通过链式法则最终得到∂e/∂φ - [ ∂π/∂P_c ] * (P_c)^其中∂π/∂P_c是投影函数关于相机坐标系下点的雅可比是一个2x3矩阵∂π/∂P_c [ 1/Z_c, 0, -X_c/Z_c^2; 0, 1/Z_c, -Y_c/Z_c^2 ]类似地我们可以推导误差关于平移扰动ρ这里我们直接对t加扰动的雅可比∂e/∂ρ - [ ∂π/∂P_c ] * I_{3x3}这样我们就得到了重投影误差关于位姿扰动旋转李代数φ和平移向量ρ的2x6雅可比矩阵。这个雅可比矩阵是迭代优化算法如高斯-牛顿法的核心输入。注意这里推导的是左扰动模型。也有右扰动模型其形式类似但物理意义不同扰动施加在坐标系上而非点上。在大多数视觉问题中左扰动模型更常用。务必在代码实现中保持一致性。3. 构建PnP非线性最小二乘问题有了数学工具我们现在可以正式定义要解决的优化问题。假设我们有n对匹配点3D点 {P_i | i1,...,n} 在世界坐标系下对应的2D像素观测 {z_i | i1,...,n}。3.1 误差函数与代价函数首先定义单个观测的重投影误差。对于第i个点使用当前位姿估计T[R|t]将世界点P_i变换到相机坐标系P_c^i R * P_i t。将相机坐标系下的点投影到归一化平面u_i [X_c^i / Z_c^i, Y_c^i / Z_c^i]^T。通常相机还有内参矩阵K焦距f_x, f_y和主点c_x, c_y需要将归一化坐标映射到像素坐标p_i K * u_i [f_x * u_x c_x, f_y * u_y c_y]^T。重投影误差就是观测像素坐标z_i与计算出的像素坐标p_i的差e_i(T) z_i - p_i(T)。这是一个二维误差向量。我们的目标是找到最优的位姿T*使得所有误差的平方和最小。这构成了一个非线性最小二乘问题T* argmin_T 0.5 * Σ_i || e_i(T) ||^2这里取0.5是为了后续求导后形式更简洁。||·||表示向量的L2范数。3.2 参数化与优化变量如前所述我们选择在无约束的空间中进行优化。因此优化变量x是一个6维向量x [φ^T, t^T]^T [φ1, φ2, φ3, t1, t2, t3]^T其中φ是旋转对应的李代数t是平移向量。在迭代过程中我们不断更新这个6维向量δx [δφ^T, δt^T]^T。迭代更新公式假设第k次迭代的位姿估计为T_k对应的李代数为φ_k满足R_k exp(φ_k^)平移为t_k。我们求解一个增量δx然后按以下方式更新φ_{k1} φ_k δφ t_{k1} t_k δt R_{k1} exp(φ_{k1}^) // 注意这里需要将更新后的李代数映射回旋转矩阵另一种等价的、更常用的更新方式是直接在李群上通过左乘扰动来更新T_{k1} exp(δξ^) * T_k其中δξ [δρ, δφ]^T 是se(3)上的增量。这两种方式在理论上对于微小增量是近似等价的但实现上略有不同。第一种“旋转-平移参数化”更直观我们后续将采用这种方式。3.3 高斯-牛顿法框架高斯-牛顿法是解决非线性最小二乘问题最常用的方法之一。其核心思想是在每次迭代的当前点x_k处将非线性误差函数e_i(x)进行一阶泰勒展开线性化然后求解一个线性最小二乘问题得到增量δx。将误差函数在x_k处展开e_i(x_k δx) ≈ e_i(x_k) J_i(x_k) * δx其中J_i是e_i关于x的雅可比矩阵在我们这里是2x6的矩阵前面已经推导。代入代价函数F(x_k δx) 0.5 * Σ_i || e_i(x_k) J_i δx ||^2 0.5 * Σ_i ( e_i^T e_i 2 e_i^T J_i δx δx^T J_i^T J_i δx )为了最小化F(x_k δx)关于δx我们令其导数为零∂F/∂(δx) Σ_i ( J_i^T e_i J_i^T J_i δx ) 0整理得到高斯-牛顿方程也称为正规方程( Σ_i J_i^T J_i ) * δx - Σ_i J_i^T e_i记 H Σ_i J_i^T J_i 为近似的海森矩阵Hessian g Σ_i J_i^T e_i 为梯度。则方程简化为H * δx -g这是一个线性方程组。求解这个6x6的线性系统得到增量δx然后按照上述更新公式更新位姿估计。重复这个过程直到增量δx的范数小于某个阈值或者代价函数的变化不再显著算法收敛。实操心得初始化的重要性。高斯-牛顿法是一个局部优化方法其收敛性和最终结果严重依赖于初始值。一个糟糕的初始值例如离真实解太远可能导致算法收敛到局部极小值甚至发散。因此在实践中我们总是先用一个快速的直接解法如EPnP计算出一个初始位姿再将其作为非线性优化的起点。这结合了直接解法速度快和优化方法精度高的优点。4. 手把手实现从理论到代码理论已经完备现在是时候将数学公式转化为实际运行的代码了。我们将使用C和Eigen库进行演示因为Eigen提供了优秀的矩阵运算和几何模块。这里会省略一些工程细节如内存管理、接口设计聚焦于核心算法流程。4.1 数据结构定义首先定义一些基本的数据结构。#include Eigen/Core #include Eigen/Geometry #include vector // 使用Eigen类型别名 using Vec2 Eigen::Vector2d; using Vec3 Eigen::Vector3d; using Mat23 Eigen::Matrixdouble, 2, 3; using Mat36 Eigen::Matrixdouble, 3, 6; using Mat66 Eigen::Matrixdouble, 6, 6; using Vec6 Eigen::Matrixdouble, 6, 1; // 一个3D-2D匹配对 struct Match { Vec3 P_w; // 世界坐标系下的3D点 Vec2 z; // 观测到的2D像素坐标 }; // 相机内参简化假设没有畸变 struct CameraIntrinsics { double fx, fy; // 焦距 double cx, cy; // 主点 };4.2 核心函数计算雅可比矩阵与误差这是整个优化器的核心。对于给定的一个位姿估计用李代数φ和平移t表示和一个匹配对计算重投影误差和关于优化变量φ, t的雅可比矩阵。/** * brief 计算单个点的重投影误差和雅可比矩阵 * param[in] P_w 世界点 * param[in] z_obs 观测像素坐标 * param[in] phi 当前旋转的李代数 (so(3)) * param[in] t 当前平移 * param[in] K 相机内参 * param[out] error 2x1 重投影误差 * param[out] jacobian 2x6 雅可比矩阵 (关于 [phi; t]) */ void ComputeReprojectionErrorAndJacobian( const Vec3 P_w, const Vec2 z_obs, const Vec3 phi, const Vec3 t, const CameraIntrinsics K, Vec2 error, Eigen::Matrixdouble, 2, 6 jacobian) { // 1. 将李代数phi转换为旋转矩阵R Eigen::AngleAxisd rotation_vector(phi.norm(), phi.normalized()); Eigen::Matrix3d R rotation_vector.toRotationMatrix(); // 注意当phi很小时可以用更高效的近似R ≈ I phi^但这里用标准转换保证精度。 // 2. 将世界点变换到相机坐标系 Vec3 P_c R * P_w t; double X P_c[0], Y P_c[1], Z P_c[2]; // 防止深度为负或零这是一个重要的鲁棒性检查 if (Z 1e-6) { // 处理异常情况例如设置一个很大的误差或跳过该点 error.setConstant(1e6); jacobian.setZero(); return; } // 3. 投影到归一化平面并计算像素坐标 double inv_Z 1.0 / Z; double inv_Z2 inv_Z * inv_Z; Vec2 u_normalized(X * inv_Z, Y * inv_Z); Vec2 z_pred(K.fx * u_normalized[0] K.cx, K.fy * u_normalized[1] K.cy); // 4. 计算重投影误差 error z_obs - z_pred; // 5. 计算投影函数关于相机坐标点P_c的雅可比 (2x3) // ∂p/∂P_c [∂p/∂u] * [∂u/∂P_c] // 其中 ∂u/∂P_c [1/Z, 0, -X/Z^2; 0, 1/Z, -Y/Z^2] Mat23 J_proj; J_proj K.fx * inv_Z, 0, -K.fx * X * inv_Z2, 0, K.fy * inv_Z, -K.fy * Y * inv_Z2; // 6. 计算P_c关于扰动φ和t的雅可比 (3x6) // 根据扰动模型P_c R * P_w t - (P_c)^ × δφ δt // 所以 ∂P_c/∂φ - (P_c)^ ∂P_c/∂t I Eigen::Matrix3d P_c_hat; P_c_hat 0, -Z, Y, Z, 0, -X, -Y, X, 0; // 这是(P_c)^的矩阵形式 Mat36 J_Pc; J_Pc.block3, 3(0, 0) -P_c_hat; // 关于φ的部分 J_Pc.block3, 3(0, 3) Eigen::Matrix3d::Identity(); // 关于t的部分 // 7. 链式法则得到最终的雅可比矩阵 (2x6) jacobian -J_proj * J_Pc; // 注意负号因为误差 e z_obs - z_pred而z_pred f(T) }关键细节与避坑指南深度检查第2步中的if (Z 1e-6)至关重要。如果点在相机后面Z为负或非常接近相机中心Z接近零投影计算会失效导致数值不稳定如除以零。在实际应用中这可能意味着初始位姿估计极差或者该匹配点是外点错误匹配。一个常见的处理策略是给该点赋予一个很大的固定误差如代码所示使其在本次迭代中对梯度贡献很大从而“拉回”位姿估计或者在后续引入鲁棒核函数来降低此类点的权重。李代数到旋转矩阵的转换我们使用了Eigen::AngleAxisd进行转换。当扰动δφ很小时也可以使用一阶近似R_new R_old * (I δφ^)来避免频繁的指数/对数映射这能提升计算速度但可能引入微小误差。在精度要求高的迭代后期建议使用精确的转换。雅可比矩阵的负号注意最后一步jacobian -J_proj * J_Pc。这是因为我们定义误差e z_obs - z_pred而z_pred是关于位姿T的函数。所以∂e/∂T - ∂z_pred/∂T。这个负号很容易被忽略导致求解的增量方向错误优化发散。4.3 高斯-牛顿迭代主循环现在我们可以实现完整的高斯-牛顿优化流程。/** * brief 使用高斯-牛顿法优化PnP位姿 * param[in] matches 3D-2D匹配对集合 * param[in] K 相机内参 * param[in, out] phi 初始旋转李代数优化后更新 * param[in, out] t 初始平移优化后更新 * param[in] max_iterations 最大迭代次数 * param[in] epsilon 收敛判据增量范数阈值 * return 最终的重投影误差总和 */ double GaussNewtonPnP(const std::vectorMatch matches, const CameraIntrinsics K, Vec3 phi, Vec3 t, int max_iterations 10, double epsilon 1e-6) { double total_cost 0.0; for (int iter 0; iter max_iterations; iter) { Mat66 H Mat66::Zero(); // 近似海森矩阵 H Σ J^T J Vec6 g Vec6::Zero(); // 梯度 g Σ J^T e double cost_this_iter 0.0; // 遍历所有匹配点累加H和g for (const auto match : matches) { Vec2 error; Eigen::Matrixdouble, 2, 6 J; ComputeReprojectionErrorAndJacobian(match.P_w, match.z, phi, t, K, error, J); // 累加海森矩阵和梯度 H J.transpose() * J; g J.transpose() * error; // 计算当前点的误差平方并累加到本轮总代价中 cost_this_iter error.squaredNorm(); } total_cost 0.5 * cost_this_iter; // 记住我们的代价函数有0.5因子 // 求解线性方程 H * δx -g Vec6 dx H.ldlt().solve(-g); // 使用LDLT分解求解H是对称半正定矩阵 // 检查求解是否成功 if (std::isnan(dx[0])) { std::cerr Iteration iter : H matrix is singular or ill-conditioned! std::endl; break; } // 更新位姿估计 Vec3 dphi dx.head3(); Vec3 dt dx.tail3(); phi dphi; t dt; // 收敛判断如果增量非常小则停止迭代 double delta_norm dx.norm(); if (delta_norm epsilon) { std::cout Converged after iter 1 iterations. std::endl; break; } std::cout Iteration iter : cost total_cost , |dx| delta_norm std::endl; } return total_cost; }4.4 初始化与调用示例最后我们需要一个合理的初始值来启动优化。通常我们会用一个快速的直接解法如EPnP来获取初始R和t然后将其转换为李代数φ。这里为了演示完整性我们假设已经有了一个初始旋转矩阵R_init和平移t_init。int main() { // 1. 准备模拟数据 CameraIntrinsics K{520.9, 521.0, 325.1, 249.7}; // 示例内参 std::vectorMatch matches; // ... 这里填充matches例如从文件读取或随机生成一些3D-2D对应点 // 2. 获取初始位姿这里用假设值实践中用EPnP等计算 Eigen::Matrix3d R_init Eigen::Matrix3d::Identity(); // 假设初始旋转为单位阵 Vec3 t_init(0.1, 0.1, 1.0); // 假设初始平移 // 3. 将初始旋转矩阵转换为李代数轴角 Eigen::AngleAxisd init_rotation(R_init); Vec3 phi_init init_rotation.angle() * init_rotation.axis(); // 4. 调用高斯-牛顿优化 Vec3 phi_opt phi_init; Vec3 t_opt t_init; double final_cost GaussNewtonPnP(matches, K, phi_opt, t_opt, 20, 1e-8); // 5. 将优化后的李代数转换回旋转矩阵 Eigen::AngleAxisd opt_rotation(phi_opt.norm(), phi_opt.normalized()); Eigen::Matrix3d R_opt opt_rotation.toRotationMatrix(); std::cout Optimization finished. Final cost: final_cost std::endl; std::cout Optimized Rotation:\n R_opt std::endl; std::cout Optimized Translation:\n t_opt.transpose() std::endl; return 0; }5. 进阶话题与工程实践中的挑战实现一个基础版本只是第一步。要让基于李代数的PnP优化在实际项目中稳定、高效、鲁棒地工作还需要考虑以下几个关键问题。5.1 鲁棒核函数应对外点干扰上面的实现有一个致命弱点它假设所有的匹配点都是正确的内点。然而在实际中特征匹配总会产生一定比例的错误匹配外点。这些外点会产生巨大的重投影误差由于最小二乘是平方项这些大误差会占据主导地位严重扭曲优化结果甚至导致算法完全失效。解决方案是使用鲁棒核函数Robust Kernel Function。其核心思想是对误差的平方项进行加权使得误差大的点可能是外点的权重降低。常用的核函数有Huber核、Cauchy核、Tukey核等。以Huber核为例其损失函数定义为L(e) { 0.5 * e^2, if |e| δ { δ * (|e| - 0.5 * δ), otherwise其中δ是一个阈值参数。在优化中这等价于给每个误差项乘以一个权重ww { 1, if |e| δ { δ / |e|, otherwise因此在构建高斯-牛顿方程时我们对每个点的雅可比矩阵J_i和误差e_i进行加权H w * J_i^T J_ig w * J_i^T e_i。实现要点需要在每次迭代中根据当前误差重新计算每个点的权重。这引入了迭代重加权最小二乘IRLS的思想。阈值δ的选择需要根据具体问题的噪声水平来调整通常取重投影误差中位数的若干倍如1.345倍标准差对应95%效率的Huber核。5.2 数值稳定性与求解器选择在我们的代码中使用H.ldlt().solve(-g)来求解线性方程组。LDLT分解适用于对称正定或半正定矩阵且比完整的LU或QR分解更高效。然而当H矩阵病态条件数很大时求解结果可能不稳定。病态性的来源观测不足匹配点数量太少或者所有点都近似共面/共线导致某些自由度如尺度、绕某个轴的旋转无法被有效约束。数值问题点的深度Z值差异巨大或者像素坐标与3D坐标的量纲差异大导致H矩阵中元素的数量级相差悬殊。解决方案预处理Preconditioning对优化变量进行缩放使其各个分量的量级大致相同。例如将平移向量t的单位从米转换为毫米或者对李代数φ进行缩放。这相当于求解(P^T H P) * (P^{-1}δx) -P^T g其中P是一个预处理矩阵通常是对角阵。使用更鲁棒的求解器对于病态问题可以使用QR分解更稳定但稍慢或奇异值分解SVD最稳定但最慢。在Eigen中可以使用H.colPivHouseholderQr().solve(-g)或H.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(-g)。列文伯格-马夸尔特Levenberg-Marquardt, LM算法LM算法是高斯-牛顿法的改进它在H矩阵上加上一个阻尼项λI即求解(H λI) δx -g。当λ很大时算法接近最速下降法稳定但慢当λ很小时算法接近高斯-牛顿法快但不稳定。LM算法能自动调整λ在稳定性和收敛速度之间取得平衡。强烈建议在实际项目中使用LM算法而非纯高斯-牛顿法。许多优化库如g2o, Ceres Solver默认使用或提供了LM算法。5.3 与优化库的集成以Ceres Solver为例手动实现优化循环有利于理解原理但在生产环境中使用成熟的优化库是更明智的选择。它们提供了经过充分测试的数值优化实现、自动求导、丰富的损失函数以及多线程支持等。下面展示如何使用Ceres Solver来实现相同的PnP优化。首先定义一个代价函数仿函数Functorstruct PnPCostFunction { PnPCostFunction(const Vec3 P_w, const Vec2 z_obs, const CameraIntrinsics K) : P_w_(P_w), z_obs_(z_obs), K_(K) {} template typename T bool operator()(const T* const pose_params, // 6维数组 [phi1, phi2, phi3, t1, t2, t3] T* residuals) const { // 1. 提取旋转和平移参数 const T* phi pose_params; const T* t pose_params 3; // 2. 将李代数(phi)转换为旋转矩阵R (使用角轴) Eigen::MatrixT, 3, 1 phi_vec(phi[0], phi[1], phi[2]); T angle phi_vec.norm(); Eigen::MatrixT, 3, 3 R; if (angle T(1e-8)) { // 小角度近似: R ≈ I phi^ R Eigen::MatrixT, 3, 3::Identity(); R(0,1) -phi[2]; R(0,2) phi[1]; R(1,0) phi[2]; R(1,2) -phi[0]; R(2,0) -phi[1]; R(2,1) phi[0]; } else { Eigen::MatrixT, 3, 1 axis phi_vec / angle; // Rodrigues rotation formula T c ceres::cos(angle); T s ceres::sin(angle); T one_minus_c T(1) - c; T x axis[0], y axis[1], z axis[2]; R x*x*one_minus_c c, x*y*one_minus_c - z*s, x*z*one_minus_c y*s, x*y*one_minus_c z*s, y*y*one_minus_c c, y*z*one_minus_c - x*s, x*z*one_minus_c - y*s, y*z*one_minus_c x*s, z*z*one_minus_c c; } // 3. 变换到相机坐标系并投影 Eigen::MatrixT, 3, 1 P_c R * P_w_.castT() Eigen::MatrixT, 3, 1(t[0], t[1], t[2]); T X P_c[0], Y P_c[1], Z P_c[2]; // 检查深度 if (Z T(1e-6)) { return false; // 深度无效返回false告知Ceres } T inv_Z T(1) / Z; T u_pred T(K_.fx) * X * inv_Z T(K_.cx); T v_pred T(K_.fy) * Y * inv_Z T(K_.cy); // 4. 计算残差 residuals[0] T(z_obs_[0]) - u_pred; residuals[1] T(z_obs_[1]) - v_pred; return true; } private: const Vec3 P_w_; const Vec2 z_obs_; const CameraIntrinsics K_; };然后构建问题并求解void OptimizePoseWithCeres(const std::vectorMatch matches, const CameraIntrinsics K, Vec6 initial_pose) { // initial_pose [phi; t] ceres::Problem problem; for (const auto match : matches) { ceres::CostFunction* cost_function new ceres::AutoDiffCostFunctionPnPCostFunction, 2, 6( new PnPCostFunction(match.P_w, match.z, K)); problem.AddResidualBlock(cost_function, nullptr, // 不使用核函数或使用 new ceres::HuberLoss(1.0) initial_pose.data()); } ceres::Solver::Options options; options.linear_solver_type ceres::DENSE_QR; // 使用QR分解更稳定 options.minimizer_progress_to_stdout true; options.max_num_iterations 50; ceres::Solver::Summary summary; ceres::Solve(options, problem, summary); std::cout summary.BriefReport() std::endl; }使用Ceres Solver的优势非常明显我们只需要定义残差如何计算而无需手动推导和编写雅可比矩阵通过自动求导也无需实现复杂的优化循环。库会帮我们处理LM算法的阻尼调整、线性求解器的选择、收敛判断等所有细节大大提高了开发效率和代码的稳健性。5.4 性能优化与尺度问题性能对于实时应用如SLAM优化速度至关重要。除了使用高效的线性代数库如Eigen和优化库外还可以减少点数在保证精度的前提下使用随机采样一致性RANSAC先剔除外点或者对点进行网格筛选避免使用过多冗余点。稀疏性PnP问题的海森矩阵H是稠密的6x6小矩阵本身没有稀疏结构。但在更大的BABundle Adjustment问题中海森矩阵是稀疏的。如果PnP被嵌入到BA中应使用专门为稀疏矩阵设计的求解器如SuiteSparse, CHOLMOD。尺度问题PnP问题从2D-3D对应中恢复的平移向量t具有尺度不确定性。也就是说如果将所有3D点坐标和t同时缩放一个因子重投影误差不变。因此优化得到的t的尺度取决于你提供的3D点坐标的尺度。在单目视觉中这是一个固有的尺度不确定性。在融合了IMU或深度传感器的系统中需要通过其他手段确定尺度。6. 总结与扩展思考基于李代数的PnP优化将复杂的带约束旋转矩阵优化问题优雅地转化到了无约束的李代数空间中进行使得标准的非线性最小二乘优化工具得以直接应用。我们从数学基础SO(3)/SE(3)与李代数、问题建模、雅可比推导到手把手代码实现完整地走通了整个流程。我个人在实际操作中的体会是理解李代数和扰动模型是打通视觉SLAM和三维重建中优化问题的关键一环。它不仅仅是PnP的专用工具更是后端BA光束法平差、位姿图优化等核心模块的基础。在实现时最容易出错的地方往往是雅可比矩阵的符号和李代数更新的方式左乘扰动还是右乘扰动是在李代数上加还是用指数映射更新李群。务必在代码中保持清晰和一致并编写充分的单元测试进行验证例如用数值微分在参数上加一个微小扰动计算误差变化来验证自己推导的解析雅可比是否正确。最后再分享一个小技巧当你的优化结果不理想时不要急于调整复杂的算法参数。首先检查你的初始值是否合理用EPnP等直接解法验证其次可视化重投影误差将优化前后的投影点与观测点画在图像上能直观地看到优化是否在向正确的方向移动最后开启优化库如Ceres的详细输出观察每次迭代的代价下降是否平滑以及LM算法的阻尼因子λ的变化情况这能帮助你判断问题是数据问题、雅可比错误还是数值病态。 从PnP到李代数：非线性优化在视觉SLAM中的原理与实现
1. 从PnP到李代数为什么我们需要另一种优化视角在计算机视觉和机器人领域PnPPerspective-n-Point问题是一个经典且基础的任务。简单来说就是已知一组3D空间点及其在2D图像上的投影点求解相机的位姿旋转和平移。这听起来像是SLAM、AR、三维重建等应用中的标准步骤。传统的解法比如直接线性变换DLT、EPnP、UPnP等已经非常成熟能快速给出一个不错的初始解。那么为什么我们还要大费周章地引入“李代数”和“基于李代数的优化”呢问题的核心在于“优化”二字。那些直接解法给出的往往是一个解析解或最小二乘解但相机成像模型本身是非线性的透视投影而且我们获取的2D观测点像素坐标不可避免地带有噪声。直接解法通常通过代数技巧或近似来绕过非线性得到一个解但这个解可能并不是在最大似然意义下最优的。换句话说它没有充分利用所有观测信息也没有对噪声模型进行最优的估计。这就引出了非线性优化。我们想找到一个相机位姿使得所有3D点投影到图像上的位置与观测到的2D点之间的“重投影误差”总和最小。这是一个典型的非线性最小二乘问题。而描述相机位姿的旋转矩阵R属于特殊正交群SO(3)平移向量t属于三维欧氏空间。在优化过程中我们需要不断调整R和t。但麻烦来了旋转矩阵R有9个参数却只有3个自由度并且必须满足正交且行列式为1的约束。直接在9维空间中对R做加法更新比如R_new R_old ΔR会破坏其正交性导致更新后的矩阵不再是合法的旋转矩阵。这就是李代数大显身手的地方。李代数这里特指so(3)可以看作是旋转矩阵在单位元处的切空间它只有3个自由度正好对应旋转的三个轴角并且其上的加法运算通过指数映射可以对应到旋转矩阵的乘法运算。这意味着我们可以用一个三维向量φ即李代数来表示一个旋转的微小扰动。在优化迭代中我们不再直接更新旋转矩阵R而是更新其对应的李代数φ。通过指数映射exp(φ^)将李代数扰动加到当前估计上得到的新旋转矩阵自动满足SO(3)的约束。平移向量t本身没有约束可以直接在其三维空间中进行更新。因此“基于李代数的PnP优化”的本质是将带有旋转矩阵约束的非线性最小二乘问题转化为在无约束的李代数空间和欧氏空间中进行数值优化的问题。这种方法通常结合高斯-牛顿法或列文伯格-马夸尔特法能迭代地找到使重投影误差最小的相机位姿其结果通常比直接解法更精确、更鲁棒尤其是在数据有噪声或存在外点的情况下。接下来我们将深入这个过程的每一个细节。2. 数学基础SO(3)、SE(3)与它们的李代数要理解基于李代数的优化必须先打好数学基础。这部分内容可能有些抽象但我会尽量用几何直观和实际意义来解释这是后续所有推导和实操的基石。2.1 旋转矩阵SO(3)与李代数so(3)旋转矩阵R的集合构成了一个李群称为特殊正交群SO(3)。“群”意味着它满足封闭性、结合律、有单位元、有逆元等性质。“特殊正交”意味着R^T R I 且 det(R) 1。李代数so(3)定义为所有三维反对称矩阵的集合so(3) { φ^ ∈ R^{3×3} | φ^ -φ^T }其中符号^表示将一个三维向量φ [φ1, φ2, φ3]^T 映射为一个反对称矩阵φ^ [ 0, -φ3, φ2; φ3, 0, -φ1; -φ2, φ1, 0 ]这个反对称矩阵φ^就是李代数so(3)的一个元素。向量φ本身可以看作so(3)空间中的一个坐标。关键的联系在于指数映射对于任意φ∈R^3有 R exp(φ^)。这个指数映射将李代数so(3)中的元素映射到李群SO(3)上。反之对数映射log(R)可以将旋转矩阵映射回李代数注意对于旋转角为π的旋转对数映射不唯一。指数映射的物理意义向量φ的模长||φ||表示旋转的角度其方向表示旋转轴遵循右手法则。exp(φ^)计算出的正是绕轴φ/||φ||旋转||φ||角度的旋转矩阵。当φ是一个微小量时指数映射有一阶近似exp(φ^) ≈ I φ^。这个近似在优化中至关重要它允许我们用简单的加法来处理旋转的局部更新。2.2 变换矩阵SE(3)与李代数se(3)相机位姿通常由旋转和平移共同构成表示为变换矩阵TT [ R t; 0^T 1 ] 其中 R ∈ SO(3) t ∈ R^3所有这样的变换矩阵T构成了特殊欧氏群SE(3)。对应的李代数se(3)是一个六维空间。一个se(3)元素可以记作ξ [ρ, φ]^T其中ρ∈R^3φ∈R^3。它对应的矩阵形式为ξ^ [ φ^ ρ; 0^T 0 ]同样存在指数映射将se(3)映射到SE(3)T exp(ξ^)。这个映射的物理意义更丰富φ部分仍然代表旋转而ρ部分在旋转的基础上还包含了与平移相关的信息。当旋转为小量时也有近似exp(ξ^) ≈ I ξ^。为什么用se(3)而不用so(3)t在优化中我们有时会将位姿的旋转和平移分开参数化即用so(3)的φ和R^3的t。这被称为“旋转-平移参数化”。而使用se(3)的ξ进行参数化被称为“李代数参数化”或“指数坐标参数化”。两者在理论上是等价的局部同胚但在实际优化中se(3)参数化有时能提供更统一的处理方式和更好的数值性质尤其是在使用某些优化库时。本文后续将主要采用更直观的“旋转-平移参数化”但原理完全相通。2.3 李代数的求导扰动模型优化算法的核心是求导。我们需要计算重投影误差关于位姿参数的雅可比矩阵导数。直接对旋转矩阵R求导非常不便。李代数提供了优雅的解决方案扰动模型。思路是给当前估计的旋转R施加一个微小的左扰动或右扰动ΔR这个扰动由李代数φ对应的微小旋转矩阵exp(φ^) ≈ I φ^来表示。然后计算误差关于这个扰动李代数φ的导数。因为φ是三维无约束向量这个导数很容易计算。具体来说假设我们有一个三维点P_w在世界坐标系下当前相机位姿为T包含R, t。点投影到相机归一化平面为u π( R * P_w t ) 其中π是投影函数[X, Y, Z]^T - [X/Z, Y/Z]^T重投影误差e u_observed - u。现在给旋转R施加一个左扰动ΔR exp(φ^)。扰动后的投影为u π( (ΔR * R) * P_w t ) ≈ π( (I φ^) * R * P_w t ) π( R*P_w t φ^ * (R*P_w) )注意φ^ * (RP_w) - (RP_w)^ × φ根据反对称矩阵的性质a^ * b - b^ * a。令P_c R * P_w t 为点在相机坐标系下的坐标则u ≈ π( P_c - (P_c)^ × φ )这里(P_c)^是P_c的反对称矩阵。接下来利用归一化坐标u [u_x, u_y]^T [X_c/Z_c, Y_c/Z_c]^T我们可以计算u关于扰动φ的雅可比矩阵。通过链式法则最终得到∂e/∂φ - [ ∂π/∂P_c ] * (P_c)^其中∂π/∂P_c是投影函数关于相机坐标系下点的雅可比是一个2x3矩阵∂π/∂P_c [ 1/Z_c, 0, -X_c/Z_c^2; 0, 1/Z_c, -Y_c/Z_c^2 ]类似地我们可以推导误差关于平移扰动ρ这里我们直接对t加扰动的雅可比∂e/∂ρ - [ ∂π/∂P_c ] * I_{3x3}这样我们就得到了重投影误差关于位姿扰动旋转李代数φ和平移向量ρ的2x6雅可比矩阵。这个雅可比矩阵是迭代优化算法如高斯-牛顿法的核心输入。注意这里推导的是左扰动模型。也有右扰动模型其形式类似但物理意义不同扰动施加在坐标系上而非点上。在大多数视觉问题中左扰动模型更常用。务必在代码实现中保持一致性。3. 构建PnP非线性最小二乘问题有了数学工具我们现在可以正式定义要解决的优化问题。假设我们有n对匹配点3D点 {P_i | i1,...,n} 在世界坐标系下对应的2D像素观测 {z_i | i1,...,n}。3.1 误差函数与代价函数首先定义单个观测的重投影误差。对于第i个点使用当前位姿估计T[R|t]将世界点P_i变换到相机坐标系P_c^i R * P_i t。将相机坐标系下的点投影到归一化平面u_i [X_c^i / Z_c^i, Y_c^i / Z_c^i]^T。通常相机还有内参矩阵K焦距f_x, f_y和主点c_x, c_y需要将归一化坐标映射到像素坐标p_i K * u_i [f_x * u_x c_x, f_y * u_y c_y]^T。重投影误差就是观测像素坐标z_i与计算出的像素坐标p_i的差e_i(T) z_i - p_i(T)。这是一个二维误差向量。我们的目标是找到最优的位姿T*使得所有误差的平方和最小。这构成了一个非线性最小二乘问题T* argmin_T 0.5 * Σ_i || e_i(T) ||^2这里取0.5是为了后续求导后形式更简洁。||·||表示向量的L2范数。3.2 参数化与优化变量如前所述我们选择在无约束的空间中进行优化。因此优化变量x是一个6维向量x [φ^T, t^T]^T [φ1, φ2, φ3, t1, t2, t3]^T其中φ是旋转对应的李代数t是平移向量。在迭代过程中我们不断更新这个6维向量δx [δφ^T, δt^T]^T。迭代更新公式假设第k次迭代的位姿估计为T_k对应的李代数为φ_k满足R_k exp(φ_k^)平移为t_k。我们求解一个增量δx然后按以下方式更新φ_{k1} φ_k δφ t_{k1} t_k δt R_{k1} exp(φ_{k1}^) // 注意这里需要将更新后的李代数映射回旋转矩阵另一种等价的、更常用的更新方式是直接在李群上通过左乘扰动来更新T_{k1} exp(δξ^) * T_k其中δξ [δρ, δφ]^T 是se(3)上的增量。这两种方式在理论上对于微小增量是近似等价的但实现上略有不同。第一种“旋转-平移参数化”更直观我们后续将采用这种方式。3.3 高斯-牛顿法框架高斯-牛顿法是解决非线性最小二乘问题最常用的方法之一。其核心思想是在每次迭代的当前点x_k处将非线性误差函数e_i(x)进行一阶泰勒展开线性化然后求解一个线性最小二乘问题得到增量δx。将误差函数在x_k处展开e_i(x_k δx) ≈ e_i(x_k) J_i(x_k) * δx其中J_i是e_i关于x的雅可比矩阵在我们这里是2x6的矩阵前面已经推导。代入代价函数F(x_k δx) 0.5 * Σ_i || e_i(x_k) J_i δx ||^2 0.5 * Σ_i ( e_i^T e_i 2 e_i^T J_i δx δx^T J_i^T J_i δx )为了最小化F(x_k δx)关于δx我们令其导数为零∂F/∂(δx) Σ_i ( J_i^T e_i J_i^T J_i δx ) 0整理得到高斯-牛顿方程也称为正规方程( Σ_i J_i^T J_i ) * δx - Σ_i J_i^T e_i记 H Σ_i J_i^T J_i 为近似的海森矩阵Hessian g Σ_i J_i^T e_i 为梯度。则方程简化为H * δx -g这是一个线性方程组。求解这个6x6的线性系统得到增量δx然后按照上述更新公式更新位姿估计。重复这个过程直到增量δx的范数小于某个阈值或者代价函数的变化不再显著算法收敛。实操心得初始化的重要性。高斯-牛顿法是一个局部优化方法其收敛性和最终结果严重依赖于初始值。一个糟糕的初始值例如离真实解太远可能导致算法收敛到局部极小值甚至发散。因此在实践中我们总是先用一个快速的直接解法如EPnP计算出一个初始位姿再将其作为非线性优化的起点。这结合了直接解法速度快和优化方法精度高的优点。4. 手把手实现从理论到代码理论已经完备现在是时候将数学公式转化为实际运行的代码了。我们将使用C和Eigen库进行演示因为Eigen提供了优秀的矩阵运算和几何模块。这里会省略一些工程细节如内存管理、接口设计聚焦于核心算法流程。4.1 数据结构定义首先定义一些基本的数据结构。#include Eigen/Core #include Eigen/Geometry #include vector // 使用Eigen类型别名 using Vec2 Eigen::Vector2d; using Vec3 Eigen::Vector3d; using Mat23 Eigen::Matrixdouble, 2, 3; using Mat36 Eigen::Matrixdouble, 3, 6; using Mat66 Eigen::Matrixdouble, 6, 6; using Vec6 Eigen::Matrixdouble, 6, 1; // 一个3D-2D匹配对 struct Match { Vec3 P_w; // 世界坐标系下的3D点 Vec2 z; // 观测到的2D像素坐标 }; // 相机内参简化假设没有畸变 struct CameraIntrinsics { double fx, fy; // 焦距 double cx, cy; // 主点 };4.2 核心函数计算雅可比矩阵与误差这是整个优化器的核心。对于给定的一个位姿估计用李代数φ和平移t表示和一个匹配对计算重投影误差和关于优化变量φ, t的雅可比矩阵。/** * brief 计算单个点的重投影误差和雅可比矩阵 * param[in] P_w 世界点 * param[in] z_obs 观测像素坐标 * param[in] phi 当前旋转的李代数 (so(3)) * param[in] t 当前平移 * param[in] K 相机内参 * param[out] error 2x1 重投影误差 * param[out] jacobian 2x6 雅可比矩阵 (关于 [phi; t]) */ void ComputeReprojectionErrorAndJacobian( const Vec3 P_w, const Vec2 z_obs, const Vec3 phi, const Vec3 t, const CameraIntrinsics K, Vec2 error, Eigen::Matrixdouble, 2, 6 jacobian) { // 1. 将李代数phi转换为旋转矩阵R Eigen::AngleAxisd rotation_vector(phi.norm(), phi.normalized()); Eigen::Matrix3d R rotation_vector.toRotationMatrix(); // 注意当phi很小时可以用更高效的近似R ≈ I phi^但这里用标准转换保证精度。 // 2. 将世界点变换到相机坐标系 Vec3 P_c R * P_w t; double X P_c[0], Y P_c[1], Z P_c[2]; // 防止深度为负或零这是一个重要的鲁棒性检查 if (Z 1e-6) { // 处理异常情况例如设置一个很大的误差或跳过该点 error.setConstant(1e6); jacobian.setZero(); return; } // 3. 投影到归一化平面并计算像素坐标 double inv_Z 1.0 / Z; double inv_Z2 inv_Z * inv_Z; Vec2 u_normalized(X * inv_Z, Y * inv_Z); Vec2 z_pred(K.fx * u_normalized[0] K.cx, K.fy * u_normalized[1] K.cy); // 4. 计算重投影误差 error z_obs - z_pred; // 5. 计算投影函数关于相机坐标点P_c的雅可比 (2x3) // ∂p/∂P_c [∂p/∂u] * [∂u/∂P_c] // 其中 ∂u/∂P_c [1/Z, 0, -X/Z^2; 0, 1/Z, -Y/Z^2] Mat23 J_proj; J_proj K.fx * inv_Z, 0, -K.fx * X * inv_Z2, 0, K.fy * inv_Z, -K.fy * Y * inv_Z2; // 6. 计算P_c关于扰动φ和t的雅可比 (3x6) // 根据扰动模型P_c R * P_w t - (P_c)^ × δφ δt // 所以 ∂P_c/∂φ - (P_c)^ ∂P_c/∂t I Eigen::Matrix3d P_c_hat; P_c_hat 0, -Z, Y, Z, 0, -X, -Y, X, 0; // 这是(P_c)^的矩阵形式 Mat36 J_Pc; J_Pc.block3, 3(0, 0) -P_c_hat; // 关于φ的部分 J_Pc.block3, 3(0, 3) Eigen::Matrix3d::Identity(); // 关于t的部分 // 7. 链式法则得到最终的雅可比矩阵 (2x6) jacobian -J_proj * J_Pc; // 注意负号因为误差 e z_obs - z_pred而z_pred f(T) }关键细节与避坑指南深度检查第2步中的if (Z 1e-6)至关重要。如果点在相机后面Z为负或非常接近相机中心Z接近零投影计算会失效导致数值不稳定如除以零。在实际应用中这可能意味着初始位姿估计极差或者该匹配点是外点错误匹配。一个常见的处理策略是给该点赋予一个很大的固定误差如代码所示使其在本次迭代中对梯度贡献很大从而“拉回”位姿估计或者在后续引入鲁棒核函数来降低此类点的权重。李代数到旋转矩阵的转换我们使用了Eigen::AngleAxisd进行转换。当扰动δφ很小时也可以使用一阶近似R_new R_old * (I δφ^)来避免频繁的指数/对数映射这能提升计算速度但可能引入微小误差。在精度要求高的迭代后期建议使用精确的转换。雅可比矩阵的负号注意最后一步jacobian -J_proj * J_Pc。这是因为我们定义误差e z_obs - z_pred而z_pred是关于位姿T的函数。所以∂e/∂T - ∂z_pred/∂T。这个负号很容易被忽略导致求解的增量方向错误优化发散。4.3 高斯-牛顿迭代主循环现在我们可以实现完整的高斯-牛顿优化流程。/** * brief 使用高斯-牛顿法优化PnP位姿 * param[in] matches 3D-2D匹配对集合 * param[in] K 相机内参 * param[in, out] phi 初始旋转李代数优化后更新 * param[in, out] t 初始平移优化后更新 * param[in] max_iterations 最大迭代次数 * param[in] epsilon 收敛判据增量范数阈值 * return 最终的重投影误差总和 */ double GaussNewtonPnP(const std::vectorMatch matches, const CameraIntrinsics K, Vec3 phi, Vec3 t, int max_iterations 10, double epsilon 1e-6) { double total_cost 0.0; for (int iter 0; iter max_iterations; iter) { Mat66 H Mat66::Zero(); // 近似海森矩阵 H Σ J^T J Vec6 g Vec6::Zero(); // 梯度 g Σ J^T e double cost_this_iter 0.0; // 遍历所有匹配点累加H和g for (const auto match : matches) { Vec2 error; Eigen::Matrixdouble, 2, 6 J; ComputeReprojectionErrorAndJacobian(match.P_w, match.z, phi, t, K, error, J); // 累加海森矩阵和梯度 H J.transpose() * J; g J.transpose() * error; // 计算当前点的误差平方并累加到本轮总代价中 cost_this_iter error.squaredNorm(); } total_cost 0.5 * cost_this_iter; // 记住我们的代价函数有0.5因子 // 求解线性方程 H * δx -g Vec6 dx H.ldlt().solve(-g); // 使用LDLT分解求解H是对称半正定矩阵 // 检查求解是否成功 if (std::isnan(dx[0])) { std::cerr Iteration iter : H matrix is singular or ill-conditioned! std::endl; break; } // 更新位姿估计 Vec3 dphi dx.head3(); Vec3 dt dx.tail3(); phi dphi; t dt; // 收敛判断如果增量非常小则停止迭代 double delta_norm dx.norm(); if (delta_norm epsilon) { std::cout Converged after iter 1 iterations. std::endl; break; } std::cout Iteration iter : cost total_cost , |dx| delta_norm std::endl; } return total_cost; }4.4 初始化与调用示例最后我们需要一个合理的初始值来启动优化。通常我们会用一个快速的直接解法如EPnP来获取初始R和t然后将其转换为李代数φ。这里为了演示完整性我们假设已经有了一个初始旋转矩阵R_init和平移t_init。int main() { // 1. 准备模拟数据 CameraIntrinsics K{520.9, 521.0, 325.1, 249.7}; // 示例内参 std::vectorMatch matches; // ... 这里填充matches例如从文件读取或随机生成一些3D-2D对应点 // 2. 获取初始位姿这里用假设值实践中用EPnP等计算 Eigen::Matrix3d R_init Eigen::Matrix3d::Identity(); // 假设初始旋转为单位阵 Vec3 t_init(0.1, 0.1, 1.0); // 假设初始平移 // 3. 将初始旋转矩阵转换为李代数轴角 Eigen::AngleAxisd init_rotation(R_init); Vec3 phi_init init_rotation.angle() * init_rotation.axis(); // 4. 调用高斯-牛顿优化 Vec3 phi_opt phi_init; Vec3 t_opt t_init; double final_cost GaussNewtonPnP(matches, K, phi_opt, t_opt, 20, 1e-8); // 5. 将优化后的李代数转换回旋转矩阵 Eigen::AngleAxisd opt_rotation(phi_opt.norm(), phi_opt.normalized()); Eigen::Matrix3d R_opt opt_rotation.toRotationMatrix(); std::cout Optimization finished. Final cost: final_cost std::endl; std::cout Optimized Rotation:\n R_opt std::endl; std::cout Optimized Translation:\n t_opt.transpose() std::endl; return 0; }5. 进阶话题与工程实践中的挑战实现一个基础版本只是第一步。要让基于李代数的PnP优化在实际项目中稳定、高效、鲁棒地工作还需要考虑以下几个关键问题。5.1 鲁棒核函数应对外点干扰上面的实现有一个致命弱点它假设所有的匹配点都是正确的内点。然而在实际中特征匹配总会产生一定比例的错误匹配外点。这些外点会产生巨大的重投影误差由于最小二乘是平方项这些大误差会占据主导地位严重扭曲优化结果甚至导致算法完全失效。解决方案是使用鲁棒核函数Robust Kernel Function。其核心思想是对误差的平方项进行加权使得误差大的点可能是外点的权重降低。常用的核函数有Huber核、Cauchy核、Tukey核等。以Huber核为例其损失函数定义为L(e) { 0.5 * e^2, if |e| δ { δ * (|e| - 0.5 * δ), otherwise其中δ是一个阈值参数。在优化中这等价于给每个误差项乘以一个权重ww { 1, if |e| δ { δ / |e|, otherwise因此在构建高斯-牛顿方程时我们对每个点的雅可比矩阵J_i和误差e_i进行加权H w * J_i^T J_ig w * J_i^T e_i。实现要点需要在每次迭代中根据当前误差重新计算每个点的权重。这引入了迭代重加权最小二乘IRLS的思想。阈值δ的选择需要根据具体问题的噪声水平来调整通常取重投影误差中位数的若干倍如1.345倍标准差对应95%效率的Huber核。5.2 数值稳定性与求解器选择在我们的代码中使用H.ldlt().solve(-g)来求解线性方程组。LDLT分解适用于对称正定或半正定矩阵且比完整的LU或QR分解更高效。然而当H矩阵病态条件数很大时求解结果可能不稳定。病态性的来源观测不足匹配点数量太少或者所有点都近似共面/共线导致某些自由度如尺度、绕某个轴的旋转无法被有效约束。数值问题点的深度Z值差异巨大或者像素坐标与3D坐标的量纲差异大导致H矩阵中元素的数量级相差悬殊。解决方案预处理Preconditioning对优化变量进行缩放使其各个分量的量级大致相同。例如将平移向量t的单位从米转换为毫米或者对李代数φ进行缩放。这相当于求解(P^T H P) * (P^{-1}δx) -P^T g其中P是一个预处理矩阵通常是对角阵。使用更鲁棒的求解器对于病态问题可以使用QR分解更稳定但稍慢或奇异值分解SVD最稳定但最慢。在Eigen中可以使用H.colPivHouseholderQr().solve(-g)或H.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(-g)。列文伯格-马夸尔特Levenberg-Marquardt, LM算法LM算法是高斯-牛顿法的改进它在H矩阵上加上一个阻尼项λI即求解(H λI) δx -g。当λ很大时算法接近最速下降法稳定但慢当λ很小时算法接近高斯-牛顿法快但不稳定。LM算法能自动调整λ在稳定性和收敛速度之间取得平衡。强烈建议在实际项目中使用LM算法而非纯高斯-牛顿法。许多优化库如g2o, Ceres Solver默认使用或提供了LM算法。5.3 与优化库的集成以Ceres Solver为例手动实现优化循环有利于理解原理但在生产环境中使用成熟的优化库是更明智的选择。它们提供了经过充分测试的数值优化实现、自动求导、丰富的损失函数以及多线程支持等。下面展示如何使用Ceres Solver来实现相同的PnP优化。首先定义一个代价函数仿函数Functorstruct PnPCostFunction { PnPCostFunction(const Vec3 P_w, const Vec2 z_obs, const CameraIntrinsics K) : P_w_(P_w), z_obs_(z_obs), K_(K) {} template typename T bool operator()(const T* const pose_params, // 6维数组 [phi1, phi2, phi3, t1, t2, t3] T* residuals) const { // 1. 提取旋转和平移参数 const T* phi pose_params; const T* t pose_params 3; // 2. 将李代数(phi)转换为旋转矩阵R (使用角轴) Eigen::MatrixT, 3, 1 phi_vec(phi[0], phi[1], phi[2]); T angle phi_vec.norm(); Eigen::MatrixT, 3, 3 R; if (angle T(1e-8)) { // 小角度近似: R ≈ I phi^ R Eigen::MatrixT, 3, 3::Identity(); R(0,1) -phi[2]; R(0,2) phi[1]; R(1,0) phi[2]; R(1,2) -phi[0]; R(2,0) -phi[1]; R(2,1) phi[0]; } else { Eigen::MatrixT, 3, 1 axis phi_vec / angle; // Rodrigues rotation formula T c ceres::cos(angle); T s ceres::sin(angle); T one_minus_c T(1) - c; T x axis[0], y axis[1], z axis[2]; R x*x*one_minus_c c, x*y*one_minus_c - z*s, x*z*one_minus_c y*s, x*y*one_minus_c z*s, y*y*one_minus_c c, y*z*one_minus_c - x*s, x*z*one_minus_c - y*s, y*z*one_minus_c x*s, z*z*one_minus_c c; } // 3. 变换到相机坐标系并投影 Eigen::MatrixT, 3, 1 P_c R * P_w_.castT() Eigen::MatrixT, 3, 1(t[0], t[1], t[2]); T X P_c[0], Y P_c[1], Z P_c[2]; // 检查深度 if (Z T(1e-6)) { return false; // 深度无效返回false告知Ceres } T inv_Z T(1) / Z; T u_pred T(K_.fx) * X * inv_Z T(K_.cx); T v_pred T(K_.fy) * Y * inv_Z T(K_.cy); // 4. 计算残差 residuals[0] T(z_obs_[0]) - u_pred; residuals[1] T(z_obs_[1]) - v_pred; return true; } private: const Vec3 P_w_; const Vec2 z_obs_; const CameraIntrinsics K_; };然后构建问题并求解void OptimizePoseWithCeres(const std::vectorMatch matches, const CameraIntrinsics K, Vec6 initial_pose) { // initial_pose [phi; t] ceres::Problem problem; for (const auto match : matches) { ceres::CostFunction* cost_function new ceres::AutoDiffCostFunctionPnPCostFunction, 2, 6( new PnPCostFunction(match.P_w, match.z, K)); problem.AddResidualBlock(cost_function, nullptr, // 不使用核函数或使用 new ceres::HuberLoss(1.0) initial_pose.data()); } ceres::Solver::Options options; options.linear_solver_type ceres::DENSE_QR; // 使用QR分解更稳定 options.minimizer_progress_to_stdout true; options.max_num_iterations 50; ceres::Solver::Summary summary; ceres::Solve(options, problem, summary); std::cout summary.BriefReport() std::endl; }使用Ceres Solver的优势非常明显我们只需要定义残差如何计算而无需手动推导和编写雅可比矩阵通过自动求导也无需实现复杂的优化循环。库会帮我们处理LM算法的阻尼调整、线性求解器的选择、收敛判断等所有细节大大提高了开发效率和代码的稳健性。5.4 性能优化与尺度问题性能对于实时应用如SLAM优化速度至关重要。除了使用高效的线性代数库如Eigen和优化库外还可以减少点数在保证精度的前提下使用随机采样一致性RANSAC先剔除外点或者对点进行网格筛选避免使用过多冗余点。稀疏性PnP问题的海森矩阵H是稠密的6x6小矩阵本身没有稀疏结构。但在更大的BABundle Adjustment问题中海森矩阵是稀疏的。如果PnP被嵌入到BA中应使用专门为稀疏矩阵设计的求解器如SuiteSparse, CHOLMOD。尺度问题PnP问题从2D-3D对应中恢复的平移向量t具有尺度不确定性。也就是说如果将所有3D点坐标和t同时缩放一个因子重投影误差不变。因此优化得到的t的尺度取决于你提供的3D点坐标的尺度。在单目视觉中这是一个固有的尺度不确定性。在融合了IMU或深度传感器的系统中需要通过其他手段确定尺度。6. 总结与扩展思考基于李代数的PnP优化将复杂的带约束旋转矩阵优化问题优雅地转化到了无约束的李代数空间中进行使得标准的非线性最小二乘优化工具得以直接应用。我们从数学基础SO(3)/SE(3)与李代数、问题建模、雅可比推导到手把手代码实现完整地走通了整个流程。我个人在实际操作中的体会是理解李代数和扰动模型是打通视觉SLAM和三维重建中优化问题的关键一环。它不仅仅是PnP的专用工具更是后端BA光束法平差、位姿图优化等核心模块的基础。在实现时最容易出错的地方往往是雅可比矩阵的符号和李代数更新的方式左乘扰动还是右乘扰动是在李代数上加还是用指数映射更新李群。务必在代码中保持清晰和一致并编写充分的单元测试进行验证例如用数值微分在参数上加一个微小扰动计算误差变化来验证自己推导的解析雅可比是否正确。最后再分享一个小技巧当你的优化结果不理想时不要急于调整复杂的算法参数。首先检查你的初始值是否合理用EPnP等直接解法验证其次可视化重投影误差将优化前后的投影点与观测点画在图像上能直观地看到优化是否在向正确的方向移动最后开启优化库如Ceres的详细输出观察每次迭代的代价下降是否平滑以及LM算法的阻尼因子λ的变化情况这能帮助你判断问题是数据问题、雅可比错误还是数值病态。