《视觉SLAM十四讲》第十讲g2o求解BA问题报错解决办法

一、问题描述

编译报错如下:

/home/xxx/slambook/ch10/g2o_custombundle/g2o_bundle.cpp: In function ‘void SetMinimizerOptions(std::shared_ptr<g2o::BlockSolver<g2o::BlockSolverTraits<9, 3> > >&, const BundleParams&, g2o::SparseOptimizer*):
/home/xxx/slambook/ch10/g2o_custombundle/g2o_bundle.cpp:136:74: error: no matching function for call to ‘g2o::OptimizationAlgorithmLevenberg::OptimizationAlgorithmLevenberg(g2o::BlockSolver<g2o::BlockSolverTraits<9, 3> >*)’
         solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr.get());
                                                                          ^
In file included from /home/xxx/slambook/ch10/g2o_custombundle/g2o_bundle.cpp:18:0:
/usr/local/include/g2o/core/optimization_algorithm_levenberg.h:47:16: note: candidate: g2o::OptimizationAlgorithmLevenberg::OptimizationAlgorithmLevenberg(std::unique_ptr<g2o::Solver>)
       explicit OptimizationAlgorithmLevenberg(std::unique_ptr<Solver> solver);
                ^
/usr/local/include/g2o/core/optimization_algorithm_levenberg.h:47:16: note:   no known conversion for argument 1 from ‘g2o::BlockSolver<g2o::BlockSolverTraits<9, 3> >*’ to ‘std::unique_ptr<g2o::Solver>/home/xxx/slambook/ch10/g2o_custombundle/g2o_bundle.cpp:139:71: error: no matching function for call to ‘g2o::OptimizationAlgorithmDogleg::OptimizationAlgorithmDogleg(g2o::BlockSolver<g2o::BlockSolverTraits<9, 3> >*)’
         solver = new g2o::OptimizationAlgorithmDogleg(solver_ptr.get());
                                                                       ^
In file included from /home/xxx/slambook/ch10/g2o_custombundle/g2o_bundle.cpp:19:0:
/usr/local/include/g2o/core/optimization_algorithm_dogleg.h:56:16: note: candidate: g2o::OptimizationAlgorithmDogleg::OptimizationAlgorithmDogleg(std::unique_ptr<g2o::BlockSolverBase>)
       explicit OptimizationAlgorithmDogleg(std::unique_ptr<BlockSolverBase> sol
                ^
/usr/local/include/g2o/core/optimization_algorithm_dogleg.h:56:16: note:   no known conversion for argument 1 from ‘g2o::BlockSolver<g2o::BlockSolverTraits<9, 3> >*’ to ‘std::unique_ptr<g2o::BlockSolverBase>/home/xxx/slambook/ch10/g2o_custombundle/g2o_bundle.cpp: In function ‘void SetSolverOptionsFromFlags(BALProblem*, const BundleParams&, g2o::SparseOptimizer*):
/home/xxx/slambook/ch10/g2o_custombundle/g2o_bundle.cpp:186:49: error: no matching function for call to ‘g2o::BlockSolver<g2o::BlockSolverTraits<9, 3> >::BlockSolver(g2o::LinearSolver<Eigen::Matrix<double, 9, 9, 0, 9, 9> >*&)’
     solver_ptr = new BalBlockSolver(linearSolver);
                                                 ^
In file included from /usr/local/include/g2o/core/block_solver.h:199:0,
                 from /home/xxx/slambook/ch10/g2o_custombundle/g2o_bundle.cpp:14:
/usr/local/include/g2o/core/block_solver.hpp:40:1: note: candidate: g2o::BlockSolver<Traits>::BlockSolver(std::unique_ptr<typename Traits::LinearSolverType>) [with Traits = g2o::BlockSolverTraits<9, 3>; typename Traits::LinearSolverType = g2o::LinearSolver<Eigen::Matrix<double, 9, 9, 0, 9, 9> >]
 BlockSolver<Traits>::BlockSolver(std::unique_ptr<LinearSolverType> linearSolver
 ^
/usr/local/include/g2o/core/block_solver.hpp:40:1: note:   no known conversion for argument 1 from ‘g2o::LinearSolver<Eigen::Matrix<double, 9, 9, 0, 9, 9> >*’ to ‘std::unique_ptr<g2o::LinearSolver<Eigen::Matrix<double, 9, 9, 0, 9, 9> >, std::default_delete<g2o::LinearSolver<Eigen::Matrix<double, 9, 9, 0, 9, 9> > > >/home/xxx/slambook/ch10/g2o_custombundle/g2o_bundle.cpp:192:68: error: no matching function for call to ‘g2o::OptimizationAlgorithmLevenberg::OptimizationAlgorithmLevenberg(BalBlockSolver*&)’
         solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
                                                                    ^
...

二、原因分析

g2o新版本和旧版本不兼容。本着有新版本不用旧版本的原则,本文用新版本g2o给出解决办法。

三、解决方案

修改g2o_bundle.cpp代码如下:

#include <Eigen/StdVector>
#include <Eigen/Core>

#include <iostream>
#include <stdint.h>

#include <unordered_set>
#include <memory>
#include <vector>
#include <stdlib.h> 

#include "g2o/stuff/sampler.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/solver.h"
#include "g2o/core/robust_kernel_impl.h"
#include "g2o/core/batch_stats.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/optimization_algorithm_dogleg.h"

#include "g2o/solvers/cholmod/linear_solver_cholmod.h"
#include "g2o/solvers/dense/linear_solver_dense.h"
#include "g2o/solvers/eigen/linear_solver_eigen.h"
#include "g2o/solvers/pcg/linear_solver_pcg.h"
#include "g2o/types/sba/types_six_dof_expmap.h"

#include "g2o/solvers/structure_only/structure_only_solver.h"

#include "common/BundleParams.h"
#include "common/BALProblem.h"
#include "g2o_bal_class.h"


using namespace Eigen;
using namespace std;

typedef Eigen::Map<Eigen::VectorXd> VectorRef;
typedef Eigen::Map<const Eigen::VectorXd> ConstVectorRef;
typedef g2o::BlockSolver<g2o::BlockSolverTraits<9,3> > BalBlockSolver;

// set up the vertexs and edges for the bundle adjustment. 
void BuildProblem(const BALProblem* bal_problem, g2o::SparseOptimizer* optimizer, const BundleParams& params)
{
    const int num_points = bal_problem->num_points();
    const int num_cameras = bal_problem->num_cameras();
    const int camera_block_size = bal_problem->camera_block_size();
    const int point_block_size = bal_problem->point_block_size();

    // Set camera vertex with initial value in the dataset.
    const double* raw_cameras = bal_problem->cameras();
    for(int i = 0; i < num_cameras; ++i)
    {
        ConstVectorRef temVecCamera(raw_cameras + camera_block_size * i,camera_block_size);
        VertexCameraBAL* pCamera = new VertexCameraBAL();
        pCamera->setEstimate(temVecCamera);   // initial value for the camera i..
        pCamera->setId(i);                    // set id for each camera vertex 
  
        // remeber to add vertex into optimizer..
        optimizer->addVertex(pCamera);
        
    }

    // Set point vertex with initial value in the dataset. 
    const double* raw_points = bal_problem->points();
    // const int point_block_size = bal_problem->point_block_size();
    for(int j = 0; j < num_points; ++j)
    {
        ConstVectorRef temVecPoint(raw_points + point_block_size * j, point_block_size);
        VertexPointBAL* pPoint = new VertexPointBAL();
        pPoint->setEstimate(temVecPoint);   // initial value for the point i..
        pPoint->setId(j + num_cameras);     // each vertex should have an unique id, no matter it is a camera vertex, or a point vertex 

        // remeber to add vertex into optimizer..
        pPoint->setMarginalized(true);
        optimizer->addVertex(pPoint);
    }

    // Set edges for graph..
    const int  num_observations = bal_problem->num_observations();
    const double* observations = bal_problem->observations();   // pointer for the first observation..

    for(int i = 0; i < num_observations; ++i)
    {
        EdgeObservationBAL* bal_edge = new EdgeObservationBAL();

        const int camera_id = bal_problem->camera_index()[i]; // get id for the camera; 
        const int point_id = bal_problem->point_index()[i] + num_cameras; // get id for the point 
        
        if(params.robustify)
        {
            g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
            rk->setDelta(1.0);
            bal_edge->setRobustKernel(rk);
        }
        // set the vertex by the ids for an edge observation
        bal_edge->setVertex(0,dynamic_cast<VertexCameraBAL*>(optimizer->vertex(camera_id)));
        bal_edge->setVertex(1,dynamic_cast<VertexPointBAL*>(optimizer->vertex(point_id)));
        bal_edge->setInformation(Eigen::Matrix2d::Identity());
        bal_edge->setMeasurement(Eigen::Vector2d(observations[2*i+0],observations[2*i + 1]));

       optimizer->addEdge(bal_edge) ;
    }

}

void WriteToBALProblem(BALProblem* bal_problem, g2o::SparseOptimizer* optimizer)
{
    const int num_points = bal_problem->num_points();
    const int num_cameras = bal_problem->num_cameras();
    const int camera_block_size = bal_problem->camera_block_size();
    const int point_block_size = bal_problem->point_block_size();

    double* raw_cameras = bal_problem->mutable_cameras();
    for(int i = 0; i < num_cameras; ++i)
    {
        VertexCameraBAL* pCamera = dynamic_cast<VertexCameraBAL*>(optimizer->vertex(i));
        Eigen::VectorXd NewCameraVec = pCamera->estimate();
        memcpy(raw_cameras + i * camera_block_size, NewCameraVec.data(), sizeof(double) * camera_block_size);
    }

    double* raw_points = bal_problem->mutable_points();
    for(int j = 0; j < num_points; ++j)
    {
        VertexPointBAL* pPoint = dynamic_cast<VertexPointBAL*>(optimizer->vertex(j + num_cameras));
        Eigen::Vector3d NewPointVec = pPoint->estimate();
        memcpy(raw_points + j * point_block_size, NewPointVec.data(), sizeof(double) * point_block_size);
    }
}

//this function is  unused yet..
void SetMinimizerOptions(std::unique_ptr<BalBlockSolver>& solver_ptr, const BundleParams& params, g2o::SparseOptimizer* optimizer)
{
    //std::cout<<"Set Minimizer  .."<< std::endl;
    g2o::OptimizationAlgorithmWithHessian* solver;
    if(params.trust_region_strategy == "levenberg_marquardt"){
        solver = new g2o::OptimizationAlgorithmLevenberg(std::move(solver_ptr));
    }
    else if(params.trust_region_strategy == "dogleg"){
        solver = new g2o::OptimizationAlgorithmDogleg(std::move(solver_ptr));
    }
    else 
    {
        std::cout << "Please check your trust_region_strategy parameter again.."<< std::endl;
        exit(EXIT_FAILURE);
    }

    optimizer->setAlgorithm(solver);
    //std::cout<<"Set Minimizer  .."<< std::endl;
}

//this function is  unused yet..
void SetLinearSolver(std::unique_ptr<BalBlockSolver>& solver_ptr, const BundleParams& params)
{
    //std::cout<<"Set Linear Solver .."<< std::endl;
    std::unique_ptr<BalBlockSolver::LinearSolverType> linearSolver;
    
    if(params.linear_solver == "dense_schur" ){
        linearSolver = g2o::make_unique<g2o::LinearSolverDense<BalBlockSolver::PoseMatrixType> >();
    }
    else if(params.linear_solver == "sparse_schur"){
        auto cholesky = g2o::make_unique<g2o::LinearSolverCholmod<BalBlockSolver::PoseMatrixType> >();
        cholesky->setBlockOrdering(true);
        linearSolver = std::move(cholesky); 
    }
    solver_ptr =  g2o::make_unique<BalBlockSolver>(std::move(linearSolver));
    std::cout <<  "Set Complete.."<< std::endl;
}

void SetSolverOptionsFromFlags(BALProblem* bal_problem, const BundleParams& params, g2o::SparseOptimizer* optimizer)
{   
    std::unique_ptr<BalBlockSolver::LinearSolverType> linearSolver;

    if(params.linear_solver == "dense_schur" ){
        linearSolver = g2o::make_unique<g2o::LinearSolverDense<BalBlockSolver::PoseMatrixType> >();
    }
    else if(params.linear_solver == "sparse_schur"){
        auto cholesky = g2o::make_unique<g2o::LinearSolverCholmod<BalBlockSolver::PoseMatrixType> >();
        cholesky->setBlockOrdering(true);
        linearSolver = std::move(cholesky); 
    }
    
    std::unique_ptr<BalBlockSolver> solver_ptr( new BalBlockSolver(std::move(linearSolver)));

    g2o::OptimizationAlgorithmWithHessian* solver;
    if(params.trust_region_strategy == "levenberg_marquardt"){
        solver = new g2o::OptimizationAlgorithmLevenberg(std::move(solver_ptr));
    }
    else if(params.trust_region_strategy == "dogleg"){
        solver = new g2o::OptimizationAlgorithmDogleg(std::move(solver_ptr));
    }
    else 
    {
        std::cout << "Please check your trust_region_strategy parameter again.."<< std::endl;
        exit(EXIT_FAILURE);
    }

    optimizer->setAlgorithm(solver);
}


void SolveProblem(const char* filename, const BundleParams& params)
{
    BALProblem bal_problem(filename);

    // show some information here ...
    std::cout << "bal problem file loaded..." << std::endl;
    std::cout << "bal problem have " << bal_problem.num_cameras() << " cameras and "
              << bal_problem.num_points() << " points. " << std::endl;
    std::cout << "Forming " << bal_problem.num_observations() << " observatoins. " << std::endl;

    // store the initial 3D cloud points and camera pose..
    if(!params.initial_ply.empty()){
        bal_problem.WriteToPLYFile(params.initial_ply);
    }

    std::cout << "beginning problem..." << std::endl;
    
    // add some noise for the intial value
    srand(params.random_seed);
    bal_problem.Normalize();
    bal_problem.Perturb(params.rotation_sigma, params.translation_sigma,
                        params.point_sigma);

    std::cout << "Normalization complete..." << std::endl;


    g2o::SparseOptimizer optimizer;
    SetSolverOptionsFromFlags(&bal_problem, params, &optimizer);
    BuildProblem(&bal_problem, &optimizer, params);

    
    std::cout << "begin optimizaiton .."<< std::endl;
    // perform the optimizaiton 
    optimizer.initializeOptimization();
    optimizer.setVerbose(true);
    optimizer.optimize(params.num_iterations);

    std::cout << "optimization complete.. "<< std::endl;
    // write the optimized data into BALProblem class
    WriteToBALProblem(&bal_problem, &optimizer);

    // write the result into a .ply file.
    if(!params.final_ply.empty()){
        bal_problem.WriteToPLYFile(params.final_ply);
    }
   
}

int main(int argc, char** argv)
{
    
    BundleParams params(argc,argv);  // set the parameters here.

    if(params.input.empty()){
        std::cout << "Usage: bundle_adjuster -input <path for dataset>";
        return 1;
    }

    SolveProblem(params.input.c_str(), params);
  
    return 0;
}

四、运行结果

在这里插入图片描述

五、优化前后对比

在这里插入图片描述

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值