双视图bundle adjustment注释程序

最近在学习高博的SLAM14讲，在g2o的部分有一些困惑，便去特别的学习了一下这部分知识，主要参考了以下几篇文章：

1、高博的博客中有一篇讲述g2o的：https://www.cnblogs.com/gaoxiang12/p/5304272.html

2、从零开始一起学习SLAM有三篇文章：总览、顶点编程和边编程

根据这几篇文章，我又把高博博客中的BA例程跑了一下，并根据我自己的理解添加了注释。

一、代码修改

由于opencv库和g2o库的版本升级，高博的例程不能直接使用，需要进行一定的修改：

1、opencv部分

opencv部分代码在函数寻找匹配点的部分，也就是findCorrespondingPoints()函数，主要是由于版本更新导致Orb变成了一个纯虚类，不能进行实例化了，源代码如下：

    cv::ORB orb;
    vector<cv::KeyPoint> kp1, kp2;
    cv::Mat desp1, desp2;
    orb( img1, cv::Mat(), kp1, desp1 );
    orb( img2, cv::Mat(), kp2, desp2 );
    cout<<"分别找到了"<<kp1.size()<<"和"<<kp2.size()<<"个特征点"<<endl;

    cv::Ptr<cv::DescriptorMatcher>  matcher = cv::DescriptorMatcher::create( "BruteForce-Hamming");

修改后：

    //由于opencv版本升级，orb现在的是一个纯虚类，无法使用原来的方法进行实例化了
    cv::Ptr<cv::FeatureDetector> orbDetector = cv::ORB::create();
    cv::Ptr<cv::DescriptorExtractor> orbExtractor = cv::ORB::create();
    cv::Ptr<cv::DescriptorMatcher> orbMatcher = cv::DescriptorMatcher::create("BruteForce-Hamming");

    vector<cv::KeyPoint> kp1, kp2;//记录关键点的容器
    cv::Mat desp1, desp2;//描述子，我的理解是把mat当矩阵来用了

    orbDetector->detect(img1, kp1);//从img1中提取关键点，并放入kp1中
    orbDetector->detect(img2, kp2);

    orbExtractor->compute(img1, kp1, desp1);//对kp1计算描述子，并放入desp1中
    orbExtractor->compute(img2, kp2, desp2);

    cout <<"分别找到了" << kp1.size() << "和" << kp2.size() <<"个点" << endl;

其次是源代码使用的是knnMatch，而我在这边根据slam14讲上orb的代码改成了match函数，同时将对于好的匹配的判断也相应的进行了修改，不过区别不大。

源码：

    double knn_match_ratio=0.8;
    vector< vector<cv::DMatch> > matches_knn;
    matcher->knnMatch( desp1, desp2, matches_knn, 2 );
    vector< cv::DMatch > matches;
    for ( size_t i=0; i<matches_knn.size(); i++ )
    {
        if (matches_knn[i][0].distance < knn_match_ratio * matches_knn[i][1].distance )
            matches.push_back( matches_knn[i][0] );
    }

修改后：

    vector<cv::DMatch> matches;//orb匹配数据
    vector<cv::DMatch> good_matches;//通过汉明距离筛选后的好的匹配数据

    orbMatcher->match(desp1, desp2, matches);//对于描述子进行匹配

    double mindistance, maxdistance;//用于求取matches的最大最小汉明距离

    for(auto m :matches)//c++11新特性，可以理解为遍历matches
    {
        if(m.distance > maxdistance)
            maxdistance = m.distance;
        if(m.distance < mindistance)
            mindistance = m.distance;
    }

    for(auto m:matches)
    {   //如果汉明距离小于2倍的最小汉明距离或者是30的话就认为是好的匹配
        if(m.distance <= max(2*mindistance, 30.0))
            good_matches.push_back(m);
    }

同时，我也加入了对于匹配图像的显示：

    cv::Mat img_match;
    cv::drawMatches(img1, kp1, img2, kp2, good_matches, img_match);//绘制匹配图像
    cv::imshow("img_match", img_match);//显示匹配图像
    cv::waitKey(10);//等待10ms（不等待不能显示）

在源程序中，这个函数在内部进行了判断匹配点是否过少，并返回布尔值，我直接改成了返回匹配点的个数，并在主函数中对其进行判断：

return good_matches.size();//返回的是匹配个数 

2、g2o部分

在对g2o进行构造时，由于g2o库的更新，线性求解器和块求解器要求使用unique_ptr

BlockSolver(std::unique_ptr<LinearSolverType> linearSolver);

(from: <block_solver.h> line:123):

而源程序使用的是普通指针：

    // 构造g2o中的图
    // 先构造求解器
    g2o::SparseOptimizer    optimizer;
    // 使用Cholmod中的线性方程求解器
    g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new  g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType> ();
    // 6*3 的参数
    g2o::BlockSolver_6_3* block_solver = new g2o::BlockSolver_6_3( linearSolver );
    // L-M 下降
    g2o::OptimizationAlgorithmLevenberg* algorithm = new g2o::OptimizationAlgorithmLevenberg( block_solver );

同时由于将unique_ptr作为参数传递给类的构造函数需要使用move指令，所以修改后的程序如下：

    //这一部分基本上固定，除非是选择不同的求解器算法
    g2o::SparseOptimizer optimizer;
    //由于g2o库的更新，线性求解器和块求解器要求使用:unique_ptr (from: <block_solver.h> line:123)
    unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver(new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>());
    //将unique_ptr作为参数传递给类的构造函数需要使用move指令
    unique_ptr<g2o::BlockSolver_6_3> BlockSolver(new g2o::BlockSolver_6_3(move(linearSolver)));
    g2o::OptimizationAlgorithmLevenberg* algorithm = new g2o::OptimizationAlgorithmLevenberg(move(BlockSolver));

至此，代码修改完毕，可以正常使用，后附完整代码。

二、代码及注释

#include <iostream>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <boost/concept_check.hpp>

#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/types/slam3d/se3quat.h>
#include <g2o/types/sba/types_six_dof_expmap.h>

using namespace std;
//相机内参K矩阵
double cx = 325.5;
double cy = 253.5;
double fx = 5180;
double fy = 519.0;

int findCorrespondingPoints(const cv::Mat& img1,const cv::Mat& img2,vector<cv::Point2f>& pts1,vector<cv::Point2f>& pts2)
{
    //由于opencv版本升级，orb现在的是一个纯虚类，无法使用原来的方法进行实例化了
    cv::Ptr<cv::FeatureDetector> orbDetector = cv::ORB::create();
    cv::Ptr<cv::DescriptorExtractor> orbExtractor = cv::ORB::create();
    cv::Ptr<cv::DescriptorMatcher> orbMatcher = cv::DescriptorMatcher::create("BruteForce-Hamming");

    vector<cv::KeyPoint> kp1, kp2;//记录关键点的容器
    cv::Mat desp1, desp2;//描述子，我的理解是把mat当矩阵来用了

    orbDetector->detect(img1, kp1);//从img1中提取关键点，并放入kp1中
    orbDetector->detect(img2, kp2);

    orbExtractor->compute(img1, kp1, desp1);//对kp1计算描述子，并放入desp1中
    orbExtractor->compute(img2, kp2, desp2);

    cout <<"分别找到了" << kp1.size() << "和" << kp2.size() <<"个点" << endl;

    vector<cv::DMatch> matches;//orb匹配数据
    vector<cv::DMatch> good_matches;//通过汉明距离筛选后的好的匹配数据

    orbMatcher->match(desp1, desp2, matches);//对于描述子进行匹配

    double mindistance, maxdistance;//用于求取matches的最大最小汉明距离

    for(auto m :matches)//c++11新特性，可以理解为遍历matches
    {
        if(m.distance > maxdistance)
            maxdistance = m.distance;
        if(m.distance < mindistance)
            mindistance = m.distance;
    }

    for(auto m:matches)
    {   //如果汉明距离小于2倍的最小汉明距离或者是30的话就认为是好的匹配
        if(m.distance <= max(2*mindistance, 30.0))
            good_matches.push_back(m);
    }

    for(auto m:good_matches)
    {//存储好的匹配的keypoints
        pts1.push_back(kp1[m.queryIdx].pt);
        pts2.push_back(kp2[m.trainIdx].pt);
    }

    cv::Mat img_match;
    cv::drawMatches(img1, kp1, img2, kp2, good_matches, img_match);//绘制匹配图像
    cv::imshow("img_match", img_match);//显示匹配图像
    cv::waitKey(10);//等待10ms（不等待不能显示）

    return good_matches.size();//返回的是匹配个数 
}

int main(int argc, char *argv[])
{
    if(argc != 3)//判断调用参数，使用方法：./ba img1 img2（因为在CMakelist里面将程序设置成了叫ba）
    {
        cout << "Usage: ba img1 img2" << endl;
        return -1;
    }
    //读取img1和img2
    cv::Mat img1 = cv::imread(argv[1]);
    cv::Mat img2 = cv::imread(argv[2]);
    //显示img1和img2
    cv::imshow("img1", img1);
    cv::imshow("img2", img2);

    vector<cv::Point2f>pts1, pts2;//点集

    if( findCorrespondingPoints(img1, img2, pts1, pts2) <= 20)//寻找匹配点
    {
        cout << "匹配点不足！"<< endl;
        return -2;
    }
    cout << "找到了" << pts1.size() << "组特征点" << endl;

    //--------------这一部分基本上固定，除非是选择不同的求解器算法------
    g2o::SparseOptimizer optimizer;
    //由于g2o库的更新，线性求解器和块求解器要求使用:unique_ptr (from: <block_solver.h> line:123)
    unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver(new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>());
    //将unique_ptr作为参数传递给类的构造函数需要使用move指令
    unique_ptr<g2o::BlockSolver_6_3> BlockSolver(new g2o::BlockSolver_6_3(move(linearSolver)));
    g2o::OptimizationAlgorithmLevenberg* algorithm = new g2o::OptimizationAlgorithmLevenberg(move(BlockSolver));

    optimizer.setAlgorithm(algorithm);
    optimizer.setVerbose(false);
    //--------------以上基本上固定-----------------------------------
    //添加位姿顶点
    for(int i=0; i < 2; i++)
    {
        g2o::VertexSE3Expmap* v = new g2o::VertexSE3Expmap();//使用的是g2o内部的定点类型
        v->setId(i);
        if(i == 0)
            v->setFixed(true);//第一幅图视作世界坐标系，设定位姿固定为I,0，不对其进行优化
        v->setEstimate(g2o::SE3Quat());//预设值,R为I,t为0
        optimizer.addVertex(v);//图中加入顶点
    }
    //添加特征点顶点
    for(size_t i=0; i < pts1.size(); i++)
    {
        g2o::VertexPointXYZ* v = new g2o::VertexPointXYZ();
        v->setId(i + 2);
        double z = 1;//深度未知，设为1
        double x = (pts1[i].x - cx) * z / fx;//通过相机内参将像素坐标转化到相机坐标系
        double y = (pts1[i].y - cy) * z / fy;//通过相机内参将像素坐标转化到相机坐标系

        v->setMarginalized(true);//在schur分解时是否进行边缘化，g2o内部通过该函数对应参数判断该点是特征点还是位姿
        v->setEstimate(Eigen::Vector3d(x,y,z));//填入观测值
        optimizer.addVertex(v);//图中加入顶点
    }
    //添加相机参数
    g2o::CameraParameters* camera = new g2o::CameraParameters(fx, Eigen::Vector2d(cx, cy), 0);
    camera->setId(0);
    optimizer.addParameter(camera);
    //添加边
    vector<g2o::EdgeProjectXYZ2UV*> edges;
    for(size_t i = 0; i < pts1.size(); i++)
    {
        g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
        edge->setVertex(0, dynamic_cast<g2o::VertexPointXYZ*> (optimizer.vertex(i+2)));//设定顶点0
        edge->setVertex(1, dynamic_cast<g2o::VertexSE3Expmap*> (optimizer.vertex(0)));//设定顶点1
        edge->setMeasurement(Eigen::Vector2d(pts1[i].x, pts1[i].y));//设置测量值为第一张图的特征点
        edge->setInformation(Eigen::Matrix2d::Identity());//信息矩阵设为0
        edge->setParameterId(0, 0);
        edge->setRobustKernel(new g2o::RobustKernelHuber());//设置鲁棒核函数
        optimizer.addEdge(edge);//添加第一张图边
        edges.push_back(edge);//记录边
    }
    for(size_t i = 0; i < pts2.size(); i++)
    {
        g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
        edge->setVertex(0, dynamic_cast<g2o::VertexPointXYZ*> (optimizer.vertex(i+2)));
        edge->setVertex(1, dynamic_cast<g2o::VertexSE3Expmap*> (optimizer.vertex(1)));
        edge->setMeasurement(Eigen::Vector2d(pts2[i].x, pts2[i].y));
        edge->setInformation(Eigen::Matrix2d::Identity());
        edge->setParameterId(0, 0);
        edge->setRobustKernel(new g2o::RobustKernelHuber());
        optimizer.addEdge(edge);//添加第二张图的边
        edges.push_back(edge);
    }
    cout << "开始优化" << endl;
    optimizer.setVerbose(true);//输出优化日志 
    optimizer.initializeOptimization();//优化初始化
    optimizer.optimize(10);//优化10次
    cout << "优化结束" << endl;

    g2o::VertexSE3Expmap* v = dynamic_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(1));//读取优化结果
    Eigen::Isometry3d pose = v->estimate();//转化成Isometry3d类型
    cout << "Pose = " << endl << pose.matrix() << endl;

    for(size_t i = 0; i < pts1.size(); i++)
    {//读取优化的特征点结果，并输出
        g2o::VertexPointXYZ* v = dynamic_cast<g2o::VertexPointXYZ*>(optimizer.vertex(2+i));
        cout << "vertex id" << i+2 << ", pos = ";
        Eigen::Vector3d pose = v->estimate();
        cout << pose(0) << "," << pose(1) << "," << pose(2) << endl;
    }

    int inliners = 0;
    for(auto e : edges)
    {//遍历所有的边，如果误差过大则认为是异常点
        e->computeError();
        if(e->chi2() > 1)
            cout << "error = " << e->chi2() << endl;
        else
            inliners++;
    }
    cout << "inliners in total points: " << inliners << "/" << pts1.size() + pts2.size() << endl;
    optimizer.save("ba.g2o");

    cv::waitKey(0);

    return 0;
}

三、学习时遇到的问题

关于BlockSolver_6_3的参数为什么是6和3

我的理解是：在进行g2o的时候放进去的两个顶点分别是pose位姿和landmark坐标点。位姿是平移和旋转，维度是6；坐标点是xyz，维度是3。

以上均为个人理解，欢迎指导、交流、讨论！