RainerKuemmerle / g2o

g2o: A General Framework for Graph Optimization
3.07k stars 1.1k forks source link

factory unable to create solver "gn_var_cholmod" #173

Closed gunshi closed 3 years ago

gunshi commented 7 years ago

#include <iostream>
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/optimization_algorithm_gauss_newton.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/solvers/csparse/linear_solver_csparse.h"
#include "g2o/solvers/cholmod/linear_solver_cholmod.h"
#include "g2o/core/optimization_algorithm_factory.h"
#include "g2o/core/factory.h"
//#include "g2o/types/slam3d/types_slam3d.h"
//#include "g2o/types/slam2d/types_slam2d.h"
#include "g2o/stuff/command_args.h"
#include "g2o/stuff/macros.h"

using namespace std;
using namespace g2o;

#define DIM_TO_SOLVER(p, l) BlockSolver< BlockSolverTraits<p, l> >
#define ALLOC_CHOLMOD(s, p, l, blockorder) \
  if (1) { \
    std::cerr << "# Using CHOLMOD poseDim " << p << " landMarkDim " << l << " blockordering " << blockorder << std::endl; \
    LinearSolverCholmod < DIM_TO_SOLVER(p, l)::PoseMatrixType >* linearSolver = new LinearSolverCholmod<DIM_TO_SOLVER(p, l)::PoseMatrixType>(); \
    linearSolver->setBlockOrdering(blockorder); \
    s = new DIM_TO_SOLVER(p, l)(linearSolver); \
} else (void)0

// we use the 2D and 3D SLAM types here
G2O_USE_TYPE_GROUP(slam2d);
G2O_USE_TYPE_GROUP(slam3d);

int main(int argc, char** argv)
{

  int maxIterations=10;
  string outputFilename="batch1.g2o";
  string inputFilename = "/home/gunshi/Downloads/Map-merging-master/build3/data/merging/traj1_traj2_quat.g2o";

  //g2o::Solver* s = 0;
  //ALLOC_CHOLMOD(s, -1,-1, false);
  std::ofstream fileout("/home/gunshi/Downloads/solvers.txt");

  g2o::OptimizationAlgorithmProperty _currentOptimizationAlgorithmProperty("gn_var_cholmod", "Gauss-Newton: Cholesky solver using CHOLMOD (variable blocksize)", "CHOLMOD", false, Eigen::Dynamic, Eigen::Dynamic);

  g2o::OptimizationAlgorithm* _currentSolver;
   OptimizationAlgorithmFactory* solverFactory = OptimizationAlgorithmFactory::instance();
   string tag="gn_var_cholmod";

   solverFactory->listSolvers(fileout);
   solverFactory->listSolvers(std::cout);

   OptimizationAlgorithmFactory::CreatorList cl = solverFactory->creatorList();
   for (OptimizationAlgorithmFactory::CreatorList::iterator it = cl.begin(); it != cl.end(); ++it) {
      const OptimizationAlgorithmProperty& sp = (*it)->property();
      std::cout<<sp.name<<std::endl;

    }

_currentSolver = solverFactory->construct(tag, _currentOptimizationAlgorithmProperty);

  //OptimizationAlgorithmGaussNewton* optimizationAlgorithm = new OptimizationAlgorithmGaussNewton(s);

  // create the optimizer to load the data and carry out the optimization
  SparseOptimizer optimizer;
  optimizer.setVerbose(true);
  //optimizer.setAlgorithm(optimizationAlgorithm);
  optimizer.setAlgorithm(_currentSolver);

  ifstream ifs(inputFilename.c_str());
  if (! ifs) {
    cerr << "unable to open " << inputFilename << endl;
    return 1;
  }
  optimizer.load(ifs);
  optimizer.initializeOptimization();
  optimizer.optimize(maxIterations);

  if (outputFilename.size() > 0) {
    if (outputFilename == "-") {
      cerr << "saving to stdout";
      optimizer.save(cout);
    } else {
      cerr << "saving " << outputFilename << " ... ";
      optimizer.save(outputFilename.c_str());
    }
    cerr << "done." << endl;
  }
  return 0;
}

This is a minimal working code of what I'm trying to do. I wanted to mimic the solver i was using in g2o_viewer(gn_var_cholmod), through API, since that was the only solver that gave me an optimised result for one specific file(maybe because of how the data is), so I looked around the source code and compiled the above code to create the gn_var_cholmod. I believe there must be something I'm still doing wrong since : 1) the creatorlist of the optimizationalgorithmfactory is empty(i tried to print out the solver names) and so i get the error: SOLVER FACTORY WARNING: Unable to create solver gn_var_cholmod (this must be since findSolver returns false since it is looking up against an empty creatorlist) I also tried using the alloc_cholmod approach (which i have commented out), and in that case i didn't get an error but the result was bizarre since over 10 iterations, the chi2 error increased dramatically instead of decreasing and so was the result when visualised in viewer.

Could anyone help out with this, (in case this is an implementation issue) since i couldn't find any resource as to how to create a solver i want and then use it? Thanks

gunshi commented 7 years ago

unopt-batch.txt (The file I was using)

will1991 commented 5 years ago

try add these lines after include: G2O_USE_OPTIMIZATION_LIBRARY(pcg) G2O_USE_OPTIMIZATION_LIBRARY(cholmod) G2O_USE_OPTIMIZATION_LIBRARY(csparse)

GarethZhang commented 3 years ago

@gunshi Were you able to resolve that issue? How did you do it? Thank you!

charon-cheung commented 1 year ago
G2O_USE_OPTIMIZATION_LIBRARY(pcg)
G2O_USE_OPTIMIZATION_LIBRARY(cholmod)
G2O_USE_OPTIMIZATION_LIBRARY(csparse)
G2O_USE_OPTIMIZATION_LIBRARY(dense)
G2O_USE_OPTIMIZATION_LIBRARY(eigen)

this should solve the problem

LightRadiance commented 1 year ago

Could you tell me how do you solve the problem?Thank you!