视觉slam十四讲第二版 pdf_视觉SLAM十四讲第七章视觉里程计1 之三角化代码解析...

最新推荐文章于 2022-10-08 19:29:06 发布

weixin_39722070

最新推荐文章于 2022-10-08 19:29:06 发布

阅读量477

点赞数

文章标签：视觉slam十四讲第二版 pdf

本文链接：https://blog.csdn.net/weixin_39722070/article/details/111384887

版权

本文详细介绍了视觉SLAM中三角化3D点和位姿估计的过程。通过对两幅图像的特征匹配，利用对极约束求取相机位姿，然后通过三角化算法得到3D点坐标。在验证部分，比较了3D点的重投影误差，确保了计算的准确性。此外，还提及了克莱姆法则在解决线性方程组中的应用。

摘要由CSDN通过智能技术生成

整体思路：

求解：

对两幅图像提取特征，通过对极约束求取相机位姿，通过三角化求取第一个相机中特征点对应的3D点坐标。

验证：

对第一幅图像，将特征点的3D点坐标做归一化，获得相机坐标，特征点的像素点坐标经过相机内参投影到相机坐标，二者做比较。对第二幅图像，将特征点的3D点坐标经过R，t变换，再归一化，获得特征点在第二个相机下的相机坐标，与特征点的像素坐标经过相机内参投影到相机坐标做比较，评价精度。

对两个位姿下相机拍摄的图片img_1提取特征点vector<KeyPoint> keypoints_1，keypoints_2;
找到匹配点vector<DMatch> matches，特征匹配：find_feature_matches(img_1,img_2,keypoints_1,keypoints_2,matches)；
估计两张图像间的运动Mat R,t；pose_estimation_2d2d(keypoints_1,keypoints_2,matches,R,t);
三角化vector<Points3d>points;trangulation(keypoints_1,keypoints_2,matches,R,t,points);
验证三角化点与特征点的重投影关系Mat K=(Mat_<double>(3,3)<<520.9,0,325.1,0,521.0,249.7,0,0,1);
1. 第一个相机
  1. 第一种途径：将特征点从像素坐标投影到相机坐标，第二种途径：三角化获得的三维点坐标进行归一化处理就是相机坐标。
2. 对第二个相机
  1. 第一种途径：将特征点从像素坐标投影到相机坐标，第二种途径：三角化获得的三维点坐标经过相机位姿的R，t变换，进行归一化处理就是第二幅图像上特征点对应的相机坐标。

#include <iostream>
#include <opencv2/opencv.hpp>
// #include "extra.h" // used in opencv2
using namespace std;
using namespace cv;

void find_feature_matches(
  const Mat &img_1, const Mat &img_2,
  std::vector<KeyPoint> &keypoints_1,
  std::vector<KeyPoint> &keypoints_2,
  std::vector<DMatch> &matches);

void pose_estimation_2d2d(
  const std::vector<KeyPoint> &keypoints_1,
  const std::vector<KeyPoint> &keypoints_2,
  const std::vector<DMatch> &matches,
  Mat &R, Mat &t);

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

///