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?\frac{{\theta }_d}{\theta }$$
$$y_d=y_n?\frac{{\theta }_d}{\theta }$$

2. 雅可比矩陣的推導

畸變坐標相對于歸一化坐標的雅可比矩陣 $H_{dz/dzn}$

我們需要計算 $\frac{?(x_d,y_d)}{?(x_n,y_n)}$。

首先，計算 $\frac{?\theta }{?x_n}$ 和 $\frac{?\theta }{?y_n}$：

$$\frac{?\theta }{?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{?\theta }{?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{?{\theta }_d}{?\theta }$：

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

接下來，計算 $\frac{?x_d}{?x_n}$ 和 $\frac{?y_d}{?y_n}$，使用鏈式法則：

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

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

類似地，可以計算 $\frac{?x_d}{?y_n}$ 和 $\frac{?y_d}{?x_n}$，它們通常不為零，因為畸變會引入坐標之間的耦合。

最終，雅可比矩陣 $H_{dz/dzn}$ 將是一個 $2\times 2$ 矩陣，包含上述偏導數。

畸變坐標相對于相機內參的雅可比矩陣 $H_{dz/dzeta}$

這個矩陣描述的是畸變像素坐標如何隨相機內參（如焦距和畸變系數）變化。

對于焦距 $f_x,f_y$，畸變像素坐標與它們的關系較為直接：

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

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

對于畸變系數 $k_i$，需要計算 $\frac{?{\theta }_d}{?k_i}$，然后使用鏈式法則計算 $\frac{?x_d}{?k_i}$ 和 $\frac{?y_d}{?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{?x_d}{?x_n}$、$\frac{?x_d}{?y_n}$、$\frac{?y_d}{?x_n}$ 和 $\frac{?y_d}{?y_n}$。
這些導數描述了當歸一化坐標發生微小變化時，畸變坐標將如何變化。
畸變坐標相對于相機內參的雅可比矩陣（H_dz_dzeta）：

這涉及到對畸變后的像素坐標（x_d, y_d）關于相機內參（如焦距 f_x, f_y 和畸變系數 k_1, k_2, k_3, k_4）求導。
具體來說，我們需要計算 $\frac{?x_d}{?f_x}$、$\frac{?x_d}{?f_y}$、$\frac{?x_d}{?k_i}$、$\frac{?y_d}{?f_x}$、$\frac{?y_d}{?f_y}$ 和 $\frac{?y_d}{?k_i}$（其中 $i=1,2,3,4$）。
這些導數描述了當相機內參發生微小變化時，畸變坐標將如何變化。
鏈式求導的應用
在計算這些導數時，我們通常會使用鏈式求導法則。例如，為了計算 $\frac{?x_d}{?x_n}$，我們可能需要先計算 $\frac{?{\theta }_d}{?\theta }$（畸變角度對無畸變角度的導數）和 $\frac{?\theta }{?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{?{\mathbf{P}}_c}{?{\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{?{\mathbf{P}}_n}{?{\mathbf{P}}_c}$
  為什么：這是透視投影的局部導數，描述相機坐標系中點的微小變化如何影響歸一化平面坐標。
  計算得：
  $$\frac{?{\mathbf{P}}_n}{?{\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{?{\mathbf{P}}_{uv}}{?{\mathbf{P}}_n}=K$
  為什么：這是內參矩陣的線性變換，描述歸一化平面坐標的微小變化如何映射到像素平面。
  計算得：
  $$\frac{?{\mathbf{P}}_{uv}}{?{\mathbf{P}}_n}=\left[\begin{array}{cc}{f}_x& 0\\ 0& f_y\end{array}\right]$$

---

4. 鏈式法則整合

最終的投影鏈式求導（例如對 ${\mathbf{P}}_w$ 求導）為：
$$\frac{?{\mathbf{P}}_{uv}}{?{\mathbf{P}}_w}=\frac{?{\mathbf{P}}_{uv}}{?{\mathbf{P}}_n}?\frac{?{\mathbf{P}}_n}{?{\mathbf{P}}_c}?\frac{?{\mathbf{P}}_c}{?{\mathbf{P}}_w}$$
即：
$$\frac{?{\mathbf{P}}_{uv}}{?{\mathbf{P}}_w}=K?{\text{Jacobian}}_{\text{proj}}?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 )	線性縮放和平移（像素化）

每一步的求導都是在當前坐標系下對上一級坐標系的坐標求導，最終通過鏈式法則整合為完整的雅可比矩陣。