三. 2d-2d三角化求路标point

int main(int argc, char** argv)

{

Mat img_1 = imread("1.png");

Mat img_2 = imread("2.png");

 

vector<KeyPoint> keypoints_1, keypoints_2;

vector<DMatch> matches;

//1. 求特征点(keypoints)和描述子之间匹配(matches)  _见上一节函数

find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);

cout << "一共找到了" << matches.size() << "组匹配点" << endl;

 

//2. 估计两张图像间运动R t   _见上一节函数

Mat R, t;

pose_estimation_2d2d(keypoints_1, keypoints_2, matches, R, t);

 

//3. 三角化求世界坐标（points(XYZ)）

vector<Point3d> points;

triangulation(keypoints_1, keypoints_2, matches, R, t, points);

 

//4. 验证三角化点与特征点的重投影关系

Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);

for (int i = 0; i<matches.size(); i++)

{

               //特征点从像素坐标系p转为相机坐标系x，  .pt是像素坐标

Point2d pt1_cam = pixel2cam(keypoints_1[matches[i].queryIdx].pt, K);//相机坐标系x,y

Point2d pt1_cam_3d(

points[i].x / points[i].z,

points[i].y / points[i].z

);

 

cout << "point in the first camera frame: " << pt1_cam << endl;

cout << "point projected from 3D " << pt1_cam_3d << ", d=" << points[i].z << endl;

 

// 第二个图

              Point2f pt2_cam = pixel2cam(keypoints_2[matches[i].trainIdx].pt, K);

              Mat pt2_trans = R*(Mat_<double>(3, 1) << points[i].x, points[i].y, points[i].z) + t;//R*x1+t

pt2_trans /= pt2_trans.at<double>(2, 0);//归一化

cout << "point in the second camera frame: " << pt2_cam << endl;

cout << "point reprojected from second frame: " << pt2_trans.t() << endl;//转置  .t()

cout << endl;

}

 

return 0;

}

//三角化triangulation函数 triangulation(keypoints_1, keypoints_2, matches, R, t, points);

void triangulation(

const vector< KeyPoint >& keypoint_1,

const vector< KeyPoint >& keypoint_2,

const std::vector< DMatch >& matches,

const Mat& R, const Mat& t,

vector< Point3d >& points)

{

Mat T1 = (Mat_<float>(3, 4) <<

1, 0, 0, 0,

0, 1, 0, 0,

0, 0, 1, 0);

Mat T2 = (Mat_<float>(3, 4) <<

R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2), t.at<double>(0, 0),

R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2), t.at<double>(1, 0),

R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2), t.at<double>(2, 0)

);

 

Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);

vector<Point2f> pts_1, pts_2;

//for的C++11新特性

for (DMatch m : matches)

{

// 将像素坐标转换至相机坐标

pts_1.push_back(pixel2cam(keypoint_1[m.queryIdx].pt, K));

pts_2.push_back(pixel2cam(keypoint_2[m.trainIdx].pt, K));

}

 

Mat pts_4d;

cv::triangulatePoints(T1, T2, pts_1, pts_2, pts_4d);

/****************************************************************************************************************/

triangulatePoints函数用法

void cv::triangulatePoints(projMatr1,projMatr2,projPoints1,projPoints2,OutputArray points4D )

/****************************************************************************************************************/

 

// 转换成非齐次坐标

for (int i = 0; i<pts_4d.cols; i++)

{

Mat x = pts_4d.col(i);

x /= x.at<float>(3, 0); // 归一化

Point3d p(

x.at<float>(0, 0),

x.at<float>(1, 0),

x.at<float>(2, 0)

);

points.push_back(p);

}

}