ORB-SLAM3源码解读：我是如何一步步推导KannalaBrandt8相机模型投影与反投影的雅可比矩阵的

ORB-SLAM3深度解析：KannalaBrandt8相机模型雅可比矩阵的数学推导与代码实现

在视觉SLAM系统中，相机模型的雅可比矩阵计算是后端优化的核心环节之一。对于采用Kannala-Brandt鱼眼模型的ORB-SLAM3系统，理解其投影与反投影过程中雅可比矩阵的推导过程，不仅能帮助开发者深入调试系统，更能为自定义相机模型的实现提供理论基础。本文将从KB模型的基本公式出发，通过链式法则逐步推导像素坐标对3D点坐标的偏导数，并与ORB-SLAM3源码中的projectJac函数实现进行逐行对照。

1. Kannala-Brandt8相机模型基础

Kannala-Brandt模型是一种适用于鱼眼镜头的通用投影模型，其核心思想是将入射光线与光轴夹角θ的多项式展开作为投影函数。给定一个3D点P=(X,Y,Z)，其投影过程可分为以下步骤：

1. 计算入射角θ和方位角ψ：

   θ = atan2(√(X²+Y²), Z) ψ = atan2(Y, X)

2. 使用多项式模型计算投影半径：

   r(θ) = θ + k₁θ³ + k₂θ⁵ + k₃θ⁷ + k₄θ⁹

3. 转换为像素坐标：

   u = f_x * r(θ) * cos(ψ) + c_x v = f_y * r(θ) * sin(ψ) + c_y

在ORB-SLAM3的实现中，这些步骤体现在KannalaBrandt8::project函数中。理解这个基础投影过程是推导雅可比矩阵的前提。

2. 雅可比矩阵的数学意义与推导框架

雅可比矩阵描述了像素坐标(u,v)对3D点坐标(X,Y,Z)的局部线性变化关系，其一般形式为：

J = [ ∂u/∂X ∂u/∂Y ∂u/∂Z ] [ ∂v/∂X ∂v/∂Y ∂v/∂Z ]

推导这个矩阵需要运用链式法则，因为u和v都是通过中间变量θ和ψ间接依赖于(X,Y,Z)。具体而言，我们需要计算：

1. ∂u/∂X = ∂u/∂r * ∂r/∂θ * ∂θ/∂X + ∂u/∂ψ * ∂ψ/∂X
2. ∂u/∂Y = ∂u/∂r * ∂r/∂θ * ∂θ/∂Y + ∂u/∂ψ * ∂ψ/∂Y
3. ∂u/∂Z = ∂u/∂r * ∂r/∂θ * ∂θ/∂Z
4. (v的偏导数同理)

下面我们将分步骤详细展开这些偏导数的计算过程。

3. 中间变量的偏导数计算

3.1 基本变量定义

首先定义一些中间变量以简化表达式：

r² = X² + Y² r = √(X² + Y²) ρ² = X² + Y² + Z² = r² + Z²

3.2 θ的偏导数

根据θ = atan2(r, Z)，我们可以计算θ对各变量的偏导：

∂θ/∂X = (Z/rρ²) * X ∂θ/∂Y = (Z/rρ²) * Y ∂θ/∂Z = -r/ρ²

3.3 ψ的偏导数

ψ = atan2(Y, X)的偏导数为：

∂ψ/∂X = -Y/r² ∂ψ/∂Y = X/r² ∂ψ/∂Z = 0

3.4 r(θ)的导数

r(θ) = θ + k₁θ³ + k₂θ⁵ + k₃θ⁷ + k₄θ⁹的导数为：

dr/dθ = 1 + 3k₁θ² + 5k₂θ⁴ + 7k₃θ⁶ + 9k₄θ⁸

这个导数对应源码中的fd变量。

4. 完整雅可比矩阵的推导

4.1 ∂u/∂X的推导

根据u = f_x * r(θ) * cos(ψ) + c_x，我们应用乘积法则：

∂u/∂X = f_x * [ (dr/dθ * ∂θ/∂X) * cos(ψ) + r(θ) * (-sin(ψ) * ∂ψ/∂X) ]

将前面计算的中间结果代入：

∂u/∂X = f_x * [ fd * (Z/rρ²)X * (X/r) + f * (-Y/r) * (-Y/r²) ] = f_x * [ fd * ZX² / (r²ρ²) + f * Y² / r³ ]

这与源码中的实现完全一致：

Jac.at<float>(0, 0) = mvParameters[0] * (fd * p3D.z * x2 / (r2 * (r2 + z2)) + f * y2 / r3);

4.2 ∂v/∂Y的推导

类似地，v = f_y * r(θ) * sin(ψ) + c_y的偏导数为：

∂v/∂Y = f_y * [ (dr/dθ * ∂θ/∂Y) * sin(ψ) + r(θ) * (cos(ψ) * ∂ψ/∂Y) ] = f_y * [ fd * (Z/rρ²)Y * (Y/r) + f * (X/r) * (X/r²) ] = f_y * [ fd * ZY² / (r²ρ²) + f * X² / r³ ]

对应源码：

Jac.at<float>(1, 1) = mvParameters[1] * (fd * p3D.z * y2 / (r2 * (r2 + z2)) + f * x2 / r3);

4.3 交叉偏导数∂u/∂Y和∂v/∂X

这两个偏导数展现了X和Y坐标之间的耦合关系：

∂u/∂Y = f_x * [ fd * (Z/rρ²)Y * (X/r) + f * (-Y/r) * (X/r²) ] = f_x * [ fd * ZXY / (r²ρ²) - f * XY / r³ ] ∂v/∂X = f_y * [ fd * (Z/rρ²)X * (Y/r) + f * (X/r) * (-Y/r²) ] = f_y * [ fd * ZXY / (r²ρ²) - f * XY / r³ ]

源码实现：

Jac.at<float>(0, 1) = mvParameters[0] * (fd * p3D.z * p3D.y * p3D.x / (r2 * (r2 + z2)) - f * p3D.y * p3D.x / r3); Jac.at<float>(1, 0) = mvParameters[1] * (fd * p3D.z * p3D.y * p3D.x / (r2 * (r2 + z2)) - f * p3D.y * p3D.x / r3);

4.4 Z方向的偏导数

由于ψ不依赖于Z，Z方向的偏导数相对简单：

∂u/∂Z = f_x * dr/dθ * ∂θ/∂Z * cos(ψ) = f_x * fd * (-r/ρ²) * (X/r) = -f_x * fd * X / ρ² ∂v/∂Z = f_y * dr/dθ * ∂θ/∂Z * sin(ψ) = f_y * fd * (-r/ρ²) * (Y/r) = -f_y * fd * Y / ρ²

对应源码：

Jac.at<float>(0, 2) = -mvParameters[0] * fd * p3D.x / (r2 + z2); Jac.at<float>(1, 2) = -mvParameters[1] * fd * p3D.y / (r2 + z2);

5. 实现细节与数值稳定性

在实际代码实现中，ORB-SLAM3采用了一些优化技巧来保证数值稳定性：

1. 中间变量预计算：提前计算θ的各次幂，避免重复计算

   float theta2 = theta * theta; float theta3 = theta2 * theta; float theta5 = theta3 * theta2; // ...

2. 分母合并：识别并合并公共分母项如r²和ρ²

   float r2 = x2 + y2; float r = sqrt(r2); float r3 = r2 * r;

3. 对称性利用：X和Y的偏导数具有对称形式，保持代码一致性

4. 矩阵存储顺序：按照OpenCV的Mat类存储约定，以行优先方式填充雅可比矩阵

理解这些实现细节对于在实际项目中修改或扩展相机模型至关重要。当需要支持新的鱼眼模型时，可以参照这个模式，先建立数学模型，然后系统地推导雅可比矩阵，最后转化为高效的代码实现。