1. What is distortion?
As the lens is added, the propagation path of the light will be changed, causing the position of the projectionimaging to be inconsistent with the actual image coordinates. Distortion is generally divided into radial distortion and tangential distortion. Please refer to Baidu for detailed explanation. I will not explain it in detail here.
2. Why should distortion be done?
The imaging process of the monocular camera is as follows:
1. There is a Pw point under world coordinates
2. Convert the Pw point to the Pc camera coordinate system through the external parameter R
3. Normalize the Pc point onto the plane of Z=1
4. If distortion occurs, calculate the distorted coordinates
5. Then obtain the image coordinates u, v through the internal parameter matrix of the camera.
Through the above general steps, we know that if we want to obtain the pixel coordinates correctly, we need to remove distortion. If the points obtained are not distorted, there will be a large error, and multi-backend optimization and pose estimation will have a great impact.
Then we know the image after distortion,How to get the Pc coordinates of the previous step 3?
Through the de-distortion formula we can easily get the correct coordinates after removing the distortion. But if we need to calculate points in three-dimensional space through triangulation, we must go back from step 5. From the formula, if we know xd and yd (abbreviation), how to calculate x, y? This is a high-order equation that can of course be solved by optimization, but it is only a small link and there is no need to spend too much computer resources to calculate it. Moreover, if there are too many feature points and each one is calculated in this way, it will not be able to meet the real-time performance of the front-end. Vins proposed an efficient calculation method!
3. The idea of vins de-distortion
At the beginning, we agreed that x_d and y_d represent the points calculated in step 4 to remove distortion. And x, y represent the original point (that is, the image point where we see distortion).
First, after we get an image after removing distortion, point A’ (x_da, y_da), we want to know its original coordinates of point A (x_a, y_a). According to a property of radial distortion, the farther you are from the optical center (center point of the image), the farther your distortion point is from the original point . From the picture above, we can see that A'A is the distance between the de-distorted point and the original point. How does vins approximately calculate point A? The steps are as follows:
1, First, point A' is regarded as point B, and point B (x_b, y_b) is the original point. Then we know the original point. According to the formula in Figure 1, we can easily get the B'(x_db,y_db) point.
2, Due to the nature of distortion, point B is obviously closer to the center of the image. We can conclude that B'B<A'A
3, After increasing the distance from point A to B to get point C, I take point C (x_c, y_c) as the original point, and pass The formula in Figure 1 calculates point C. According to the properties, we can get B'B<C'C<A'A, because point C is farther from the center of the image than point B but not as far as point A.
4, Then start from point A' and increase the C'C distance to get a D (x_D, y_D) [Note: capitalized D is used to distinguish the distortion removal points], Then get the D' point through the formula, according to the property B'B<C'C<D'D<A'A. Loop in sequence, and approach A'A until it reaches a threshold, which is considered to meet the accuracy requirements.
The author of vins believes that looping 8 times means that this accuracy requirement can be met. Once we understand the theoretical part, it is very simple to look at the code. Here is the original code, with the author's comments for understanding. This function in the source code is called 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;
}Image Source:
Efficient de-distortion method in VINS-MONO_marinepig's blog-CSDN blog_vins de-distortion code
Efficient de-distortion method in VINS-mono_Like a new blog-CSDN blog_vins de-distortion