专栏首页深度学习和计算机视觉高翔Slambook第七讲代码解读(3d-2d位姿估计)

高翔Slambook第七讲代码解读(3d-2d位姿估计)

上回咱们读完了pose_estimation_2d2d.cpp这个文件,基本上明白了通过对极几何计算相机位姿变换的过程,简单地说就是:你给我两帧图像,我给你算个R、t。

我们按部就班,跟着小绿来看一下接下来要读的程序——pose_estimation_3d2d。

这里小绿简单的拿两张图来看一下2d-2d与3d-2d在本质上的区别:

↑两张平面图

↑一张平面图+一张深度图 与一张平面图

这个程序,顾名思义,便是已知一帧图像中特征点的3d位置信息,以及另一帧图像中特征点的2d位置信息,进行相机的位姿变换计算。其中,3d位置信息是指该特征点所对应的真实物体点,在当前相机坐标系下的坐标;2d位置信息则是特征点的像素坐标。这里3d位置信息是由RGB-D相机提供的深度信息进行计算得到的。

我们来看一下子函数声明和主函数

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <chrono>

using namespace std;
using namespace cv;

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

// 像素坐标转相机归一化坐标
Point2d pixel2cam ( const Point2d& p, const Mat& K );

void bundleAdjustment (
    const vector<Point3f> points_3d,
    const vector<Point2f> points_2d,
    const Mat& K,
    Mat& R, Mat& t
);

int main ( int argc, char** argv )
{
    if ( argc != 5 )
    {
        cout<<"usage: pose_estimation_3d2d img1 img2 depth1 depth2"<<endl;
        return 1;
    }
    //-- 读取图像
    Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );
    Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );

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

    // 建立3D点
    Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED );       // 深度图为16位无符号数,单通道图像
    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
    vector<Point3f> pts_3d;
    vector<Point2f> pts_2d;
    for ( DMatch m:matches )
    {
        ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
        if ( d == 0 )   // bad depth
            continue;
        float dd = d/5000.0;
        Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
        pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );
        pts_2d.push_back ( keypoints_2[m.trainIdx].pt );
    }

    cout<<"3d-2d pairs: "<<pts_3d.size() <<endl;

    Mat r, t;
    solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false ); // 调用OpenCV 的 PnP 求解,可选择EPNP,DLS等方法
    Mat R;
    cv::Rodrigues ( r, R ); // r为旋转向量形式,用Rodrigues公式转换为矩阵

    cout<<"R="<<endl<<R<<endl;
    cout<<"t="<<endl<<t<<endl;

    cout<<"calling bundle adjustment"<<endl;

    bundleAdjustment ( pts_3d, pts_2d, K, R, t );
    return 0;
}

可以看到一共使用了三个子函数:find_feature_matches、pixel2cam和bundleAdjustment。这里引用白白的一张图:

图中的BA指的就是Bundle Adjustment(光束平差法,即最小化重投影误差),在这里针对本程序则封装为一个函数进行调用。在这三个子函数中,find_feature_matches即特征点匹配,用来匹配两帧图像中的特征点;pixel2cam即像素坐标到归一化平面坐标变换,用来转换坐标:这两杆数是我们研读过的,在此不做赘述。只有第三个子函数bundleAdjustment是个新面孔,的确,他是3d-2d位姿估计中的重点。bundleAdjustment函数无返回值,形参为存储Point3f类对象的容器points_3d、存储Point2f类对象的容器points_2d、Mat类矩阵K、R和t。通过const限定符可以推算该函数是要修改引用调用的R和t,即通过一组点的3d坐标、一组点的2d坐标求取相机位姿变换。我们先来看看主函数,最后再对bundleAdjustment进行梳理。

主函数

if ( argc != 5 )

    {
       cout<<"usage:pose_estimation_3d2d img1 img2 depth1 depth2"<<endl;
        return 1;
}

这里表示需要额外传入四个参数:img1、img2、depth1和depth2。其实在3d-2d匹配过程中,我们只需要前一帧的深度信息,因此可以将argc的判断改为4,不再传入depth2也是可以的。

Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED );

读入了argv[3]对应的参数,即depth_1.png,前一帧的深度信息图,调用cv::imread存入一个480*640的Mat类变量d1。

// 建立3D点
   vector<Point3f> pts_3d;
   vector<Point2f> pts_2d;
    for ( DMatch m:matches )
    {
        ushortd = d1.ptr<unsigned short>(int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
        if ( d == 0 )  // bad depth

            continue;

        float dd = d/5000.0;
        Point2dp1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
        pts_3d.push_back ( Point3f ( p1.x*dd,p1.y*dd, dd ) );
        pts_2d.push_back ( keypoints_2[m.trainIdx].pt );
    }

首先定义了两个容器,分别用于存储点的3d坐标和2d坐标,故容器内元素类型分别为Point3f和Point2f。注意,这里将容器命名为pts_3d与pts_2d并不是说其坐标值是double类型的,而是3-dimention与2-dimention。

那么在接下来的循环中,我们就是要将matches中每一对点的坐标分别存入3d容器与2d容器中去。这里我们需要使用前一帧图像中特征点的深度信息,因此需要在深度信息矩阵d中提取出来该特征点所对应的深度信息,于是定义了一个无符号整型变量d,并进行了如下初始化:

ushort d = d1.ptr<unsignedshort> (int( keypoints_1[m.queryIdx].pt.y )) [ int (keypoints_1[m.queryIdx].pt.x ) ];

我们分离出其原型:

ushort d = d1.ptr<unsignedshort> ( row )[ cloumn ];

其中使用.ptr函数访问Mat类对象d1的第row行首地址,[ column ]表示本行的第column个对象,整体来看就是获取了d1内第row行第column列的元素的值,存储为uchar类型。注意,图像中第m行第n列的数据(即像素坐标为(m,n))存储在Mat类对象中,其数据将位于第n行第m列,因此比方说我们要看看像素坐标为(0,1)的灰度值,就需要去找一下灰度矩阵第2行第1列的值,即img.ptr<uchar>(1)[0]。在写程序时不要写反行和列,因为编译时不会报错,但得到的结果会出错,甚至产生越界。

其实这里我们也可以用上一期所学的,使用.at()函数来访问Mat类对象内部的值:

ushort d = d1.ptr<unsignedshort> (int( keypoints_1[m.queryIdx].pt.y )) [ int (keypoints_1[m.queryIdx].pt.x ) ];

其结果是一样的(注意keypoints内存储的特征点横纵坐标虽然是像素坐标,为整数,但仍是float类型的,需要强转为int类型)。

float dd = d/5000.0;
Point2dp1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );
pts_2d.push_back ( keypoints_2[m.trainIdx].pt );

在得到深度信息后,我们将其除以一个常数方便计算(取5000也可能是内定的,这个数可以改,成反比例影响t的求取,对R的结果无影响)。进而,将需要计算3d坐标点(前一帧)的特征点的像素坐标转化为归一化平面坐标,并结合深度信息计算相机坐标系下的坐标:

最终存于Point3f类的容器pts_3d中。

  // 调用OpenCV 的 PnP 求解,可选择EPNP,DLS等方法
    Mat r, t;
    solvePnP (pts_3d, pts_2d, K, Mat(), r, t, false );
    Mat R;
    cv::Rodrigues ( r, R ); // r为旋转向量形式,用Rodrigues公式转换为矩阵

这里神奇的事情出现了:我们调用OpenCV提供的solvePnP函数(并结合罗德里格斯变换),直接求出了旋转矩阵R和平移向量t。好了,大功告成,3d-2d位姿求取拿pnp就直接搞定了。然而,只使用pnp解算出的R、t往往具有较大误差,只能作为估计值,实际应用中还需要构建最小二乘优化问题对估计值进行调整(Bundle Adjustment)(高翔Slambook原话)。下面我们来看一下这个最关键的子函数bundleAdjustment。

bundleAdjustment

void bundleAdjustment (
    const vector< Point3f > points_3d,
    const vector< Point2f > points_2d,
    const Mat& K,
    Mat& R, Mat& t )
{
    // 初始化g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  
    // pose 维度为 6, landmark 维度为 3
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); 
    // 线性方程求解器
   
    
    
//     Block* solver_ptr = new Block ( linearSolver );     // 矩阵块求解器
//     g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
    
       Block* solver_ptr = new Block( std::unique_ptr<Block::LinearSolverType>(linearSolver) );
       g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<Block>(solver_ptr) );
    
    
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm ( solver );

    // vertex
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose
    Eigen::Matrix3d R_mat;
    R_mat <<
          R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ),
               R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ),
               R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 );
    pose->setId ( 0 );
    pose->setEstimate ( g2o::SE3Quat (
                            R_mat,
                            Eigen::Vector3d ( t.at<double> ( 0,0 ), t.at<double> ( 1,0 ), t.at<double> ( 2,0 ) )
                        ) );
    optimizer.addVertex ( pose );

    int index = 1;
    for ( const Point3f p:points_3d )   // landmarks
    {
        g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
        point->setId ( index++ );
        point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );
        point->setMarginalized ( true ); // g2o 中必须设置 marg 参见第十讲内容
        optimizer.addVertex ( point );
    }

    // parameter: camera intrinsics
    g2o::CameraParameters* camera = new g2o::CameraParameters (
        K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0
    );
    camera->setId ( 0 );
    optimizer.addParameter ( camera );

    // edges
    index = 1;
    for ( const Point2f p:points_2d )
    {
        g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
        edge->setId ( index );
        edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
        edge->setVertex ( 1, pose );
        edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );
        edge->setParameterId ( 0,0 );
        edge->setInformation ( Eigen::Matrix2d::Identity() );
        optimizer.addEdge ( edge );
        index++;
    }

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.setVerbose ( true );
    optimizer.initializeOptimization();
    optimizer.optimize ( 100 );
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );
    cout<<"optimization costs time: "<<time_used.count() <<" seconds."<<endl;

    cout<<endl<<"after optimization:"<<endl;
    cout<<"T="<<endl<<Eigen::Isometry3d ( pose->estimate() ).matrix() <<endl;
}

这里需要注意,由于g2o版本改动,高翔的代码会报错(我给注释上了),按照网上的方案修改后运行无误。

将节点定义为李代数形式的第二帧相机位姿,与所有特征点的空间位置;将边定义为每个3D点在第二个相机中的投影。位姿使用李代数形式,为6自由度;空间坐标点为3自由度,因而参数为6、3。这里将参数块求解器重命名为Block(也有的程序将其命名为BlockSlover_6_3等等)。

迭代方式选择列文伯格马夸尔特(LM法),将之前使用PnP求解出的R、t进行传入后,在此作为BundleAdjustment的迭代初值。进而设置好图模型中的节点与边,打开求解器,并指定迭代次数上限为100次,开始迭代求解。最后解出优化后的变换矩阵T。

其实关于使用g2o的BA模块,小绿实在难以深入阐述甚至理解其中的很多代码。包括3d-2d中使用的BA模块,以及下一个.cpp中3d-3d的BA模块,小绿认为完全可以在定义好所需要的类后(视情况需要),将BA模块作为一个函数封装进行调用,即输入给定的3d或2d坐标和相机内参(视情况需要),以及迭代初值R、t,就可以按照已经设定好的图模型进行图优化处理。

本程序在传入R、t之后,虽然没有加const限定符并使用引用调用&,但事实上没有修改(优化)R、t的值,而是直接cout了优化后的变换矩阵T。我们来看一下程序的运行结果:

对比T的左上方3×3矩阵,与PnP直接求得的R,我们可以发现差距并不大;对比T右侧3×1矩阵,与PnP直接求得的t,我们发现差距也不大。但毕竟是经过了一个最小二乘优化问题的求解,既然有些微小的变化,我们宁愿相信BA是起着优化R、t求解的作用的。

好了,本次的3d-2d位姿求解程序解读就这样草草收尾(g2o看不懂,小绿强行将其黑箱化)。在实际的SLAM工程中,优化部分占着极大的比重,不仅因为其计算量巨大,更在于定位、建图的精确程度基本由后端优化的精良所决定。使用图优化理论进行非线性优化其实特别重要,因此g2o库的使用、图模型的建立、边与节点的定义、求解器的定义与初始化与具体使用等细节操作...等等等等,这些小绿都需要日后完善。

最后,希望在SLAM道路上的各位敢于攻坚。共勉。

相关阅读

视觉SLAM十四讲第七章代码解读(一)

视觉SLAM十四讲第七章代码解读(二)

图像特征点|Moravec特征点

Ubuntu 16.04安装ROS教程

Windows&Ubuntu16.04 双系统图文教程

本文分享自微信公众号 - 小白学视觉(NoobCV),作者:小绿

原文出处及转载信息见文内详细说明,如有侵权,请联系 yunjia_community@tencent.com 删除。

原始发表时间:2018-11-08

本文参与腾讯云自媒体分享计划,欢迎正在阅读的你也加入,一起分享。

我来说两句

0 条评论
登录 后参与评论

相关文章

  • 【OpenCV教程】core 模块 - Mat - 基本图像容器

    从真实世界中获取数字图像有很多方法,比如数码相机、扫描仪、CT或者磁共振成像。无论哪种方法,我们(人类)看到的是图像,而让数字设备来“看“的时候,都是在记录图像...

    小白学视觉
  • 人眼疲倦检测开源算法

    这次小白为小伙伴们带来了和生活相关性很大的一个系统。司机疲劳驾驶一直都是交通事故高发的主要原因。因此本项目的开发者计划通过视觉观测人的眼睛来判断被观察者是否存在...

    小白学视觉
  • 三维重建新应用:精确还原凶杀现场细节!

    SLAM和三维重建的落地应用一直走的比较艰难,之前华为mate30 发布三维重建应用时,还是很亮眼的,计算机视觉life 当时也第一时间进行了解读(看这里:华为...

    小白学视觉
  • 计算三角形面积

    汐楓
  • tkinter -- Event(1)

    分别测试鼠标的 Relase 事件,只有当鼠标被 Relase 后移动才回产生 Relase 事件

    py3study
  • 币圈用户请小心!WebInjects可以清空你的加密货币账户

    网络犯罪分子正在尝试各种各样的方法来将他们的“罪恶之手”伸向广大用户的加密货币钱包。研究人员发现,攻击者正在利用能够劫持浏览器(中间人攻击的一种)的恶意软件来劫...

    FB客服
  • 1.7编程基础之字符串03:基因相关性

    #include<iostream> #include<cstring> #include<cstdio> #include<cstdlib> using na...

    attack
  • 如何把微信二维码藏进命令行里

    landv
  • RPA应用于IT服务的十大场景

    大量企业的实践表明,在IT项目的生命周期中,大约80%的时间与IT项目运营维护有关,而该阶段的投资仅占整个IT投资的20%,由此形成了典型的“轻服务、重技术”现...

    蕉黄
  • 《数据可视化基础》第七章:分布可视化:经验累积分布函数和q-q图

    以下部分是基于《Fundamentals of Data Visualization》学习笔记,要是有兴趣的话,可以直接看原版书籍:https://serial...

    医学数据库百科

扫码关注云+社区

领取腾讯云代金券