【SLAM】不同相机模型及其常见的链式求导推导

1. 鱼眼相机模型链式求导

该接口compute_distort_jacobian实现了计算鱼眼相机模型中畸变坐标相对于归一化坐标和相机内参的雅可比矩阵的功能。雅可比矩阵在优化和校正相机模型中非常重要，因为它们描述了输出（畸变坐标）如何随输入（归一化坐标和相机内参）的变化而变化。
当然，我们可以从数学上推导这些雅可比矩阵。首先，我们需要理解鱼眼相机的畸变模型以及它是如何影响像素坐标的。

1. 鱼眼相机畸变模型

鱼眼相机的畸变通常可以用以下公式表示：

$${\theta }_d=\theta +k_1{\theta }^3+k_2{\theta }^5+k_3{\theta }^7+k_4{\theta }^9$$

其中，$\theta$ 是入射光线与相机光轴的夹角（无畸变），${\theta }_d$ 是畸变后的夹角，$k_1,k_2,k_3,k_4$ 是畸变系数。

归一化像素坐标 $(x_n,y_n)$ 和角度 $\theta$ 的关系为：

$$\theta =\arctan \left(\sqrt{x_n^2+y_n^2}\right)$$

畸变后的像素坐标 $(x_d,y_d)$ 可以通过以下公式计算：

$$x_d=x_n\cdot \frac{{\theta }_d}{\theta }$$
$$y_d=y_n\cdot \frac{{\theta }_d}{\theta }$$

2. 雅可比矩阵的推导

畸变坐标相对于归一化坐标的雅可比矩阵 $H_{dz/dzn}$

我们需要计算 $\frac{\partial (x_d,y_d)}{\partial (x_n,y_n)}$。

首先，计算 $\frac{\partial \theta }{\partial x_n}$ 和 $\frac{\partial \theta }{\partial y_n}$：

$$\frac{\partial \theta }{\partial x_n}=\frac{x_n}{\sqrt{x_n^2+y_n^2}\left(1+\frac{x_n^2+y_n^2}{x_n^2+y_n^2}\right)}=\frac{x_n}{r^2+x_n^2}$$
$$\frac{\partial \theta }{\partial y_n}=\frac{y_n}{\sqrt{x_n^2+y_n^2}\left(1+\frac{x_n^2+y_n^2}{x_n^2+y_n^2}\right)}=\frac{y_n}{r^2+x_n^2}$$

其中 $r=\sqrt{x_n^2+y_n^2}$。

然后，计算 $\frac{\partial {\theta }_d}{\partial \theta }$：

$$\frac{\partial {\theta }_d}{\partial \theta }=1+3k_1{\theta }^2+5k_2{\theta }^4+7k_3{\theta }^6+9k_4{\theta }^8$$

接下来，计算 $\frac{\partial x_d}{\partial x_n}$ 和 $\frac{\partial y_d}{\partial y_n}$，使用链式法则：

$$\frac{\partial x_d}{\partial x_n}=\frac{{\theta }_d}{\theta }+x_n\left(\frac{{\theta }_d^{\prime }\theta -{\theta }_d{\theta }^{\prime }}{{\theta }^2}\right)$$
$$\frac{\partial y_d}{\partial y_n}=\frac{{\theta }_d}{\theta }+y_n\left(\frac{{\theta }_d^{\prime }\theta -{\theta }_d{\theta }^{\prime }}{{\theta }^2}\right)$$

其中 ${\theta }^{\prime }=\frac{\partial \theta }{\partial x_n}$ 或 $\frac{\partial \theta }{\partial y_n}$，${\theta }_d^{\prime }=\frac{\partial {\theta }_d}{\partial \theta }$。

类似地，可以计算 $\frac{\partial x_d}{\partial y_n}$ 和 $\frac{\partial y_d}{\partial x_n}$，它们通常不为零，因为畸变会引入坐标之间的耦合。

最终，雅可比矩阵 $H_{dz/dzn}$ 将是一个 $2\times 2$ 矩阵，包含上述偏导数。

畸变坐标相对于相机内参的雅可比矩阵 $H_{dz/dzeta}$

这个矩阵描述的是畸变像素坐标如何随相机内参（如焦距和畸变系数）变化。

对于焦距 $f_x,f_y$，畸变像素坐标与它们的关系较为直接：

$$x_d=f_x\cdot x_n\cdot \frac{{\theta }_d}{\theta }$$
$$y_d=f_y\cdot y_n\cdot \frac{{\theta }_d}{\theta }$$

因此，$\frac{\partial x_d}{\partial f_x}=x_n\cdot \frac{{\theta }_d}{\theta }$，$\frac{\partial y_d}{\partial f_y}=y_n\cdot \frac{{\theta }_d}{\theta }$。

对于畸变系数 $k_i$，需要计算 $\frac{\partial {\theta }_d}{\partial k_i}$，然后使用链式法则计算 $\frac{\partial x_d}{\partial k_i}$ 和 $\frac{\partial y_d}{\partial k_i}$。

最终，雅可比矩阵 $H_{dz/dzeta}$ 将是一个 $2\times (2+4)$ 矩阵（假设有2个焦距和4个畸变系数），包含上述所有偏导数。

3. 注意

上述推导是简化的，并且假设了某些变量（如焦距）是常数。在实际应用中，可能需要考虑更多因素，并且计算可能会更加复杂。此外，上述公式中的部分导数可能需要根据具体情况进行调整，以确保它们的准确性。

4. 输入输出含义

• 输入:

  • const Eigen::Vector2f &uv_norm: 一个包含归一化像素坐标的二维向量。这些坐标通常是图像平面上的点，经过焦距归一化处理。
  • Eigen::MatrixXf &H_dz_dzn: 一个引用，用于存储畸变坐标相对于归一化坐标的雅可比矩阵。
  • Eigen::MatrixXf &H_dz_dzeta: 一个引用，用于存储畸变坐标相对于相机内参（包括焦距和畸变系数）的雅可比矩阵。

• 输出:

  • 通过引用参数H_dz_dzn和H_dz_dzeta，函数计算并填充了相应的雅可比矩阵。

5. 算法流程

1. 获取相机参数: 从全局或类作用域的camera_values矩阵中提取相机内参，包括焦距和畸变系数。

2. 计算畸变坐标:

   • 计算归一化坐标到图像中心的距离（半径r）。
   • 计算无畸变的入射光线与相机光轴的夹角（theta）。
   • 应用畸变模型计算畸变后的夹角（theta_d）。

3. 处理特殊情况: 当半径r非常小时（接近零），使用近似值以避免数值不稳定。

4. 计算雅可比矩阵:

   • 相对于归一化坐标的雅可比矩阵（H_dz_dzn）:
     • 计算归一化坐标到“标准”像素的雅可比矩阵（duv_dxy）。
     • 计算“标准”像素到归一化像素、归一化像素到半径、半径到归一化像素、以及“标准”像素到畸变角度的雅可比矩阵。
     • 使用链式法则组合这些雅可比矩阵，得到最终的H_dz_dzn。
   • 相对于相机内参的雅可比矩阵（H_dz_dzeta）:
     • 直接计算畸变坐标对焦距和畸变系数的偏导数，并填充到H_dz_dzeta中。

5. 返回结果: 函数通过修改引用参数H_dz_dzn和H_dz_dzeta来返回计算结果，而不是返回新的矩阵对象。

6. 总结

该接口是计算机视觉和摄影测量中处理鱼眼相机畸变的关键部分。通过计算雅可比矩阵，可以更有效地进行相机校正、三维重建和图像配准等任务。这些矩阵提供了关于如何调整输入参数以最小化输出误差的重要信息。

谁对谁求导
在compute_distort_jacobian函数中，我们主要计算以下两类雅可比矩阵：

畸变坐标相对于归一化坐标的雅可比矩阵（H_dz_dzn）：

这涉及到对畸变后的像素坐标（x_d, y_d）关于归一化像素坐标（x_n, y_n）求导。
具体来说，我们需要计算 $\frac{\partial x_d}{\partial x_n}$、$\frac{\partial x_d}{\partial y_n}$、$\frac{\partial y_d}{\partial x_n}$ 和 $\frac{\partial y_d}{\partial y_n}$。
这些导数描述了当归一化坐标发生微小变化时，畸变坐标将如何变化。
畸变坐标相对于相机内参的雅可比矩阵（H_dz_dzeta）：

这涉及到对畸变后的像素坐标（x_d, y_d）关于相机内参（如焦距 f_x, f_y 和畸变系数 k_1, k_2, k_3, k_4）求导。
具体来说，我们需要计算 $\frac{\partial x_d}{\partial f_x}$、$\frac{\partial x_d}{\partial f_y}$、$\frac{\partial x_d}{\partial k_i}$、$\frac{\partial y_d}{\partial f_x}$、$\frac{\partial y_d}{\partial f_y}$ 和 $\frac{\partial y_d}{\partial k_i}$（其中 $i=1,2,3,4$）。
这些导数描述了当相机内参发生微小变化时，畸变坐标将如何变化。
链式求导的应用
在计算这些导数时，我们通常会使用链式求导法则。例如，为了计算 $\frac{\partial x_d}{\partial x_n}$，我们可能需要先计算 $\frac{\partial {\theta }_d}{\partial \theta }$（畸变角度对无畸变角度的导数）和 $\frac{\partial \theta }{\partial x_n}$（无畸变角度对归一化坐标的导数），然后通过链式法则将它们组合起来。

7. 实现

  void compute_distort_jacobian(const Eigen::Vector2f &uv_norm, Eigen::MatrixXf &H_dz_dzn, Eigen::MatrixXf &H_dz_dzeta) {// Get our camera parametersEigen::MatrixXf cam_d = camera_values;// Calculate distorted coordinates for fisheyefloat r = std::sqrt(uv_norm(0) * uv_norm(0) + uv_norm(1) * uv_norm(1));float theta = std::atan(r);float theta_d = theta + cam_d(4) * std::pow(theta, 3) + cam_d(5) * std::pow(theta, 5) + cam_d(6) * std::pow(theta, 7) +cam_d(7) * std::pow(theta, 9);// Handle when r is small (meaning our xy is near the camera center)float inv_r = (r > 1e-8) ? 1.0 / r : 1.0;float cdist = (r > 1e-8) ? theta_d * inv_r : 1.0;// Jacobian of distorted pixel to "normalized" pixelEigen::Matrix<float, 2, 2> duv_dxy = Eigen::Matrix<float, 2, 2>::Zero();duv_dxy << cam_d(0), 0, 0, cam_d(1);// Jacobian of "normalized" pixel to normalized pixelEigen::Matrix<float, 2, 2> dxy_dxyn = Eigen::Matrix<float, 2, 2>::Zero();dxy_dxyn << theta_d * inv_r, 0, 0, theta_d * inv_r;// Jacobian of "normalized" pixel to rEigen::Matrix<float, 2, 1> dxy_dr = Eigen::Matrix<float, 2, 1>::Zero();dxy_dr << -uv_norm(0) * theta_d * inv_r * inv_r, -uv_norm(1) * theta_d * inv_r * inv_r;// Jacobian of r pixel to normalized xyEigen::Matrix<float, 1, 2> dr_dxyn = Eigen::Matrix<float, 1, 2>::Zero();dr_dxyn << uv_norm(0) * inv_r, uv_norm(1) * inv_r;// Jacobian of "normalized" pixel to theta_dEigen::Matrix<float, 2, 1> dxy_dthd = Eigen::Matrix<float, 2, 1>::Zero();dxy_dthd << uv_norm(0) * inv_r, uv_norm(1) * inv_r;// Jacobian of theta_d to thetafloat dthd_dth = 1 + 3 * cam_d(4) * std::pow(theta, 2) + 5 * cam_d(5) * std::pow(theta, 4) + 7 * cam_d(6) * std::pow(theta, 6) +9 * cam_d(7) * std::pow(theta, 8);// Jacobian of theta to rfloat dth_dr = 1 / (r * r + 1);// Total Jacobian wrt normalized pixel coordinatesH_dz_dzn = Eigen::MatrixXf::Zero(2, 2);H_dz_dzn = duv_dxy * (dxy_dxyn + (dxy_dr + dxy_dthd * dthd_dth * dth_dr) * dr_dxyn);// Calculate distorted coordinates for fisheyefloat x1 = uv_norm(0) * cdist;float y1 = uv_norm(1) * cdist;// Compute the Jacobian in respect to the intrinsicsH_dz_dzeta = Eigen::MatrixXd::Zero(2, 8);H_dz_dzeta(0, 0) = x1;H_dz_dzeta(0, 2) = 1;H_dz_dzeta(0, 4) = cam_d(0) * uv_norm(0) * inv_r * std::pow(theta, 3);H_dz_dzeta(0, 5) = cam_d(0) * uv_norm(0) * inv_r * std::pow(theta, 5);H_dz_dzeta(0, 6) = cam_d(0) * uv_norm(0) * inv_r * std::pow(theta, 7);H_dz_dzeta(0, 7) = cam_d(0) * uv_norm(0) * inv_r * std::pow(theta, 9);H_dz_dzeta(1, 1) = y1;H_dz_dzeta(1, 3) = 1;H_dz_dzeta(1, 4) = cam_d(1) * uv_norm(1) * inv_r * std::pow(theta, 3);H_dz_dzeta(1, 5) = cam_d(1) * uv_norm(1) * inv_r * std::pow(theta, 5);H_dz_dzeta(1, 6) = cam_d(1) * uv_norm(1) * inv_r * std::pow(theta, 7);H_dz_dzeta(1, 7) = cam_d(1) * uv_norm(1) * inv_r * std::pow(theta, 9);}

2. 针孔相机模型链式求导

在针孔相机模型中，投影链式求导的坐标系变化与每一步的求导关系如下。关键点在于：每一步求导都是在描述“当前坐标系下的坐标”相对于“上一级坐标系下的坐标”的变化率，即链式法则中的“局部导数”。

---

1. 世界坐标系 → 相机坐标系（外参数求导）

• 坐标系变化：世界坐标系 ${\mathbf{P}}_w=[X_w,Y_w,Z_w{]}^T$ 通过刚体变换（旋转 $R$ 和平移 $t$）转换为相机坐标系 ${\mathbf{P}}_c=[X_c,Y_c,Z_c{]}^T$。
  $${\mathbf{P}}_c=R{\mathbf{P}}_w+t$$
• 求导对象：$\frac{\partial {\mathbf{P}}_c}{\partial {\mathbf{P}}_w}=R$
  为什么：这是世界坐标系到相机坐标系的坐标变换，求导结果是旋转矩阵 ( R )，表示世界坐标系中点的微小变化如何映射到相机坐标系中。

---

2. 相机坐标系 → 归一化平面坐标（投影求导）

• 坐标系变化：相机坐标系 ${\mathbf{P}}_c=[X_c,Y_c,Z_c{]}^T$通过透视投影转换为归一化平面坐标 ${\mathbf{P}}_n=[X_n,Y_n,1{]}^T$，其中
  $$X_n=\frac{X_c}{Z_c},Y_n=\frac{Y_c}{Z_c}$$
• 求导对象：$\frac{\partial {\mathbf{P}}_n}{\partial {\mathbf{P}}_c}$
  为什么：这是透视投影的局部导数，描述相机坐标系中点的微小变化如何影响归一化平面坐标。
  计算得：
  $$\frac{\partial {\mathbf{P}}_n}{\partial {\mathbf{P}}_c}=\left[\begin{array}{ccc}\frac{1}{Z_c}& 0& -\frac{X_c}{Z_c^2}\\ 0& \frac{1}{Z_c}& -\frac{Y_c}{Z_c^2}\end{array}\right]$$

---

3. 归一化平面坐标 → 像素坐标（内参数求导）

• 坐标系变化：归一化平面坐标 ${\mathbf{P}}_n=[X_n,Y_n,1{]}^T$ 通过内参数矩阵 ( K ) 转换为像素坐标 ${\mathbf{P}}_{uv}=[u,v{]}^T$：
  $${\mathbf{P}}_{uv}=K{\mathbf{P}}_n=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{X}_n\\ Y_n\\ 1\end{array}\right]$$
• 求导对象：$\frac{\partial {\mathbf{P}}_{uv}}{\partial {\mathbf{P}}_n}=K$
  为什么：这是内参矩阵的线性变换，描述归一化平面坐标的微小变化如何映射到像素平面。
  计算得：
  $$\frac{\partial {\mathbf{P}}_{uv}}{\partial {\mathbf{P}}_n}=\left[\begin{array}{cc}{f}_x& 0\\ 0& f_y\end{array}\right]$$

---

4. 链式法则整合

最终的投影链式求导（例如对 ${\mathbf{P}}_w$ 求导）为：
$$\frac{\partial {\mathbf{P}}_{uv}}{\partial {\mathbf{P}}_w}=\frac{\partial {\mathbf{P}}_{uv}}{\partial {\mathbf{P}}_n}\cdot \frac{\partial {\mathbf{P}}_n}{\partial {\mathbf{P}}_c}\cdot \frac{\partial {\mathbf{P}}_c}{\partial {\mathbf{P}}_w}$$
即：
$$\frac{\partial {\mathbf{P}}_{uv}}{\partial {\mathbf{P}}_w}=K\cdot {\text{Jacobian}}_{\text{proj}}\cdot R$$
其中 ${\text{Jacobian}}_{\text{proj}}$ 是步骤2中的透视投影雅可比矩阵。

---

总结：坐标系与求导关系表

步骤	坐标系变化	求导对象	物理意义
外参数	世界 → 相机	( \frac{\partial \mathbf{P}_c}{\partial \mathbf{P}_w} = R )	世界坐标系到相机坐标系的刚体变换
投影	相机 → 归一化平面	( \frac{\partial \mathbf{P}_n}{\partial \mathbf{P}_c} )	透视投影的局部导数
内参数	归一化平面 → 像素平面	( \frac{\partial \mathbf{P}_{uv}}{\partial \mathbf{P}_n} = K )	线性缩放和平移（像素化）

每一步的求导都是在当前坐标系下对上一级坐标系的坐标求导，最终通过链式法则整合为完整的雅可比矩阵。