视觉SLAM实践入门——（12）三角测量

前面通过对极约束估计了相机的 R，t，这一节通过三角测量可以恢复深度值，得到特征点的空间位置（估计值）

利用opencv进行三角测量的步骤：

1、定义旋转矩阵和平移向量组成的增广矩阵

2、计算特征点在两个坐标系下的归一化坐标（取前两维）

3、调用triangulatePoints，得到空间点的齐次坐标

4、归一化处理，取前三维作为空间点的非齐次坐标

需要注意的是：三角测量是由平移得到的，纯旋转是无法使用三角测量的，因为对极约束将永远满足。在平移存在的情况下，还要关心三角测量的不确定性，这会引出一个三角测量的矛盾

如果特征点运动一个像素 δx，使得视线角变化了一个角度 δθ，那么测量到深度值将有 δd 的变化。从几何关系可以看到，当 t 较大时，δd 将明显变小，这说明平移较大时，在同样的相机分辨率下，三角化测量将更精确

要增加三角化的精度，其一是提高特征点的提取精度，也就是提高图像分辨率——但这会导致图像变大，提高计算成本

另一方式是使平移量增大。但是，平移量增大会导致图像的外观发生明显的变化，比如箱子原先被挡住的侧面显示出来，比如反射光发生变化等等。外观变化会使得特征提取与匹配变得困难

因此出现了三角化的矛盾：平移增大，可能导致匹配失效；平移减小，三角化精度不够

#include <iostream>
using namespace std;#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/opencv.hpp>
using namespace cv;void OrbFeaturesMatch(const Mat &img1, const Mat &img2,vector<KeyPoint> &kp1, vector<KeyPoint> &kp2,vector<DMatch> &matches)
{Mat desc1, desc2;Ptr<FeatureDetector> detector = ORB::create();Ptr<DescriptorExtractor> desc = ORB::create();Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");//1、检测角点detector->detect(img1, kp1);detector->detect(img2, kp2);//2、计算描述子desc->compute(img1, kp1, desc1);desc->compute(img2, kp2, desc2);//3、匹配点对vector<DMatch> match;matcher->match(desc1, desc2, match);//4、检测误匹配double minDist = 10000, maxDist = 0;for(int i = 0; i < (int)match.size(); i++){double dist = match[i].distance;minDist = minDist > dist ? dist : minDist;maxDist = maxDist < dist ? dist : maxDist;}cout << "minDist and maxDist = " << minDist << " and " << maxDist << endl;double matchDist = max(30.0, minDist * 2);for(int i = 0; i < (int)match.size(); i++)if(match[i].distance <= matchDist)matches.push_back(match[i]);
}void estimateMoving(const vector<KeyPoint> &kp1, const vector<KeyPoint> &kp2, const vector<DMatch> &matches, Mat &R, Mat &t)
{//取出匹配点对坐标vector<Point2f> p1, p2;for(int i = 0; i < matches.size(); i++){//DMatch中有3个成员，queryIdx，trainIdx，distance//queryIdx是特征点第一张图的索引，trainIdx是特征点在第二张图的索引p1.push_back(kp1[matches[i].queryIdx].pt);p2.push_back(kp2[matches[i].trainIdx].pt);}//1、计算基础矩阵、本质矩阵、单应矩阵Mat fundamentalMat = findFundamentalMat(p1, p2, CV_FM_8POINT);   //8点法cout << "fundamentalMat :\n" << fundamentalMat << endl;double focalLength = 520.9;Point2d principalPoint(325.1, 249.7);Mat essentialMat = findEssentialMat(p1, p2, focalLength, principalPoint);cout << "essentialMax :\n" << essentialMat << endl;//场景不是平面时，单应矩阵意义不大Mat homographyMat = findHomography(p1, p2, RANSAC, 3);  //基于RANSAC的鲁棒算法，最大允许重投影错误阈值是3cout << "homographyMat :\n" << homographyMat << endl;//2、分解矩阵，求解相机运动R，trecoverPose(essentialMat, p1, p2, R, t, focalLength, principalPoint);cout << "R :\n" << R << endl;cout << "t :" << t.t() << endl;
}Point2d pixelToNor(const Point2d &p, const Mat &k)
{//像素坐标 = 内参矩阵 * 归一化坐标：p = K * x（齐次坐标运算）//下式计算的原理：x = fx * p_x + cx，y = fy * p_y + cy（记像素坐标(x, y)，归一化坐标(p_x, p_y)）//p_x = (x - cx) / fx，p_y = (y - cy) / fyreturn Point2d((p.x - k.at<double>(0, 2)) / k.at<double>(0, 0), (p.y - k.at<double>(1, 2)) / k.at<double>(1, 1));
}void triangulation(const vector<KeyPoint> &kp1, const vector<KeyPoint> &kp2,const vector<DMatch> &matches,const Mat &R, const Mat &t,vector<Point3d> &p)
{//T是旋转矩阵和平移向量拼接的增广矩阵，不是变换矩阵//以img1作为参考系，因此T1表示不旋转不平移，T2是img1到img2的旋转、平移Mat T1 = (Mat_<double>(3, 4) << 1, 0, 0, 0,0, 1, 0, 0,0, 0, 1, 0,0, 0, 0, 0);Mat T2 = (Mat_<double>(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);//计算归一化坐标的(x, y)vector<Point2d> p1, p2;for(auto m : matches){p1.push_back(pixelToNor(kp1[m.queryIdx].pt, k));p2.push_back(pixelToNor(kp2[m.trainIdx].pt, k));}Mat p4d;    //空间点的齐次坐标triangulatePoints(T1, T2, p1, p2, p4d);//转换为非齐次坐标for(int i = 0; i < p4d.cols; i++){Mat x = p4d.col(i); //取出一列x = x / x.at<double>(3, 0);    //归一化Point3d pt(x.at<double>(0, 0), x.at<double>(1, 0), x.at<double>(2, 0));p.push_back(pt);}
}inline cv::Scalar get_color(float depth) {float up_th = 50, low_th = 10, th_range = up_th - low_th;if (depth > up_th) depth = up_th;if (depth < low_th) depth = low_th;return cv::Scalar(255 * depth / th_range, 0, 255 * (1 - depth / th_range));
}int main(int argc, char **argv)
{if(argc != 3){cout << "input 2 images" << endl;return -1;}Mat img1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);Mat img2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);//1、匹配特征点vector<KeyPoint> kp1, kp2;vector<DMatch> matches;OrbFeaturesMatch(img1, img2, kp1, kp2, matches);//2、估计相机运动Mat R, t;estimateMoving(kp1, kp2, matches, R, t);//三角测量，得到特征点的空间坐标vector<Point3d> p;triangulation(kp1, kp2, matches, R, t, p);Mat k = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );//验证三角化点与特征点的重投影关系for(int i = 0; i < (int)matches.size(); i++){Point2d p1 = pixelToNor(kp1[matches[i].queryIdx].pt, k);    //img1特征点的归一化坐标的(x, y)      double p1Depth = p[i].z;   //深度        cv::circle(img1, kp1[matches[i].queryIdx].pt, 2, get_color(p1Depth), 2);    //画圈Mat p2_3d = R * (Mat_<double>(3, 1) << p[i].x, p[i].y, p[i].z) + t;   //特征点经过旋转平移double p2Depth = p2_3d.at<double>(2, 0);cv::circle(img2, kp2[matches[i].trainIdx].pt, 2, get_color(p2Depth), 2);Point2d p1Projected(p[i].x, p[i].y);  //经过三角测量恢复深度后，提取点在坐标系1中的坐标的前两维p1Projected = p1Projected / p[i].z;       //归一化处理，得到重投影的坐标（投影到相机1的归一化平面上）printf("第%03d个特征点在img1中：", i);cout << p1 << "，重投影后：" << p1Projected << "深度：" << p1Depth << endl;Point2d p2Projected(p2_3d.at<double>(0, 0), p2_3d.at<double>(1, 0));  //点在坐标系2中的坐标的前两维p2Projected = p2Projected / p2Depth;   //归一化处理，得到重投影的坐标（投影到相机2的归一化平面上）Point2d p2 = pixelToNor(kp2[matches[i].trainIdx].pt, k);   //img2特征点的归一化坐标的(x, y)printf("第%03d个特征点在img2中：", i);cout << p2 << "，重投影后：" << p2Projected << "深度：" << p2Depth << endl;}imshow("img1", img1);imshow("img2", img2);waitKey(0);return 0;
}

视觉SLAM实践入门——（12）三角测量相关推荐

视觉SLAM实践入门——（3）运动的可视化演示
旋转矩阵等数学表示方式不直观,考虑将相机的运动轨迹画到一个窗口中. 假设轨迹文件已经存储在trajectory.txt,每一行用下面格式存储: time,tx,ty,tz,qx,qy,qz,qw, 其 ...
视觉SLAM实践入门——（15）使用g2o求解PnP
使用g2o求解PnP的步骤: 1.定义顶点和边的数据类型 2.提取ORB特征.匹配点对 3.构造顶点.边 4.定义.设置求解器 5.调用求解器的 initializeOptimization 函数求解 ...
视觉SLAM笔记（12）四元数
视觉SLAM笔记(12) 四元数 1. 定义 2. 运算 2.1. 加法和减法 2.2. 乘法 2.3. 共轭 2.4. 模长 2.5. 逆 2.6. 数乘与点乘 3. 旋转表示 4. 旋转矩阵转换 ...
视觉SLAM十四讲学习笔记-第七讲-视觉里程计-三角测量和实践
专栏汇总视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第 ...
视觉SLAM十四讲 ch3 Ubuntu18.04 KDevelop的使用及Eigen实践入门笔记
视觉SLAM十四讲 ch3 Ubuntu18.04 KDevelop的使用及Eigen实践入门笔记一.创建KDevelop项目二.编写程序一.创建KDevelop项目你的电脑上如果还没有安装 ...
视觉SLAM十四讲学习笔记-第七讲-视觉里程计-PnP和实践
专栏汇总视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记- ...
视觉SLAM十四讲学习笔记-第七讲-视觉里程计-特征点法和特征提取和匹配实践
专栏系列文章如下: 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习 ...
视觉SLAM十四讲学习笔记——第十三讲实践：设计SLAM系统
1.如何运行示例代码首先是如何运行示例代码,这里遇到了很多问题: (1)首先要下载Kitti数据集,并在config/default.yaml文件内修改路径. (2)安装Glog.GTest.GFl ...
视觉SLAM：一直在入门，从未到精通
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达上周的组会上,我给研一的萌新们讲解什么是SLAM,为了能让他们在没有任何基础的情况下大致听懂,PPT只 ...

视觉SLAM实践入门——（12）三角测量

视觉SLAM实践入门——（12）三角测量相关推荐

最新文章

热门文章