一、什么是畸变
由于添加镜头,会改变光线的传播路径,导致投影成像的位置与实际的像坐标不一致。畸变一般分为径向畸变和切向畸变,具体的讲解请自行百度,此处不做详细讲解。
二、为什么要去畸变
单目相机的成像过程如下:
1、世界坐标下有一个Pw点
2、通过外参R将Pw点转换到Pc相机坐标系下
3、将Pc点归一化投影到Z=1的平面上
4、若出现畸变,计算畸变后的坐标
5、再通过相机的内参矩阵得到图像坐标u,v
通过上面的大致步骤我们知道,如果要正确的得到像素坐标,就需要去畸变。如果不去畸变得到的点会出现很大的误差,多后端优化和位姿估计会有很大的影响。
那我们已知去畸变之后的图像,如何得到之前步骤3的Pc坐标呢?
通过去畸变的公式我们很容易得到除去畸变后的正确坐标。但是如果我们需要通过三角测量来计算三维空间下的点,就一定需要从步骤5反推回去。从公式看出,如果我们已知xd和yd(简写)如何计算出x,y呢?这是一个高阶方程如果用优化当然可以求解出来,但是这只是一个小环节没必要花费计算机太多资源去计算。而且特征点数目如果过多每一个都这样计算根本无法满足前端的实时性。vins就提出了一种高效的计算方法!
三、vins去畸变的思想
开始我们约定x_d、y_d表示经过步骤4计算出来去畸变的点。而x,y表示原始点(就是我们看见失真的图像点)。
首先在我们得到一张除去畸变后图像,A’(x_da,y_da)点,我们想知道它的原始坐标A(x_a,y_a)点。根据径向畸变的一个性质,如果说距离光心(图像中心点)越远,你的畸变点距离原始点则就越远。从上图我们可以看出A'A就是去畸变后的点和原始点之间的距离。vins是如何近似计算出A点的呢?步骤如下:
1、首先将A'点看为B点,B(x_b,y_b)点为原始点。那我们知道原始点根据图1的公式,我们很容易得到B'(x_db,y_db)点。
2、因为畸变的性质,B点明显更加靠近图像中心点。我们可以得出B'B<A'A
3、在从A'点以B'B距离增加得到C点,我又以C(x_c,y_c)点为原始点,通过图1公式计算出C'点。根据性质可以得B'B<C'C<A'A,因为C点比B点距离图像中心更远但是没有A点远。
4、再从A’点出发以C'C距离增加得到一个D(x_D,y_D)【注:以大写D区分去畸变点】,再通过公式得到D'点,根据性质B'B<C'C<D'D<A'A。依次循环,这样一直逼近A'A直到达到一个阈值则认为满足精度要求。
vins的作者认为循环8次则表示可以满足这个精度要求。我们懂了理论部分看代码就十分简单了,一下为原始代码,有笔者的注释加以理解。源代码中这个功能函数名为liftProjective
/// @brief 得到原始坐标
/// @param p 除去畸变的点A‘
/// @param P 得到原始点A
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;
// m_inv_K11 = 1.0 / mParameters.fx();
// m_inv_K13 = -mParameters.cx() / mParameters.fx();
// m_inv_K22 = 1.0 / mParameters.fy();
// m_inv_K23 = -mParameters.cy() / mParameters.fy();
// Lift points to normalised plane
//从2纬转化为3纬,这里为了方便计算提前将可以计算的数计算出来
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)
{
double k1 = mParameters.k1();
double k2 = mParameters.k2();
double p1 = mParameters.p1();
double p2 = mParameters.p2();
// Apply inverse distortion model
// proposed by Heikkila
mx2_d = mx_d*mx_d;
my2_d = my_d*my_d;
mxy_d = mx_d*my_d;
rho2_d = mx2_d+my2_d;
rho4_d = rho2_d*rho2_d;
radDist_d = k1*rho2_d+k2*rho4_d;
Dx_d = mx_d*radDist_d + p2*(rho2_d+2*mx2_d) + 2*p1*mxy_d;
Dy_d = my_d*radDist_d + p1*(rho2_d+2*my2_d) + 2*p2*mxy_d;
inv_denom_d = 1/(1+4*k1*rho2_d+6*k2*rho4_d+8*p1*my_d+8*p2*mx_d);
mx_u = mx_d - inv_denom_d*Dx_d;
my_u = my_d - inv_denom_d*Dy_d;
}
else
{
// Recursive distortion model
//作者定义8次循环可以达到精度
int n = 8;
//定义A'A
Eigen::Vector2d d_u;
//通过公式计算出A'A
distortion(Eigen::Vector2d(mx_d, my_d), d_u);
// Approximate value
mx_u = mx_d - d_u(0);
my_u = my_d - d_u(1);
for (int i = 1; i < n; ++i)
{
//循环计算
distortion(Eigen::Vector2d(mx_u, my_u), d_u);
mx_u = mx_d - d_u(0);
my_u = my_d - d_u(1);
}
}
}
// Obtain a projective ray
//返回A点的归一化坐标
P << mx_u, my_u, 1.0;
}
图片来源: