前文分析了如何进行特征检测匹配，接下来分析如何求解摄像机矩阵。
I：求解基础矩阵
首先介绍两个代码段，关于关键点和Point2f相互转化的函数
void KeyPointsToPoints(const vector<KeyPoint>& kps, vector<Point2f>& ps) {
ps.clear();
for (unsigned int i=0; i<kps.size(); i++) ps.push_back(kps[i].pt);
}
void PointsToKeyPoints(const vector<Point2f>& ps, vector<KeyPoint>& kps) {
kps.clear();
for (unsigned int i=0; i<ps.size(); i++) kps.push_back(KeyPoint(ps[i],1.0f));
}这两个函数在求解基础矩阵的时候用的特别多。这里提前放着。
下面是求解每两幅图像的基础矩阵
void MultiCameraPnP::PruneMatchesBasedOnF() {
//prune the match between <_i> and all views using the Fundamental matrix to prune
//#pragma omp parallel for
//求解每两幅图像的基础矩阵
for (int _i=0; _i < imgs.size() - 1; _i++)
{
for (unsigned int _j=_i+1; _j < imgs.size(); _j++) {
int older_view = _i, working_view = _j;
GetFundamentalMat( imgpts[older_view],
imgpts[working_view],
imgpts_good[older_view],
imgpts_good[working_view],
matches_matrix[std::make_pair(older_view,working_view)]
#ifdef __SFM__DEBUG__
,imgs_orig[older_view],imgs_orig[working_view]
#endif
);
//update flip matches as well
#pragma omp critical
matches_matrix[std::make_pair(working_view,older_view)] = FlipMatches(matches_matrix[std::make_pair(older_view,working_view)]);
}
}
}核心函数GetFundamentalMat的实现：
计算出两幅图像的基础矩阵，并优化匹配组，基本去除错误匹配
Mat GetFundamentalMat(const vector<KeyPoint>& imgpts1,
const vector<KeyPoint>& imgpts2,
vector<KeyPoint>& imgpts1_good,
vector<KeyPoint>& imgpts2_good,
vector<DMatch>& matches
#ifdef __SFM__DEBUG__
,const Mat& img_1,
const Mat& img_2
#endif
)
{
//Try to eliminate keypoints based on the fundamental matrix
//(although this is not the proper way to do this)
vector<uchar> status(imgpts1.size());
#ifdef __SFM__DEBUG__
std::vector< DMatch > good_matches_;
std::vector<KeyPoint> keypoints_1, keypoints_2;
#endif
// undistortPoints(imgpts1, imgpts1, cam_matrix, distortion_coeff);
// undistortPoints(imgpts2, imgpts2, cam_matrix, distortion_coeff);
//
imgpts1_good.clear(); imgpts2_good.clear();
vector<KeyPoint> imgpts1_tmp;
vector<KeyPoint> imgpts2_tmp;
if (matches.size() <= 0) {
//points already aligned...
imgpts1_tmp = imgpts1;
imgpts2_tmp = imgpts2;
} else {
GetAlignedPointsFromMatch(imgpts1, imgpts2, matches, imgpts1_tmp, imgpts2_tmp);//选取特征点集中匹配的特征点
}
Mat F;
{
vector<Point2f> pts1,pts2;
KeyPointsToPoints(imgpts1_tmp, pts1);//特征点转化成Point2f点
KeyPointsToPoints(imgpts2_tmp, pts2);
#ifdef __SFM__DEBUG__
cout << "pts1 " << pts1.size() << " (orig pts " << imgpts1_tmp.size() << ")" << endl;
cout << "pts2 " << pts2.size() << " (orig pts " << imgpts2_tmp.size() << ")" << endl;
#endif
double minVal,maxVal;
cv::minMaxIdx(pts1,&minVal,&maxVal);
F = findFundamentalMat(pts1, pts2, FM_RANSAC, 0.006 * maxVal, 0.99, status); //threshold from [Snavely07 4.1]//核心函数，计算F，并得到优化的匹配组模板status
}
vector<DMatch> new_matches;
cout << "F keeping " << countNonZero(status) << " / " << status.size() << endl;
for (unsigned int i=0; i<status.size(); i++) {
if (status[i]) //根据模板，重新优化匹配组及特征点，找到最优的匹配组，基本就没什么错误匹配了。
{
imgpts1_good.push_back(imgpts1_tmp[i]);
imgpts2_good.push_back(imgpts2_tmp[i]);
if (matches.size() <= 0) { //points already aligned...
new_matches.push_back(DMatch(matches[i].queryIdx,matches[i].trainIdx,matches[i].distance));
} else {
new_matches.push_back(matches[i]);
}
#ifdef __SFM__DEBUG__
good_matches_.push_back(DMatch(imgpts1_good.size()-1,imgpts1_good.size()-1,1.0));
keypoints_1.push_back(imgpts1_tmp[i]);
keypoints_2.push_back(imgpts2_tmp[i]);
#endif
}
}
cout << matches.size() << " matches before, " << new_matches.size() << " new matches after Fundamental Matrix\n";
matches = new_matches; //keep only those points who survived the fundamental matrix
#if 0
//-- Draw only "good" matches
#ifdef __SFM__DEBUG__
if(!img_1.empty() && !img_2.empty()) {
vector<Point2f> i_pts,j_pts;
Mat img_orig_matches;
{ //draw original features in red绘制原始特征点用红色
vector<uchar> vstatus(imgpts1_tmp.size(),1);
vector<float> verror(imgpts1_tmp.size(),1.0);
img_1.copyTo(img_orig_matches);
KeyPointsToPoints(imgpts1_tmp, i_pts);
KeyPointsToPoints(imgpts2_tmp, j_pts);
drawArrows(img_orig_matches, i_pts, j_pts, vstatus, verror, Scalar(0,0,255));
}
{ //superimpose filtered features in green用绿色绘制经过此次优化的特征点
vector<uchar> vstatus(imgpts1_good.size(),1);
vector<float> verror(imgpts1_good.size(),1.0);
i_pts.resize(imgpts1_good.size());
j_pts.resize(imgpts2_good.size());
KeyPointsToPoints(imgpts1_good, i_pts);
KeyPointsToPoints(imgpts2_good, j_pts);
drawArrows(img_orig_matches, i_pts, j_pts, vstatus, verror, Scalar(0,255,0));
imshow( "Filtered Matches", img_orig_matches );
}
int c = waitKey(0);
if (c=='s') {
imwrite("fundamental_mat_matches.png", img_orig_matches);
}
destroyWindow("Filtered Matches");
}
#endif
#endif
return F;
}其中从每幅图像的所有的特征集点中选取匹配的特征点函数
void GetAlignedPointsFromMatch(const std::vector<cv::KeyPoint>& imgpts1,
const std::vector<cv::KeyPoint>& imgpts2,
const std::vector<cv::DMatch>& matches,
std::vector<cv::KeyPoint>& pt_set1,
std::vector<cv::KeyPoint>& pt_set2)
{
for (unsigned int i=0; i<matches.size(); i++) {
// cout << "matches[i].queryIdx " << matches[i].queryIdx << " matches[i].trainIdx " << matches[i].trainIdx << endl;
assert(matches[i].queryIdx < imgpts1.size());
pt_set1.push_back(imgpts1[matches[i].queryIdx]);
assert(matches[i].trainIdx < imgpts2.size());
pt_set2.push_back(imgpts2[matches[i].trainIdx]);
}
}以上就求出了每两幅图像最佳匹配组，接下来就是对每两幅图像的最佳匹配进行处理。
2：计算两幅图像的单应内点
找到在两幅图像中的内点数，通过RANSAC筛选，得到单应矩阵收敛时符合单应变换的内点
//Following Snavely07 4.2 - find how many inliers are in the Homography between 2 views
//找到在两幅图像中的内点数，通过RANSAC筛选
int MultiCameraPnP::FindHomographyInliers2Views(int vi, int vj)
{
vector<cv::KeyPoint> ikpts,jkpts; vector<cv::Point2f> ipts,jpts;
GetAlignedPointsFromMatch(imgpts[vi],imgpts[vj],matches_matrix[make_pair(vi,vj)],ikpts,jkpts);
KeyPointsToPoints(ikpts,ipts); KeyPointsToPoints(jkpts,jpts);
double minVal,maxVal; cv::minMaxIdx(ipts,&minVal,&maxVal); //TODO flatten point2d?? or it takes max of width and height
vector<uchar> status;
cv::Mat H = cv::findHomography(ipts,jpts,status,CV_RANSAC, 0.004 * maxVal); //threshold from Snavely07
return cv::countNonZero(status); //number of inliers
}使用map容器，按内点概率从高到底依次排列
list
//sort pairwise matches to find the lowest Homography inliers [Snavely07 4.2]
cout << "Find highest match...";
list<pair<int,pair<int,int> > > matches_sizes;
//TODO: parallelize!
for(std::map<std::pair<int,int> ,std::vector<cv::DMatch> >::iterator i = matches_matrix.begin(); i != matches_matrix.end(); ++i) {
if((*i).second.size() < 100)
matches_sizes.push_back(make_pair(100,(*i).first));
else {
int Hinliers = FindHomographyInliers2Views((*i).first.first,(*i).first.second);
int percent = (int)(((double)Hinliers) / ((double)(*i).second.size()) * 100.0);
cout << "[" << (*i).first.first << "," << (*i).first.second << " = "<<percent<<"] ";
matches_sizes.push_back(make_pair((int)percent,(*i).first));
}
}
cout << endl;
matches_sizes.sort(sort_by_first);//按内点概率从大到小排列匹配组3：根据上面排好序的匹配组，依次计算摄像机矩阵
bool goodF = false;
int highest_pair = 0;
m_first_view = m_second_view = 0;
//reverse iterate by number of matches
for(list<pair<int,pair<int,int> > >::iterator highest_pair = matches_sizes.begin();
highest_pair != matches_sizes.end() && !goodF;
++highest_pair)
{
m_second_view = (*highest_pair).second.second;
m_first_view = (*highest_pair).second.first;
std::cout << " -------- " << imgs_names[m_first_view] << " and " << imgs_names[m_second_view] << " -------- " <<std::endl;
//what if reconstrcution of first two views is bad? fallback to another pair
//See if the Fundamental Matrix between these two views is good
goodF = FindCameraMatrices(K, Kinv, distortion_coeff,
imgpts[m_first_view],
imgpts[m_second_view],
imgpts_good[m_first_view],
imgpts_good[m_second_view],
P,
P1,
matches_matrix[std::make_pair(m_first_view,m_second_view)],
tmp_pcloud
#ifdef __SFM__DEBUG__
,imgs[m_first_view],imgs[m_second_view]
#endif
);这里预设
cv::Matx34d P(1,0,0,0,
0,1,0,0,
0,0,1,0),
P1(1,0,0,0,
0,1,0,0,
0,0,1,0);4：计算基础矩阵，本质矩阵
Mat F = GetFundamentalMat(imgpts1,imgpts2,imgpts1_good,imgpts2_good,matches
#ifdef __SFM__DEBUG__
,img_1,img_2
#endif
);
if(matches.size() < 100) { // || ((double)imgpts1_good.size() / (double)imgpts1.size()) < 0.25
cerr << "not enough inliers after F matrix" << endl;
return false;
}
//Essential matrix: compute then extract cameras [R|t]
Mat_<double> E = K.t() * F * K; //according to HZ (9.12)
//according to http://en.wikipedia.org/wiki/Essential_matrix#Properties_of_the_essential_matrix
if(fabsf(determinant(E)) > 1e-07) {
cout << "det(E) != 0 : " << determinant(E) << "\n";
P1 = 0;
return false;
}通过计算出的基础矩阵计算本质矩阵E。
5：接下来，就是把本质矩阵进行SVD奇异值分解，得到旋转部分和平移部分。首先若fabsf(determinant(E)) > 1e-07，则E无法分解。若得出的determinant(R1)+1.0 < 1e-09，就把E取负值重新进行分解。若fabsf(determinant(R))-1.0 > 1e-07，那么分解得到的R1是错误的。
bool DecomposeEtoRandT(
Mat_<double>& E,
Mat_<double>& R1,
Mat_<double>& R2,
Mat_<double>& t1,
Mat_<double>& t2)
{
#ifdef DECOMPOSE_SVD
//Using HZ E decomposition
Mat svd_u, svd_vt, svd_w;
TakeSVDOfE(E,svd_u,svd_vt,svd_w);//进行SVD分解
//check if first and second singular values are the same (as they should be)检查第一个和第二个奇异值是不是相同，应该相同的。
double singular_values_ratio = fabsf(svd_w.at<double>(0) / svd_w.at<double>(1));
if(singular_values_ratio>1.0) singular_values_ratio = 1.0/singular_values_ratio; // flip ratio to keep it [0,1]
if (singular_values_ratio < 0.7) {
cout << "singular values are too far apart\n";
return false;
}
Matx33d W(0,-1,0, //HZ 9.13
1,0,0,
0,0,1);
Matx33d Wt(0,1,0,
-1,0,0,
0,0,1);
R1 = svd_u * Mat(W) * svd_vt; //HZ 9.19
R2 = svd_u * Mat(Wt) * svd_vt; //HZ 9.19
t1 = svd_u.col(2); //u3
t2 = -svd_u.col(2); //u3
#else
//Using Horn E decomposition，使用另一种方法分解
DecomposeEssentialUsingHorn90(E[0],R1[0],R2[0],t1[0],t2[0]);
#endif
return true;
}其中TakeSVDOfE函数实现了E的SVD分解，得到u，vt，w。
void TakeSVDOfE(Mat_<double>& E, Mat& svd_u, Mat& svd_vt, Mat& svd_w) {
#if 1
//Using OpenCV's SVD
SVD svd(E,SVD::MODIFY_A);
svd_u = svd.u;
svd_vt = svd.vt;
svd_w = svd.w;
#else
//Using Eigen's SVD
cout << "Eigen3 SVD..\n";
Eigen::Matrix3f e = Eigen::Map<Eigen::Matrix<double,3,3,Eigen::RowMajor> >((double*)E.data).cast<float>();
Eigen::JacobiSVD<Eigen::MatrixXf> svd(e, Eigen::ComputeThinU | Eigen::ComputeThinV);
Eigen::MatrixXf Esvd_u = svd.matrixU();
Eigen::MatrixXf Esvd_v = svd.matrixV();
svd_u = (Mat_<double>(3,3) << Esvd_u(0,0), Esvd_u(0,1), Esvd_u(0,2),
Esvd_u(1,0), Esvd_u(1,1), Esvd_u(1,2),
Esvd_u(2,0), Esvd_u(2,1), Esvd_u(2,2));
Mat_<double> svd_v = (Mat_<double>(3,3) << Esvd_v(0,0), Esvd_v(0,1), Esvd_v(0,2),
Esvd_v(1,0), Esvd_v(1,1), Esvd_v(1,2),
Esvd_v(2,0), Esvd_v(2,1), Esvd_v(2,2));
svd_vt = svd_v.t();
svd_w = (Mat_<double>(1,3) << svd.singularValues()[0] , svd.singularValues()[1] , svd.singularValues()[2]);
#endif
cout << "----------------------- SVD ------------------------\n";
cout << "U:\n"<<svd_u<<"\nW:\n"<<svd_w<<"\nVt:\n"<<svd_vt<<endl;
cout << "----------------------------------------------------\n";
}下面是另一种方法进行分解的E，得到R1，R2，T1，T2。
void DecomposeEssentialUsingHorn90(double _E[9], double _R1[9], double _R2[9], double _t1[3], double _t2[3]) {
//from : http://people.csail.mit.edu/bkph/articles/Essential.pdf
#ifdef USE_EIGEN
using namespace Eigen;
Matrix3d E = Map<Matrix<double,3,3,RowMajor> >(_E);
Matrix3d EEt = E * E.transpose();
Vector3d e0e1 = E.col(0).cross(E.col(1)),e1e2 = E.col(1).cross(E.col(2)),e2e0 = E.col(2).cross(E.col(2));
Vector3d b1,b2;
#if 1
//Method 1
Matrix3d bbt = 0.5 * EEt.trace() * Matrix3d::Identity() - EEt; //Horn90 (12)
Vector3d bbt_diag = bbt.diagonal();
if (bbt_diag(0) > bbt_diag(1) && bbt_diag(0) > bbt_diag(2)) {
b1 = bbt.row(0) / sqrt(bbt_diag(0));
b2 = -b1;
} else if (bbt_diag(1) > bbt_diag(0) && bbt_diag(1) > bbt_diag(2)) {
b1 = bbt.row(1) / sqrt(bbt_diag(1));
b2 = -b1;
} else {
b1 = bbt.row(2) / sqrt(bbt_diag(2));
b2 = -b1;
}
#else
//Method 2
if (e0e1.norm() > e1e2.norm() && e0e1.norm() > e2e0.norm()) {
b1 = e0e1.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
b2 = -b1;
} else if (e1e2.norm() > e0e1.norm() && e1e2.norm() > e2e0.norm()) {
b1 = e1e2.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
b2 = -b1;
} else {
b1 = e2e0.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
b2 = -b1;
}
#endif
//Horn90 (19)
Matrix3d cofactors; cofactors.col(0) = e1e2; cofactors.col(1) = e2e0; cofactors.col(2) = e0e1;
cofactors.transposeInPlace();
//B = [b]_x , see Horn90 (6) and http://en.wikipedia.org/wiki/Cross_product#Conversion_to_matrix_multiplication
Matrix3d B1; B1 << 0,-b1(2),b1(1),
b1(2),0,-b1(0),
-b1(1),b1(0),0;
Matrix3d B2; B2 << 0,-b2(2),b2(1),
b2(2),0,-b2(0),
-b2(1),b2(0),0;
Map<Matrix<double,3,3,RowMajor> > R1(_R1),R2(_R2);
//Horn90 (24)
R1 = (cofactors.transpose() - B1*E) / b1.dot(b1);
R2 = (cofactors.transpose() - B2*E) / b2.dot(b2);
Map<Vector3d> t1(_t1),t2(_t2);
t1 = b1; t2 = b2;
cout << "Horn90 provided " << endl << R1 << endl << "and" << endl << R2 << endl;
#endif
}到这里就已经分解出R1,R2,t1,t2了，P0就是预设的单位矩阵，P1可以由这四个值组合成四种。应该取哪一个，下一章再分析