畸变模型求解
畸变模型
在上一篇博客，我提到了我们用的畸变模型大多数是布朗模型的一种近似（当然还存在很多畸变模型，如鱼眼模型、根据镜头特点提出的模型等等）。
同时，在上一篇博客也指出了以下两种模型其实是等价的：
x
u
n
d
i
s
t
=
f
(
x
d
i
s
t
,
y
d
i
s
t
)
(1)
x_{undist} = f(x_{dist},y_{dist}) \tag{1}
xundist?=f(xdist?,ydist?)(1)
x
d
i
s
t
=
g
(
x
u
n
d
i
s
t
,
y
u
n
d
i
s
t
)
(2)
x_{dist} = g(x_{undist},y_{undist}) \tag{2}
xdist?=g(xundist?,yundist?)(2)
对于opencv，其采用的是第二种，原因是利用反向映射进行去畸变，一方面避免求解上述二元高次方程，另一方面可以避免第一种模型去畸变后产生“奶酪图”。
那么，如果我们要想求解第二种模型的方程，应该怎么做呢？
在opencv中给出了一种迭代法求解的方法，且收敛速度非常快。答案就在函数undistortPoints()中。
undistortPoints()
对于一些场景，我们并不需要对整幅图进行去畸变。比如对于光流法中我们跟踪的一些点（a sparse set of points），我们只需要对这些点进行畸变矫正即可，相比全图去畸变，节约算力。
同时，该函数还是projectPoints的逆变换。因为投影时，3D点经过刚体变换后的坐标就是理想的坐标，经过透镜后的坐标才是畸变后的坐标，进而通过乘以内参矩阵，得到像素点坐标。
对于给定的畸变点 ( u , v ) (u,v) (u,v)，计算其去畸变后的坐标点 ( x ′ ′ , y ′ ′ ) (x^{''},y^{''}) (x′′,y′′)，主要步骤如下：
1、根据相机内参，将坐标转化到相机坐标系下，即
2、迭代求解畸变模型，得到去畸变后的坐标，即
3、再根据相机内参，将坐标系转化为像素坐标系，得到去畸变后的像素坐标，即
之所以要经过上述一圈，是因为我们在相机坐标系下，利用透镜前（无畸变）和透镜后（畸变）对应的坐标点进行优化得到畸变系数。
迭代法求解非线性方程组
对于相机坐标系下的畸变坐标
(
x
d
i
s
t
,
y
d
i
s
t
)
(x_{dist},y_{dist})
(xdist?,ydist?)，我们有如下畸变方程：
x
d
i
s
t
=
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
+
p
1
?
(
2
?
x
?
y
)
+
p
2
?
(
r
2
+
2
?
x
2
)
+
s
1
?
r
2
+
s
2
?
r
4
(3)
x_{dist} = x * \frac{1 + k1 * r^2 + k2 * r^4 + k3 * r^6}{1 + k4* r^2 + k5 * r^4 + k6 * r^6} + p1 * (2*x*y)+p2*(r^2+2*x^2)+s1*r^2+s2*r^4 \tag{3}
xdist?=x?1+k4?r2+k5?r4+k6?r61+k1?r2+k2?r4+k3?r6?+p1?(2?x?y)+p2?(r2+2?x2)+s1?r2+s2?r4(3)
简化一下上式，令：
A
=
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
B
=
p
1
?
(
2
?
x
?
y
)
+
p
2
?
(
r
2
+
2
?
x
2
)
+
s
1
?
r
2
+
s
2
?
r
4
C
=
p
1
?
(
r
2
+
2
?
y
2
)
+
p
2
?
(
2
?
x
?
y
)
+
s
3
?
r
2
+
s
4
?
r
4
(4)
A = \frac{1 + k1 * r^2 + k2 * r^4 + k3 * r^6}{1 + k4* r^2 + k5 * r^4 + k6 * r^6} \\ B = p1 * (2*x*y)+p2*(r^2+2*x^2)+s1*r^2+s2*r^4 \\ C = p1 * (r^2+2*y^2)+p2*(2*x*y)+s3*r^2+s4*r^4 \tag{4}
A=1+k4?r2+k5?r4+k6?r61+k1?r2+k2?r4+k3?r6?B=p1?(2?x?y)+p2?(r2+2?x2)+s1?r2+s2?r4C=p1?(r2+2?y2)+p2?(2?x?y)+s3?r2+s4?r4(4)
那么，畸变方程简化为：
x
d
i
s
t
=
x
?
A
+
B
(5)
x_{dist} = x * A + B \tag{5}
xdist?=x?A+B(5)
迭代法求解非线性方程组，首先就是要分离出
x
x
x和
y
y
y，即
x
=
(
x
d
i
s
t
?
B
)
/
A
y
=
(
y
d
i
s
t
?
C
)
/
A
(6)
x = (x_{dist} - B) / A \\ y = (y_{dist} - C) / A \tag{6}
x=(xdist??B)/Ay=(ydist??C)/A(6)
这相当于我们利用畸变坐标，得到了去畸变后的坐标位置。但是，这个结果并不足够准确，需要进行多次迭代。
既然需要迭代进行求解，我们就需要设定迭代结束条件，即：
- 满足残差
- 达到最大迭代次数
残差
每一次迭代，我们通过公式（6）得到了“去畸变后的坐标位置”。
那么，将其带入到公式（2），我们就可以得到新的”畸变坐标位置“。
再借助相机内参，将其转化到像素坐标系下，这样我们就可以计算和真实的畸变坐标位置的欧氏距离作为最终的error，也就是重投影误差。
代码
void undistorPointCore(Point2f& point)
{
// point 畸变点坐标
cv::TermCriteria criteria = cv::TermCriteria(cv::TermCriteria::EPS, 50, 0.0001);
double k[3] = { 8.754e-05,3.669e-07,-1.36e-07 }; // x
//double k[3] = { 0.0001003,-6.175e-07,-1.186e-07 }; // y
double xu, yu, xd = 0, yd = 0;
xd = xu = point.x;
yd = yu = point.y;
double error = std::numeric_limits<double>::max();
for (int j = 0; ; j++)
{
if ((criteria.type & cv::TermCriteria::COUNT) && j >= criteria.maxCount)
{
//cout << __LINE__ << " exceed max iterative num" << endl;
break;
}
if ((criteria.type & cv::TermCriteria::EPS) && error < criteria.epsilon)
{
//cout << __LINE__ << " reach residual error!" << endl;
break;
}
double r2 = xu * xu + yu * yu;
double icdist = 1 + ((k[2] * r2 + k[1]) * r2 + k[0]) * r2;
if (icdist < 0)
{
xu = point.x;
yu = point.y;
break;
}
// xu yu 可以认为是去畸变后的坐标位置
// xd yd 可以认为是畸变前的坐标位置
// xd = xu(1+k1*rd^2+k2*rd^4+k3*rd^6)
// rd = sqrt(xu * xu + yu * yu)
xu = xd / icdist;
yu = yd / icdist;
// 然后再对上述去畸变后的坐标再加畸变，然后再转换到像素坐标系，并计算误差
if (criteria.type & cv::TermCriteria::EPS)
{
double r4, r6, cdist;
double xu0, yu0;
r2 = xu * xu + yu * yu;
r4 = r2 * r2;
r6 = r4 * r2;
cdist = 1 + k[0] * r2 + k[1] * r4 + k[2] * r6;
xd0 = xu * cdist;
yd0 = yu * cdist;
error = sqrt(pow(xd0 - point.x, 2) + pow(yd0 - point.y, 2));
//cout << j << " " << error << endl;
}
}
cout << point.x << " " << point.y << " " << xu << " " << yu << endl;
point.x = xu;
point.y = yu;
// point 去畸变点坐标
}
opencv中undistortPoints()在内部调用cvUndistortPointsInternal()，上述代码做了一些精简，比如：
- 舍弃了大量的类型检查，以及Mat与cvMat之间的转化
- 将畸变模型简化，只考虑三阶径向模型
这种方法在去畸变的时候，收敛速度非常快，一般几次就能得到去畸变后的坐标位置。opencv中提到，默认迭代次数为5次。
Default version of undistortPoints does 5 iterations to compute undistorted points
beacuse, criteria = cv::TermCriteria(cv::TermCriteria::EPS, 5, 0.01);
最后
刚开始在做反投影的时候，卡在了畸变方程的求解。于是乎，就在网上找了一个牛顿法求解非线性方程组的demo。相比牛顿法去求解，opencv中给出的这种方法，直观、收敛速度快、精度比较高，且实现起来比较容易（牛顿法需要求雅可比矩阵和海森矩阵）。