图像去畸变与对极几何估计相机运动
之前做了手机相机的标定,得到了相机的内参矩阵和径向畸变系数 k 1 , k 2 k_1,k_2 k1,k2 ,于是想先尝试一下从两张相邻图片估计相机的运动,步骤大致如下:
- 图像去畸变处理。
- 匹配相邻图像特征点。
- 从匹配点估计相机的运动。
1. 图像去畸变处理
1.1. 原理
在标定手机相机内参时,就已经考虑到了图像的畸变。公式如下:
径向畸变+切向畸变:
{
x
c
o
r
r
e
c
t
e
d
=
x
(
1
+
k
1
r
2
+
k
2
r
2
+
k
3
r
6
)
+
2
p
1
x
y
+
p
2
(
r
2
+
2
x
2
)
y
c
o
r
r
e
c
t
e
d
=
y
(
1
+
k
1
r
2
+
k
2
r
2
+
k
3
r
6
)
+
p
1
(
r
2
+
2
y
2
)
+
2
p
2
x
y
\left\{\begin{matrix} x_{corrected}=x(1+k_1r^2+k_2r^2+k_3r^6)+2p_1xy+p_2(r^2+2x^2)\\ \\ y_{corrected}=y(1+k_1r^2+k_2r^2+k_3r^6)+p_1(r^2+2y^2)+2p_2xy \end{matrix}\right.
⎩⎨⎧xcorrected=x(1+k1r2+k2r2+k3r6)+2p1xy+p2(r2+2x2)ycorrected=y(1+k1r2+k2r2+k3r6)+p1(r2+2y2)+2p2xy
则有:
{
u
=
f
x
x
c
o
r
r
e
c
t
e
d
+
c
x
v
=
f
y
y
c
o
r
r
e
c
t
e
d
+
c
y
\left\{\begin{matrix} u=f_x x_{corrected}+c_x\\ \\ v=f_yy_{corrected}+c_y \end{matrix}\right.
⎩⎨⎧u=fxxcorrected+cxv=fyycorrected+cy
这里 x x x, y y y, x c o r r e c t e d x_{corrected} xcorrected, y c o r r e c t e d y_{corrected} ycorrected 均为归一平面上的点。
我们需要没有畸变的图像,则相当于把去畸变的图像上的每一点与原图像相对应,即相当于把去畸变图像上的一点加上畸变,与原图对应的点相对应。
设去畸变的图像上一点
p
c
=
[
u
c
,
v
c
]
T
p_c=[u_c,v_c]^T
pc=[uc,vc]T ,则把此点转化到相机的归一化平面为:
[
x
,
y
,
1
]
=
[
u
c
−
c
x
f
x
,
v
c
−
c
y
f
y
,
1
]
T
[x,y,1]=[\frac{u_c-c_x}{f_x},\frac{v_c-c_y}{f_y},1]^T
[x,y,1]=[fxuc−cx,fyvc−cy,1]T
再用畸变关系得到加上畸变后的图像位置 p = [ u , v ] T p=[u,v]^T p=[u,v]T 。
这里有一个问题:我们计算得到的 u , v u, v u,v 不一定为整数,而图像上的点的RGB(或灰度)值则相当于图像转化乘的矩阵在第 v v v 行、第 u u u 列的值, u , v u,v u,v 应为整数。故我们这里要用二维线性插值来确定这个点 [ u c , v c ] [u_c,v_c] [uc,vc] 的值。
这里有一篇介绍二维线性插值的文章。
大致的内容是这样的,如上图,我们要求对应的原图项上的点 (v+dv, u+du) 的值,即根据与之相邻的四个点的值来计算为 (与一维线性方法一样) :
P
=
(
1
−
d
u
)
⋅
(
1
−
d
v
)
⋅
A
+
d
u
⋅
(
1
−
d
v
)
⋅
B
+
d
u
⋅
d
v
⋅
C
+
(
1
−
d
u
)
⋅
d
v
⋅
D
P=(1-du)\cdot(1-dv)\cdot A+du\cdot(1-dv)\cdot B+du\cdot dv\cdot C+(1-du)\cdot dv\cdot D
P=(1−du)⋅(1−dv)⋅A+du⋅(1−dv)⋅B+du⋅dv⋅C+(1−du)⋅dv⋅D
如此,我们便能求出去畸变图像上每一个点的像素值。
1.2. OpenCV 实现
void cv::undistort( InputArray src,
OutputArray dst,
InputArray cameraMatrix,
InputArray distCoeffs,
InputArray newCameraMatrix = noArray()
)
//newCameraMatrix 为去畸变后图像对应的相机内参矩阵,默认为与之前相同
Mat cv::getOptimalNewCameraMatrix( InputArray cameraMatrix,
InputArray distCoeffs,
Size imageSize,
double alpha,
Size newImgSize = Size(),
Rect * validPixROI = 0,
bool centerPrincipalPoint = false
)
//alpha的值为0-1,0则输出内矩阵,即去畸变图像没有黑色边框。1则输出外矩阵,即去畸变图像包括原图像所有点。
下为直接用cv::undistort
做的图像去畸变:
左为原图,右为去畸变后的图像。因为 k 1 k_1 k1 很小, k 2 k_2 k2 稍微大一点,所以可以看到图像中心附近基本没有什么改变,离中心较远的地方略微有点变化。
2. 匹配相邻图像特征点
具体原理见《SLAM 十四讲》,书上讲的极其详细。
代码思路如下:
//1--初始化
//两个vector放两个图像的角点,两个Mat存储对应的描述子
std::vector<KeyPoint> k1;
std::vector<KeyPoint> k2;
Mat descriptors_1, descriptors_2;
//用于提取角点和提取对应的描述子
Ptr<FeatureDetector> detector = ORB::create();
Ptr<DescriptorExtractor> descriptor = ORB::create();
//用于匹配特征点,距离为Hamming
Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create ( "BruteForce-Hamming" );
//用于存放所有匹配点和筛选后的匹配点
std::vector<DMatch> match;
std::vector<DMatch> matches;
//2--提取并匹配特征点
//检测角点并提取相应描述子
detector->detectAndCompute( img_1, noArray(), keypoints_1, descriptors_1 );
detector->detectAndCompute( img_2, noArray(), keypoints_2, descriptors_2 );
//匹配特征点
matcher->match( descriptors_1, descriptors_2, match );
//3--筛选特征点
//找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
double min_dist=10000, max_dist=0;
for ( int i = 0; i < descriptors_1.rows; i++ ) {
double dist = match[i].distance;
if ( dist < min_dist ) min_dist = dist;
if ( dist > max_dist ) max_dist = dist;
}
printf ( "-- Max dist : %f \n", max_dist );
printf ( "-- Min dist : %f \n", min_dist );
//当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值25作为下限.
for ( int i = 0; i < descriptors_1.rows; i++ ) {
if ( match[i].distance <= max ( 2*min_dist, 25.0 ) ) {
matches.push_back ( match[i] );
}
}
//4--绘制匹配结果
Mat img_match;
drawMatches( img_1, keypoints_1, img_2, keypoints_2, matches, img_match );
imshow ( "所有匹配点对", img_match );
这里 k1[ matches[i].queryIdex ].pt
与 k2[ matches[i].queryIdex ].pt
就是两幅图对应的特征点的坐标,输出类型是Point2f
。
匹配运行结果如下:
3. 从匹配点估计相机运动
3.1. 对极几何
如上图: p 1 , p 2 p_1,p_2 p1,p2 为两个匹配好的特征点。 Π O 1 O 2 P \Pi_{O_1O_2P} ΠO1O2P 是极平面, O 1 O 2 O_1O_2 O1O2 与平面 I 1 , I 2 I_1,I_2 I1,I2 分别交于 e 1 , e 2 e_1,e_2 e1,e2 ,这两点被称为极点。 O 1 O 2 O_1O_2 O1O2 被称为基线,直线 l 1 = p 1 e 1 , l 2 = p 2 e 2 l_1=p_1e_1,l_2=p_2e_2 l1=p1e1,l2=p2e2 被称为极线。
具体推导过程见《SLAM 十四讲》,这里直接给出结论:
p
2
T
K
−
T
t
∧
R
K
−
1
p
1
=
0
p^T_2 K^{−T} t^∧ RK^{−1} p_1 = 0
p2TK−Tt∧RK−1p1=0
其中 K K K 为相机的内参矩阵, R , t R,t R,t 为相机从 O 1 O_1 O1 运动到 O 2 O_2 O2 的旋转矩阵和平移向量。
这里可以简化:本质矩阵 E = t ∧ R E = t^∧ R E=t∧R ,基础矩阵 F = K − T E K − 1 F = K^{−T} EK^{ −1} F=K−TEK−1 。
于是,在解出本质矩阵后,我们就可以用奇异值分解的方法解出旋转矩阵和平移向量。
3.2. 实现
//把匹配点转换为vector<Point2f>的形式
vector<Point2f> points1;
vector<Point2f> points2;
for ( int i = 0; i < ( int ) matches.size(); i++ ) {
points1.push_back ( keypoints_1[matches[i].queryIdx].pt );
points2.push_back ( keypoints_2[matches[i].trainIdx].pt );
}
//计算基础矩阵
Mat fundamental_matrix;
fundamental_matrix = findFundamentalMat ( points1, points2, CV_FM_8POINT );
cout<<"fundamental_matrix is "<<endl<< fundamental_matrix<<endl;
//计算本质矩阵
Point2d principal_point ( cam.cx_, cam.cy_ ); //相机光心, TUM dataset标定值
double focal_length = ( cam.fx_+cam.fy_ )/2; //相机焦距, TUM dataset标定值
Mat essential_matrix;
essential_matrix = findEssentialMat ( points1, points2, focal_length, principal_point );
E = essential_matrix;
cout<<"essential_matrix is "<<endl<< essential_matrix<<endl;
//从本质矩阵中恢复旋转和平移信息.
recoverPose ( essential_matrix, points1, points2, R, t, focal_length, principal_point );
cout<<"R is "<<endl<<R<<endl;
cout<<"t is "<<endl<<t<<endl;
这里cam
为一个存储相机参数的类,keypoints_1
,keypoints_2
和matches
都是上一步做好的特征点匹配。
下面介绍一下 findEssentialMat
:
Mat cv::findEssentialMat( InputArray points1,
InputArray points2,
double focal = 1.0,
Point2d pp = Point2d(0, 0),
int method = RANSAC,
double prob = 0.999,
double threshold = 1.0,
OutputArray mask = noArray()
)
- 这里
method
包括RANSAC
和LMEDS
,是处理那一对堆匹配点的算法,具体参见RANSAC LMedS 详细分析。 prob
是置信区间,范围是 0-1 。threshold
是RANSAC
算法里的误差阈值,即为点到极线的最大距离,这个用来判断点是 inlier 还是 outlier 。
介绍一下findFundamentalMat
:
Mat cv::findFundamentalMat( InputArray points1,
InputArray points2,
int method = FM_RANSAC,
double ransacReprojThreshold = 3,
double confidence = 0.99,
OutputArray mask = noArray()
)
method
:
CV_FM_7POINT
:7点算法,输入点的数量为7。
CV_FM_8POINT
:8点算法,输入点的数量大于等于8。
CV_FM_RANSAC
:RANSAC 算法, N ≥ 8 N≥8 N≥8 。
CV_FM_LMEDS
:LMedS 算法, N ≥ 8 N≥8 N≥8 。ransacReprojThreshold
:同之前的threshold
。