图像去畸变与对极几何估计相机运动
之前做了手机相机的标定,得到了相机的内参矩阵和径向畸变系数 ,于是想先尝试一下从两张相邻图片估计相机的运动,步骤大致如下:
- 图像去畸变处理。
- 匹配相邻图像特征点。
- 从匹配点估计相机的运动。
1. 图像去畸变处理
1.1. 原理
在标定手机相机内参时,就已经考虑到了图像的畸变。公式如下:
径向畸变+切向畸变:
则有:
这里 , , , 均为归一平面上的点。
我们需要没有畸变的图像,则相当于把去畸变的图像上的每一点与原图像相对应,即相当于把去畸变图像上的一点加上畸变,与原图对应的点相对应。
设去畸变的图像上一点
,则把此点转化到相机的归一化平面为:
再用畸变关系得到加上畸变后的图像位置 。
这里有一个问题:我们计算得到的 不一定为整数,而图像上的点的RGB(或灰度)值则相当于图像转化乘的矩阵在第 行、第 列的值, 应为整数。故我们这里要用二维线性插值来确定这个点 的值。
这里有一篇介绍二维线性插值的文章。
大致的内容是这样的,如上图,我们要求对应的原图项上的点 (v+dv, u+du) 的值,即根据与之相邻的四个点的值来计算为 (与一维线性方法一样) :
如此,我们便能求出去畸变图像上每一个点的像素值。
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
做的图像去畸变:
左为原图,右为去畸变后的图像。因为 很小, 稍微大一点,所以可以看到图像中心附近基本没有什么改变,离中心较远的地方略微有点变化。
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. 对极几何
如上图: 为两个匹配好的特征点。 是极平面, 与平面 分别交于 ,这两点被称为极点。 被称为基线,直线 被称为极线。
具体推导过程见《SLAM 十四讲》,这里直接给出结论:
其中 为相机的内参矩阵, 为相机从 运动到 的旋转矩阵和平移向量。
这里可以简化:本质矩阵 ,基础矩阵 。
于是,在解出本质矩阵后,我们就可以用奇异值分解的方法解出旋转矩阵和平移向量。
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 算法, 。
CV_FM_LMEDS
:LMedS 算法, 。ransacReprojThreshold
:同之前的threshold
。