VINS_Mono代码阅读记录--liftProjective函数

在VINS_Mono中的视觉前端里,经常会使用liftProjective这个函数,如下所示:

for (unsigned int i = 0; i < cur_pts.size(); i++)
{
    Eigen::Vector2d a(cur_pts[i].x, cur_pts[i].y); //a为像素坐标
    Eigen::Vector3d b;
    m_camera->liftProjective(a, b); //b为无畸变的归一化坐标
    cur_un_pts.push_back(cv::Point2f(b.x() / b.z(), b.y() / b.z()));
    //...
}

这个函数的作用是将像素坐标转化为无畸变的归一化坐标

那么这个函数具体是怎么实现的呢?其实,OpenCV也有相应的函数是将像素坐标转化为无畸变的归一化坐标,这个函数是cv::undistortPoints(),那么OpenCV又是怎么实现这个函数的呢?在看这个函数之前,我们先看下Pinhole相机的一个成像过程(参考来源于OpenCV,同时只讨论前12个畸变系数)。

成像过程

假设相机坐标系下有一空间点 ( x , y , z ) T (x,y,z)^{T} (x,y,z)T,先得到无畸变的归一化坐标 ( x ′ , y ′ ) T (x',y')^{T} (x,y)T
( x ′ y ′ ) = ( x / z y / z ) (1) \begin{aligned} \begin{pmatrix} x'\\y' \end{pmatrix} = \begin{pmatrix} x/z\\y/z \end{pmatrix} \tag{1} \end{aligned} (xy)=(x/zy/z)(1)
然后将无畸变的归一化坐标 ( x ′ , y ′ ) T (x',y')^{T} (x,y)T转化为有畸变的归一化坐标 ( x ′ ′ , y ′ ′ ) T (x'',y'')^{T} (x,y)T
( x ′ ′ y ′ ′ ) = ( x ′ 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 1 + k 4 r 2 + k 5 r 4 + k 6 r 6 + 2 p 1 x ′ y ′ + p 2 ( r 2 + 2 x ′ 2 ) + s 1 r 2 + s 2 r 4 y ′ 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 1 + k 4 r 2 + k 5 r 4 + k 6 r 6 + p 1 ( r 2 + 2 y ′ 2 ) + 2 p 2 x ′ y ′ + s 3 r 2 + s 4 r 4 ) (2) \begin{aligned} \begin{pmatrix} x''\\y'' \end{pmatrix} = \begin{pmatrix} x' \frac{1+k_1r^2+k_2r^4+k_3r^6}{1+k_4r^2+k_5r^4+k_6r^6}+2p_1x'y'+p_2(r^2+2x'^2)+s_1r^2+s_2r^4 \\ y' \frac{1+k_1r^2+k_2r^4+k_3r^6}{1+k_4r^2+k_5r^4+k_6r^6}+p_1(r^2+2y'^2)+2p_2x'y'+s_3r^2+s_4r^4 \end{pmatrix} \tag{2} \end{aligned} (xy)=(x1+k4r2+k5r4+k6r61+k1r2+k2r4+k3r6+2p1xy+p2(r2+2x2)+s1r2+s2r4y1+k4r2+k5r4+k6r61+k1r2+k2r4+k3r6+p1(r2+2y2)+2p2xy+s3r2+s4r4)(2)
其中 r 2 = x ′ 2 + y ′ 2 r^2=x'^2+y'^2 r2=x2+y2。最后将有畸变的归一化坐标 ( x ′ ′ , y ′ ′ ) T (x'',y'')^{T} (x,y)T转化为像素坐标 ( u , v ) T (u,v)^{T} (u,v)T
( u v ) = ( f x ∗ x ′ ′ + c x f y ∗ y ′ ′ + c y ) (3) \begin{aligned} \begin{pmatrix} u\\v \end{pmatrix} = \begin{pmatrix} f_x * x'' + c_x \\ f_y * y'' + c_y \end{pmatrix} \tag{3} \end{aligned} (uv)=(fxx+cxfyy+cy)(3)
这里的像素坐标 ( u , v ) T (u,v)^{T} (u,v)T就是带有畸变的图像上的像素坐标。

cv::undistortPoints()代码实现

那么cv::undistortPoints()这个函数是怎么将像素坐标 ( u , v ) T (u,v)^{T} (u,v)T转化到无畸变的归一化坐标 ( x ′ , y ′ ) T (x',y')^{T} (x,y)T呢?

首先第一步就是将像素坐标 ( u , v ) T (u,v)^{T} (u,v)T转化为有畸变的归一化坐标 ( x ′ ′ , y ′ ′ ) T (x'',y'')^{T} (x,y)T,就是公式(3)的逆过程:
( x ′ ′ y ′ ′ ) = ( ( u − c x ) / f x ( v − c y ) / f y ) (4) \begin{aligned} \begin{pmatrix} x''\\y'' \end{pmatrix} = \begin{pmatrix} (u-c_x)/f_x \\ (v-c_y)/f_y \end{pmatrix} \tag{4} \end{aligned} (xy)=((ucx)/fx(vcy)/fy)(4)
然后就是要将有畸变的归一化坐标 ( x ′ ′ , y ′ ′ ) T (x'',y'')^{T} (x,y)T转化为无畸变的归一化坐标 ( x ′ , y ′ ) T (x',y')^{T} (x,y)T,理论上应该是公式(2)的逆过程。但是可以看到,这个逆过程很复杂,OpenCV中采用了近似迭代的方法来求解。

首先用一个临时变量 ( x 0 , y 0 ) T (x_0,y_0)^T (x0,y0)T来存储有畸变的归一化坐标,即 ( x 0 , y 0 ) T = ( x ′ ′ , y ′ ′ ) T (x_0,y_0)^T=(x'',y'')^T (x0,y0)T=(x,y)T。然后是一个迭代过程:
( x ′ ′ y ′ ′ ) ⟸ ( { x 0 − [ 2 p 1 x ′ ′ y ′ ′ + p 2 ( r 2 + 2 x ′ ′ 2 ) + s 1 r 2 + s 2 r 4 ] } 1 + k 4 r 2 + k 5 r 4 + k 6 r 6 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 { y 0 − [ p 1 ( r 2 + 2 y ′ ′ 2 ) + 2 p 2 x ′ ′ y ′ ′ + s 3 r 2 + s 4 r 4 ] } 1 + k 4 r 2 + k 5 r 4 + k 6 r 6 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) (5) \begin{aligned} \begin{pmatrix} x''\\y'' \end{pmatrix} \overset{}{\Longleftarrow} \begin{pmatrix} \{x_0 - [2p_1x''y''+p_2(r^2+2x''^2)+s_1r^2+s_2r^4]\} \frac{1+k_4r^2+k_5r^4+k_6r^6}{1+k_1r^2+k_2r^4+k_3r^6} \\ \{y_0 - [p_1(r^2+2y''^2)+2p_2x''y''+s_3r^2+s_4r^4]\} \frac{1+k_4r^2+k_5r^4+k_6r^6}{1+k_1r^2+k_2r^4+k_3r^6} \end{pmatrix} \tag{5} \end{aligned} (xy)({x0[2p1xy+p2(r2+2x2)+s1r2+s2r4]}1+k1r2+k2r4+k3r61+k4r2+k5r4+k6r6{y0[p1(r2+2y2)+2p2xy+s3r2+s4r4]}1+k1r2+k2r4+k3r61+k4r2+k5r4+k6r6)(5)其中 r 2 = x ′ ′ 2 + y ′ ′ 2 r^2=x''^2+y''^2 r2=x2+y2。这个迭代过程中, ( x 0 , y 0 ) T (x_0,y_0)^T (x0,y0)T保持不变,而 ( x ′ ′ , y ′ ′ ) T (x'',y'')^T (x,y)T不断被更新。达到迭代终止条件后,无畸变的归一化坐标 ( x ′ , y ′ ) T = ( x ′ ′ , y ′ ′ ) T (x',y')^{T}=(x'',y'')^T (x,y)T=(x,y)T

这种近似迭代的过程为什么可以这样算,我看到有人说可以参考文献[1]中的Backward Model这节或公式(16)的推导过程(我个人是看不太懂=。=!!)。

现在我们来看一下OpenCV中的具体实现。源代码在/modules/imgproc/src/undistort.cpp里面,而 cv::undistortPoints()函数中又调用了同一源文件下的cvUndistortPointsInternal()这个函数。在过滤掉一些不相关的代码后,下面是cvUndistortPointsInternal()这个函数的解析:

//首先获得相机参数和畸变系数
double fx;
double fy;
double ifx = 1./fx;
double ify = 1./fy;
double cx;
double cy;
//畸变系数共有14个,但我们这里只用前面12个系数
double k[14]; //(k1,k2,p1,p2[,k3[,k4,k5,k6[,s1,s2,s3,s4[,τx,τy]]]])

//以下只针对一个像素坐标进行说明
double x, y, x0 = 0, y0 = 0, u, v;
x = 得到像素坐标x;
y = 得到像素坐标y;
u = x; v = y; //此时u,v为像素坐标
x = (x - cx)*ifx; //此时x,y为有畸变的归一化坐标
y = (y - cy)*ify;

if( _distCoeffs ) {
    // compensate tilt distortion
    // 这里是剩下的两个畸变系数的逆过程,不使用,我注释了
    //cv::Vec3d vecUntilt = invMatTilt * cv::Vec3d(x, y, 1);
    //double invProj = vecUntilt(2) ? 1./vecUntilt(2) : 1;
    //x0 = x = invProj * vecUntilt(0);
    //y0 = y = invProj * vecUntilt(1);
    x0 = x; //此时x0,y0存储最初的有畸变的归一化坐标
    y0 = y;
	
    double error = std::numeric_limits<double>::max(); //重投影误差,预设为无穷大
    // compensate distortion iteratively
	// 从这里开始进行公式(5)的迭代过程
    for( int j = 0; ; j++ )
    {
    	//如果迭代次数大于等于criteria.maxCount,则退出迭代。criteria.maxCount为5
        if ((criteria.type & cv::TermCriteria::COUNT) && j >= criteria.maxCount)
            break;
        //如果重投影误差小于criteria.epsilon,则退出迭代。criteria.epsilon为0.01
        if ((criteria.type & cv::TermCriteria::EPS) && error < criteria.epsilon)
            break;
        //这里的x,y为最始的有畸变的归一化坐标,它们会不断被更新,对应公式(5)中的x''和y''
        double r2 = x*x + y*y;
        //这里的icdist相当于公式(5)中最右边的乘子
        double icdist = (1 + ((k[7]*r2 + k[6])*r2 + k[5])*r2)/(1 + ((k[4]*r2 + k[1])*r2 + k[0])*r2);
        //这里的deltaX和deltaY相当于公式(5)中的中括号对应的内容
        double deltaX = 2*k[2]*x*y + k[3]*(r2 + 2*x*x)+ k[8]*r2+k[9]*r2*r2;
        double deltaY = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y+ k[10]*r2+k[11]*r2*r2;
        //这里的x0和y0为最初的有畸变的归一化坐标,迭代过程中保持不变
        //而x和y则会不断被更新,其对应公式(5)中的x''和y''
        x = (x0 - deltaX)*icdist;
        y = (y0 - deltaY)*icdist;
        //更新后的x和y是可能的、无畸变的归一化坐标

		//下面是重投影误差的计算
		//方法是将更新的x和y坐标重新经过畸变模型变为有畸变的归一化坐标,
		//再经过相机内参得到像素坐标,然后与原本的像素坐标进行误差计算
        if(criteria.type & cv::TermCriteria::EPS)
        {
            double r4, r6, a1, a2, a3, cdist, icdist2;
            double xd, yd, xd0, yd0;
            cv::Vec3d vecTilt;
			//这里的x和y是被更新的、可能的、无畸变的归一化坐标
			//下面是进行畸变过程,即公式(2)的计算
            r2 = x*x + y*y;
            r4 = r2*r2;
            r6 = r4*r2;
            a1 = 2*x*y;
            a2 = r2 + 2*x*x;
            a3 = r2 + 2*y*y;
            cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6;
            icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6);

            xd0 = x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4;
            yd0 = y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4;
            //此时,xd0和yd0为有畸变的归一化坐标

			//下面是剩下的两个畸变系数的正过程,不使用,我注释了
            //vecTilt = matTilt*cv::Vec3d(xd0, yd0, 1);
            //invProj = vecTilt(2) ? 1./vecTilt(2) : 1;
            //xd = invProj * vecTilt(0);
            //yd = invProj * vecTilt(1);
            xd = xd0; //此时xd和yd为有畸变的归一化坐标
            yd = yd0;

			//经过相机参数后,x_proj和y_proj为重投影的像素坐标
            double x_proj = xd*fx + cx;
            double y_proj = yd*fy + cy;
			//将重投影的像素坐标和原本的像素坐标(u,v)进行误差计算
            error = sqrt( pow(x_proj - u, 2) + pow(y_proj - v, 2) );
        }
        //在下一次的迭代之前,会先判断迭代次数和误差是否达到阈值,若达到则停止迭代
    }
    //跳出迭代过程后,x和y就是要求的、经过迭代的、近似的、无畸变的归一化坐标
}

从OpenCV的代码中可以看到,若不考虑剩下的两个畸变系数,则迭代过程与博文中的迭代过程一致。而每次迭代都会判断是否达到迭代次数或重投影误差是否达到阈值。

VINS_Mono中的liftProjective()函数

接下来,我们看一下VINS_Mono中Pinhole相机类型是如何把像素坐标转化为无畸变的归一化坐标

代码中只使用了 k 1 k_1 k1 k 2 k_2 k2 p 1 p_1 p1 p 2 p_2 p2这4个畸变系数。首先我们来看一下正向过程,即将无畸变的归一化坐标 ( x ′ , y ′ ) T (x',y')^{T} (x,y)T转化为有畸变的归一化坐标 ( x ′ ′ , y ′ ′ ) T (x'',y'')^{T} (x,y)T的公式:
( x ′ ′ y ′ ′ ) = ( x ′ ( 1 + k 1 r 2 + k 2 r 4 ) + 2 p 1 x ′ y ′ + p 2 ( r 2 + 2 x ′ 2 ) y ′ ( 1 + k 1 r 2 + k 2 r 4 ) + p 1 ( r 2 + 2 y ′ 2 ) + 2 p 2 x ′ y ′ ) = ( x ′ + x ′ ( k 1 r 2 + k 2 r 4 ) + 2 p 1 x ′ y ′ + p 2 ( r 2 + 2 x ′ 2 ) y ′ + y ′ ( k 1 r 2 + k 2 r 4 ) + p 1 ( r 2 + 2 y ′ 2 ) + 2 p 2 x ′ y ′ ) = ( x ′ + Δ x ′ y ′ + Δ y ′ ) (6) \begin{aligned} \begin{pmatrix} x''\\y'' \end{pmatrix} & = \begin{pmatrix} x' (1+k_1r^2+k_2r^4)+2p_1x'y'+p_2(r^2+2x'^2) \\ y' (1+k_1r^2+k_2r^4)+p_1(r^2+2y'^2)+2p_2x'y' \end{pmatrix} \\ & = \begin{pmatrix} x'+ x' (k_1r^2+k_2r^4)+2p_1x'y'+p_2(r^2+2x'^2) \\ y'+ y' (k_1r^2+k_2r^4)+p_1(r^2+2y'^2)+2p_2x'y' \end{pmatrix} \\ & = \begin{pmatrix} x'+ \Delta x' \\ y'+ \Delta y' \end{pmatrix} \tag{6} \end{aligned} (xy)=(x(1+k1r2+k2r4)+2p1xy+p2(r2+2x2)y(1+k1r2+k2r4)+p1(r2+2y2)+2p2xy)=(x+x(k1r2+k2r4)+2p1xy+p2(r2+2x2)y+y(k1r2+k2r4)+p1(r2+2y2)+2p2xy)=(x+Δxy+Δy)(6)其中 r 2 = x ′ 2 + y ′ 2 r^2=x'^2+y'^2 r2=x2+y2

接下来看一下VINS_Mono中实际的逆向过程,即有畸变的归一化坐标 ( x ′ ′ , y ′ ′ ) T (x'',y'')^{T} (x,y)T转化为无畸变的归一化坐标 ( x ′ , y ′ ) T (x',y')^{T} (x,y)T的迭代过程。

首先用一个临时变量 ( x 0 , y 0 ) T (x_0,y_0)^T (x0,y0)T来存储有畸变的归一化坐标,即 ( x 0 , y 0 ) T = ( x ′ ′ , y ′ ′ ) T (x_0,y_0)^T=(x'',y'')^T (x0,y0)T=(x,y)T。然后是一个迭代过程:
( x ′ ′ y ′ ′ ) ⟸ ( x 0 − [ x ′ ′ ( k 1 r 2 + k 2 r 4 ) + 2 p 1 x ′ ′ y ′ ′ + p 2 ( r 2 + 2 x ′ ′ 2 ) ] y 0 − [ y ′ ′ ( k 1 r 2 + k 2 r 4 ) + p 1 ( r 2 + 2 y ′ ′ 2 ) + 2 p 2 x ′ ′ y ′ ′ ] ) ⟸ ( x 0 − Δ x ′ ′ y 0 − Δ y ′ ′ ) (7) \begin{aligned} \begin{pmatrix} x''\\y'' \end{pmatrix} &\overset{}{\Longleftarrow} \begin{pmatrix} x_0 - [x'' (k_1r^2+k_2r^4)+2p_1x''y''+p_2(r^2+2x''^2)] \\ y_0 - [y'' (k_1r^2+k_2r^4)+p_1(r^2+2y''^2)+2p_2x''y''] \end{pmatrix} \\ & \overset{}{\Longleftarrow} \begin{pmatrix} x_0 - \Delta x'' \\ y_0 - \Delta y'' \end{pmatrix} \tag{7} \end{aligned} (xy)(x0[x(k1r2+k2r4)+2p1xy+p2(r2+2x2)]y0[y(k1r2+k2r4)+p1(r2+2y2)+2p2xy])(x0Δxy0Δy)(7)其中 r 2 = x ′ ′ 2 + y ′ ′ 2 r^2=x''^2+y''^2 r2=x2+y2。这个迭代过程中, ( x 0 , y 0 ) T (x_0,y_0)^T (x0,y0)T保持不变,而 ( x ′ ′ , y ′ ′ ) T (x'',y'')^T (x,y)T不断被更新。达到迭代终止条件后,无畸变的归一化坐标 ( x ′ , y ′ ) T = ( x ′ ′ , y ′ ′ ) T (x',y')^{T}=(x'',y'')^T (x,y)T=(x,y)T

把VINS_Mono的正向过程和逆向过程与OpenCV的进行对比可以发现略有不同。在正向过程中,VINS_Mono中把 x ′ ( 1 + k 1 r 2 + k 2 r 4 ) x'(1+k_1r^2+k_2r^4) x(1+k1r2+k2r4)中的1单独乘了出来变为 x ′ + x ′ ( k 1 r 2 + k 2 r 4 ) x'+x'(k_1r^2+k_2r^4) x+x(k1r2+k2r4),然后把 x ′ ( k 1 r 2 + k 2 r 4 ) x'(k_1r^2+k_2r^4) x(k1r2+k2r4)这一项与后面的几个加项组成了 Δ x ′ \Delta x' Δx。因此,逆向过程只要减去一个 Δ x ′ ′ \Delta x'' Δx即可。同理对y也有。

由于只讨论Pinhole相机类型,则liftProjective()函数的实现文件在PinholeCamera.cc中。liftProjective()函数中会调用同文件中的distortion()函数。distortion()函数的作用是计算公式(7)中的 Δ x ′ ′ \Delta x'' Δx Δ y ′ ′ \Delta y'' Δy

首先是liftProjective()函数的解析:

//首先从函数参数来看,它的输入是一个二维的像素坐标,它的输出是一个三维的无畸变的归一化坐标
void PinholeCamera::liftProjective(const Eigen::Vector2d& p, Eigen::Vector3d& P) const
{
    double mx_d, my_d,mx2_d, mxy_d, my2_d, mx_u, my_u;
    double rho2_d, rho4_d, radDist_d, Dx_d, Dy_d, inv_denom_d;
    //double lambda;

    // Lift points to normalised plane
    // 这里的mx_d和my_d就是最初的、有畸变的归一化坐标
    mx_d = m_inv_K11 * p(0) + m_inv_K13;
    my_d = m_inv_K22 * p(1) + m_inv_K23;

    if (m_noDistortion)
    { //如果没有畸变,则直接返回归一化坐标
        mx_u = mx_d;
        my_u = my_d;
    }
    else
    { //如果有畸变,则计算无畸变的归一化坐标
        if (0) //if语句为0,那就执行else语句
        {
            //......
        }
        else
        {
            // Recursive distortion model
            int n = 8; //迭代8次
            Eigen::Vector2d d_u;
            //distortion()函数的作用是计算公式(7)中的delta_x''和delta_y''
            distortion(Eigen::Vector2d(mx_d, my_d), d_u);
            //下面是公式(7)的计算。此处的mx_d和my_d相当于公式(7)中的x0和y0,它们在迭代过程保持不变
            mx_u = mx_d - d_u(0);
            my_u = my_d - d_u(1);
			//此时的mx_u和my_u相当于公式(7)中被更新了一次的x''和y''
            for (int i = 1; i < n; ++i)
            { //上面已经迭代了1次,for循环中迭代7次
            	//此处distortion()函数的输入是使用被更新的mx_u和my_u(即x''和y'')来计算新的delta_x''和delta_y''
                distortion(Eigen::Vector2d(mx_u, my_u), d_u);
                //下面是公式(7)的计算。mx_d和my_d(即x0和y0)保持不变
                mx_u = mx_d - d_u(0);
                my_u = my_d - d_u(1);
            }
        }
    }

    // 迭代结束后,返回无畸变的归一化坐标
    P << mx_u, my_u, 1.0;
}

然后是distortion()函数的解析:

//输入参数p_u是公式(7)中不断被更新的x''和y'',输出参数d_u是公式(7)中的delta_x''和delta_y''
void PinholeCamera::distortion(const Eigen::Vector2d& p_u, Eigen::Vector2d& d_u) const
{
    double k1 = mParameters.k1();
    double k2 = mParameters.k2();
    double p1 = mParameters.p1();
    double p2 = mParameters.p2();

    double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;
	//下面是公式(7)中delta_x''和delta_y''的计算,对照公式看应该是很直观的了
    mx2_u = p_u(0) * p_u(0);
    my2_u = p_u(1) * p_u(1);
    mxy_u = p_u(0) * p_u(1);
    rho2_u = mx2_u + my2_u;
    rad_dist_u = k1 * rho2_u + k2 * rho2_u * rho2_u;
    d_u << p_u(0) * rad_dist_u + 2.0 * p1 * mxy_u + p2 * (rho2_u + 2.0 * mx2_u),
           p_u(1) * rad_dist_u + 2.0 * p2 * mxy_u + p1 * (rho2_u + 2.0 * my2_u);
}

从liftProjective()函数的实现代码中与OpenCV的代码比较可以看到,除了计算公式不太一样外,VINS_Mono是直接迭代8次后得到无畸变的归一化坐标,其中没有重投影误差的计算;而OpenCV是默认迭代5次,每次迭代后都会进行重投影误差的计算,并判断误差是否达到阈值,若达到则停止迭代。

参考文献

[1] Kannala, Juho, and Sami S. Brandt. “A generic camera model and calibration method for conventional, wide-angle, and fish-eye lenses.” IEEE transactions on pattern analysis and machine intelligence 28.8 (2006): 1335-1340.

你可能感兴趣的:(SLAM,slam)