说实话，聊到李代数在PnP里的玩意儿，我这心里还真是有点小激动。

Zui小二乘嘛，就是把一堆误差平方和给压到Zui低。

在PnP里我们的误差就是三维点投影到图像上跟真实像素坐标的差。

所以目标函数长得像：

$J={\sum }_{}^{}{i}_1{\left\{x\right\}}^{}$_i2

哈哈，这玩意儿听起来hen严肃，其实背后全是线性代数的套路。

记得上一篇我说过旋转的“加法”并不像普通向量那样工作。

我们常常写 ，结果却不一定等于两次旋转的合成。

尤其是当Δr大一点，矩阵马上就跑偏了——正交性不保了。

这时候，你会发现自己像在走钢丝，随时可Neng掉进坑里。

别慌，咱们有李群和它的切空间——李代数。

简单说so是一个三维向量空间，用来描述“微小旋转”。

而SO是所有合法旋转矩阵组成的集合。

关键是有一个叫指数映射的桥梁：

${\phi }^{-1}$=exp

把so里的扰动φ送进去，就Neng得到SO里的新旋转矩阵。

于是geng新公式变成：

害，这下子我们不再盲目相加，而是用矩阵左乘保证合法性。

先说咱们的状态向量：

T = ∈ SE，R∈SO，t∈ℝ³。

PnP 的投影函数是：

x̂ = K··X₃d，把三维点X₃d映射到相机坐标再投影到像素平面。

K 是内参矩阵，这块大家dou懂啦。

误差 eᵢ = xᵢ - x̂ᵢ，目标就是Zui小化 Σ‖eᵢ‖²。

先算出每个点对姿态的导数——这一步Zui关键，也是Zui容易出错的地方。

// 下面直接写结果，不跟你细碎推导了哈哈。

Aha！这就是所谓的“左扰动雅可比”。它天然落在SE切空间上，不会跑偏。

L-M 本质上是在高斯牛顿基础上加了阻尼项，用来防止步长太大闯祸。

L-M 的增量方程变成：

 Δξ = -Jᵀe

Solve Δξ，用指数映射把它塞回姿态：

T_new = exp ⊗ T_old

*不对不对*，其实右乘也是Ke以的，只要保持一致就行。咱们这里统一左乘，因为左扰动在文献里geng常见，你懂的。

零角度附近梯度会失真——要ZuoTaylor展开或者直接用Rodrigues近似；

λ调太大会让优化停滞——经验上先设个1e‑3，然后根据残差自适应；

特征点分布太集中会导致雅可比秩亏——多加点远处特征就好啦；

别忘了把相机内参固定，否则会出现耦合噪声。

// pseudo code
T = Identity
while not converged:
    compute residuals e_i and Jacobian J_i using left perturbation
    solve  * dxi = -J.T@e
    T = Exp @ T   # left multiplication
    update lambda based on cost reduction
end
print

自然保留正交性，避免手动归一化；

微分在切空间上定义明确，雅可比geng稳健；

统一处理旋转和平移，一步到位；

兼容LM等主流优化器，无缝衔接。

CIAO！Ru果你现在还在用那套“r←r+Δr”的老办法，那真的该考虑升级啦。咱们今天聊得够爽吧？懂得dou懂，不懂的赶紧去翻翻前几篇关于SLAM里李群李代数的文章，把概念捧热了再回来玩优化。下回见~ 哈哈！