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^{\prime },y^{\prime }{)}^T$：
$$\begin{array}{r}\left(\begin{array}{c}{x}^{\prime }\\ y^{\prime }\end{array}\right)=\left(\begin{array}{c}x/z\\ y/z\end{array}\right)\end{array}\text{\hspace{1em}(1)}$$
然后将无畸变的归一化坐标$(x^{\prime },y^{\prime }{)}^T$转化为有畸变的归一化坐标$(x^{\prime \prime },y^{\prime \prime }{)}^T$：
$$\begin{array}{r}\left(\begin{array}{c}{x}^{\prime \prime }\\ y^{\prime \prime }\end{array}\right)=\left(\begin{array}{c}{x}^{\prime }\frac{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}+2p_1{x}^{\prime }{y}^{\prime }+p_2(r^2+2x^{\prime 2})+s_1{r}^2+s_2{r}^4\\ y^{\prime }\frac{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+2y^{\prime 2})+2p_2{x}^{\prime }{y}^{\prime }+s_3{r}^2+s_4{r}^4\end{array}\right)\end{array}\text{\hspace{1em}(2)}$$
其中$r^2=x^{\prime 2}+y^{\prime 2}$。最后将有畸变的归一化坐标$(x^{\prime \prime },y^{\prime \prime }{)}^T$转化为像素坐标$(u,v{)}^T$：
$$\begin{array}{r}\left(\begin{array}{c}u\\ v\end{array}\right)=\left(\begin{array}{c}{f}_x\ast x^{\prime \prime }+c_x\\ f_y\ast y^{\prime \prime }+c_y\end{array}\right)\end{array}\text{\hspace{1em}(3)}$$
这里的像素坐标$(u,v{)}^T$就是带有畸变的图像上的像素坐标。

cv::undistortPoints()代码实现

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

首先第一步就是将像素坐标$(u,v{)}^T$转化为有畸变的归一化坐标$(x^{\prime \prime },y^{\prime \prime }{)}^T$，就是公式(3)的逆过程：
$$\begin{array}{r}\left(\begin{array}{c}{x}^{\prime \prime }\\ y^{\prime \prime }\end{array}\right)=\left(\begin{array}{c}(u-c_x)/f_x\\ (v-c_y)/f_y\end{array}\right)\end{array}\text{\hspace{1em}(4)}$$
然后就是要将有畸变的归一化坐标$(x^{\prime \prime },y^{\prime \prime }{)}^T$转化为无畸变的归一化坐标$(x^{\prime },y^{\prime }{)}^T$，理论上应该是公式(2)的逆过程。但是可以看到，这个逆过程很复杂，OpenCV中采用了近似迭代的方法来求解。

首先用一个临时变量$(x_0,y_0{)}^T$来存储有畸变的归一化坐标，即$(x_0,y_0{)}^T=(x^{\prime \prime },y^{\prime \prime }{)}^T$。然后是一个迭代过程：
$$\begin{array}{r}\left(\begin{array}{c}{x}^{\prime \prime }\\ y^{\prime \prime }\end{array}\right)⟸\left(\begin{array}{c}\{x_0-[2p_1{x}^{\prime \prime }{y}^{\prime \prime }+p_2(r^2+2x^{\prime \prime 2})+s_1{r}^2+s_2{r}^4]\}\frac{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+2y^{\prime \prime 2})+2p_2{x}^{\prime \prime }{y}^{\prime \prime }+s_3{r}^2+s_4{r}^4]\}\frac{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}\end{array}\right)\end{array}\text{\hspace{1em}(5)}$$其中$r^2=x^{\prime \prime 2}+y^{\prime \prime 2}$。这个迭代过程中，$(x_0,y_0{)}^T$保持不变，而$(x^{\prime \prime },y^{\prime \prime }{)}^T$不断被更新。达到迭代终止条件后，无畸变的归一化坐标$(x^{\prime },y^{\prime }{)}^T=(x^{\prime \prime },y^{\prime \prime }{)}^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_2$、$p_1$、$p_2$这4个畸变系数。首先我们来看一下正向过程，即将无畸变的归一化坐标$(x^{\prime },y^{\prime }{)}^T$转化为有畸变的归一化坐标$(x^{\prime \prime },y^{\prime \prime }{)}^T$的公式：
$$\begin{array}{rl}\left(\begin{array}{c}{x}^{\prime \prime }\\ y^{\prime \prime }\end{array}\right)& =\left(\begin{array}{c}{x}^{\prime }(1+k_1{r}^2+k_2{r}^4)+2p_1{x}^{\prime }{y}^{\prime }+p_2(r^2+2x^{\prime 2})\\ y^{\prime }(1+k_1{r}^2+k_2{r}^4)+p_1(r^2+2y^{\prime 2})+2p_2{x}^{\prime }{y}^{\prime }\end{array}\right)\\ & =\left(\begin{array}{c}{x}^{\prime }+x^{\prime }(k_1{r}^2+k_2{r}^4)+2p_1{x}^{\prime }{y}^{\prime }+p_2(r^2+2x^{\prime 2})\\ y^{\prime }+y^{\prime }(k_1{r}^2+k_2{r}^4)+p_1(r^2+2y^{\prime 2})+2p_2{x}^{\prime }{y}^{\prime }\end{array}\right)\\ & =\left(\begin{array}{c}{x}^{\prime }+\Delta x^{\prime }\\ y^{\prime }+\Delta y^{\prime }\end{array}\right)\end{array}\text{\hspace{1em}(6)}$$其中$r^2=x^{\prime 2}+y^{\prime 2}$。

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

首先用一个临时变量$(x_0,y_0{)}^T$来存储有畸变的归一化坐标，即$(x_0,y_0{)}^T=(x^{\prime \prime },y^{\prime \prime }{)}^T$。然后是一个迭代过程：
$$\begin{array}{rl}\left(\begin{array}{c}{x}^{\prime \prime }\\ y^{\prime \prime }\end{array}\right)& ⟸\left(\begin{array}{c}{x}_0-[x^{\prime \prime }(k_1{r}^2+k_2{r}^4)+2p_1{x}^{\prime \prime }{y}^{\prime \prime }+p_2(r^2+2x^{\prime \prime 2})]\\ y_0-[y^{\prime \prime }(k_1{r}^2+k_2{r}^4)+p_1(r^2+2y^{\prime \prime 2})+2p_2{x}^{\prime \prime }{y}^{\prime \prime }]\end{array}\right)\\ & ⟸\left(\begin{array}{c}{x}_0-\Delta x^{\prime \prime }\\ y_0-\Delta y^{\prime \prime }\end{array}\right)\end{array}\text{\hspace{1em}(7)}$$其中$r^2=x^{\prime \prime 2}+y^{\prime \prime 2}$。这个迭代过程中，$(x_0,y_0{)}^T$保持不变，而$(x^{\prime \prime },y^{\prime \prime }{)}^T$不断被更新。达到迭代终止条件后，无畸变的归一化坐标$(x^{\prime },y^{\prime }{)}^T=(x^{\prime \prime },y^{\prime \prime }{)}^T$。

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

由于只讨论Pinhole相机类型，则liftProjective()函数的实现文件在PinholeCamera.cc中。liftProjective()函数中会调用同文件中的distortion()函数。distortion()函数的作用是计算公式(7)中的$\Delta x^{\prime \prime }$和$\Delta y^{\prime \prime }$。

首先是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.