視覺SLAM作業（四） 相機模型與非線性優化

一 圖像去畸變

現實生活中的圖像總存在畸變。原則上來說，針孔透視相機應該將三維世界中的直線投影成直線，但是當我們使用廣角和魚眼鏡頭時，由於畸變的原因，直線在圖像里看起來是扭曲的。本次作業，你將嘗試如何對一張圖像去畸變，得到畸變前的圖像。

圖1 是本次習題的測試圖像（code/test.png），來自EuRoC 數據集[1]。可以明顯看到實際的柱子、箱子的直線邊緣在圖像中被扭曲成了曲線。這就是由相機畸變造成的。根據我們在課上的介紹，畸變前后的坐標變換為：
$x_{distorted} = x(1 + k_1r^2 + k_2r^4)+ 2p_1xy + p_2(r^2 + 2x^2)\\ y_{distorted} = y(1 + k_1r^2 + k_2r^4)+ p_1(r^2 + 2y^2)+ 2p_2xy$
其中x; y 為去畸變后的坐標， $x_{distorted}$，$ y_{distroted}$ 為去畸變前的坐標。

現給定參數：
$k_1= 0.28340811; k2 = 0.07395907; p_1 = 0.00019359; p_2 = 1.76187114e^{-5}:$
以及相機內參
$f_x = 458.654; f_y = 457.296; c_x = 367.215; c_y = 248.375:$
請根據undistort_image.cpp 文件中內容，完成對該圖像的去畸變操作。

答： 去畸變過程主要包括以下步驟：

1. 將圖像的像素坐標系通過內參矩陣轉換到相機歸一化坐標系
   $x = (u-c_x)/f_x\\ y = (v-c_y)/f_y$

2. 在相機坐標系下進行去畸變操作
   $r = \sqrt{x^2+y^2}\\ x' = x*(1+k_1*r^2+k_2*r^4)+2*p_1*x*y+p_2*(r^2+2*x^2)\\ y' = y*(1+k_1*r^2+k_2*r^4)+2*p_2*x*y+p_1*(r^2+2*y^2)\\$

3. 去畸變操作結束后，將相機坐標系重新轉換到圖像像素坐標系
   $u'=x'*f_x+c_x\\ v'=y'*f_y+c_y$

4. 用源圖像的像素值對新圖像的像素點進行插值

代碼修改部分

// u(x) 列 v(y) 行
double u_distorted = 0, v_distorted = 0;            
// TODO 按照公式，計算點(u,v)對應到畸變圖像中的坐標
// start your code here

// 把像素坐標系的點投影到歸一化平面
double x = (u-cx)/fx, y = (v-cy)/fy; 

// 計算圖像點坐標到光心的距離；
double r = sqrt(x*x+y*y);

// 計算投影點畸變后的點
double x_distorted = x*(1+k1*r+k2*r*r)+2*p1*x*y+p2*(r+2*x*x); 
double y_distorted = y*(1+k1*r+k2*r*r)+2*p2*x*y+p1*(r+2*y*y); 

// 把畸變后的點投影回去
u_distorted = x_distorted*fx+cx;
v_distorted = y_distorted*fy+cy;
// end your code here

運行結果截圖

二 雙目視差的使用​

雙目相機的一大好處是可以通過左右目的視差來恢復深度。課程中我們介紹了由視差計算深度的過程。本題，你需要根據視差計算深度，進而生成點雲數據。本題的數據來自Kitti 數據集[2]。
​ Kitti 中的相機部分使用了一個雙目模型。雙目采集到左圖和右圖，然后我們可以通過左右視圖恢復出深度。經典雙目恢復深度的算法有BM(Block Matching), SGBM(Semi-Global Block Matching)[3, 4] 等，
但本題不探討立體視覺內容（那是一個大問題）。我們假設雙目計算的視差已經給定，請你根據雙目模型，畫出圖像對應的點雲，並顯示到Pangolin 中。
​ 本題給定的左右圖見code/left.png 和code/right.png，視差圖亦給定，見code/right.png。雙目的參數如下：
$f_x = 718.856; f_y = 718.856; c_x = 607.1928; c_y = 185.2157:$
且雙目左右間距（即基線）為：
$d = 0.573 m:$
請根據以上參數，計算相機數據對應的點雲，並顯示到Pangolin 中。程序請參考code/disparity.cpp 文件。

答：課本中的雙目相機模型如下：

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-qQqTRudg-1592674995380)(曾是少年-第四章作業.assets/image-20200605134649792.png)]

深度計算公式為：
$depth = \frac{f*b}{d}$
在程序中，視差disp由深度圖提供(uchar類型)。，f焦距由 $f_x$給出，b是基線距離（程序中由d表示，可能會有一點混淆）。

課本中提到。雖然由視差計算深度的公式很簡潔，但視差d 本身的計算卻比較困難。本程序中已經提供了視差圖因此很容易計算得到深度。

注意事項：

• 計算點的時候需要把像素點先轉換到相機坐標系。
• 程序中基線距離的表示符號為d
• 視差圖中數據類型為uchar
• 平時中焦距 $f$與 $f_x$差不多

點雲計算代碼

// TODO 根據雙目模型計算點雲
// 如果你的機器慢，請把后面的v++和u++改成v+=2, u+=2
for (int v = 0; v < left.rows; v++)
    for (int u = 0; u < left.cols; u++) {

        Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0); // 前三維為xyz,第四維為顏色
        // start your code here (~6 lines)
        // 根據雙目模型計算 point 的位置
        double x = (u-cx)/fx;
        double y = (v-cy)/fy;
        float disp = disparity.at<uchar>(v,u); //視差
        double depth = fx*d/(disp);//  d是基線
        point[0] = x*depth;
        point[1] = y*depth;
        point[2] = 1*depth;
        pointcloud.push_back(point);
        // end your code here
    }

生成的點雲截圖如下所示：

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-JX6J9Rrr-1592674995382)(image/點雲結果.png)]

三 矩陣運算微分

在優化中經常會遇到矩陣微分的問題。例如，當自變量為向量x，求標量函數u(x) 對x 的導數時，即為矩陣微分。通常線性代數教材不會深入探討此事，這往往是矩陣論的內容。我在ppt/目錄下為你准備了一份清華研究生課的矩陣論課件（僅矩陣微分部分）。閱讀此ppt，回答下列問題：
設變量為 $x \in R^N$，(x是列向量) 那么：

1. 矩陣 $A \in R^{N\times N}$，那么d(Ax)/dx 是什么？

答： $x$是 $n\times1$列向量

令矩陣 $A = [a_1,a_2,...,a_n]$ , $A = [a_1';a_2';...;a_n']$。

$\begin{aligned} \frac{\partial{{Ax}}}{\partial x} &= \left[ \begin{array}{ccc} \frac{\partial{{Ax}_1}}{\partial x_1}& \frac{\partial{Ax}_2}{\partial x_1}& ...& \frac{\partial{Ax}_n}{\partial x_1}\\ \frac{\partial{{Ax}_1}}{\partial x_2}& \frac{\partial{Ax}_2}{\partial x_2}& ...& \frac{\partial{Ax}_n}{\partial x_2}\\ ... & ... &...&...\\ \frac{\partial{{Ax}_1}}{\partial x_n}& \frac{\partial{Ax}_2}{\partial x_n}& ...& \frac{\partial{Ax}_n}{\partial x_n}\\ \end{array} \right] \end{aligned}$
先對x的第i個分量求導：
$\begin{aligned} \frac{\partial{Ax}_i}{\partial x_k} &= \frac{\partial{a_ix}}{\partial x_k} =a_{ik} \end{aligned}$
導入前式有：
$\begin{aligned} \frac{\partial{{Ax}}}{\partial x} &= \left[ \begin{array}{ccc} a_{11} & a_{21} & ...& a_{n1}\\ a_{12} & a_{22} & ... & a_{n2}\\ ... & ... &...&...\\ a_{1n} & a_{2n} & ...& a_{nn}\\ \end{array} \right] \end{aligned} = A^T$

2. 矩陣 $A \in R^{N\times N}$，那么 $d(x^TAx)/dx$ 是什么？

答：
$\begin{aligned} \frac{\partial{x^TAx}}{\partial x} &= \left[ \begin{array}{ccc} \frac{\partial{x^TAx}}{\partial x_1}& \frac{\partial{x^TAx}}{\partial x_2}& ...& \frac{\partial{x^TAx}}{\partial x_n} \end{array} \right] \end{aligned}$
先對x的第k個分量求導，結果如下：
$\begin{aligned} \frac{\partial{x^TAx}}{\partial x_k} &= \frac{\partial{\sum^n_{i=1}\sum_{j=1}^nx_{i}A_{ij}x_j}}{\partial x_k}\\ &=\sum^n_{i=1} A_{ik}x_i+\sum^n_{j=1}A_{kj}x_j\\ &=a^T_kx+a'_kx \end{aligned}$
可以看出第一部分是矩陣A的第k列轉置后和x相乘得到，第二部分是矩陣A的第k行和x相乘得到，排列好就是:
$\frac{\partial{x ^ T Ax}}{\partial x} = A^Tx+Ax$

3. 證明： $x^TAx = tr(Axx^T)$

證明：

設a,b都是n維列向量，顯然有
$ab^T= \left[ \begin{array}{ccc} a_1b_1&a_1b_2&...&a_1b_n\\ a_2b_1&a_2b_2&...&a_2b_n\\ ...&...&...&...\\ a_nb_1&a_nb_2&...&a_nb_n \end{array} \right]$

$b^Ta=\sum^{n}_{i=1}a_ib_i$

顯然，可以得到：
$tr(ab^T)=b^Ta$
令 $a=Ax$ , $b=x$ 可得
$tr(Axx^T)=tr((Ax)x^T)=x^TAx$
證畢

附加參考：

四 高斯牛頓法的曲線擬合實驗

我們在課上演示了用Ceres 和g2o 進行曲線擬合的實驗，可以看到優化框架給我們帶來了諸多便利。
本題中你需要自己實現一遍高斯牛頓的迭代過程，求解曲線的參數。我們將原題復述如下。設有曲線滿足以下方程：
$y = \exp(ax^2 + bx + c) + w.$
其中 $a, b, c$ 為曲線參數，w為噪聲。現有N個數據點 $(x,y)$，希望通過此N個點來擬合 $a, b, c$。實驗中取 $N = 100$。
那么，定義誤差為 $e_i = y_i - \exp(ax^2_i+bx_i + c)$，於是 $(a, b,c)$ 的最優解可通過解以下最小二乘獲得：
$\min_{a,b,c}\frac{1}{2}\sum^{N}_{i=1}||y_i\exp(ax_i^2+bx_i+c)||^2$
現在請你書寫Gauss-Newton 的程序以解決此問題。程序框架見code/gaussnewton.cpp，請填寫程序內容以完成作業。作為驗證，按照此程序的設定，估計得到的a; b; c 應為： $a = 0.890912; b = 2.1719; c = 0.943629,$
這和書中的結果是吻合的。

答：先回顧高斯牛頓法求解最小二乘問題的步驟：
$\Delta x^{*} = \arg \min_{\Delta x}\frac{1}{2}||f(x)+J(x)^T\Delta x||^2$

1. 給定初始值 $x_0$。
2. 對於第k 次迭代，求出當前的雅可比矩陣 $J(x_k)$ 和誤差 $f(x_k)$。
3. 求解增量方程： $HΔx_k = g$。
4. 若 $Δx_k$ 足夠小，則停止。否則，令 $x_{k+1} = x_k + Δx_k$，返回第2 步。

可以按照以上步驟來修改代碼

1. 設置初始值

double ae = 2.0, be = -1.0, ce = 5.0;

2. 計算雅可比矩陣 $J(x_k)$ 和誤差 $f(x_k)$。

計算誤差 $error = f(x_i)-f_e(x_i)$

error = yi - exp(ae * xi * xi + be * xi + ce);

計算雅可比矩陣$J = \frac{\partial error} {\partial x} $

Vector3d J; // 雅可比矩陣
J[0] = - exp(ae * xi * xi + be * xi + ce)* xi * xi;  // de/da
J[1] = - exp(ae * xi * xi + be * xi + ce)* xi;  // de/db
J[2] = - exp(ae * xi * xi + be * xi + ce);  // de/dc

3. 求解增量方程

計算增量矩陣H

H += J * J.transpose(); // GN近似的H

計算g

b += -error * J;

用EIgen中的ldlt求解 $H\Delta x =b$。

Vector3d dx;
dx = H.ldlt().solve(b);

4. 若 $Δx_k$ 足夠小，則停止。否則，令 $x_{k+1} = x_k + Δx_k$，返回第2 步。

if (iter > 0 && cost > lastCost) {
    // 誤差增長了，說明近似的不夠好
    cout << "cost: " << cost << ", last cost: " << lastCost << endl;
    break;
}

至此，代碼修改完畢。

運行結果：

/home/guoben/Project/SLAM-homework/ch4/GaussNewton/bin/GN
total cost: 3.19575e+06
total cost: 376785
total cost: 35673.6
total cost: 2195.01
total cost: 174.853
total cost: 102.78
total cost: 101.937
total cost: 101.937
total cost: 101.937
total cost: 101.937
total cost: 101.937
total cost: 101.937
total cost: 101.937
cost: 101.937, last cost: 101.937
estimated abc = 0.890912, 2.1719, 0.943629

Process finished with exit code 0

運行截圖

附加題 五* 批量最大似然估計

考慮離散時間系統：
$x_k = x_{k-1} + v_k + w_k; w\sim N (0;Q)\\ y_k = x_k + n_k; n_k \sim N (0;R)$
這可以表達一輛沿x 軸前進或后退的汽車。第一個公式為運動方程， $v_k$ 為輸入， $w_k$ 為噪聲；第二個公式為觀測方程， $y_k$ 為路標點。取時間 $k = 1,...,3$，現希望根據已有的 $v,y$ 進行狀態估計。設初始狀態 $x_0$ 已知。
請根據本題題設，推導批量（batch）最大似然估計。首先，令批量狀態變量為

$x = [x_0, x_1, x_2, x_3]^T$，令批量觀測為 $z = [v_1, v_2, v_3, y_1, y_2, y_3]^T$，那么：

1. 可以定義矩陣 H，使得批量誤差為 $e = z - Hx$。請給出此處H的具體形式。

答：該線性系統很簡單，很容易的寫成以下形式
$v_k = x_k-x_{k-1} + w_k\\ y_k= x_k + n_k\\$
而 $z-Hx=e\sim N(0,\Sigma)$, 向量化上式可以得到：
$H= \left[ \begin{array}{ccc} -1& 1& 0& 0\\ 0 &-1& 1& 0\\ 0 & 0&-1& 1\\ 0&1&0&0\\ 0&0&1&0\\ 0&0&0&1\\ \end{array} \right]$

2. 據上問，最大似然估計可轉換為最小二乘問題, 請給出此問題下信息矩陣W 的具體取值。

$x^{*} = \arg \min \frac{1}{2}(z - Hx)^TW^{-1}(z-Hx)$

其中W 為此問題的信息矩陣，可以從最大似然的概率定義給出。

答： $W=diag(Q,R)$
$\begin{aligned} x^{*} &= \arg \max P(x|z) = \arg \max P(z|x)\\ &=\prod^{3}_{k=1}P(v_k|x_{k-1},x_k)\prod^{3}_{k=1}P(y_k|x_k) \end{aligned}$
其中 $P(v_k|x_{k-1},x_k)=N(x_k-x_{k-1},Q)$，

$P(y_k|x_k) = N(x_k,R)$。

誤差變量如下：
$e_{v,k}=x_k-x_{k-1}-v_k, e_{z,k}=y_k-x_k$
對概率取對數，可以把最小二乘的目標函數化為如下形式：
$\min\sum^3_{k=1} e^{T}_{v,k}Q^{-1}e_{v,k}+\sum^3_{k=1}e^T_{y,k}R^{-1}e_{y,k}$
因此 $W=diag(Q,Q,Q,R,R,R)$; 即
$W = \left[ \begin{array}{ccc} Q & 0 & 0 & 0 & 0 & 0\\ 0 & Q & 0 & 0 & 0 & 0\\ 0 & 0 & Q & 0 & 0 & 0\\ 0 & 0 & 0 & R & 0 & 0\\ 0 & 0 & 0 & 0 & R & 0\\ 0 & 0 & 0 & 0 & 0 & R\\ \end{array} \right]$
此時，最小二乘問題可以寫為：
$x^{*} =\arg \min e^T W^{-1} e$

3. 假設所有噪聲相互無關，該問題存在唯一的解嗎？若有，唯一解是什么？若沒有，說明理由。

答: 當噪聲相互無關的時候，該問題存在唯一解。

因為 $Hx=z$ 這個式子中H是6*4矩陣，方程個數大於未知量個數的方程組，是一個超定矩陣。而系數矩陣超定時，最小二乘問題可以得到唯一解。
唯一最小二乘解如下：
$x=(H^TH)^{-1}H^Tz$

助教點評：假設所有噪聲相互無關，那么H的秩是等於4的，所以問題存在唯一解，那根據本題定義，我們可以將目標函數寫成圖中14式所示，因為JX剛好是一個拋物面，我們能解析的找到它的最小值，這只需要讓目標函數相對於自變量的偏導數為零即可得到啊，如圖中所示，我們可以得到最后的一個X最優解。