视觉SLAM学习【3】—–视觉SLAM通过三角测量和PnP法估计特征点的空间位置目录
- 一、G2O的安装
- 1、g2o的下载
- 2、文件上传ubuntu
- 3、安装依赖库
- 4、g2o的编译
- 二、项目创建
- 1、创建项目文件夹
- 2、创建三角测量的cpp文件
- 3、创建PnP法的cpp文件
- 4、创建CMakeLists.txt配置文件
- 三、编译项目
- 1、文件准备
- 2、项目进行编译
- 2、结果运行
嵌入式开发学习也慢慢的进入尾声了，从之前的ros学习，到gazebo再到机械臂的学习，到现在的slam视觉学习，层次逐渐上升，难度逐渐加大，本次博客，林君学长将打大家理解，如何通过三角测量和PnP法估计特征点的空间位置，并且自己创建项目工程用make方式编译，不用g++编译，一起来看如下步骤吧！
一、G2O的安装
g2o的核里带有各种各样的求解器，而它的顶点、边的类型则多种多样。通过自定义顶点和边，事实上，只要一个优化问题能够表达成图，那么就可以用g2o去求解它。常见的，比如bundle adjustment，ICP，数据拟合，都可以用g2o来做; 本次博客的内容，我们需要用到G20,所以我们首先需要进度G2o的安装，一起看步骤吧！
1、g2o的下载
（1）、方法一，通过如下链接在GitHub上面下载： https://github.com/RainerKuemmerle/g2o
（2）、方法二，林君学长已经将g2o的源代码(未编译)包上传到CSDN我的资源模块，小伙伴们可以去下载，链接如下所示： https://download.csdn.net/download/qq_42451251/12413010
2、文件上传ubuntu
（1）、将下载好的g2o的源代码资源包上传至ubuntu的任意位置解压并重新命名为g2o，下面以林君学长的路径为例
3、安装依赖库
<code class="prism language-bash has-numbering"><span class="token function">sudo</span> <span class="token function">apt-get</span> <span class="token function">install</span> libeigen3-dev libsuitesparse-dev libqt4-dev qt4-qmake libqglviewer-dev </code>
4、g2o的编译
（1）、进入g2o文件夹，创建build文件夹，存放编译产生的文件
<code class="prism language-bash has-numbering"><span class="token function">cd</span> ~/lenovo/g2o <span class="token function">mkdir</span> build <span class="token function">cd</span> build </code>
（2）、编译
<code class="prism language-bash has-numbering">cmake <span class="token punctuation">..</span> <span class="token function">make</span> -j8 </code>
以上的编译，林君学长是用的8线程编译的，如果小伙伴的电脑不支持8线程的话，可以改为make-j4和make
如上则为成功编译！ （3）、安装
<code class="prism language-bash has-numbering"><span class="token function">sudo</span> <span class="token function">make</span> <span class="token function">install </span> </code>
通过以上，我们的g2o就安装好了，编译的时候有点慢，我们耐心等待就好了！
二、项目创建
1、创建项目文件夹
新建终端，创建项目并创建编译文件夹
<code class="prism language-bash has-numbering"><span class="token function">cd</span> ~/lenovo/opencv-3.4.1 <span class="token function">mkdir</span> <span class="token function">test</span> <span class="token function">cd</span> <span class="token function">test</span> <span class="token function">mkdir</span> build <span class="token function">cd</span> build </code>
2、创建三角测量的cpp文件
（1）、创建三角测量算法的cpp文件夹
<code class="prism language-bash has-numbering"><span class="token function">touch</span> testTriang.cpp gedit testTriang.cpp </code>
（2）、编写三角测量算法代码
<code class="prism language-cpp has-numbering"><span class="token macro property">#<span class="token directive keyword">include</span> <span class="token string"><iostream></span></span>
<span class="token macro property">#<span class="token directive keyword">include</span> <span class="token string"><opencv2/core/core.hpp></span></span>
<span class="token macro property">#<span class="token directive keyword">include</span> <span class="token string"><opencv2/features2d/features2d.hpp></span></span>
<span class="token macro property">#<span class="token directive keyword">include</span> <span class="token string"><opencv2/highgui/highgui.hpp></span></span>
<span class="token macro property">#<span class="token directive keyword">include</span> <span class="token string"><opencv2/calib3d/calib3d.hpp></span></span>
<span class="token keyword">using</span> <span class="token keyword">namespace</span> std<span class="token punctuation">;</span>
<span class="token keyword">using</span> <span class="token keyword">namespace</span> cv<span class="token punctuation">;</span>
<span class="token keyword">void</span> <span class="token function">find_feature_matches</span><span class="token punctuation">(</span><span class="token keyword">const</span> Mat<span class="token operator">&</span> img_1<span class="token punctuation">,</span><span class="token keyword">const</span> Mat<span class="token operator">&</span> img_2<span class="token punctuation">,</span>
vector<span class="token operator"><</span>KeyPoint<span class="token operator">></span><span class="token operator">&</span> keypoints_1<span class="token punctuation">,</span>
vector<span class="token operator"><</span>KeyPoint<span class="token operator">></span><span class="token operator">&</span> keypoints_2<span class="token punctuation">,</span>
vector<span class="token operator"><</span>DMatch<span class="token operator">></span><span class="token operator">&</span> matches<span class="token punctuation">)</span>
<span class="token punctuation">{</span>
<span class="token comment">//初始化</span>
Mat descriptors_1<span class="token punctuation">,</span>descriptors_2<span class="token punctuation">;</span>
Ptr<span class="token operator"><</span>FeatureDetector<span class="token operator">></span> detector<span class="token operator">=</span>ORB<span class="token operator">::</span><span class="token function">create</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
Ptr<span class="token operator"><</span>DescriptorExtractor<span class="token operator">></span> descriptor<span class="token operator">=</span>ORB<span class="token operator">::</span><span class="token function">create</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
Ptr<span class="token operator"><</span>DescriptorMatcher<span class="token operator">></span> matcher<span class="token operator">=</span>DescriptorMatcher<span class="token operator">::</span><span class="token function">create</span><span class="token punctuation">(</span><span class="token string">"BruteForce-Hamming"</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token comment">//第一步:检测 Oriented FAST 角点位置</span>
detector<span class="token operator">-</span><span class="token operator">></span><span class="token function">detect</span><span class="token punctuation">(</span>img_1<span class="token punctuation">,</span>keypoints_1<span class="token punctuation">)</span><span class="token punctuation">;</span>
detector<span class="token operator">-</span><span class="token operator">></span><span class="token function">detect</span><span class="token punctuation">(</span>img_2<span class="token punctuation">,</span>keypoints_2<span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token comment">//第二步:根据角点位置计算 BRIEF 描述子</span>
descriptor<span class="token operator">-</span><span class="token operator">></span><span class="token function">compute</span><span class="token punctuation">(</span>img_1<span class="token punctuation">,</span>keypoints_1<span class="token punctuation">,</span>descriptors_1<span class="token punctuation">)</span><span class="token punctuation">;</span>
descriptor<span class="token operator">-</span><span class="token operator">></span><span class="token function">compute</span><span class="token punctuation">(</span>img_2<span class="token punctuation">,</span>keypoints_2<span class="token punctuation">,</span>descriptors_2<span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token comment">//第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离</span>
vector<span class="token operator"><</span>DMatch<span class="token operator">></span> match<span class="token punctuation">;</span>
matcher<span class="token operator">-</span><span class="token operator">></span><span class="token function">match</span><span class="token punctuation">(</span>descriptors_1<span class="token punctuation">,</span>descriptors_2<span class="token punctuation">,</span>match<span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token comment">//第四步:匹配点对筛选</span>
<span class="token keyword">double</span> min_dist<span class="token operator">=</span><span class="token number">1000</span><span class="token punctuation">,</span>max_dist<span class="token operator">=</span><span class="token number">0</span><span class="token punctuation">;</span>
<span class="token comment">//找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离</span>
<span class="token keyword">for</span><span class="token punctuation">(</span><span class="token keyword">int</span> i<span class="token operator">=</span><span class="token number">0</span><span class="token punctuation">;</span>i<span class="token operator"><</span>descriptors_1<span class="token punctuation">.</span>rows<span class="token punctuation">;</span><span class="token operator">++</span>i<span class="token punctuation">)</span>
<span class="token punctuation">{</span>
<span class="token keyword">double</span> dist<span class="token operator">=</span>match<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>distance<span class="token punctuation">;</span>
<span class="token keyword">if</span><span class="token punctuation">(</span>dist<span class="token operator"><</span>min_dist<span class="token punctuation">)</span> min_dist<span class="token operator">=</span>dist<span class="token punctuation">;</span>
<span class="token keyword">if</span><span class="token punctuation">(</span>dist<span class="token operator">></span>max_dist<span class="token punctuation">)</span> max_dist<span class="token operator">=</span>dist<span class="token punctuation">;</span>
<span class="token punctuation">}</span>
<span class="token function">printf</span><span class="token punctuation">(</span><span class="token string">"Max dist :%f\n"</span><span class="token punctuation">,</span>max_dist<span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token function">printf</span><span class="token punctuation">(</span><span class="token string">"Min dist :%f\n"</span><span class="token punctuation">,</span>min_dist<span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token comment">//当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.</span>
<span class="token keyword">for</span><span class="token punctuation">(</span><span class="token keyword">int</span> i<span class="token operator">=</span><span class="token number">0</span><span class="token punctuation">;</span>i<span class="token operator"><</span>descriptors_1<span class="token punctuation">.</span>rows<span class="token punctuation">;</span><span class="token operator">++</span>i<span class="token punctuation">)</span>
<span class="token punctuation">{</span>
<span class="token keyword">if</span><span class="token punctuation">(</span>match<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>distance<span class="token operator"><=</span><span class="token function">max</span><span class="token punctuation">(</span><span class="token number">2</span><span class="token operator">*</span>min_dist<span class="token punctuation">,</span><span class="token number">30.0</span><span class="token punctuation">)</span><span class="token punctuation">)</span>
<span class="token punctuation">{</span>
matches<span class="token punctuation">.</span><span class="token function">push_back</span><span class="token punctuation">(</span>match<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token punctuation">}</span>
<span class="token punctuation">}</span>
cout<span class="token operator"><<</span><span class="token string">"一共找到了"</span><span class="token operator"><<</span>matches<span class="token punctuation">.</span><span class="token function">size</span><span class="token punctuation">(</span><span class="token punctuation">)</span> <span class="token operator"><<</span><span class="token string">"组匹配点"</span><span class="token operator"><<</span>endl<span class="token punctuation">;</span>
<span class="token punctuation">}</span>
<span class="token comment">//估计两张图像间运动</span>
<span class="token keyword">void</span> <span class="token function">pose_estimation_2d2d</span><span class="token punctuation">(</span><span class="token keyword">const</span> vector<span class="token operator"><</span>KeyPoint<span class="token operator">></span><span class="token operator">&</span> keypoints_1<span class="token punctuation">,</span>
<span class="token keyword">const</span> vector<span class="token operator"><</span>KeyPoint<span class="token operator">></span><span class="token operator">&</span> keypoints_2<span class="token punctuation">,</span>
<span class="token keyword">const</span> vector<span class="token operator"><</span>DMatch<span class="token operator">></span><span class="token operator">&</span> matches<span class="token punctuation">,</span>
Mat<span class="token operator">&</span> R<span class="token punctuation">,</span>Mat<span class="token operator">&</span> t<span class="token punctuation">)</span>
<span class="token punctuation">{</span>
<span class="token comment">//相机内参,TUM Freiburg2</span>
Mat K<span class="token operator">=</span><span class="token punctuation">(</span>Mat_<span class="token operator"><</span><span class="token keyword">double</span><span class="token operator">></span> <span class="token punctuation">(</span><span class="token number">3</span><span class="token punctuation">,</span><span class="token number">3</span><span class="token punctuation">)</span><span class="token operator"><<</span><span class="token number">520.9</span><span class="token punctuation">,</span><span class="token number">325.1</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">,</span><span class="token number">521.0</span><span class="token punctuation">,</span><span class="token number">249.7</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">,</span><span class="token number">1</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token comment">//把匹配点转换为vector<Point2f>的形式</span>
vector<span class="token operator"><</span>Point2f<span class="token operator">></span> points1<span class="token punctuation">;</span>
vector<span class="token operator"><</span>Point2f<span class="token operator">></span> points2<span class="token punctuation">;</span>
<span class="token keyword">for</span><span class="token punctuation">(</span><span class="token keyword">int</span> i<span class="token operator">=</span><span class="token number">0</span><span class="token punctuation">;</span>i<span class="token operator"><</span><span class="token punctuation">(</span><span class="token keyword">int</span><span class="token punctuation">)</span>matches<span class="token punctuation">.</span><span class="token function">size</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span><span class="token operator">++</span>i<span class="token punctuation">)</span>
<span class="token punctuation">{</span>
points1<span class="token punctuation">.</span><span class="token function">push_back</span><span class="token punctuation">(</span>keypoints_1<span class="token punctuation">[</span>matches<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>queryIdx<span class="token punctuation">]</span><span class="token punctuation">.</span>pt<span class="token punctuation">)</span><span class="token punctuation">;</span><span class="token comment">//?</span>
points2<span class="token punctuation">.</span><span class="token function">push_back</span><span class="token punctuation">(</span>keypoints_2<span class="token punctuation">[</span>matches<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>trainIdx<span class="token punctuation">]</span><span class="token punctuation">.</span>pt<span class="token punctuation">)</span><span class="token punctuation">;</span><span class="token comment">//?</span>
<span class="token punctuation">}</span>
<span class="token comment">//计算基础矩阵</span>
Mat fundamental_matrix<span class="token punctuation">;</span>
fundamental_matrix<span class="token operator">=</span><span class="token function">findFundamentalMat</span><span class="token punctuation">(</span>points1<span class="token punctuation">,</span>points2<span class="token punctuation">,</span>CV_FM_8POINT<span class="token punctuation">)</span><span class="token punctuation">;</span>
cout<span class="token operator"><<</span><span class="token string">"fundamental_matrix ="</span><span class="token operator"><<</span>endl<span class="token operator"><<</span>fundamental_matrix<span class="token operator"><<</span>endl<span class="token punctuation">;</span>
<span class="token comment">//计算本质矩阵</span>
Point2d <span class="token function">principal_point</span><span class="token punctuation">(</span><span class="token number">325.1</span><span class="token punctuation">,</span><span class="token number">249.7</span><span class="token punctuation">)</span><span class="token punctuation">;</span><span class="token comment">//相机光心，TUM dataset标定值</span>
<span class="token keyword">int</span> focal_length<span class="token operator">=</span><span class="token number">521</span><span class="token punctuation">;</span><span class="token comment">相机焦距, TUM dataset标定值</span>
Mat essential_matrix<span class="token punctuation">;</span>
essential_matrix<span class="token operator">=</span><span class="token function">findEssentialMat</span><span class="token punctuation">(</span>points1<span class="token punctuation">,</span>points2<span class="token punctuation">,</span>focal_length<span class="token punctuation">,</span>principal_point<span class="token punctuation">)</span><span class="token punctuation">;</span>
cout<span class="token operator"><<</span><span class="token string">"essential_matrix = "</span><span class="token operator"><<</span>endl<span class="token operator"><<</span>essential_matrix<span class="token operator"><<</span>endl<span class="token punctuation">;</span>
<span class="token comment">//计算单应矩阵</span>
Mat homography_matrix<span class="token punctuation">;</span>
homography_matrix<span class="token operator">=</span><span class="token function">findHomography</span><span class="token punctuation">(</span>points1<span class="token punctuation">,</span>points2<span class="token punctuation">,</span>RANSAC<span class="token punctuation">,</span><span class="token number">3</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
cout<span class="token operator"><<</span><span class="token string">"homography_matrix = "</span><span class="token operator"><<</span>homography_matrix<span class="token operator"><<</span>endl<span class="token punctuation">;</span>
<span class="token comment">//从本质矩阵中恢复旋转和平移信息</span>
<span class="token function">recoverPose</span><span class="token punctuation">(</span>essential_matrix<span class="token punctuation">,</span>points1<span class="token punctuation">,</span>points2<span class="token punctuation">,</span>R<span class="token punctuation">,</span>t<span class="token punctuation">,</span>focal_length<span class="token punctuation">,</span>principal_point<span class="token punctuation">)</span><span class="token punctuation">;</span>
cout<span class="token operator"><<</span><span class="token string">"R = "</span><span class="token operator"><<</span>endl<span class="token operator"><<</span>R<span class="token operator"><<</span>endl<span class="token punctuation">;</span>
cout<span class="token operator"><<</span><span class="token string">"t = "</span><span class="token operator"><<</span>endl<span class="token operator"><<</span>t<span class="token operator"><<</span>endl<span class="token punctuation">;</span>
<span class="token punctuation">}</span>
Point2f <span class="token function">pixel2cam</span><span class="token punctuation">(</span><span class="token keyword">const</span> Point2d<span class="token operator">&</span> p<span class="token punctuation">,</span><span class="token keyword">const</span> Mat<span class="token operator">&</span> K<span class="token punctuation">)</span>
<span class="token punctuation">{</span>
<span class="token keyword">return</span> <span class="token function">Point2f</span><span class="token punctuation">(</span>
<span class="token punctuation">(</span>p<span class="token punctuation">.</span>x<span class="token operator">-</span>K<span class="token punctuation">.</span>at<span class="token operator"><</span><span class="token keyword">double</span><span class="token operator">></span><span class="token punctuation">(</span><span class="token number">0</span><span class="token punctuation">,</span><span class="token number">2</span><span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token operator">/</span>K<span class="token punctuation">.</span>at<span class="token operator"><</span><span class="token keyword">double</span><span class="token operator">></span><span class="token punctuation">(</span><span class="token number">0</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">)</span><span class="token punctuation">,</span>
<span class="token punctuation">(</span>p<span class="token punctuation">.</span>y<span class="token operator">-</span>K<span class="token punctuation">.</span>at<span class="token operator"><</span><span class="token keyword">double</span><span class="token operator">></span><span class="token punctuation">(</span><span class="token number">1</span><span class="token punctuation">,</span><span class="token number">2</span><span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token operator">/</span>K<span class="token punctuation">.</span>at<span class="token operator"><</span><span class="token keyword">double</span><span class="token operator">></span><span class="token punctuation">(</span><span class="token number">1</span><span class="token punctuation">,</span><span class="token number">1</span><span class="token punctuation">)</span>
<span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token punctuation">}</span>
<span class="token keyword">void</span> <span class="token function">triangulation</span><span class="token punctuation">(</span><span class="token keyword">const</span> vector<span class="token operator"><</span>KeyPoint<span class="token operator">></span><span class="token operator">&</span> keypoint_1<span class="token punctuation">,</span>
<span class="token keyword">const</span> vector<span class="token operator"><</span>KeyPoint<span class="token operator">></span><span class="token operator">&</span> keypoint_2<span class="token punctuation">,</span>
<span class="token keyword">const</span> vector<span class="token operator"><</span>DMatch<span class="token operator">></span><span class="token operator">&</span> matches<span class="token punctuation">,</span>
<span class="token keyword">const</span> Mat<span class="token operator">&</span> R<span class="token punctuation">,</span><span class="token keyword">const</span> Mat<span class="token operator">&</span> t<span class="token punctuation">,</span>
vector<span class="token operator"><</span>Point3d<span class="token operator">></span><span class="token operator">&</span> points<span class="token punctuation">)</span>
<span class="token punctuation">{</span>
Mat T1<span class="token operator">=</span><span class="token punctuation">(</span>Mat_<span class="token operator"><</span><span class="token keyword">float</span><span class="token operator">></span> <span class="token punctuation">(</span><span class="token number">3</span><span class="token punctuation">,</span><span class="token number">4</span><span class="token punctuation">)</span><span class="token operator"><<</span>
<span class="token number">1</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">,</span>
<span class="token number">0</span><span class="token punctuation">,</span><span class="token number">1</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">,</span>
<span class="token number">0</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">,</span><span class="token number">1</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
Mat T2<span class="token operator">=</span><span class="token punctuation">(</span>Mat_<span class="token operator"><</span><span class="token keyword">float</span><span class="token operator">></span><span class="token punctuation">(</span><span class="token number">3</span><span class="token punctuation">,</span><span class="token number">4</span><span class="token punctuation">)</span><span class="token operator"><<</span>
R<span class="token punctuation">.</span>at<span class="token operator"><</span><span class="token keyword">double</span><span class="token operator">></span><span class="token punctuation">(</span><span class="token number">0</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">)</span><span class="token punctuation">,</span>R<span class="token punctuation">.</span>at<span class="token operator"><</span><span class="token keyword">double</span><span class="token operator">></span><span class="token punctuation">(</span><span class="token number">0</span><span class="token punctuation">,</span><span class="token number">1</span><span class="token punctuation">)</span><span class="token punctuation">,</span>R<span class="token punctuation">.</span>at<span class="token operator"><</span><span class="token keyword">double</span><span class="token operator">></span><span class="token punctuation">(</span><span class="token number">0</span><span class="token punctuation">,</span><span class="token number">2</span><span class="token punctuation">)</span><span class="token punctuation">,</span>t<span class="token punctuation">.</span>at<span class="token operator"><</span><span class="token keyword">double</span><span class="token operator">></span><span class="token punctuation">(</span><span class="token number">0</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">)</span><span class="token punctuation">,</span>
R<span class="token punctuation">.</span>at<span class="token operator"><</span><span class="token keyword">double</span><span class="token operator">></span><span class="token punctuation">(</span><span class="token number">1</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">)</span><span class="token punctuation">,</span>R<span class="token punctuation">.</span>at<span class="token operator"><</span><span class="token keyword">double</span><span class="token operator">></span><span class="token punctuation">(</span><span class="token number">1</span><span class="token punctuation">,</span><span class="token number">1</span><span class="token punctuation">)</span><span class="token punctuation">,</span>R<span class="token punctuation">.</span>at<span class="token operator"><</span><span class="token keyword">double</span><span class="token operator">></span><span class="token punctuation">(</span><span class="token number">1</span><span class="token punctuation">,</span><span class="token number">2</span><span class="token punctuation">)</span><span class="token punctuation">,</span>t<span class="token punctuation">.</span>at<span class="token operator"><</span><span class="token keyword">double</span><span class="token operator">></span><span class="token punctuation">(</span><span class="token number">1</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">)</span><span class="token punctuation">,</span>
R<span class="token punctuation">.</span>at<span class="token operator"><</span><span class="token keyword">double</span><span class="token operator">></span><span class="token punctuation">(</span><span class="token number">2</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">)</span><span class="token punctuation">,</span>R<span class="token punctuation">.</span>at<span class="token operator"><</span><span class="token keyword">double</span><span class="token operator">></span><span class="token punctuation">(</span><span class="token number">2</span><span class="token punctuation">,</span><span class="token number">1</span><span class="token punctuation">)</span><span class="token punctuation">,</span>R<span class="token punctuation">.</span>at<span class="token operator"><</span><span class="token keyword">double</span><span class="token operator">></span><span class="token punctuation">(</span><span class="token number">2</span><span class="token punctuation">,</span><span class="token number">2</span><span class="token punctuation">)</span><span class="token punctuation">,</span>t<span class="token punctuation">.</span>at<span class="token operator"><</span><span class="token keyword">double</span><span class="token operator">></span><span class="token punctuation">(</span><span class="token number">2</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">)</span>
<span class="token punctuation">)</span><span class="token punctuation">;</span>
Mat K<span class="token operator">=</span><span class="token punctuation">(</span>Mat_<span class="token operator"><</span><span class="token keyword">double</span><span class="token operator">></span><span class="token punctuation">(</span><span class="token number">3</span><span class="token punctuation">,</span><span class="token number">3</span><span class="token punctuation">)</span><span class="token operator"><<</span><span class="token number">520.9</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">,</span><span class="token number">325.1</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">,</span><span class="token number">521.0</span><span class="token punctuation">,</span><span class="token number">249.7</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">,</span><span class="token number">1</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
vector<span class="token operator"><</span>Point2f<span class="token operator">></span> pts_1<span class="token punctuation">,</span>pts_2<span class="token punctuation">;</span>
<span class="token keyword">for</span><span class="token punctuation">(</span>DMatch m<span class="token operator">:</span>matches<span class="token punctuation">)</span>
<span class="token punctuation">{</span>
<span class="token comment">//将像素坐标转换至相机坐标</span>
pts_1<span class="token punctuation">.</span><span class="token function">push_back</span><span class="token punctuation">(</span><span class="token function">pixel2cam</span><span class="token punctuation">(</span>keypoint_1<span class="token punctuation">[</span>m<span class="token punctuation">.</span>queryIdx<span class="token punctuation">]</span><span class="token punctuation">.</span>pt<span class="token punctuation">,</span>K<span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
pts_2<span class="token punctuation">.</span><span class="token function">push_back</span><span class="token punctuation">(</span><span class="token function">pixel2cam</span><span class="token punctuation">(</span>keypoint_2<span class="token punctuation">[</span>m<span class="token punctuation">.</span>trainIdx<span class="token punctuation">]</span><span class="token punctuation">.</span>pt<span class="token punctuation">,</span>K<span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token punctuation">}</span>
Mat pts_4d<span class="token punctuation">;</span>
<span class="token function">triangulatePoints</span><span class="token punctuation">(</span>T1<span class="token punctuation">,</span>T2<span class="token punctuation">,</span>pts_1<span class="token punctuation">,</span>pts_2<span class="token punctuation">,</span>pts_4d<span class="token punctuation">)</span><span class="token punctuation">;</span>
cout<span class="token operator"><<</span>pts_4d<span class="token punctuation">.</span>cols<span class="token operator"><<</span>endl<span class="token punctuation">;</span>
<span class="token comment">//转换成非齐次坐标</span>
<span class="token keyword">for</span><span class="token punctuation">(</span><span class="token keyword">int</span> i<span class="token operator">=</span><span class="token number">0</span><span class="token punctuation">;</span>i<span class="token operator"><</span>pts_4d<span class="token punctuation">.</span>cols<span class="token punctuation">;</span><span class="token operator">++</span>i<span class="token punctuation">)</span>
<span class="token punctuation">{</span>
Mat x<span class="token operator">=</span>pts_4d<span class="token punctuation">.</span><span class="token function">col</span><span class="token punctuation">(</span>i<span class="token punctuation">)</span><span class="token punctuation">;</span>
x<span class="token operator">/</span><span class="token operator">=</span>x<span class="token punctuation">.</span>at<span class="token operator"><</span><span class="token keyword">float</span><span class="token operator">></span><span class="token punctuation">(</span><span class="token number">3</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">)</span><span class="token punctuation">;</span><span class="token comment">//归一化</span>
Point3d <span class="token function">p</span><span class="token punctuation">(</span>
x<span class="token punctuation">.</span>at<span class="token operator"><</span><span class="token keyword">float</span><span class="token operator">></span><span class="token punctuation">(</span><span class="token number">0</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">)</span><span class="token punctuation">,</span>
x<span class="token punctuation">.</span>at<span class="token operator"><</span><span class="token keyword">float</span><span class="token operator">></span><span class="token punctuation">(</span><span class="token number">1</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">)</span><span class="token punctuation">,</span>
x<span class="token punctuation">.</span>at<span class="token operator"><</span><span class="token keyword">float</span><span class="token operator">></span><span class="token punctuation">(</span><span class="token number">2</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">)</span>
<span class="token punctuation">)</span><span class="token punctuation">;</span>
cout<span class="token operator"><<</span><span class="token string">"p = "</span><span class="token operator"><<</span>endl<span class="token operator"><<</span>p<span class="token operator"><<</span>endl<span class="token punctuation">;</span>
points<span class="token punctuation">.</span><span class="token function">push_back</span><span class="token punctuation">(</span>p<span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token punctuation">}</span>
<span class="token punctuation">}</span>
<span class="token keyword">int</span> <span class="token function">main</span><span class="token punctuation">(</span><span class="token keyword">int</span> argc<span class="token punctuation">,</span><span class="token keyword">char</span><span class="token operator">*</span><span class="token operator">*</span> argv<span class="token punctuation">)</span>
<span class="token punctuation">{</span>
<span class="token keyword">if</span><span class="token punctuation">(</span>argc<span class="token operator">!=</span><span class="token number">3</span><span class="token punctuation">)</span>
<span class="token punctuation">{</span>
cout<span class="token operator"><<</span><span class="token string">"usage:triangulation img1 img2"</span><span class="token operator"><<</span>endl<span class="token punctuation">;</span>
<span class="token keyword">return</span> <span class="token number">1</span><span class="token punctuation">;</span>
<span class="token punctuation">}</span>
<span class="token comment">//读取图像</span>
Mat img_1<span class="token operator">=</span><span class="token function">imread</span><span class="token punctuation">(</span>argv<span class="token punctuation">[</span><span class="token number">1</span><span class="token punctuation">]</span><span class="token punctuation">,</span>CV_LOAD_IMAGE_COLOR<span class="token punctuation">)</span><span class="token punctuation">;</span>
Mat img_2<span class="token operator">=</span><span class="token function">imread</span><span class="token punctuation">(</span>argv<span class="token punctuation">[</span><span class="token number">2</span><span class="token punctuation">]</span><span class="token punctuation">,</span>CV_LOAD_IMAGE_COLOR<span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token comment">//寻找匹配点对</span>
vector<span class="token operator"><</span>KeyPoint<span class="token operator">></span> keypoints_1<span class="token punctuation">,</span>keypoints_2<span class="token punctuation">;</span>
vector<span class="token operator"><</span>DMatch<span class="token operator">></span> matches<span class="token punctuation">;</span>
<span class="token function">find_feature_matches</span><span class="token punctuation">(</span>img_1<span class="token punctuation">,</span>img_2<span class="token punctuation">,</span>keypoints_1<span class="token punctuation">,</span>keypoints_2<span class="token punctuation">,</span>matches<span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token comment">//估计两张图像间运动</span>
Mat R<span class="token punctuation">,</span>t<span class="token punctuation">;</span>
<span class="token function">pose_estimation_2d2d</span><span class="token punctuation">(</span>keypoints_1<span class="token punctuation">,</span>keypoints_2<span class="token punctuation">,</span>matches<span class="token punctuation">,</span>R<span class="token punctuation">,</span>t<span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token comment">//三角化</span>
vector<span class="token operator"><</span>Point3d<span class="token operator">></span> points<span class="token punctuation">;</span>
<span class="token function">triangulation</span><span class="token punctuation">(</span>keypoints_1<span class="token punctuation">,</span>keypoints_2<span class="token punctuation">,</span>matches<span class="token punctuation">,</span>R<span class="token punctuation">,</span>t<span class="token punctuation">,</span>points<span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token comment">//验证三角化点与特征点的重投影关系</span>
Mat K<span class="token operator">=</span><span class="token punctuation">(</span>Mat_<span class="token operator"><</span><span class="token keyword">double</span><span class="token operator">></span><span class="token punctuation">(</span><span class="token number">3</span><span class="token punctuation">,</span><span class="token number">3</span><span class="token punctuation">)</span><span class="token operator"><<</span><span class="token number">520.9</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">,</span><span class="token number">325.1</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">,</span><span class="token number">521.0</span><span class="token punctuation">,</span><span class="token number">249.7</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">,</span><span class="token number">1</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token keyword">for</span><span class="token punctuation">(</span><span class="token keyword">int</span> i<span class="token operator">=</span><span class="token number">0</span><span class="token punctuation">;</span>i<span class="token operator"><</span>matches<span class="token punctuation">.</span><span class="token function">size</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span><span class="token operator">++</span>i<span class="token punctuation">)</span>
<span class="token punctuation">{</span>
Point2d pt1_cam<span class="token operator">=</span><span class="token function">pixel2cam</span><span class="token punctuation">(</span>keypoints_1<span class="token punctuation">[</span>matches<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>queryIdx<span class="token punctuation">]</span><span class="token punctuation">.</span>pt<span class="token punctuation">,</span>K<span class="token punctuation">)</span><span class="token punctuation">;</span>
Point2d <span class="token function">pt1_cam_3d</span><span class="token punctuation">(</span>
points<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>x<span class="token operator">/</span>points<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>z<span class="token punctuation">,</span>
points<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>y<span class="token operator">/</span>points<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>z
<span class="token punctuation">)</span><span class="token punctuation">;</span>
cout<span class="token operator"><<</span><span class="token string">"point in the first camera frame: "</span><span class="token operator"><<</span>pt1_cam<span class="token operator"><<</span>endl<span class="token punctuation">;</span>
cout<span class="token operator"><<</span><span class="token string">"point projected from 3D"</span><span class="token operator"><<</span>pt1_cam_3d<span class="token operator"><<</span><span class="token string">",d="</span><span class="token operator"><<</span>points<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>z<span class="token operator"><<</span>endl<span class="token punctuation">;</span>
<span class="token comment">//第二个图</span>
Point2f pt2_cam<span class="token operator">=</span><span class="token function">pixel2cam</span><span class="token punctuation">(</span>keypoints_2<span class="token punctuation">[</span>matches<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>trainIdx<span class="token punctuation">]</span><span class="token punctuation">.</span>pt<span class="token punctuation">,</span>K<span class="token punctuation">)</span><span class="token punctuation">;</span>
Mat pt2_trans<span class="token operator">=</span>R<span class="token operator">*</span><span class="token punctuation">(</span>Mat_<span class="token operator"><</span><span class="token keyword">double</span><span class="token operator">></span><span class="token punctuation">(</span><span class="token number">3</span><span class="token punctuation">,</span><span class="token number">1</span><span class="token punctuation">)</span><span class="token operator"><<</span>points<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>x<span class="token punctuation">,</span>points<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>y<span class="token punctuation">,</span>points<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>z<span class="token punctuation">)</span><span class="token operator">+</span>t<span class="token punctuation">;</span>
pt2_trans<span class="token operator">/</span><span class="token operator">=</span>pt2_trans<span class="token punctuation">.</span>at<span class="token operator"><</span><span class="token keyword">double</span><span class="token operator">></span><span class="token punctuation">(</span><span class="token number">2</span><span class="token punctuation">,</span><span class="token number">0</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
cout<span class="token operator"><<</span><span class="token string">"point in the second camera a frame: "</span><span class="token operator"><<</span>pt2_cam<span class="token operator"><<</span>endl<span class="token punctuation">;</span>
cout<span class="token operator"><<</span><span class="token string">"point projected from second frame: "</span><span class="token operator"><<</span>pt2_trans<span class="token punctuation">.</span><span class="token function">t</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token operator"><<</span>endl<span class="token punctuation">;</span>
cout<span class="token operator"><<</span>endl<span class="token punctuation">;</span>
<span class="token punctuation">}</span>
<span class="token keyword">return</span> <span class="token number">0</span><span class="token punctuation">;</span>
<span class="token punctuation">}</span>
</code>
3、创建PnP法的cpp文件
1）、创建PnP法的cpp文件夹
<code class="prism language-bash has-numbering"><span class="token function">touch</span> testPnp.cpp gedit testPnp.cpp </code>
2）、编写PnP法的c++代码
<code class="prism language-cpp has-numbering"><span class="token macro property">#<span class="token directive keyword">include</span> <span class="token string"><iostream></span></span>
<span class="token macro property">#<span class="token directive keyword">include</span> <span class="token string"><opencv2/core/core.hpp></span></span>
<span class="token macro property">#<span class="token directive keyword">include</span> <span class="token string"><opencv2/features2d/features2d.hpp></span></span>
<span class="token macro property">#<span class="token directive keyword">include</span> <span class="token string"><opencv2/highgui/highgui.hpp></span></span>
<span class="token macro property">#<span class="token directive keyword">include</span> <span class="token string"><opencv2/calib3d/calib3d.hpp></span></span>
<span class="token macro property">#<span class="token directive keyword">include</span> <span class="token string"><Eigen/Core></span></span>
<span class="token macro property">#<span class="token directive keyword">include</span> <span class="token string"><Eigen/Geometry></span></span>
<span class="token macro property">#<span class="token directive keyword">include</span> <span class="token string"><g2o/core/base_vertex.h></span></span>
<span class="token macro property">#<span class="token directive keyword">include</span> <span class="token string"><g2o/core/base_unary_edge.h></span></span>
<span class="token macro property">#<span class="token directive keyword">include</span> <span class="token string"><g2o/core/block_solver.h></span></span>
<span class="token macro property">#<span class="token directive keyword">include</span> <span class="token string"><g2o/core/optimization_algorithm_levenberg.h></span></span>
<span class="token macro property">#<span class="token directive keyword">include</span> <span class="token string"><g2o/solvers/csparse/linear_solver_csparse.h></span></span>
<span class="token macro property">#<span class="token directive keyword">include</span> <span class="token string"><g2o/types/sba/types_six_dof_expmap.h></span></span>
<span class="token macro property">#<span class="token directive keyword">include</span> <span class="token string"><chrono></span></span>
<span class="token keyword">using</span> <span class="token keyword">namespace</span> std<span class="token punctuation">;</span>
<span class="token keyword">using</span> <span class="token keyword">namespace</span> cv<span class="token punctuation">;</span>
<span class="token keyword">void</span> find_feature_matches <span class="token punctuation">(</span>
<span class="token keyword">const</span> Mat<span class="token operator">&</span> img_1<span class="token punctuation">,</span> <span class="token keyword">const</span> Mat<span class="token operator">&</span> img_2<span class="token punctuation">,</span>
std<span class="token operator">::</span>vector<span class="token operator"><</span>KeyPoint<span class="token operator">></span><span class="token operator">&</span> keypoints_1<span class="token punctuation">,</span>
std<span class="token operator">::</span>vector<span class="token operator"><</span>KeyPoint<span class="token operator">></span><span class="token operator">&</span> keypoints_2<span class="token punctuation">,</span>
std<span class="token operator">::</span>vector<span class="token operator"><</span> DMatch <span class="token operator">></span><span class="token operator">&</span> matches <span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token comment">// 像素坐标转相机归一化坐标</span>
Point2d pixel2cam <span class="token punctuation">(</span> <span class="token keyword">const</span> Point2d<span class="token operator">&</span> p<span class="token punctuation">,</span> <span class="token keyword">const</span> Mat<span class="token operator">&</span> K <span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token keyword">int</span> main <span class="token punctuation">(</span> <span class="token keyword">int</span> argc<span class="token punctuation">,</span> <span class="token keyword">char</span><span class="token operator">*</span><span class="token operator">*</span> argv <span class="token punctuation">)</span>
<span class="token punctuation">{</span>
<span class="token keyword">if</span> <span class="token punctuation">(</span> argc <span class="token operator">!=</span> <span class="token number">4</span> <span class="token punctuation">)</span>
<span class="token punctuation">{</span>
cout<span class="token operator"><<</span><span class="token string">"usage: pose_estimation_3d2d img1 img2 depth1"</span><span class="token operator"><<</span>endl<span class="token punctuation">;</span>
<span class="token keyword">return</span> <span class="token number">1</span><span class="token punctuation">;</span>
<span class="token punctuation">}</span>
<span class="token comment">//-- 读取图像</span>
Mat img_1 <span class="token operator">=</span> imread <span class="token punctuation">(</span> argv<span class="token punctuation">[</span><span class="token number">1</span><span class="token punctuation">]</span><span class="token punctuation">,</span> CV_LOAD_IMAGE_COLOR <span class="token punctuation">)</span><span class="token punctuation">;</span>
Mat img_2 <span class="token operator">=</span> imread <span class="token punctuation">(</span> argv<span class="token punctuation">[</span><span class="token number">2</span><span class="token punctuation">]</span><span class="token punctuation">,</span> CV_LOAD_IMAGE_COLOR <span class="token punctuation">)</span><span class="token punctuation">;</span>
vector<span class="token operator"><</span>KeyPoint<span class="token operator">></span> keypoints_1<span class="token punctuation">,</span> keypoints_2<span class="token punctuation">;</span>
vector<span class="token operator"><</span>DMatch<span class="token operator">></span> matches<span class="token punctuation">;</span>
find_feature_matches <span class="token punctuation">(</span> img_1<span class="token punctuation">,</span> img_2<span class="token punctuation">,</span> keypoints_1<span class="token punctuation">,</span> keypoints_2<span class="token punctuation">,</span> matches <span class="token punctuation">)</span><span class="token punctuation">;</span>
cout<span class="token operator"><<</span><span class="token string">"一共找到了"</span><span class="token operator"><<</span>matches<span class="token punctuation">.</span><span class="token function">size</span><span class="token punctuation">(</span><span class="token punctuation">)</span> <span class="token operator"><<</span><span class="token string">"组匹配点"</span><span class="token operator"><<</span>endl<span class="token punctuation">;</span>
<span class="token comment">// 建立3D点</span>
Mat d1 <span class="token operator">=</span> imread <span class="token punctuation">(</span> argv<span class="token punctuation">[</span><span class="token number">3</span><span class="token punctuation">]</span><span class="token punctuation">,</span> CV_LOAD_IMAGE_UNCHANGED <span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token comment">// 深度图为16位无符号数，单通道图像</span>
Mat K <span class="token operator">=</span> <span class="token punctuation">(</span> Mat_<span class="token operator"><</span><span class="token keyword">double</span><span class="token operator">></span> <span class="token punctuation">(</span> <span class="token number">3</span><span class="token punctuation">,</span><span class="token number">3</span> <span class="token punctuation">)</span> <span class="token operator"><<</span> <span class="token number">520.9</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">325.1</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">521.0</span><span class="token punctuation">,</span> <span class="token number">249.7</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">1</span> <span class="token punctuation">)</span><span class="token punctuation">;</span>
vector<span class="token operator"><</span>Point3f<span class="token operator">></span> pts_3d<span class="token punctuation">;</span>
vector<span class="token operator"><</span>Point2f<span class="token operator">></span> pts_2d<span class="token punctuation">;</span>
<span class="token keyword">for</span> <span class="token punctuation">(</span> DMatch m<span class="token operator">:</span>matches <span class="token punctuation">)</span>
<span class="token punctuation">{</span>
ushort d <span class="token operator">=</span> d1<span class="token punctuation">.</span>ptr<span class="token operator"><</span><span class="token keyword">unsigned</span> <span class="token keyword">short</span><span class="token operator">></span> <span class="token punctuation">(</span><span class="token keyword">int</span> <span class="token punctuation">(</span> keypoints_1<span class="token punctuation">[</span>m<span class="token punctuation">.</span>queryIdx<span class="token punctuation">]</span><span class="token punctuation">.</span>pt<span class="token punctuation">.</span>y <span class="token punctuation">)</span><span class="token punctuation">)</span> <span class="token punctuation">[</span> <span class="token keyword">int</span> <span class="token punctuation">(</span> keypoints_1<span class="token punctuation">[</span>m<span class="token punctuation">.</span>queryIdx<span class="token punctuation">]</span><span class="token punctuation">.</span>pt<span class="token punctuation">.</span>x <span class="token punctuation">)</span> <span class="token punctuation">]</span><span class="token punctuation">;</span>
<span class="token keyword">if</span> <span class="token punctuation">(</span> d <span class="token operator">==</span> <span class="token number">0</span> <span class="token punctuation">)</span> <span class="token comment">// bad depth</span>
<span class="token keyword">continue</span><span class="token punctuation">;</span>
<span class="token keyword">float</span> dd <span class="token operator">=</span> d<span class="token operator">/</span><span class="token number">5000.0</span><span class="token punctuation">;</span>
Point2d p1 <span class="token operator">=</span> pixel2cam <span class="token punctuation">(</span> keypoints_1<span class="token punctuation">[</span>m<span class="token punctuation">.</span>queryIdx<span class="token punctuation">]</span><span class="token punctuation">.</span>pt<span class="token punctuation">,</span> K <span class="token punctuation">)</span><span class="token punctuation">;</span>
pts_3d<span class="token punctuation">.</span>push_back <span class="token punctuation">(</span> Point3f <span class="token punctuation">(</span> p1<span class="token punctuation">.</span>x<span class="token operator">*</span>dd<span class="token punctuation">,</span> p1<span class="token punctuation">.</span>y<span class="token operator">*</span>dd<span class="token punctuation">,</span> dd <span class="token punctuation">)</span> <span class="token punctuation">)</span><span class="token punctuation">;</span>
pts_2d<span class="token punctuation">.</span>push_back <span class="token punctuation">(</span> keypoints_2<span class="token punctuation">[</span>m<span class="token punctuation">.</span>trainIdx<span class="token punctuation">]</span><span class="token punctuation">.</span>pt <span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token punctuation">}</span>
cout<span class="token operator"><<</span><span class="token string">"3d-2d pairs: "</span><span class="token operator"><<</span>pts_3d<span class="token punctuation">.</span><span class="token function">size</span><span class="token punctuation">(</span><span class="token punctuation">)</span> <span class="token operator"><<</span>endl<span class="token punctuation">;</span>
Mat r<span class="token punctuation">,</span> t<span class="token punctuation">;</span>
solvePnP <span class="token punctuation">(</span> pts_3d<span class="token punctuation">,</span> pts_2d<span class="token punctuation">,</span> K<span class="token punctuation">,</span> <span class="token function">Mat</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">,</span> r<span class="token punctuation">,</span> t<span class="token punctuation">,</span> <span class="token boolean">false</span> <span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token comment">// 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法</span>
Mat R<span class="token punctuation">;</span>
cv<span class="token operator">::</span>Rodrigues <span class="token punctuation">(</span> r<span class="token punctuation">,</span> R <span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token comment">// r为旋转向量形式，用Rodrigues公式转换为矩阵</span>
cout<span class="token operator"><<</span><span class="token string">"R="</span><span class="token operator"><<</span>endl<span class="token operator"><<</span>R<span class="token operator"><<</span>endl<span class="token punctuation">;</span>
cout<span class="token operator"><<</span><span class="token string">"t="</span><span class="token operator"><<</span>endl<span class="token operator"><<</span>t<span class="token operator"><<</span>endl<span class="token punctuation">;</span>
<span class="token punctuation">}</span>
<span class="token keyword">void</span> find_feature_matches <span class="token punctuation">(</span> <span class="token keyword">const</span> Mat<span class="token operator">&</span> img_1<span class="token punctuation">,</span> <span class="token keyword">const</span> Mat<span class="token operator">&</span> img_2<span class="token punctuation">,</span>
std<span class="token operator">::</span>vector<span class="token operator"><</span>KeyPoint<span class="token operator">></span><span class="token operator">&</span> keypoints_1<span class="token punctuation">,</span>
std<span class="token operator">::</span>vector<span class="token operator"><</span>KeyPoint<span class="token operator">></span><span class="token operator">&</span> keypoints_2<span class="token punctuation">,</span>
std<span class="token operator">::</span>vector<span class="token operator"><</span> DMatch <span class="token operator">></span><span class="token operator">&</span> matches <span class="token punctuation">)</span>
<span class="token punctuation">{</span>
<span class="token comment">//-- 初始化</span>
Mat descriptors_1<span class="token punctuation">,</span> descriptors_2<span class="token punctuation">;</span>
<span class="token comment">// used in OpenCV3</span>
Ptr<span class="token operator"><</span>FeatureDetector<span class="token operator">></span> detector <span class="token operator">=</span> ORB<span class="token operator">::</span><span class="token function">create</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
Ptr<span class="token operator"><</span>DescriptorExtractor<span class="token operator">></span> descriptor <span class="token operator">=</span> ORB<span class="token operator">::</span><span class="token function">create</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token comment">// use this if you are in OpenCV2</span>
<span class="token comment">// Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );</span>
<span class="token comment">// Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );</span>
Ptr<span class="token operator"><</span>DescriptorMatcher<span class="token operator">></span> matcher <span class="token operator">=</span> DescriptorMatcher<span class="token operator">::</span>create <span class="token punctuation">(</span> <span class="token string">"BruteForce-Hamming"</span> <span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token comment">//-- 第一步:检测 Oriented FAST 角点位置</span>
detector<span class="token operator">-</span><span class="token operator">></span>detect <span class="token punctuation">(</span> img_1<span class="token punctuation">,</span>keypoints_1 <span class="token punctuation">)</span><span class="token punctuation">;</span>
detector<span class="token operator">-</span><span class="token operator">></span>detect <span class="token punctuation">(</span> img_2<span class="token punctuation">,</span>keypoints_2 <span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token comment">//-- 第二步:根据角点位置计算 BRIEF 描述子</span>
descriptor<span class="token operator">-</span><span class="token operator">></span>compute <span class="token punctuation">(</span> img_1<span class="token punctuation">,</span> keypoints_1<span class="token punctuation">,</span> descriptors_1 <span class="token punctuation">)</span><span class="token punctuation">;</span>
descriptor<span class="token operator">-</span><span class="token operator">></span>compute <span class="token punctuation">(</span> img_2<span class="token punctuation">,</span> keypoints_2<span class="token punctuation">,</span> descriptors_2 <span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token comment">//-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离</span>
vector<span class="token operator"><</span>DMatch<span class="token operator">></span> match<span class="token punctuation">;</span>
<span class="token comment">// BFMatcher matcher ( NORM_HAMMING );</span>
matcher<span class="token operator">-</span><span class="token operator">></span>match <span class="token punctuation">(</span> descriptors_1<span class="token punctuation">,</span> descriptors_2<span class="token punctuation">,</span> match <span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token comment">//-- 第四步:匹配点对筛选</span>
<span class="token keyword">double</span> min_dist<span class="token operator">=</span><span class="token number">10000</span><span class="token punctuation">,</span> max_dist<span class="token operator">=</span><span class="token number">0</span><span class="token punctuation">;</span>
<span class="token comment">//找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离</span>
<span class="token keyword">for</span> <span class="token punctuation">(</span> <span class="token keyword">int</span> i <span class="token operator">=</span> <span class="token number">0</span><span class="token punctuation">;</span> i <span class="token operator"><</span> descriptors_1<span class="token punctuation">.</span>rows<span class="token punctuation">;</span> i<span class="token operator">++</span> <span class="token punctuation">)</span>
<span class="token punctuation">{</span>
<span class="token keyword">double</span> dist <span class="token operator">=</span> match<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>distance<span class="token punctuation">;</span>
<span class="token keyword">if</span> <span class="token punctuation">(</span> dist <span class="token operator"><</span> min_dist <span class="token punctuation">)</span> min_dist <span class="token operator">=</span> dist<span class="token punctuation">;</span>
<span class="token keyword">if</span> <span class="token punctuation">(</span> dist <span class="token operator">></span> max_dist <span class="token punctuation">)</span> max_dist <span class="token operator">=</span> dist<span class="token punctuation">;</span>
<span class="token punctuation">}</span>
printf <span class="token punctuation">(</span> <span class="token string">"-- Max dist : %f \n"</span><span class="token punctuation">,</span> max_dist <span class="token punctuation">)</span><span class="token punctuation">;</span>
printf <span class="token punctuation">(</span> <span class="token string">"-- Min dist : %f \n"</span><span class="token punctuation">,</span> min_dist <span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token comment">//当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.</span>
<span class="token keyword">for</span> <span class="token punctuation">(</span> <span class="token keyword">int</span> i <span class="token operator">=</span> <span class="token number">0</span><span class="token punctuation">;</span> i <span class="token operator"><</span> descriptors_1<span class="token punctuation">.</span>rows<span class="token punctuation">;</span> i<span class="token operator">++</span> <span class="token punctuation">)</span>
<span class="token punctuation">{</span>
<span class="token keyword">if</span> <span class="token punctuation">(</span> match<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">.</span>distance <span class="token operator"><=</span> max <span class="token punctuation">(</span> <span class="token number">2</span><span class="token operator">*</span>min_dist<span class="token punctuation">,</span> <span class="token number">30.0</span> <span class="token punctuation">)</span> <span class="token punctuation">)</span>
<span class="token punctuation">{</span>
matches<span class="token punctuation">.</span>push_back <span class="token punctuation">(</span> match<span class="token punctuation">[</span>i<span class="token punctuation">]</span> <span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token punctuation">}</span>
<span class="token punctuation">}</span>
<span class="token punctuation">}</span>
Point2d pixel2cam <span class="token punctuation">(</span> <span class="token keyword">const</span> Point2d<span class="token operator">&</span> p<span class="token punctuation">,</span> <span class="token keyword">const</span> Mat<span class="token operator">&</span> K <span class="token punctuation">)</span>
<span class="token punctuation">{</span>
<span class="token keyword">return</span> Point2d
<span class="token punctuation">(</span>
<span class="token punctuation">(</span> p<span class="token punctuation">.</span>x <span class="token operator">-</span> K<span class="token punctuation">.</span>at<span class="token operator"><</span><span class="token keyword">double</span><span class="token operator">></span> <span class="token punctuation">(</span> <span class="token number">0</span><span class="token punctuation">,</span><span class="token number">2</span> <span class="token punctuation">)</span> <span class="token punctuation">)</span> <span class="token operator">/</span> K<span class="token punctuation">.</span>at<span class="token operator"><</span><span class="token keyword">double</span><span class="token operator">></span> <span class="token punctuation">(</span> <span class="token number">0</span><span class="token punctuation">,</span><span class="token number">0</span> <span class="token punctuation">)</span><span class="token punctuation">,</span>
<span class="token punctuation">(</span> p<span class="token punctuation">.</span>y <span class="token operator">-</span> K<span class="token punctuation">.</span>at<span class="token operator"><</span><span class="token keyword">double</span><span class="token operator">></span> <span class="token punctuation">(</span> <span class="token number">1</span><span class="token punctuation">,</span><span class="token number">2</span> <span class="token punctuation">)</span> <span class="token punctuation">)</span> <span class="token operator">/</span> K<span class="token punctuation">.</span>at<span class="token operator"><</span><span class="token keyword">double</span><span class="token operator">></span> <span class="token punctuation">(</span> <span class="token number">1</span><span class="token punctuation">,</span><span class="token number">1</span> <span class="token punctuation">)</span>
<span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token punctuation">}</span>
</code>
4、创建CMakeLists.txt配置文件
1）、创建CMakeLists.txt配置文件
<code class="prism language-bash has-numbering"><span class="token function">touch</span> CMakeLists.txt gedit CMakeLists.txt </code>
2）、编写CMakeLists.txt配置文件内容
<code class="prism language-bash has-numbering">cmake_minimum_required<span class="token punctuation">(</span>VERSION 2.8<span class="token punctuation">)</span>
project<span class="token punctuation">(</span>test<span class="token punctuation">)</span>
list<span class="token punctuation">(</span>APPEND CMAKE_MODULE_PATH <span class="token variable">${PROJECT_SOURCE_DIR}</span>/cmake_modules<span class="token punctuation">)</span>
set<span class="token punctuation">(</span>CMAKE_CXX_STANDARD 11<span class="token punctuation">)</span>
set<span class="token punctuation">(</span> G2O_ROOT /usr/local/include/g2o <span class="token punctuation">)</span>
find_package<span class="token punctuation">(</span> OpenCV REQUIRED<span class="token punctuation">)</span>
find_package<span class="token punctuation">(</span>Eigen3 REQUIRED<span class="token punctuation">)</span>
find_package<span class="token punctuation">(</span>G2O REQUIRED<span class="token punctuation">)</span>
find_package<span class="token punctuation">(</span> CSparse<span class="token punctuation">)</span>
include_directories<span class="token punctuation">(</span>
<span class="token variable">${EIGEN3_INCLUDE_DIR}</span>
<span class="token variable">${OpenCV_INCLUDE_DIRS}</span>
<span class="token variable">${G2O_INCLUDE_DIRS}</span>
<span class="token variable">${CSPARSE_INCLUDE_DIR}</span>
<span class="token punctuation">)</span>
add_executable<span class="token punctuation">(</span>testPnp testPnp.cpp<span class="token punctuation">)</span>
add_executable<span class="token punctuation">(</span>testTriang testTriang.cpp<span class="token punctuation">)</span>
target_link_libraries<span class="token punctuation">(</span> testTriang <span class="token variable">${OpenCV_LIBS}</span><span class="token punctuation">)</span>
target_link_libraries<span class="token punctuation">(</span> testPnp <span class="token variable">${EIGEN3_LIB}</span> <span class="token variable">${OpenCV_LIBS}</span> <span class="token variable">${G2O_LIB}</span> <span class="token variable">${CSPARSE_LIB}</span><span class="token punctuation">)</span>
target_link_libraries<span class="token punctuation">(</span>testPnp g2o_core g2o_stuff<span class="token punctuation">)</span>
</code>
三、编译项目
1、文件准备
(1）、将上面我们编译g2o中的cmake_modules文件夹复制到我们test项目文件夹中，如下所示：
(2）、在test项目下的build中添加两张图片，如下所示：
(3）、将上面的两张拍摄的图片变为深度图(灰度图)，后存放该处：
什么是深度图： 深度图为16位无符号数，单通道图像，也就是，我们只需要将我们的图片变为灰度图就行了，方法很多，小伙伴们可以自己摸索哦！ 图片的拍摄必须是同一个物体，不同的角度，也就是要具有相同的特征点
2、项目进行编译
(1）、进入build文件夹进行编译
<code class="prism language-bash has-numbering"><span class="token function">cd</span> build </code>
2）、编译
<code class="prism language-bash has-numbering">cmake <span class="token punctuation">..</span> <span class="token function">make</span> -j4 </code>
(3）、编译后的整体项目如下： test目录下：
build目录下：
如上所示出现百分百则为成功，由于林君学长是编译了一次的，所以编译很快，小伙伴则需要等待一小会儿
2、结果运行
(1）、三角测量算法的结果运行
<code class="prism language-bash has-numbering">./testTriang 1.jpg 2.jpg </code>
（2）、PnP法的结果运行
<code class="prism language-bash has-numbering">./testPnp 1.jpg 2.jpg 11.jpg </code>
开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明视觉SLAM学习【3】—–视觉SLAM通过三角测量和PnP法估计特征点的空间位置(采用make编译方式)！
