基于李群与李代数的PnP优化方法解析

1. 项目概述

在计算机视觉和机器人领域，Perspective-n-Point（PnP）问题是一个经典的基础性问题。它旨在通过一组3D空间点及其在2D图像上的投影，求解相机的位姿（位置和朝向）。这个问题在SLAM、AR/VR、摄影测量等领域有着广泛的应用。

传统的PnP求解方法通常采用旋转向量（axis-angle）参数化来表示相机旋转，并在欧氏空间中直接对旋转向量进行优化。然而，这种方法存在一些固有缺陷：

1. 旋转向量的加法运算不符合旋转群的实际几何结构
2. 在零附近数值不稳定
3. 缺乏统一的微小旋转定义方法

本文将介绍如何利用李群（Lie Group）和李代数（Lie Algebra）理论，构建一个严格保持旋转合法性的SE(3)优化框架，从根本上解决这些问题。

2. 理论基础：李群与李代数

2.1 特殊正交群SO(3)

所有合法的三维旋转矩阵构成一个集合：

SO(3) =

这个集合不仅是一个群（满足封闭性、结合律、单位元、逆元），更是一个光滑流形。我们称SO(3)为特殊正交群，它是描述三维旋转的李群。

关键点在于：SO(3)不是向量空间。不能简单地将两个旋转矩阵相加，结果很可能不再属于SO(3)。

2.2 李代数so(3)

为了在SO(3)上进行优化，我们需要它的李代数so(3)。so(3)是SO(3)在单位元处的切空间，也是一个向量空间。so(3)中的元素可以用一个三维向量φ∈ℝ³来表示：

φ ↔ [φ]× = [ 0 -φ₃ φ₂
φ₃ 0 -φ₁
-φ₂ φ₁ 0 ]

这个向量φ的方向代表旋转轴，模长‖φ‖代表旋转角度（弧度）。

2.3 指数映射与对数映射

通过指数映射，我们可以将李代数中的向量"提升"回李群中的旋转矩阵：

R = exp([φ]×) = Exp(φ)

这个公式就是著名的Rodrigues旋转公式。它建立了从平坦的ℝ³（李代数坐标）到弯曲的SO(3)（李群）的光滑映射。

反之，通过对数映射，我们可以将任意旋转矩阵R∈SO(3)"拉回"到其对应的李代数向量：

φ = Log(R)

2.4 特殊欧氏群SE(3)