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个畸变系数)。
成像过程
cv::undistortPoints()代码实现
现在我们来看一下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::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()函数
首先是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.