当前位置：网站首页>Deep Blue Academy - Fourteen Lectures of Visual SLAM - Chapter 4 Homework

Deep Blue Academy - Fourteen Lectures of Visual SLAM - Chapter 4 Homework

2022-08-02 05:23:00 【hello689】

Fourth class homework

2.图像去畸变

The main content of this question is to achieve image de-distortion according to the provided formulas and parameters.

Coordinate change formula before and after distortion：
$\begin{cases} 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 \end{cases}$
where the given parameters are ：
$k_1 = -0.28340811,k_2 = 0.07395907,p_1 = 0.00019359,p_2=1.76187114e-05 \\ 相机内参：\\ f_x = 458.654,f_y=457.296,c_x=367.215,c_y=248.375$
Dewarping the core part of the code：

 // start your code here
//计算归一化坐标
double x = (u - cx)/fx;
double y = (v - cy)/fy;
double r = sqrt(x*x + y*y);
//Calculate radial and tangentially distorted coordinates
double x_distorted = x * (1 + k1 * r * r + k2 * r * r * r * r) + 2 * p1 * x * y + p2 * (r * r + 2 * x * x);
double y_distorted = y * (1 + k1 * r * r + k2 * r * r * r * r) + p1 * (r * r + 2 * y * y) + 2 * p2 * x * y;
//计算像素
u_distorted = fx * x_distorted + cx;
v_distorted = fy * y_distorted + cy;
// end your code here

De-distortion rendering：

在这里插入图片描述

3.鱼眼模型与去畸变

请说明鱼眼相机相比于普通针孔相机在SLAM 方面的优势.
One of the most important advantages of fisheye cameras is that they have a wider field of view than ordinary pinhole cameras.So you can be sure for a while,Bring as many visual features as possible into the camera's field of view,Thereby improving the perception of the surrounding environment.
请整理并描述OpenCV 中使用的Fisheye Distortion模型（等距投影）是如何定义的,它与上题的畸变模型
有何不同.
参考资料：
- OpenCV: Fisheye camera model
- https://blog.csdn.net/KYJL888/article/details/117423950
- https://blog.csdn.net/weixin_43304707/article/details/113261307
模型定义(也叫kannala-brandt模型,There are various fisheye distortion models)：
Set a point in the world coordinate systemP,The coordinates of the point are used in a matrixX表示.Vector coordinates in the camera coordinate systemP为：
$X c = R X + T$
其中,R是旋转矩阵,旋转向量omAfter Rodrigues transformation.XcThe three quantities are respectivelyx,y,z;
$x = Xc_1 \\ y = Yc_2\\ z = Xc_3$
PThe pinhole projection coordinates of [a,b]：
$\space and \space b = y/z\\ r^2=a^2+b^2\\ \theta = \arctan(r)$
注：在OpenCV的文档中,如下图所示, $\theta = atan(r)$ ,represents the arc tangent,也可以描述成 $\theta = \arctan(r)$ .
Fisheye Distortion：
$\theta_d = \theta(1+k_1\theta^2+k_2\theta^3+k_3\theta^6+k_4\theta^8)$
The coordinates of the distorted point are $x^{'};y^{'}]$ :
$x^{'} = (\theta_d/r)a\\ y^{'} = (\theta_d/r)b$
最后,Convert to pixel coordinate system,The final pixel coordinate vector is [u;v], $\alpha$ 为偏度系数:
$f_x(x^{'}+\alpha y^{'})+c_x\\ v = f_y y^{'}+c_y$
The difference from the distortion model in the previous question：The distortion model of the above question includes radial and tangential distortions.The fisheye camera of this question is only given $\theta_d$ A type of distortion.我注意到,Solving for the final pixel coordinates $u$ 时,Added one to the fisheye camera conversion $\alpha y^{'}$ 与y有关的变量,Different from the previous model.

完成fisheye.cpp 文件中的内容.针对给定的图像,实现它的畸变校正.要求：通过手写方式实现,
不允许调用OpenCV 的API.

通过上述公式,The core code to implement dewarping is shown below：

 double a = (u-cx)/fx, b = (v-cy)/fy;
double r = sqrt(a*a + b*b);
double theta = atan(r);
double theta_2 = theta*theta;
double theta_4 = theta_2*theta_2;
double theta_6 = theta_4*theta_2;
double theta_8 = theta_4*theta_4;
double theta_d = theta*(1+k1*theta_2+k2*theta_4+k3*theta_6+k4*theta_8);

double x_distorted = (theta_d / r)*a;
double y_distorted = (theta_d / r)*b;

u_distorted = fx*(x_distorted + 0.01*y_distorted)+cx;
v_distorted = fy*y_distorted + cy;

The effect of removing fisheye distortion：

在这里插入图片描述

Why in this image,我们令畸变参数k1, . . . , k4 = 0,依然可以产生去畸变的效果？
$\theta_d = \theta(1+k_1\theta^2+k_2\theta^3+k_3\theta^6+k_4\theta^8)$
The fisheye camera finally reaches the imaging unit through the refraction of multiple lenses.Kannala-BrandtFisheye Distortion模型,It's actually about the angle of incidence $\theta$ 的奇函数,因此鱼眼镜头的畸变也是对 $\theta$ 的畸变,It cannot be described by simple radial and tangential distortion polynomials.而 $k_1,\cdots ,k_4$ ,is the radial distortion coefficient determined by camera calibration,这里可以暂时忽略.因此令 $k_1,\cdots ,k_4$ 为０,The effect of removing the distortion can still be clearly displayed.
正确答案：Take the first five terms of Taylor expansion to approximate the fisheye model,k1-k4取0,Equivalent to only approximating the first term,So the de-distortion operation can still be done.
在鱼眼模型中,去畸变是否带来了图像内容的损失？如何避免这种图像内容上的损失呢？
Fisheye diagrams are generally circular,The information at the edge is compressed very densely,After removing the distortion, the middle part of the original image will be well preserved,The edge position is generally stretched very seriously、视觉效果差,So excision is usually done,Therefore, there will definitely be a loss of image content.Increases the size of the image when dewarped,Or use monocular and fisheye camera images for fusion,Complete missing information.

4.双目视差的使用

理论部分

推导双目相机模型下,视差与 $X Y Z$ 坐标的关系式.请给出由像素坐标加视差 $u, v, d$ 推导 $X Y Z$
与已知 $X Y Z$ 推导 $u, v, d$ 两个关系.
First give the world coordinate system、相机坐标系、图像坐标系、像素坐标系的关系：
img source:https://zhuanlan.zhihu.com/p/421453976
set a pointP在成像平面上的两个点 $P_L、P_R$ 的坐标分别是 $x^l,y^l)(x^r,y^r)$ ：
根据三角形 $\bigtriangleup PP_LP_R$ 和 $\bigtriangleup PO_LO_R$ There are similar relationships：
$\frac{z-f}{z} = \frac{b-u_L+u_R}{b}$
整理可得：
$\frac{fb}{d},d=x_l-x_r$
其中d定义为左右图的横坐标之差,称为视差.f为相机的焦距,bis the distance between the two cameras,也就是基线.
得到深度值 $z$ 后,即可得出目标点的三维坐标：
$\begin{cases} u = \frac{bx_l}{d}\\ v = \frac{by_1}{d}\\ z = \frac{fb}{d} \end{cases}$
推导在右目相机下该模型将发生什么改变.
In the parallax piece,Use the right eye camera coordinates to subtract the left eye camera coordinates,d = x_r - x_l.Use the projection point of the right eye camera,Also use the extrinsic parameters of the left eye camera.

编程部分

核心部分代码：

// start your code here (~6 lines)
// 根据双目模型计算 point 的位置
double x = (u - cx) / fx;
double y = (v - cy) / fy;
double depth = fx * d / (disparity.at<char>(v, u));
point[0] = x * depth;
point[1] = y * depth;
point[2] = depth;

pointcloud.push_back(point);
// end your code here

运行效果：

在这里插入图片描述

5.矩阵运算微分

设变量为 $\in \mathbb{R}^N$ ,那么：

矩阵 $\in \mathbb{R}^{N \times N}$ ,那么d(Ax)/dx 是什么？
$\frac{d(AX)}{dx}= A^T$
矩阵 $\in \mathbb{R}^{N \times N}$ ,那么 $d(x^TAx)/dx$ 是什么？
$\frac{d(x^TAx)}{dx} = \frac{(dx)^TAx+x^TAdx}{dx}= \frac{dx^TAx+dx^TA^Tx}{dx} = (A+A^T)x$
证明：
$x^T Ax = tr(Axx^T)\\ 证明：\\ \begin{aligned} 左式 &= x^TAx\\ &= \begin{bmatrix} x_1,x_2,\cdots,x_n \end{bmatrix} \cdot \begin{bmatrix} a_{11},a_{12},\cdots,a_{1n}\\ a_{21},a_{22},\cdots,a_{2n}\\ \cdots\\ a_{n1},a_{n2},\cdots,a_{nn}\\ \end{bmatrix}\cdot \begin{bmatrix} x_1\\x_2\\ \cdots\\x_n \end{bmatrix} \\&= x_1\sum_{i=1 }^{n}a_{1i}x_i+x_2\sum_{i=1 }^{n}a_{2i}x_i+\cdots+x_n\sum_{i=1 }^{n}a_{ni}x_i\\ 右式 &=tr(Axx^T) \\&= tr\begin{pmatrix} \begin{bmatrix} a_{11},a_{12},\cdots,a_{1n}\\ a_{21},a_{22},\cdots,a_{2n}\\ \cdots\\ a_{n1},a_{n2},\cdots,a_{nn}\\ \end{bmatrix}\cdot \begin{bmatrix} x_1\\x_2\\ \cdots\\x_n \end{bmatrix} \cdot \begin{bmatrix} x_1,x_2,\cdots,x_n \end{bmatrix} \end{pmatrix}\\&= tr\begin{pmatrix} \begin{bmatrix} a_{11},a_{12},\cdots,a_{1n}\\ a_{21},a_{22},\cdots,a_{2n}\\ \cdots\\ a_{n1},a_{n2},\cdots,a_{nn}\\ \end{bmatrix}\cdot \begin{bmatrix} x_1x_1,x_1x_2,\cdots,x_1x_n\\ x_2x_1,a_2x_2,\cdots,x_2x_n\\ \cdots\\ x_nx_1,x_nx_2,\cdots,x_nx_n\\ \end{bmatrix} \end{pmatrix}\\&= x_1\sum_{i=1 }^{n}a_{1i}x_i+x_2\sum_{i=1 }^{n}a_{2i}x_i+\cdots+x_n\sum_{i=1 }^{n}a_{ni}x_i\\ \end{aligned}\\ 得证$

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

定义误差为：
$e_i = y_i-\exp(ax_i^2+bx_i+c)$
每个误差项对于状态变量的导数：
$\frac{\partial e_i}{\partial a} = -x_i^2\exp(ax_i^2+bx_i+c)\\ \frac{\partial e_i}{\partial b} = -x_i\exp(ax_i^2+bx_i+c)\\ \frac{\partial e_i}{\partial c} = -\exp(ax_i^2+bx_i+c)\\$
根据该公式,Data fitting was performed using the Gauss-Newton method,主要部分代码如下：

// start your code here
double error = 0;   // 第i个数据点的计算误差
error = yi - exp(ae * xi * xi + be*xi + ce); // 填写计算error的表达式
Vector3d J; // 雅可比矩阵
J[0] = -xi*xi*exp(ae * xi * xi + be*xi + ce);  // de/da
J[1] = -xi* exp(ae * xi * xi + be*xi + ce);  // de/db
J[2] = -exp(ae * xi * xi + be*xi + ce);  // de/dc

H += J * J.transpose(); // GN近似的H
b += -error * J;
// end your code here

// 求解线性方程 Hx=b,建议用ldlt
// start your code here
Vector3d dx = H.ldlt().solve(b);
// end your code here

运行结果：

在这里插入图片描述

7.批量最大似然估计

1.可以定义矩阵 $H$ ,使得批量误差为 $e = z - H x$ .请给出此处 $H$ 的具体形式.
$e = z-Hx\\ x = [x_0,x_1,x_2,x_3]^T\\ z=[v_1,v_2,v_3,y_1,y_2,y_3]^T$
所以H的大小应该是 $6\times4$ ,且需要满足：
$x_k = x_{k-1}+v_k+w_k\\ y_k = x_k+n_k$

$\begin{aligned} e &= z-Hx\\&= \begin{bmatrix} v_1\\v_2\\v_3\\y_1\\y_2\\y_3 \end{bmatrix}-H\cdot \begin{bmatrix} x_0\\x_1\\x_2\\x_3 \end{bmatrix}\\&= \begin{bmatrix} v_1-(x_1-x_0)\\ v_2-(x_2-x_1) \\ v_3-(x_3-x_2)\\ y_1-x_1\\ y_2-x_2\\ y_3-x_3 \end{bmatrix}\\ \end{aligned}$
所以H为：
$\begin{bmatrix} -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{bmatrix}$
参考博客：https://blog.csdn.net/floatinglong/article/details/116202102