ORB-SLAM2系列-相机成像与畸变_orbslam 世界坐标系到相机坐标系的变换矩阵

坐标转换

坐标的转换在视觉slam十四讲中讲解的很清楚，这里就只是展示一下转化的公式：

根据上图，由世界坐标系中的点转换到相机坐标系中：

( x c y c z c 1 ) = [ R t 0 1 ] ( x w y w z w 1 )

$$\begin{pmatrix} x_c\\ y_c\\ z_c\\ 1 \end{pmatrix}$$ = $$\begin{bmatrix} R & t\\ 0 & 1\\ \end{bmatrix}$$ $$\begin{pmatrix} x_w\\ y_w\\ z_w\\ 1 \end{pmatrix}$$ ​xc​yc​zc​1​ ​=[R0​t1​] ​xw​yw​zw​1​ ​

相机成像

根据视觉slam十四讲中，相机坐标系下的点$P(X,Y,Z)$在相机成像平面上的点$P^{\prime }(X^{\prime },Y^{\prime },Z^{\prime })$

根据右图的三角形相似的原理，得到如下式子：

Z f = X X ′ = Y Y ′ 得到 { X ′ = f X Z Y ′ = f Y Z \frac{Z}{f}=\frac{X}{X'}=\frac{Y}{Y'}得到

$$\begin{cases} X'=f\frac{X}{Z}\\\\ Y'=f\frac{Y}{Z} \end{cases}$$ fZ​=X′X​=Y′Y​得到⎩ ⎨ ⎧​X′=fZX​Y′=fZY​​

上面公式就是将相机坐标系中的点P，投影在相机成像平面。虽然相机成像的平面和像素坐标在同一个平面，但是他们的坐标系不同（我们所知道的相机像素都是左上角才是0,0点），因此需要一个平移的操作，即引入了平移参数$c_x,c_y$

{ u = α X ′ + c x v = β Y ′ + c y 得到 { u = f x X Z + c x v = f y Y Z + c y

$$\begin{cases} u=\alpha X' + c_x\\\\ v=\beta Y' + c_y \end{cases}$$ 得到 $$\begin{cases} u = f_x\frac{X}{Z} + c_x\\\\ v = f_y\frac{Y}{Z} + c_y \end{cases}$$ ⎩ ⎨ ⎧​u=αX′+cx​v=βY′+cy​​得到⎩ ⎨ ⎧​u=fx​ZX​+cx​v=fy​ZY​+cy​​

这个公式很重要，其中$\alpha 、\beta$指的是在$x、y$方向上的缩放。这个时候就完成了世界坐标系中的点，转换为像素点坐标的过程：

( u v 1 ) = 1 Z ( f x 0 c x 0 f y c y 0 0 1 ) [ R t ] ( x w y w z w 1 ) = 1 Z K T ( x w y w z w 1 )

$$\begin{pmatrix} u\\ v\\ 1 \end{pmatrix}$$ = \frac{1}{Z} $$\begin{pmatrix} f_x & 0 & c_x\\ 0 & f_y & c_y\\ 0 & 0 & 1 \end{pmatrix}$$ $$\begin{bmatrix} R & t \end{bmatrix}$$ $$\begin{pmatrix} x_w\\ y_w\\ z_w\\ 1 \end{pmatrix}$$ = \frac{1}{Z}KT $$\begin{pmatrix} x_w\\ y_w\\ z_w\\ 1 \end{pmatrix}$$ ​uv1​ ​=Z1​ ​fx​00​0fy​0​cx​cy​1​ ​[R​t​] ​xw​yw​zw​1​ ​=Z1​KT ​xw​yw​zw​1​ ​

其中$f_x、f_y$是x，y方向的焦距，一般都是相等的，$c_x、c_y$是光心在像素坐标的位置，一般就是长宽的一半，这些都是称为内参。而$R、t$是相机坐标系在世界坐标系的转换，称为外参。

因此，世界坐标系中的一点乘外参之后，就得到相机坐标系下的空间坐标（如果这个空间坐标除以 $Z$ 就是归一化平面的坐标），再乘内参（三角形相似定理）就是在图像上的像素坐标

这个内参，在实际中应用过程中，是经过相机标定提前设置好的，因为ORBSLAM2的程序中，读取的配置文件（yaml文件）中存放这相机的内参参数

相机畸变

畸变的原因

根据针孔相机模型中，我们是直接将相机光心想象成了一个平面，但是在实际中，相机前面是有一个透镜的，为了能够拍摄到更大的范围，这个透镜一般都是中间很厚两边薄，因此之前的假设就不能满足。换句话说，就是透镜不能满足针孔模型假设，如下图显示（径向畸变），然而畸变并不仅仅是这样，还有其他的畸变：

矫正畸变的方法

径向畸变

上面的图片其实就是径向畸变，主要是由于透镜的形状改变了光线传播造成的畸变，这也是最常见的需要解决的畸变问题，主要分为桶形畸变和枕形畸变

径向畸变可以看成坐标点沿着长度方向发生了变化，也就是其距离原点的长度发生了变化。下面考虑在归一化平面（就是在相机平面上的点除以Z）上的一点$p$的坐标$[x,y]$径向畸变（这其实就是做了个泰勒级数展开）：

{ x d i s t o r t e d = x ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) y d i s t o r t e d = y ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 )

$$\begin{cases} x_{distorted}=x(1+k_1r^2+k_2r^4+k_3r^6)\\\\ y_{distorted}=y(1+k_1r^2+k_2r^4+k_3r^6) \end{cases}$$ ⎩ ⎨ ⎧​xdistorted​=x(1+k1​r2+k2​r4+k3​r6)ydistorted​=y(1+k1​r2+k2​r4+k3​r6)​

其中的$x_{distorted}$就是矫正畸变前的坐标值（就是已知的坐标），$k_1,k_2,k_3$都是畸变参数，$r$是该点到中心的距离，因此$r^2=x^2+y^2$

切向畸变

切向畸变一般是在机械组装的过程，透镜和成像平面不平行导致的。

切向畸变可以看成坐标点沿着长度方向发生了变化，也就是水平夹角发生了变化。下面依然考虑在归一化平面的切向畸变：

{ x d i s t o r t e d = x + [ 2 p 1 y + p 2 ( r 2 + 2 x 2 ) ] y d i s t o r t e d = y + [ p 1 ( r 2 + 2 y 2 ) + 2 p 2 x ]

$$\begin{cases} x_{distorted}=x+[2p_1y+p_2(r^2+2x^2)]\\\\ y_{distorted}=y+[p_1(r^2+2y^2)+2p_2x] \end{cases}$$ ⎩ ⎨ ⎧​xdistorted​=x+[2p1​y+p2​(r2+2x2)]ydistorted​=y+[p1​(r2+2y2)+2p2​x]​

跟径向畸变一样，其中$p_1,p_2$是畸变参数。

实际上畸变不仅仅只有这两种，但是影响较大的主要是这两种，或者说主要是径向畸变，而这些畸变参数也是跟内参一样，存放在yaml文件中

矫正畸变的过程

矫正畸变就是将径向畸变和切向畸变的转换公式合起来，重新求得每个像素的位置坐标：等式左边是已知的坐标，而右边是未知的，那么求解这个未知量的时候就非常麻烦，因此选择反过来求，假设已经有了矫正畸变后的图像，而这个图像每一点$[x,y]$我们就都知道了，将其带入公式，求得$x_{distorted},y_{distorted}$就是原来扭曲的图像里的坐标，那么将这个扭曲图像坐标对应的像素值赋值给矫正畸变后的图像$[x,y]$中，就实现了矫正畸变的流程，下面使用视觉slam十四讲中的代码进行简单阐述：

#include <opencv2/opencv.hpp>
#include <string>
using namespace std;
string image_file = "./imageBasics/distorted.png";   // 图像路径
int main(int argc, char **argv) {
    // 手撸去畸变流程
    // 畸变参数
    double k1 = -0.28340811, k2 = 0.07395907, p1 = 0.00019359, p2 = 1.76187114e-05;
    // 内参
    double fx = 458.654, fy = 457.296, cx = 367.215, cy = 248.375;

    cv::Mat image = cv::imread(image_file, 0);   // 图像是灰度图，CV_8UC1
    int rows = image.rows, cols = image.cols;
    cv::Mat image_undistort = cv::Mat(rows, cols, CV_8UC1);   // 假设的去畸变以后的图

    // 假设我们知道了去畸变图像，所以我们遍历去畸变后的图像坐标：
    // 1、遍历假设的去畸变后的图像坐标，求得这个坐标对应的在去畸变前的图像中的坐标
    // 2、将去畸变前的图像中的坐标对应的像素值，赋值给去畸变后的图像坐标的像素值
    // 3、每个去畸变后的图像都经过上面的流程，因此每个去畸变后的像素值就都有了
    for (int v = 0; v < rows; v++) {
        for (int u = 0; u < cols; u++) {
            // 这里是将去畸变的图像坐标，转换成了归一化坐标
            // 因为畸变的公式都是对归一化坐标进行计算的
            double x = (u - cx) / fx, y = (v - cy) / fy;
            // 下面就是畸变的公式，就是将径向畸变和切向畸变和到一起
            double r = sqrt(x * x + y * y);
            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;
            // 求得坐标值，依然是归一化坐标，因此需要转换成像素坐标
            // 此时需要注意，得到的像素坐标实际上是去畸变之前的图像坐标
            double u_distorted = fx * x_distorted + cx;
            double v_distorted = fy * y_distorted + cy;

            // 赋值 (最近邻插值)
            if (u_distorted >= 0 && v_distorted >= 0 && u_distorted < cols && v_distorted < rows) {
				// 将去畸变前的图像中的坐标对应的像素值，赋值给去畸变后的图像坐标的像素值
                // 所以看的是将v_distorted，u_distorted中在去畸变前的图像中的像素值
                // 赋值给v,u去畸变后的图像中的像素值
                image_undistort.at<uchar>(v, u) = image.at<uchar>((int) v_distorted, (int) u_distorted);
            } else {
                image_undistort.at<uchar>(v, u) = 0;
            }
        }
    }
    // 画图去畸变后图像
    cv::imshow("distorted", image);
    cv::imshow("undistorted", image_undistort);
    cv::waitKey();
    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50

上面的代码可以用下面的图简单展示：

其中像素坐标转换成归一化坐标就是将上面相机成像的公式反过来使用：

{ u = f x X Z + c x v = f y Y Z + c y ⇔ { x = ( u − c x ) / f x y = ( u − c y ) / f y

$$\begin{cases} u = f_x\frac{X}{Z} + c_x\\\\ v = f_y\frac{Y}{Z} + c_y \end{cases}$$ \Leftrightarrow $$\begin{cases} x = (u-c_x)/f_x\\\\ y = (u-c_y)/f_y \end{cases}$$ ⎩ ⎨ ⎧​u=fx​ZX​+cx​v=fy​ZY​+cy​​⇔⎩ ⎨ ⎧​x=(u−cx​)/fx​y=(u−cy​)/fy​​

综合考虑成像过程

• 世界坐标系下的点$P$，坐标为$P_w$
• 将点由世界坐标系转换成相机坐标系：$P_c^{\prime }=R{P}_w+t$
• 将相机坐标系下的点投影到归一化平面$Z=1$上：$P_c=[X/Z,Y/Z,1]$
• 矫正畸变，根据畸变参数计算矫正后的坐标，得到$P_{c-distorted}$
• 经过内参转换，使其对应到像素坐标：$P_{uv}=K{P}_{c-distorted}$

标定相机

上面的成像过程中，无论是内参还是畸变参数，都不是随便取的，都是经过相机标定确定的。对于针孔相机模型中，需要确定$f_x,f_y,c_x,c_y,k_1,k_2,p_1,p_2,k_3$一共9个参数，其中前4个是内参，后5个是畸变参数（$k_3$放在最后是因为有时候并不会去考虑$k_3$，因为泰勒展开到$r_4$的精度就够了）

标定的过程较为复杂，其中涉及到H矩阵的计算，因为不在这里阐述，放几个我觉得不错的讲解：

最详细、最完整的相机标定讲解
相机标定究竟在标定什么？
从零开始学习「张氏相机标定法」（二）单应矩阵

ORBSLAM2中矫正畸变

OpenCV中矫正畸变函数

ORBSLAM2中使用了OpenCV库中的函数，对图像进行去畸变，主要有两种函数：

• 使用 initUndistortRectifyMap 函数得到映射参数，再使用 remap 函数将畸变的图像映射到新的图像中，得到去畸变的效果。

//计算无畸变和修正转换映射
//参数很多，但是除了输入的映射表，剩下都是配置文件给出的标定好的参数
cv::initUndistortRectifyMap(            
    K_l,       //输入的相机内参矩阵 (单目标定阶段得到的相机内参矩阵)
    D_l,       //单目标定阶段得到的相机的去畸变参数
    R_l,       //可选的修正变换矩阵,3*3, 从 cv::stereoRectify 得来
    P_l.rowRange(0,3).colRange(0,3),    //新的相机内参矩阵
    cv::Size(cols_l,rows_l),            //在去畸变之前的图像尺寸
    CV_32F,                             //第一个输出映射的类型
    M1l,                                //第一个输出映射表
    M2l);                               //第二个输出映射
//参数R_l是第一个相机和第二个相机之间的旋转矩阵，可以设置为空，就是单位矩阵
//参数P_l可以理解为对于双目相机中假想的一个相机的参数
//上面函数主要得到的就是M1l和M2l
//下面就是使用这两个映射表，将图像映射到新的图像中
cv::remap(              //重映射，就是把一幅图像中某位置的像素放置到另一个图片指定位置的过程。
    imLeft,             //输入图像
    imLeftRect,         //输出图像
    M1l,                //第一个映射矩阵表
    M2l,                //第二个映射矩阵
    cv::INTER_LINEAR);  //插值方式
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

• 使用 undistortPoints 函数，只对我们感兴趣的点（特征点）进行去畸变

cv::undistortPoints(	
    mat,				//输入的特征点坐标
    mat,				//输出的校正后的特征点坐标覆盖原矩阵
    mK,					//相机的内参数矩阵
    mDistCoef,			//相机畸变参数矩阵
    cv::Mat(),			//一个空矩阵，对应为函数原型中的R
    mK); 				//新内参数矩阵，对应为函数原型中的P（这里就设置为内参矩阵）
1
2
3
4
5
6
7

ORBSLAM2的畸变函数

Mono单目相机

单目相机的去畸变是在提取特征点之后，仅对特征点进行去畸变操作，使用的undistortPoints 函数。

这里没有选择先将图像去畸变后提取特征点，而是选择仅对特征点进行去畸变，主要是为了减少计算量。然而先提取特征点会出现的问题是，后面的图像可能跟踪不到这个点，因为在畸变的时候计算特征点的描述子会有些许差别。ORBSLAM2中在单目相机使用的先提取特征点，还有一个原因就是对于畸变不大的相机，可以这么操作（毕竟slam要求实时性），而对于畸变较大的相机（广角相机或者鱼眼相机等），就需要先去畸变。

Stereo双目相机

双目相机的去畸变是在提取特征之前，对整个图像进行去畸变操作，使用 initUndistortRectifyMap 和 remap 函数。

注意：initUndistortRectifyMap 是求映射表的，而对于相同的相机，这个映射表只需要求解一次就够了，因此在ORBSLAM2中可以看到，initUndistortRectifyMap 函数仅仅在开始运行过，之后对每个图像的遍历中使用的都是remap 函数，这也是这种函数与undistortPoints 函数不同的地方

下面是对矫正畸变函数的讲解：

OpenCV畸变校正原理以及损失有效像素原理分析
「OpenCV3.4」图像去畸变代码实战
Opencv中的两种去畸变函数
【ORB_SLAM2源码解读】EuRoc双目数据集图像去畸变（2）
OpenCV笔记（10） 相机模型与标定