SLAM--G2o实现BA优化

最新推荐文章于 2024-03-15 13:53:25 发布
最新推荐文章于 2024-03-15 13:53:25 发布
阅读量3.6k 收藏 22
点赞数 1
分类专栏： SLAM 文章标签： slam
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qq_42995327/article/details/118523360
版权

利用G2o实现BA优化—学习笔记

目录

前言

使用G2o需要图论的知识。一般来说，要优化的参数就是顶点，边就是顶点之间的映射关系。
BA优化，即Bundle Adjustment，需要对相机姿态和特征点同时进行优化，以求得到最优的状态。
在这里插入图片描述

对于优化的参数：
x = [ ξ 1 ξ 2 . . . ξ m ∣ p 1 p 2 . . . p n ] T \pmb x=[\xi_1 \quad \xi_2 ...\xi_m| \quad p_1 \quad p_2...p_n]^T xxx=[ξ1​ξ2​...ξm​∣p1​p2​...pn​]T
我们需要利用到g2o的类来分别定义（当然我们可以自己定义），并定义相关的边，最后利用优化器优化。
 

一、G2o简单例程

官方例程：

// g2o - General Graph Optimization
// Copyright (C) 2012 R. Kümmerle
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <Eigen/Core>
#include <iostream>

#include "g2o/stuff/sampler.h"
#include "g2o/stuff/command_args.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/solver.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/base_vertex.h"
#include "g2o/core/base_unary_edge.h"
#include "g2o/solvers/dense/linear_solver_dense.h"
#include <memory>
#include <boost/shared_array.hpp>
#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>

using namespace std;

/**
 * \brief the params, a, b, and lambda for a * exp(-lambda * t) + b
 */
class VertexParams : public g2o::BaseVertex<3, Eigen::Vector3d>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    VertexParams()
    {
    }

    virtual bool read(std::istream& /*is*/)
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }

    virtual bool write(std::ostream& /*os*/) const
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }

    virtual void setToOriginImpl()
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
    }

    virtual void oplusImpl(const double* update)
    {
      Eigen::Vector3d::ConstMapType v(update);
      _estimate += v;
    }
};

/**
 * \brief measurement for a point on the curve
 *
 * Here the measurement is the point which is lies on the curve.
 * The error function computes the difference between the curve
 * and the point.
 */
class EdgePointOnCurve : public g2o::BaseUnaryEdge<1, Eigen::Vector2d, VertexParams>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    EdgePointOnCurve()
    {
    }
    virtual bool read(std::istream& /*is*/)
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }
    virtual bool write(std::ostream& /*os*/) const
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }

    void computeError()
    {
      const VertexParams* params = static_cast<const VertexParams*>(vertex(0));
      const double& a = params->estimate()(0);
      const double& b = params->estimate()(1);
      const double& lambda = params->estimate()(2);
      double fval = a * exp(-lambda * measurement()(0)) + b;
      _error(0) = fval - measurement()(1);
    }
};

int main(int argc, char** argv)
{
  int numPoints;
  int maxIterations;
  bool verbose;
  std::vector<int> gaugeList;
  string dumpFilename;
  g2o::CommandArgs arg;
  arg.param("dump", dumpFilename, "", "dump the points into a file");
  arg.param("numPoints", numPoints, 50, "number of points sampled from the curve");
  arg.param("i", maxIterations, 10, "perform n iterations");
  arg.param("v", verbose, true, "verbose output of the optimization process");

  arg.parseArgs(argc, argv);

  // generate random data
  double a = 2.;
  double b = 0.4;
  double lambda = 0.2;

  //std::unique_ptr<Eigen::Vector2d[]> points(new Eigen::Vector2d[numPoints]);
  boost::shared_array<Eigen::Vector2d> points(new Eigen::Vector2d[numPoints]);
  for (int i = 0; i < numPoints; ++i) {
    double x = g2o::Sampler::uniformRand(0, 10);
    double y = a * exp(-lambda * x) + b;
    // add Gaussian noise
    y += g2o::Sampler::gaussRand(0, 0.02);
    points[i].x() = x;
    points[i].y() = y;
  }

  if (dumpFilename.size() > 0) {
    ofstream fout(dumpFilename.c_str());
    for (int i = 0; i < numPoints; ++i)
      fout << points[i].transpose() << endl;
  }

  // some handy typedefs
  typedef g2o::BlockSolver< g2o::BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic> >  MyBlockSolver;
  typedef g2o::LinearSolverDense<MyBlockSolver::PoseMatrixType> MyLinearSolver;

  // setup the solver
  g2o::SparseOptimizer optimizer;
  optimizer.setVerbose(true);
  MyLinearSolver* linearSolver = new MyLinearSolver();
  MyBlockSolver* solver_ptr = new MyBlockSolver(linearSolver);
  g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
  optimizer.setAlgorithm(solver);

  // build the optimization problem given the points
  // 1. add the parameter vertex
  VertexParams* params = new VertexParams();
  params->setId(0);
  params->setEstimate(Eigen::Vector3d(1,1,1)); // some initial value for the params
  optimizer.addVertex(params);
  // 2. add the points we measured to be on the curve
  for (int i = 0; i < numPoints; ++i) {
    EdgePointOnCurve* e = new EdgePointOnCurve;
    e->setInformation(Eigen::Matrix<double, 1, 1>::Identity());
    e->setVertex(0, params);
    e->setMeasurement(points[i]);
    optimizer.addEdge(e);
  }

  // perform the optimization
  optimizer.initializeOptimization();
  optimizer.setVerbose(verbose);
  optimizer.optimize(maxIterations);

  if (verbose)
    cout << endl;

  // print out the result
  cout << "Target curve" << endl;
  cout << "a * exp(-lambda * x) + b" << endl;
  cout << "Iterative least squares solution" << endl;
  cout << "a      = " << params->estimate()(0) << endl;
  cout << "b      = " << params->estimate()(1) << endl;
  cout << "lambda = " << params->estimate()(2) << endl;
  cout << endl;

  // clean up
  //delete[] points;

  return 0;
}

高翔在书中给出的程序：

#include <iostream>
#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/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>
using namespace std;

// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    virtual void setToOriginImpl() // 重置
    {
        _estimate << 0,0,0;
    }

    virtual void oplusImpl( const double* update ) // 更新
    {
        _estimate += Eigen::Vector3d(update);
    }
    // 存盘和读盘：留空
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
};

// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
    // 计算曲线模型误差
    void computeError()
    {
        const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
        const Eigen::Vector3d abc = v->estimate();
        _error(0,0) = measurement()- std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;
    }
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
public:
    double _x;  // x 值， y 值为 _measurement
};

int main( int argc, char** argv )
{
    double a=1.0, b=2.0, c=1.0;         // 真实参数值
    int N=100;                          // 数据点
    double w_sigma=1.0;                 // 噪声Sigma值
    cv::RNG rng;                        // OpenCV随机数产生器
    double abc[3] = {0,0,0};            // abc参数的估计值

    vector<double> x_data, y_data;      // 数据

    cout<<"generating data: "<<endl;
    for ( int i=0; i<N; i++ )
    {
        double x = i/100.0;
        x_data.push_back ( x );
        y_data.push_back (
            exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
        );
        cout<<x_data[i]<<" "<<y_data[i]<<endl;
    }

    // 构建图优化，先设定g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  // 每个误差项优化变量维度为3，误差值维度为1
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器
    Block* solver_ptr = new Block( linearSolver );      // 矩阵块求解器
    // 梯度下降方法，从GN, LM, DogLeg 中选
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
    // g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );
    g2o::SparseOptimizer optimizer;     // 图模型
    optimizer.setAlgorithm( solver );   // 设置求解器
    optimizer.setVerbose( true );       // 打开调试输出

    // 往图中增加顶点
    CurveFittingVertex* v = new CurveFittingVertex();
    v->setEstimate( Eigen::Vector3d(0,0,0) );
    v->setId(0);
    optimizer.addVertex( v );

    // 往图中增加边
    for ( int i=0; i<N; i++ )
    {
        CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
        edge->setId(i);
        edge->setVertex( 0, v );                // 设置连接的顶点
        edge->setMeasurement( y_data[i] );      // 观测数值
        edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵：协方差矩阵之逆
        optimizer.addEdge( edge );
    }

    // 执行优化
    cout<<"start optimization"<<endl;
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    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<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;

    // 输出优化值
    Eigen::Vector3d abc_estimate = v->estimate();
    cout<<"estimated model: "<<abc_estimate.transpose()<<endl;

    return 0;
}

二、利用G2o实现BA优化

基本方法：
g2o::VertexSE3Expmap定义相机的位姿顶点；
g2o::VertexSBAPointXYZ定义特征点的空间向量；g2o::EdgeProjectXYZ2UV 定义边；
优化器中添加顶点和边；
设置相机参数；
优化。

2.1 初始化G2o求解器

	//构造求解器
    g2o::SparseOptimizer optimizer;
    //线性方程求解器
    //g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>();
    g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>();
    //矩阵块求解器
    g2o::BlockSolver_6_3* block_solver = new g2o::BlockSolver_6_3(linearSolver);
    //L-M优化算法
    g2o::OptimizationAlgorithmLevenberg* algorithm = new g2o::OptimizationAlgorithmLevenberg(block_solver);
	optimizer.setAlgorithm(algorithm);

2.2 添加顶点

    for(int i = 0;i<num;i++)
    {
        g2o::VertexSE3Expmap* v = new g2o::VertexSE3Expmap;
        v->setId(i);
        v->setFixed(i==0);
        v->setEstimate(g2o::SE3Quat());
        optimizer.addVertex(v);
    }

    //添加特征点顶点
    for(int i=0;i<points_1.size();i++)
    {
        g2o::VertexSBAPointXYZ* v = new g2o::VertexSBAPointXYZ();
        Point2d cam = pixel2cam(points_1[i],K);
        v->setId(i+num); //已经添加过num个位姿的顶点了
        v->setEstimate(Eigen::Vector3d(cam.x,cam.y,1));
        v->setMarginalized(true);//把矩阵块分成两个部分，分别求解微量
        optimizer.addVertex(v);
    }

添加的参数示意：
x = [ ξ 1 ξ 2 . . . ξ m ∣ p 1 p 2 . . . p n ] T \pmb x=[\xi_1 \quad \xi_2 ...\xi_m| \quad p_1 \quad p_2...p_n]^T xxx=[ξ1​ξ2​...ξm​∣p1​p2​...pn​]T

2.3 添加边

    g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
    edge->setVertex(0,dynamic_cast<g2o::VertexSBAPointXYZ*> (optimizer.vertex(i+2)));
    edge->setVertex(1,dynamic_cast<g2o::VertexSE3Expmap*> (optimizer.vertex(0)));
    edge->setMeasurement(Eigen::Vector2d(points_1[i].x,points_1[i].y));
    edge->setRobustKernel(new g2o::RobustKernelHuber());
    edge->setInformation(Eigen::Matrix2d::Identity());
    edge->setParameterId(0,0);
    optimizer.addEdge(edge);

2.4 优化

	optimizer.setVerbose(true);
    optimizer.initializeOptimization();
    optimizer.optimize(20);

2.5 获取优化的值

	g2o::VertexSE3Expmap* v = dynamic_cast<g2o::VertexSE3Expmap*>( optimizer.vertex(1) );
	Eigen::Isometry3d pose = v->estimate();
	cout<<"Pose="<<endl<<pose.matrix()<<endl;

2.6 保存为g2o文件

	optimizer.save("ba.g2o");

三、优化后的程序

main.cpp源码：

#include <iostream>

#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>

#include <g2o/core/robust_kernel.h>
#include <g2o/core/robust_kernel_impl.h>

#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>

#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>

#include <g2o/types/sba/types_six_dof_expmap.h>
#include <g2o/types/slam3d/se3quat.h>

#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/cudafeatures2d.hpp>
#include <opencv2/cudaimgproc.hpp>

#include <Eigen/Core>

#include <vector>
#include <chrono>

using namespace std;
using namespace cv;

void find_feature_matches_GPU(const Mat &img_1, const Mat &img_2,
                          std::vector<KeyPoint> &keypoints_1,
                          std::vector<KeyPoint> &keypoints_2,
                          std::vector<DMatch> &matches);
void find_feature_matches_CPU(const Mat &img_1, const Mat &img_2,
                          std::vector<KeyPoint> &keypoints_1,
                          std::vector<KeyPoint> &keypoints_2,
                          std::vector<DMatch> &matches);
void BASolver(vector<Point2f> &points_1,vector<Point2f> &points_2,Mat &K);

//相机内参
Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);

Point2d pixel2cam(const Point2d &p, const Mat &K)
{
  return Point2d
    (
      (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
      (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
    );
}

int main(int argc,char**argv)
{
    std::string path1 = argv[1];
    std::string path2 = argv[2];
    if(argc !=3)
    {
        cout<<"输入参数方法：**/testOptimizerG2o ./1.png ./2.png\n";
        return 0;
    }
    Mat img_1 = imread(path1);
    Mat img_2 = imread(path2);

    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<Point2f> points_1,points_2;
    vector<DMatch> matches;

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    //use GPU
    //find_feature_matches_GPU(img_1,img_2,keypoints_1,keypoints_2,matches);
    //use cpu
    find_feature_matches_CPU(img_1,img_2,keypoints_1,keypoints_2,matches);

    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> delay_time = chrono::duration_cast<chrono::duration<double>>(t2 - t1); //milliseconds 毫秒

    cout<<"匹配耗时:"<<delay_time.count()<<"秒"<<endl;


    for(auto m:matches)
    {
        points_1.push_back(keypoints_1[m.queryIdx].pt);
        points_2.push_back(keypoints_2[m.trainIdx].pt);
    }

    cout<<"特征点的数量："<<points_1.size()<<"  "<<points_2.size()<<endl;

    //BA优化
    BASolver(points_1,points_2,K);

    Mat img_keypoints;
    drawMatches(img_1,keypoints_1,img_2,keypoints_2,matches,img_keypoints);
    imshow("matches",img_keypoints);
    waitKey(0);

    return 0;
}

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

  cuda::Stream myStream;
  // 利用GPU加速提取特征点
  cuda::GpuMat d_img1,d_img2;
  d_img1.upload(img_1,myStream);
  d_img2.upload(img_2,myStream);

  cuda::GpuMat d_img1Gray,d_img2Gray;
  cuda::cvtColor(d_img1,d_img1Gray,COLOR_BGR2GRAY,0,myStream);
  cuda::cvtColor(d_img2,d_img2Gray,COLOR_BGR2GRAY,0,myStream);


  //-- 初始化
  cuda::GpuMat d_keypoints1, d_keypoints2;
  cuda::GpuMat descriptors_1, descriptors_2,descriptors_1_32F,descriptors_2_32F;

  //创建ORB检测
  Ptr<cuda::ORB> detector_ORB = cuda::ORB::create();

  //-- 第一步:检测 Oriented FAST 角点位置 计算 BRIEF 描述子
  detector_ORB->detectAndComputeAsync(d_img1Gray,cuda::GpuMat(),d_keypoints1,descriptors_1,false,myStream);
  detector_ORB->convert(d_keypoints1,keypoints_1);
  descriptors_1.convertTo(descriptors_1_32F,CV_32F);

  detector_ORB->detectAndComputeAsync(d_img2Gray,cuda::GpuMat(),d_keypoints2,descriptors_2,false,myStream);
  detector_ORB->convert(d_keypoints2,keypoints_2);
  descriptors_2.convertTo(descriptors_2_32F,CV_32F);

  //-- 第二步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离

  //*******************************************利用Match方法进行匹配************************************/
  //可取消注释选择此方法
  /*
  vector<DMatch> match;
  Ptr<cuda::DescriptorMatcher> matcher = cuda::DescriptorMatcher::createBFMatcher(NORM_L2);
  // BFMatcher matcher ( NORM_HAMMING );
  matcher->match(descriptors_1_32F, descriptors_2_32F, match);
  //-- 第三步:匹配点对筛选
  double min_dist = 1000, max_dist = 0;
  for (int i = 0; i < descriptors_1.rows; i++) {
    double dist = match[i].distance;
    if (dist < min_dist) min_dist = dist;
    if (dist > max_dist) max_dist = dist;
  }
  cout<<"-- Max dist : "<< max_dist<<endl;
  cout<<"-- Min dist : "<< min_dist<<endl;
  for (int i = 0; i < descriptors_1.rows; i++)
  {
    if (match[i].distance <= 0.7*max_dist)
    {
      matches.push_back(match[i]);
    }
  }
  */



  //*******************************************利用KnnMatch方法进行匹配（效果较好）**************************/
  vector< vector<DMatch>> knnmatch;
  Ptr<cuda::DescriptorMatcher> matcher = cuda::DescriptorMatcher::createBFMatcher(NORM_L2);
  // BFMatcher matcher ( NORM_HAMMING );
  matcher->knnMatch(descriptors_1_32F, descriptors_2_32F, knnmatch,2);

  //-- 第三步:匹配点对筛选

    for (int i = 0; i < knnmatch.size(); i++)
    {
        if (knnmatch[i][0].distance <= 0.8*knnmatch[i][1].distance)
        {
            matches.push_back(knnmatch[i][0]);
        }
    }


}

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

    //-- 初始化
    Mat descriptors_1, descriptors_2;
    Mat imgGray_1,imgGray_2;
    cvtColor(img_1,imgGray_1,COLOR_BGR2GRAY);
    cvtColor(img_2,imgGray_2,COLOR_BGR2GRAY);

    //创建ORB检测
    Ptr<ORB> detector_ORB = ORB::create();

    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
    //-- 第一步:检测 Oriented FAST 角点位置 计算 BRIEF 描述子
    detector_ORB->detectAndCompute(imgGray_1,Mat(),keypoints_1,descriptors_1);

    detector_ORB->detectAndCompute(imgGray_2,Mat(),keypoints_2,descriptors_2);


    //-- 第二步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    vector<DMatch> match;
    // BFMatcher matcher ( NORM_HAMMING );
    matcher->match(descriptors_1, descriptors_2, match);

    //-- 第三步:匹配点对筛选
    double min_dist = 1000, max_dist = 0;

    for (int i = 0; i < descriptors_1.rows; i++)
    {
      double dist = match[i].distance;
      if (dist < min_dist) min_dist = dist;
      if (dist > max_dist) max_dist = dist;
    }

    cout<<"-- Max dist : "<< max_dist<<endl;
    cout<<"-- Min dist : "<< min_dist<<endl;


  for (int i = 0; i < descriptors_1.rows; i++)
  {
    if (match[i].distance <= 0.4*max_dist)
    {
      matches.push_back(match[i]);
    }
  }

}

void BASolver(vector<Point2f> &points_1,vector<Point2f> &points_2,Mat &K)
{
    //****************************BA优化过程*********************
    //构造求解器
    g2o::SparseOptimizer optimizer;
    //线性方程求解器
    //g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>();
    g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>();
    //矩阵块求解器
    g2o::BlockSolver_6_3* block_solver = new g2o::BlockSolver_6_3(linearSolver);
    //L-M优化算法
    g2o::OptimizationAlgorithmLevenberg* algorithm = new g2o::OptimizationAlgorithmLevenberg(block_solver);
    //
    optimizer.setAlgorithm(algorithm);
    optimizer.setVerbose(true);

    //添加顶点就是添加优化的参数，这里位姿和特征点都要优化；
    //边实际上就是两个参数之间的关系，在这里是两者参数映射的关系
    //添加位姿顶点
    for(int i = 0;i<2;i++)
    {
        g2o::VertexSE3Expmap* v = new g2o::VertexSE3Expmap;
        v->setId(i);
        v->setFixed(i==0);
        v->setEstimate(g2o::SE3Quat());
        optimizer.addVertex(v);
    }

    //添加特征点顶点
    for(int i=0;i<points_1.size();i++)
    {
        g2o::VertexSBAPointXYZ* v = new g2o::VertexSBAPointXYZ();
        Point2d cam = pixel2cam(points_1[i],K);
        v->setId(i+2); //已经添加过两个位姿的顶点了
        v->setEstimate(Eigen::Vector3d(cam.x,cam.y,1));
        v->setMarginalized(true);//把矩阵块分成两个部分，分别求解微量
        optimizer.addVertex(v);
    }

    //添加相机参数
    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);

    //添加边,第一帧和第二帧
    for(size_t i = 0;i<points_1.size();i++)
    {

        g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
        edge->setVertex(0,dynamic_cast<g2o::VertexSBAPointXYZ*> (optimizer.vertex(i+2)));
        edge->setVertex(1,dynamic_cast<g2o::VertexSE3Expmap*> (optimizer.vertex(0)));
        edge->setMeasurement(Eigen::Vector2d(points_1[i].x,points_1[i].y));
        edge->setRobustKernel(new g2o::RobustKernelHuber());
        edge->setInformation(Eigen::Matrix2d::Identity());
        edge->setParameterId(0,0);//这句必要
        optimizer.addEdge(edge);
    }

    for(size_t i = 0;i<points_2.size();i++)
    {

        g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
        edge->setVertex(0,dynamic_cast<g2o::VertexSBAPointXYZ*> (optimizer.vertex(i+2)));
        edge->setVertex(1,dynamic_cast<g2o::VertexSE3Expmap*> (optimizer.vertex(1)));
        edge->setMeasurement(Eigen::Vector2d(points_2[i].x,points_2[i].y));
        edge->setRobustKernel(new g2o::RobustKernelHuber());
        edge->setInformation(Eigen::Matrix2d::Identity());
        edge->setParameterId(0,0);
        optimizer.addEdge(edge);
    }
    optimizer.setVerbose(false);
    optimizer.initializeOptimization();
    optimizer.optimize(20);

    //变换矩阵
    g2o::VertexSE3Expmap * v1 = dynamic_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(0)) ;
    g2o::VertexSE3Expmap * v2 = dynamic_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(1)) ;
    Eigen::Isometry3d pose1 = v1->estimate();
    Eigen::Isometry3d pose2 = v2->estimate();
    cout<<"The Pose1 from fram 1=\n"<<pose1.matrix()<<endl;
    cout<<"The Pose2 from fram 2(or the frame1 to frame2)=\n"<<pose2.matrix()<<endl;

    //****************************BA优化过程*********************
}

CMakeLists.txt:

cmake_minimum_required(VERSION 3.5)

project(testOptimizerG2o LANGUAGES CXX)

set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_STANDARD_REQUIRED ON)


set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )

set(CMAKE_INCLUDE_CURRENT_DIR ON)
list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )

# OpenCV
find_package( OpenCV 4.3.0 REQUIRED)
# Eigen3
find_package( Eigen3 REQUIRED )
# 寻找G2O
find_package( G2O REQUIRED )
#Sophus
find_package( Sophus REQUIRED)
#Cholmod
find_package( Cholmod REQUIRED)

INCLUDE_DIRECTORIES(
                ${CHOLMOD_INCLUDE_DIR}
                ${OpenCV_INCLUDE_DIRS}
                ${EIGEN3_INCLUDE_DIRS}
                ${Sophus_INCLUDE_DIRS})

add_executable(testOptimizerG2o main.cpp)
TARGET_LINK_LIBRARIES(testOptimizerG2o
         ${OpenCV_LIBS}
         ${Sophus_LIBRARIES}
         ${CHOLMOD_LIBRARIES}
         g2o_core g2o_types_slam3d g2o_solver_csparse g2o_stuff g2o_csparse_extension g2o_types_sim3 g2o_types_sba
         )

 message(STATUS "OPENCV is :${OpenCV_INCLUDE_DIRS}")
 message(STATUS "OPENCV version is :${OpenCV_VERSION}")
 message(STATUS "the cholmod is ${CHOLMOD_INCLUDE_DIR} ${CHOLMOD_LIBRARIES}")

完整的文件在参考链接里。

 

参考链接

https://www.cnblogs.com/gaoxiang12/p/5304272.html

https://github.com/zouyuelin/SLAM_Learning_notes/tree/main/testOptimizerG2o

机器人学渣
关注
  • 1
    点赞
  • 踩
  • 22
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
g2opy:SLAM图优化框架g2o的Python绑定
04-29
g2o是用于优化基于图的非线性误差函数的开源C ++框架。 g2o被设计为易于扩展到各种问题，通常可以在几行代码中指定一个新问题。 当前的实现为SLAM和BA的多个变体提供了解决方案。 机器人技术和计算机视觉中的许多...
深蓝学院-视觉SLAM课程-第7讲作业：SLAM中g2o入门详解，直接法BA
qq_37746927的博客
03-28 2739
深蓝学院-视觉SLAM课程-第7讲作业：SLAM中g2o入门详解，直接法BA
g2o报错
Chen__16的博客
04-08 383
解决SLAM十四讲中ch7遇到的大部分问题 eigen版本建议3.4.0
ceres库，g2o库做BA问题，效果对比
蓝羽飞鸟的博客
04-18 2517
ply文件的显示用meshlab 效果图： 没有优化之前的： ceres优化之后的 g2o优化后的 看下初始状态和优化后的对比，可看到初始状态噪声比较大，而优化后较聚集 看下ceres和g2o的对比吧，都会收敛一些，但是位置会有一些错开 ...
【SLAM】G2OBA优化及ICP的学习实践
heroybc的博客
05-26 1364
1.G2O 在之前的章节我已经讲过了G2O（General Graph Optimization）的相关原理以及应用，这次的BA优化问题呢，我们也可以转化为一个G2O来求解。 G2O（General Graph Optimization）———— 通用图优化。 G2O（General Graph Optimization）的核里带有种类多样的求解器，而它的顶点、边的类型也是多种多样。我们可以自己定义顶点和边。总的来说，如果一个优化问题能够表达成图（顶点与边），那么这个问题就可以用G2O（General G
ubuntu20编译运行orb-slam3踩坑
热门推荐
qq_39533374的博客
03-20 1万+
orb3-编译网上教程很多，写一下自己安装编译过程中踩的坑，一个半星期，终于可以跑demo了。 1.出现如下问题，或者在Build target g2o时 卡住。 make[2]: *** [CMakeFiles/ORB_SLAM3.dir/build.make:375：CMakeFiles/ORB_SLAM3.dir/src/MLPnPsolver.cpp.o] 错误 1 make[2]: *** [CMakeFiles/ORB_SLAM3.dir/build.make:76：CMake
使用 g2o 进行BA调整：优化相机位姿和三维点
DeepLearnerZxf的博客
03-15 695
g2o实现BA，提供代码与数据
<视觉SLAM>opencv/手写位姿估计/G2O的BA优化
08-19
- 当知道一个图像的3维信息与另一个图像的2维信息（单目相机拍摄，不含深度信息），就可以估计相机在两幅图像位姿，并利用BA对位姿进行优化。 - 本次实例中的代码均为高翔老师《视觉SLAM十四讲》第一版与第二版的...
SLAM-基于序列的ORB-SLAM-可加速闭环线程-loop-closure-thread-优质项目实战.zip
03-17
8. **数据结构与优化算法**：项目可能涉及图优化技术，如G2O（Graph Optimization）或Ceres Solver，用于地图点和关键帧的优化。 9. **实时性能与鲁棒性**：项目实战可能涵盖了如何在保证实时性的同时，提高系统对...
SLAM-使用C++基于激光点云实现的SLAM算法端到端系统设计-优质项目实战.zip
03-14
这通常涉及图优化框架，如G2O（General Graph Optimization）或iSAM（incremental Smoothing and Mapping），它们能够处理复杂的非线性优化问题。 6. 端到端设计：项目实战中，SLAM系统将涵盖从数据采集、预处理、...
ubuntu下g2o安装
qq_56716304的博客
06-27 1776
使用VBOX下新建Linux虚拟机（Linux类型，ubuntu版本）,下载Ubuntu桌面版镜像->配置虚拟盘。 进入虚拟机安装Ubuntu(每次进入时都要安装Ubuntu->将虚拟盘置为空)。 配置g2o安装环境，可下载可克隆安装包(最新版)，五连击(mkdir build,cd build,cmake ..,make,sudo make install),检查g2o_viewer能否启动，不能启动看一下是不是版本不对，或者是安装各种包起了冲突(第一次踩坑，惨痛教训！) ...
视觉十四讲:第九讲_BA优化_g2o
penuel
09-02 1877
1.投影模型和BA代价函数 这个流程就是观测方程 之前抽象的记为： \(z = h(x, y)\) 现在给出具体的参数话过程，x指此时相机的位姿R,t，它对应的李代数为\(\xi\)。路标y即为这里的三维点p，而观测数据则是像素坐标(u,v)。 此次观测的误差为： \(e = z - h(\xi, p)\) 如果把其他时刻的观测量也考虑进来，则整体的代价函数为： 相当于对位姿和3D...
g2o--ba代码解析
zhanglehes的专栏
01-25 451
g2o是常用的图优化理论c++库，其自带了很多example讲解如何使用该库文件，本文分析其中ba的示例代码。所谓的图优化，就是把一个常规的优化问题，以图（Graph）的形式来表述。在图中，以顶点表示优化变量，以边表示观测方程。于是总体优化问题变为n条边加和的形式边是约束在具体编写g2o代码时，我们也需要明确哪些是顶点（优化项），哪些是边（约束项）。
在windows下vs c++运行g2o的BA优化程序示例
JAMES费的空间
11-16 485
本篇博客主要介绍如何在Windows下安装g2o，并利用g2o和OpenCV库实现一个两帧之间的ORB特征点检测和BA优化的C++程序。ORB是一种既能检测特征点，又能描述特征点的算法，BA是一种优化算法，可以优化相机位姿和三维点云。本文将详细介绍如何在Windows下安装g2o，并利用g2o和OpenCV库实现一个两帧之间的ORB特征点检测和BA优化的C++程序。首先，我们需要在Windows下安装g2o。g2o是一个用于图优化的C++库，可以用于SLAM、机器人、计算机视觉等领域。
Ubuntu18中cuda11.6+cuDNN8.4.1+tensorRT8.4.3.1+opencv4.2.0配置（AirVO配置）(超详细)
llllldm的博客
07-04 2166
注意：我没有尝试过cuda11.4+cuDnn8.4.1+tensorRT8.4.3是否可以，但是根据cuda和cudnn的对应、tensorRT8.4.3支持列表，其实是支持的。我是重新装了cuda。下载opencv4.2和opencv-contrib-4.2，分别解压缩，opencv-contrib文件夹放进opencv-4.2文件夹中，我的显卡是2080Ti，所以bin是7.5。已有cuda11.4，eigen3.3.4，ros melodic，ceres 1.14，boost 1.65。
【从零开始学习SLAM】BA问题用g2o求解重新投影误差优化相机位姿和路标点
小秋 AI SLAM 入门实战教程
05-18 1825
文章目录前言编译运行算法解析源码解读g2o_bundle.cpp第二版 g2o_bal_class.h第一版 g2o_bal_class.h 前言 视觉SLAM十四讲系列之第十讲g2o_bundle.cpp源码解读 编译运行 mkdir build cd build cmake .. make cd build ./g2o_customBundle -input /home/q/projects/slambook/ch10/g2o_custombundle/data/problem-16-22106-.
ubuntu下g2o的安装编译，以及遇到问题的解决
Numberors的博客
10-15 2131
由于在做ros相关的项目时产生了一些依赖的问题，ros自带的g2o可能不太好用，所以就删除重新安装之类的，删除的方法可以去搜索，不再叙述 1、下载g2o 去g2o的github网站去git clone就可以，网站上也详细说明了依赖、安装的步骤。在你的home路径下打开终端： git clone https://github.com/RainerKuemmerle/g2o 2、安装依赖 如果你的PC已经玩了很久ros，其实这些依赖基本上都有，任意路径下打开终端： sudo apt-get install c
实践：g2o求解BA(2)
weixin_46417419的博客
04-24 534
g2o使用步骤： - 创建线性求解器LinearSolver; - 创建BlockSolver,用于上面定义的线性求解器初始化； - 创建总求解器solver,使用GN/LM/DogLey任意一种增量求解方法； - 创建图优化核心：稀疏优化器； - 定义节点和边，并添加到稀疏优化器中； - 设置优化参数，开始优化；
ORB-SLAM-VI
最新发布
05-21
ORB-SLAM-VI是一种基于单目相机的即时定位与地图构建（SLAM）算法，它是ORB-SLAM系列算法的最新版本。它使用光流和IMU数据来预测相机的姿态，并使用ORB特征点来进行特征匹配和地图点的三角化。与先前版本相比，ORB-SLAM-VI引入了双目和RGB-D相机支持、自适应视觉里程计和增量式回归等新特性，从而提高了定位和地图构建的精度和鲁棒性。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值