一、问题描述
编译报错如下:
/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;
}