视觉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$