视觉SLAM作业（四） 相机模型与非线性优化

视觉SLAM作业（四） 相机模型与非线性优化

一 图像去畸变

现实生活中的图像总存在畸变。原则上来说，针孔透视相机应该将三维世界中的直线投影成直线，但是当我们使用广角和鱼眼镜头时，由于畸变的原因，直线在图像里看起来是扭曲的。本次作业，你将尝试如何对一张图像去畸变，得到畸变前的图像。

图1 是本次习题的测试图像（code/test.png），来自EuRoC 数据集[1]。可以明显看到实际的柱子、箱子的直线边缘在图像中被扭曲成了曲线。这就是由相机畸变造成的。根据我们在课上的介绍，畸变前后的坐标变换为：
$$\begin{gathered} x_{distorted}=x(1+k_1{r}^2+k_2{r}^4)+2p_{1}xy+p_2(r^2+2x^2) \\ {y}_{distorted}=y(1+k_1{r}^2+k_2{r}^4)+p_1(r^2+2y^2)+2p_{2}xy \end{gathered}$$
其中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. 将图像的像素坐标系通过内参矩阵转换到相机归一化坐标系
   $$\begin{gathered} x=(u-c_x)/f_x \\ y=(v-c_y)/f_y \end{gathered}$$

2. 在相机坐标系下进行去畸变操作
   $$\begin{gathered} r=\sqrt{x^2+y^2} \\ {x}^{\prime }=x\ast (1+k_1\ast r^2+k_2\ast r^4)+2\ast p_1\ast x\ast y+p_2\ast (r^2+2\ast x^2) \\ {y}^{\prime }=y\ast (1+k_1\ast r^2+k_2\ast r^4)+2\ast p_2\ast x\ast y+p_1\ast (r^2+2\ast y^2) \end{gathered}$$

3. 去畸变操作结束后，将相机坐标系重新转换到图像像素坐标系
   $$\begin{gathered} u^{\prime }=x^{\prime }\ast f_x+c_x \\ {v}^{\prime }=y^{\prime }\ast f_y+c_y \end{gathered}$$

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.573m:$$
请根据以上参数，计算相机数据对应的点云，并显示到Pangolin 中。程序请参考code/disparity.cpp 文件。

答：课本中的双目相机模型如下：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-qQqTRudg-1592674995380)(曾是少年-第四章作业.assets/image-20200605134649792.png)]

深度计算公式为：
$$depth=\frac{f\ast 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(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(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\times 1$列向量

令矩阵$A=[a_1,a_2,...,a_n]$ , $A=[a_1^{\prime };a_2^{\prime };...;a_n^{\prime }]$。

$$\begin{array}{rl}\frac{\partial Ax}{\partial x}& =\left[\begin{array}{cccc}\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{array}$$
先对x的第i个分量求导：
$$\begin{array}{rl}\frac{\partial {Ax}_i}{\partial x_k}& =\frac{\partial a_{i}x}{\partial x_k}=a_{ik}\end{array}$$
导入前式有：
$$\begin{array}{rl}\frac{\partial Ax}{\partial x}& =\left[\begin{array}{cccc}{a}_{11}& a_{21}& ...& a_{n1}\\ a_{12}& a_{22}& ...& a_{n2}\\ ...& ...& ...& ...\\ a_{1n}& a_{2n}& ...& a_{nn}\end{array}\right]\end{array}=A^T$$

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

答：
$$\begin{array}{rl}\frac{\partial x^{T}Ax}{\partial x}& =\left[\begin{array}{cccc}\frac{\partial x^{T}Ax}{\partial x_1}& \frac{\partial x^{T}Ax}{\partial x_2}& ...& \frac{\partial x^{T}Ax}{\partial x_n}\end{array}\right]\end{array}$$
先对x的第k个分量求导，结果如下：
$$\begin{array}{rl}\frac{\partial x^{T}Ax}{\partial x_k}& =\frac{\partial {\sum }_{i=1}^n{\sum }_{j=1}^n{x}_i{A}_{ij}{x}_j}{\partial x_k}\\ & =\sum _{i=1}^n{A}_{ik}{x}_i+\sum _{j=1}^n{A}_{kj}{x}_j\\ & =a_k^{T}x+a_k^{\prime }x\end{array}$$
可以看出第一部分是矩阵A的第k列转置后和x相乘得到，第二部分是矩阵A的第k行和x相乘得到，排列好就是:
$\frac{\partial x^{T}Ax}{\partial x}=A^{T}x+Ax$

3. 证明：$x^{T}Ax=tr(Ax{x}^T)$

证明：

设a,b都是n维列向量，显然有
$$a{b}^T=\left[\begin{array}{cccc}{a}_1{b}_1& a_1{b}_2& ...& a_1{b}_n\\ a_2{b}_1& a_2{b}_2& ...& a_2{b}_n\\ ...& ...& ...& ...\\ a_n{b}_1& a_n{b}_2& ...& a_n{b}_n\end{array}\right]$$

$$b^{T}a=\sum _{i=1}^n{a}_i{b}_i$$

显然，可以得到：
$$tr(a{b}^T)=b^{T}a$$
令 $a=Ax$ , $b=x$ 可得
$$tr(Ax{x}^T)=tr((Ax)x^T)=x^{T}Ax$$
证毕

附加参考：

四 高斯牛顿法的曲线拟合实验

我们在课上演示了用Ceres 和g2o 进行曲线拟合的实验，可以看到优化框架给我们带来了诸多便利。
本题中你需要自己实现一遍高斯牛顿的迭代过程，求解曲线的参数。我们将原题复述如下。设有曲线满足以下方程：
$$y=\exp (a{x}^2+bx+c)+w.$$
其中$a,b,c$ 为曲线参数，w为噪声。现有N个数据点$(x,y)$，希望通过此N个点来拟合$a,b,c$。实验中取$N=100$。
那么，定义误差为$e_i=y_i-\exp (a{x}_i^2+b{x}_i+c)$，于是$(a,b,c)$ 的最优解可通过解以下最小二乘获得：
$$\underset{a,b,c}{\min }\frac{1}{2}\sum _{i=1}^N||y_i\exp (a{x}_i^2+b{x}_i+c)|{|}^2$$
现在请你书写Gauss-Newton 的程序以解决此问题。程序框架见code/gaussnewton.cpp，请填写程序内容以完成作业。作为验证，按照此程序的设定，估计得到的a; b; c 应为：$a=0.890912;b=2.1719;c=0.943629,$
这和书中的结果是吻合的。

答：先回顾高斯牛顿法求解最小二乘问题的步骤：
$$\Delta x^{\ast }=\arg \underset{\Delta x}{\min }\frac{1}{2}||f(x)+J(x{)}^T\Delta x|{|}^2$$

1. 给定初始值$x_0$。
2. 对于第k 次迭代，求出当前的雅可比矩阵$J(x_k)$ 和误差$f(x_k)$。
3. 求解增量方程：$H\Delta x_k=g$。
4. 若$\Delta x_k$ 足够小，则停止。否则，令$x_{k+1}=x_k+\Delta 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. 若$\Delta x_k$ 足够小，则停止。否则，令$x_{k+1}=x_k+\Delta 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

运行截图

附加题 五* 批量最大似然估计

考虑离散时间系统：
$$\begin{gathered} 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) \end{gathered}$$
这可以表达一辆沿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的具体形式。

答：该线性系统很简单，很容易的写成以下形式
$$\begin{gathered} v_k=x_k-x_{k-1}+w_k \\ {y}_k=x_k+n_k \end{gathered}$$
而$z-Hx=e\sim N(0,\Sigma )$, 向量化上式可以得到：
$$H=\left[\begin{array}{cccc}-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^{\ast }=\arg \min \frac{1}{2}(z-Hx{)}^T{W}^{-1}(z-Hx)$$

其中W 为此问题的信息矩阵，可以从最大似然的概率定义给出。

答：$W=diag(Q,R)$
$$\begin{array}{rl}{x}^{\ast }& =\arg \max P(x|z)=\arg \max P(z|x)\\ & =\prod _{k=1}^{3}P(v_k|x_{k-1},x_k)\prod _{k=1}^{3}P(y_k|x_k)\end{array}$$
其中 $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 _{k=1}^3{e}_{v,k}^T{Q}^{-1}{e}_{v,k}+\sum _{k=1}^3{e}_{y,k}^T{R}^{-1}{e}_{y,k}$$
因此$W=diag(Q,Q,Q,R,R,R)$; 即
$$W=\left[\begin{array}{cccccc}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^{\ast }=\arg \min e^T{W}^{-1}e$$

3. 假设所有噪声相互无关，该问题存在唯一的解吗？若有，唯一解是什么？若没有，说明理由。

答: 当噪声相互无关的时候，该问题存在唯一解。

因为$Hx=z$ 这个式子中H是6*4矩阵，方程个数大于未知量个数的方程组，是一个超定矩阵。而系数矩阵超定时，最小二乘问题可以得到唯一解。
唯一最小二乘解如下：
$$x=(H^{T}H{)}^{-1}{H}^{T}z$$

助教点评：假设所有噪声相互无关，那么H的秩是等于4的，所以问题存在唯一解，那根据本题定义，我们可以将目标函数写成图中14式所示，因为JX刚好是一个抛物面，我们能解析的找到它的最小值，这只需要让目标函数相对于自变量的偏导数为零即可得到啊，如图中所示，我们可以得到最后的一个X最优解。