# 2. 三角测量函数

2D-2D姿态估计函数 pose_estimation_2d2d视觉SLAM笔记（33） 对极约束求解相机运动 相似

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)
);

// 相机内参,TUM Freiburg2
double cx = 325.1;  // 像素坐标系与成像平面之间的原点平移
double cy = 249.7;
double fx = 520.9;  // 焦距
double fy = 521.0;
Mat K = (Mat_<double>(3, 3) << fx, 0, cx, 0, fy, cy, 0, 0, 1);

vector<Point2f> pts_1, pts_2;
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);

// 转换成非齐次坐标
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);
}
}


# 3. 求特征点的空间位置

int main(int argc, char** argv)
{
if (argc != 3)
{
cout << "usage: triangulation img1 img2" << endl;
return 1;
}
//-- 读取图像

//-- 特征提取
vector<KeyPoint> keypoints_1, keypoints_2;
vector<DMatch> matches;
find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
cout << "一共找到了" << matches.size() << "组匹配点" << endl;

//-- 估计两张图像间运动
Mat R, t;
pose_estimation_2d2d(keypoints_1, keypoints_2, matches, R, t);

//-- 三角化
vector<Point3d> points;
triangulation(keypoints_1, keypoints_2, matches, R, t, points);

//-- 验证三角化点与特征点的重投影关系
// 相机内参,TUM Freiburg2
double cx = 325.1;  // 像素坐标系与成像平面之间的原点平移
double cy = 249.7;
double fx = 520.9;  // 焦距
double fy = 521.0;
Mat K = (Mat_<double>(3, 3) << fx, 0, cx, 0, fy, cy, 0, 0, 1);
for (int i = 0; i < matches.size(); i++)
{
// 第一个图
Point2d pt1_cam = pixel2cam(keypoints_1[matches[i].queryIdx].pt, K);
cout << "point in the first camera frame: " << pt1_cam << endl;

// 空间位置
Point2d pt1_cam_3d(points[i].x / points[i].z, points[i].y / points[i].z);
cout << "point projected from 3D " << pt1_cam_3d << ", d=" << points[i].z << endl;

// 第二个图
Point2f pt2_cam = pixel2cam(keypoints_2[matches[i].trainIdx].pt, K);
cout << "point in the second camera frame: " << pt2_cam << endl;

// 重投影
Mat pt2_trans = R * (Mat_<double>(3, 1) << points[i].x, points[i].y, points[i].z) + t;
pt2_trans /= pt2_trans.at<double>(2, 0);
cout << "point reprojected from second frame: " << pt2_trans.t() << endl;
cout << endl;
}

return 0;
}


《视觉SLAM十四讲》

©️2019 CSDN 皮肤主题: Age of Ai 设计师: meimeiellie