Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- /*
- * intrinsic:3x3 double matrix
- */
- void pose_estimation_2d2d ( const cv::Mat& intrinsic, double focal_length, std::vector<KeyPoint> keypoints_1,
- std::vector<KeyPoint> keypoints_2,
- std::vector< DMatch > matches,
- Mat& R, Mat& t )
- {
- // 相机内参,TUM Freiburg2
- Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
- //-- 把匹配点转换为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 ( intrinsic.at<double>(0,2), intrinsic.at<double>(1,2) ); //相机光心, TUM dataset标定值
- //double focal_length = 521; //相机焦距, TUM dataset标定值
- Mat essential_matrix;
- essential_matrix = findEssentialMat ( points1, points2, focal_length, principal_point );
- cout<<"essential_matrix is "<<endl<< essential_matrix<<endl;
- //-- 计算单应矩阵
- Mat homography_matrix;
- homography_matrix = findHomography ( points1, points2, RANSAC, 3 );
- cout<<"homography_matrix is "<<endl<<homography_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;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement