• 欢迎访问开心洋葱网站,在线教程,推荐使用最新版火狐浏览器和Chrome浏览器访问本网站,欢迎加入开心洋葱 QQ群
  • 为方便开心洋葱网用户,开心洋葱官网已经开启复制功能!
  • 欢迎访问开心洋葱网站,手机也能访问哦~欢迎加入开心洋葱多维思维学习平台 QQ群
  • 如果您觉得本站非常有看点,那么赶紧使用Ctrl+D 收藏开心洋葱吧~~~~~~~~~~~~~!
  • 由于近期流量激增,小站的ECS没能经的起亲们的访问,本站依然没有盈利,如果各位看如果觉着文字不错,还请看官给小站打个赏~~~~~~~~~~~~~!

视觉SLAM学习【3】—–视觉SLAM通过三角测量和PnP法估计特征点的空间位置(采用make编译方式)

人工智能 陈一月的编程岁月 2900次浏览 0个评论

视觉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  
视觉SLAM学习【3】-----视觉SLAM通过三角测量和PnP法估计特征点的空间位置(采用make编译方式)

2、文件上传ubuntu

  (1)、将下载好的g2o的源代码资源包上传至ubuntu的任意位置解压并重新命名为g2o,下面以林君学长的路径为例  
视觉SLAM学习【3】-----视觉SLAM通过三角测量和PnP法估计特征点的空间位置(采用make编译方式)

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-j4make  
视觉SLAM学习【3】-----视觉SLAM通过三角测量和PnP法估计特征点的空间位置(采用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项目文件夹中,如下所示:  
视觉SLAM学习【3】-----视觉SLAM通过三角测量和PnP法估计特征点的空间位置(采用make编译方式)
视觉SLAM学习【3】-----视觉SLAM通过三角测量和PnP法估计特征点的空间位置(采用make编译方式)   (2)、在test项目下的build中添加两张图片,如下所示:  
视觉SLAM学习【3】-----视觉SLAM通过三角测量和PnP法估计特征点的空间位置(采用make编译方式)   (3)、将上面的两张拍摄的图片变为深度图(灰度图),后存放该处:  
视觉SLAM学习【3】-----视觉SLAM通过三角测量和PnP法估计特征点的空间位置(采用make编译方式)   什么是深度图: 深度图为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>

 
视觉SLAM学习【3】-----视觉SLAM通过三角测量和PnP法估计特征点的空间位置(采用make编译方式)   (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编译方式)


开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明视觉SLAM学习【3】—–视觉SLAM通过三角测量和PnP法估计特征点的空间位置(采用make编译方式)
喜欢 (0)

您必须 登录 才能发表评论!

加载中……