namespace g2o {
/**
* helper function for allocating
*/
static OptimizationAlgorithm* createSolver(const std::string& fullSolverName)
{
if (fullSolverName != "2dlinear")
return 0;
g2o::Solver* s = 0;
ALLOC_CSPARSE(s, 3, 2, true);
OptimizationAlgorithm* snl = 0;
snl = new SolverSLAM2DLinear(s);
return snl;
}
class SLAM2DLinearSolverCreator : public AbstractOptimizationAlgorithmCreator
{
public:
SLAM2DLinearSolverCreator(const OptimizationAlgorithmProperty& p) : AbstractOptimizationAlgorithmCreator(p) {}
virtual OptimizationAlgorithm* construct()
{
return createSolver(property().name);
}
};
G2O_REGISTER_OPTIMIZATION_LIBRARY(slam2d_linear);
G2O_REGISTER_OPTIMIZATION_ALGORITHM(2dlinear, new SLAM2DLinearSolverCreator(OptimizationAlgorithmProperty("2dlinear", "Solve Orientation + Gauss-Newton: Works only on 2D pose graphs!!", "CSparse", false, 3, 3)));
} // end namespace
namespace g2o {
static OptimizationAlgorithm* createSolver(const std::string& fullSolverName)
{
g2o::Solver* s = 0;
string methodName = fullSolverName.substr(0, 2);
string solverName = fullSolverName.substr(3);
if (solverName == "dense") {
ALLOC_DENSE(s, -1, -1);
}
else if (solverName == "dense3_2") {
ALLOC_DENSE(s, 3, 2);
}
else if (solverName == "dense6_3") {
ALLOC_DENSE(s, 6, 3);
}
else if (solverName == "dense7_3") {
ALLOC_DENSE(s, 7, 3);
}
OptimizationAlgorithm* snl = 0;
if (methodName == "gn") {
snl = new OptimizationAlgorithmGaussNewton(s);
}
else if (methodName == "lm") {
snl = new OptimizationAlgorithmLevenberg(s);
}
return snl;
}
class DenseSolverCreator : public AbstractOptimizationAlgorithmCreator
{
public:
DenseSolverCreator(const OptimizationAlgorithmProperty& p) : AbstractOptimizationAlgorithmCreator(p) {}
virtual OptimizationAlgorithm* construct()
{
return createSolver(property().name);
}
};
G2O_REGISTER_OPTIMIZATION_LIBRARY(dense);
G2O_REGISTER_OPTIMIZATION_ALGORITHM(gn_dense, new DenseSolverCreator(OptimizationAlgorithmProperty("gn_dense", "Gauss-Newton: Dense solver (variable blocksize)", "Dense", false, Eigen::Dynamic, Eigen::Dynamic)));
G2O_REGISTER_OPTIMIZATION_ALGORITHM(gn_dense3_2, new DenseSolverCreator(OptimizationAlgorithmProperty("gn_dense3_2", "Gauss-Newton: Dense solver (fixed blocksize)", "Dense", true, 3, 2)));
G2O_REGISTER_OPTIMIZATION_ALGORITHM(gn_dense6_3, new DenseSolverCreator(OptimizationAlgorithmProperty("gn_dense6_3", "Gauss-Newton: Dense solver (fixed blocksize)", "Dense", true, 6, 3)));
G2O_REGISTER_OPTIMIZATION_ALGORITHM(gn_dense7_3, new DenseSolverCreator(OptimizationAlgorithmProperty("gn_dense7_3", "Gauss-Newton: Dense solver (fixed blocksize)", "Dense", true, 7, 3)));
G2O_REGISTER_OPTIMIZATION_ALGORITHM(lm_dense, new DenseSolverCreator(OptimizationAlgorithmProperty("lm_dense", "Levenberg: Dense solver (variable blocksize)", "Dense", false, -1, -1)));
G2O_REGISTER_OPTIMIZATION_ALGORITHM(lm_dense3_2, new DenseSolverCreator(OptimizationAlgorithmProperty("lm_dense3_2", "Levenberg: Dense solver (fixed blocksize)", "Dense", true, 3, 2)));
G2O_REGISTER_OPTIMIZATION_ALGORITHM(lm_dense6_3, new DenseSolverCreator(OptimizationAlgorithmProperty("lm_dense6_3", "Levenberg: Dense solver (fixed blocksize)", "Dense", true, 6, 3)));
G2O_REGISTER_OPTIMIZATION_ALGORITHM(lm_dense7_3, new DenseSolverCreator(OptimizationAlgorithmProperty("lm_dense7_3", "Levenberg: Dense solver (fixed blocksize)", "Dense", true, 7, 3)));
} // end namespace
void init()
{
static bool initialized = false;
if (initialized)
return;
OptimizationAlgorithmFactory* factory = OptimizationAlgorithmFactory::instance();
factory->registerSolver(new SLAM2DLinearSolverCreator(OptimizationAlgorithmProperty("2dlinear", "Solve Orientation + Gauss-Newton: Works only on 2D pose graphs!!", "CSparse", false, 3, 3)));
initialized = 1;
}
void init()
{
static bool initialized;
if (initialized)
return;
OptimizationAlgorithmFactory* factory = OptimizationAlgorithmFactory::instance();
factory->registerSolver(new DenseSolverCreator(OptimizationAlgorithmProperty("gn_dense", "Gauss-Newton: Dense solver (variable blocksize)", "Dense", false, -1, -1)));
factory->registerSolver(new DenseSolverCreator(OptimizationAlgorithmProperty("gn_dense3_2", "Gauss-Newton: Dense solver (fixed blocksize)", "Dense", true, 3, 2)));
factory->registerSolver(new DenseSolverCreator(OptimizationAlgorithmProperty("gn_dense6_3", "Gauss-Newton: Dense solver (fixed blocksize)", "Dense", true, 6, 3)));
factory->registerSolver(new DenseSolverCreator(OptimizationAlgorithmProperty("gn_dense7_3", "Gauss-Newton: Dense solver (fixed blocksize)", "Dense", true, 7, 3)));
factory->registerSolver(new DenseSolverCreator(OptimizationAlgorithmProperty("lm_dense", "Levenberg: Dense solver (variable blocksize)", "Dense", false, -1, -1)));
factory->registerSolver(new DenseSolverCreator(OptimizationAlgorithmProperty("lm_dense3_2", "Levenberg: Dense solver (fixed blocksize)", "Dense", true, 3, 2)));
factory->registerSolver(new DenseSolverCreator(OptimizationAlgorithmProperty("lm_dense6_3", "Levenberg: Dense solver (fixed blocksize)", "Dense", true, 6, 3)));
factory->registerSolver(new DenseSolverCreator(OptimizationAlgorithmProperty("lm_dense7_3", "Levenberg: Dense solver (fixed blocksize)", "Dense", true, 7, 3)));
initialized = true;
}
void init()
{
static bool initialized = false;
if (initialized)
return;
OptimizationAlgorithmFactory* factory = OptimizationAlgorithmFactory::instance();
factory->registerSolver(new PCGSolverCreator(OptimizationAlgorithmProperty("gn_pcg", "Gauss-Newton: PCG solver using block-Jacobi pre-conditioner (variable blocksize)", "PCG", false, -1, -1)));
factory->registerSolver(new PCGSolverCreator(OptimizationAlgorithmProperty("gn_pcg3_2", "Gauss-Newton: PCG solver using block-Jacobi pre-conditioner (fixed blocksize)", "PCG", true, 3, 2)));
factory->registerSolver(new PCGSolverCreator(OptimizationAlgorithmProperty("gn_pcg6_3", "Gauss-Newton: PCG solver using block-Jacobi pre-conditioner (fixed blocksize)", "PCG", true, 6, 3)));
factory->registerSolver(new PCGSolverCreator(OptimizationAlgorithmProperty("gn_pcg7_3", "Gauss-Newton: PCG solver using block-Jacobi pre-conditioner (fixed blocksize)", "PCG", true, 7, 3)));
factory->registerSolver(new PCGSolverCreator(OptimizationAlgorithmProperty("lm_pcg", "Levenberg: PCG solver using block-Jacobi pre-conditioner (variable blocksize)", "PCG", false, -1, -1)));
factory->registerSolver(new PCGSolverCreator(OptimizationAlgorithmProperty("lm_pcg3_2", "Levenberg: PCG solver using block-Jacobi pre-conditioner (fixed blocksize)", "PCG", true, 3, 2)));
factory->registerSolver(new PCGSolverCreator(OptimizationAlgorithmProperty("lm_pcg6_3", "Levenberg: PCG solver using block-Jacobi pre-conditioner (fixed blocksize)", "PCG", true, 6, 3)));
factory->registerSolver(new PCGSolverCreator(OptimizationAlgorithmProperty("lm_pcg7_3", "Levenberg: PCG solver using block-Jacobi pre-conditioner (fixed blocksize)", "PCG", true, 7, 3)));
initialized = true;
}
namespace g2o {
namespace
{
template<int p, int l, bool blockorder>
std::unique_ptr<BlockSolverBase> AllocateSolver()
{
std::cerr << "# Using CSparse poseDim " << p << " landMarkDim " << l << " blockordering " << blockorder << std::endl;
auto linearSolver = g2o::make_unique<LinearSolverCSparse<typename BlockSolverPL<p, l>::PoseMatrixType>>();
linearSolver->setBlockOrdering(blockorder);
return g2o::make_unique<BlockSolverPL<p, l>>(std::move(linearSolver));
}
}
/**
* helper function for allocating
*/
static OptimizationAlgorithm* createSolver(const std::string& fullSolverName)
{
static const std::map<std::string, std::function<std::unique_ptr<BlockSolverBase>()>> solver_factories{
{ "var", &AllocateSolver<-1, -1, false> },
{ "fix3_2", &AllocateSolver<3, 2, true> },
{ "fix6_3", &AllocateSolver<6, 3, true> },
{ "fix7_3", &AllocateSolver<7, 3, true> },
{ "fix3_2_scalar", &AllocateSolver<3, 2, false> },
{ "fix6_3_scalar", &AllocateSolver<6, 3, false> },
{ "fix7_3_scalar", &AllocateSolver<7, 3, false> },
};
string solverName = fullSolverName.substr(3);
auto solverf = solver_factories.find(solverName);
if (solverf == solver_factories.end())
return nullptr;
string methodName = fullSolverName.substr(0, 2);
if (methodName == "gn")
{
return new OptimizationAlgorithmGaussNewton(solverf->second());
}
else if (methodName == "lm")
{
return new OptimizationAlgorithmLevenberg(solverf->second());
}
else if (methodName == "dl")
{
return new OptimizationAlgorithmDogleg(solverf->second());
}
return nullptr;
}
class CSparseSolverCreator : public AbstractOptimizationAlgorithmCreator
{
public:
CSparseSolverCreator(const OptimizationAlgorithmProperty& p) : AbstractOptimizationAlgorithmCreator(p) {}
virtual OptimizationAlgorithm* construct()
{
return createSolver(property().name);
}
};
G2O_REGISTER_OPTIMIZATION_LIBRARY(csparse);
G2O_REGISTER_OPTIMIZATION_ALGORITHM(gn_var, new CSparseSolverCreator(OptimizationAlgorithmProperty("gn_var", "Gauss-Newton: Cholesky solver using CSparse (variable blocksize)", "CSparse", false, Eigen::Dynamic, Eigen::Dynamic)));
G2O_REGISTER_OPTIMIZATION_ALGORITHM(gn_fix3_2, new CSparseSolverCreator(OptimizationAlgorithmProperty("gn_fix3_2", "Gauss-Newton: Cholesky solver using CSparse (fixed blocksize)", "CSparse", true, 3, 2)));
G2O_REGISTER_OPTIMIZATION_ALGORITHM(gn_fix6_3, new CSparseSolverCreator(OptimizationAlgorithmProperty("gn_fix6_3", "Gauss-Newton: Cholesky solver using CSparse (fixed blocksize)", "CSparse", true, 6, 3)));
G2O_REGISTER_OPTIMIZATION_ALGORITHM(gn_fix7_3, new CSparseSolverCreator(OptimizationAlgorithmProperty("gn_fix7_3", "Gauss-Newton: Cholesky solver using CSparse (fixed blocksize)", "CSparse", true, 7, 3)));
G2O_REGISTER_OPTIMIZATION_ALGORITHM(lm_var, new CSparseSolverCreator(OptimizationAlgorithmProperty("lm_var", "Levenberg: Cholesky solver using CSparse (variable blocksize)", "CSparse", false, Eigen::Dynamic, Eigen::Dynamic)));
G2O_REGISTER_OPTIMIZATION_ALGORITHM(lm_fix3_2, new CSparseSolverCreator(OptimizationAlgorithmProperty("lm_fix3_2", "Levenberg: Cholesky solver using CSparse (fixed blocksize)", "CSparse", true, 3, 2)));
G2O_REGISTER_OPTIMIZATION_ALGORITHM(lm_fix6_3, new CSparseSolverCreator(OptimizationAlgorithmProperty("lm_fix6_3", "Levenberg: Cholesky solver using CSparse (fixed blocksize)", "CSparse", true, 6, 3)));
G2O_REGISTER_OPTIMIZATION_ALGORITHM(lm_fix7_3, new CSparseSolverCreator(OptimizationAlgorithmProperty("lm_fix7_3", "Levenberg: Cholesky solver using CSparse (fixed blocksize)", "CSparse", true, 7, 3)));
#ifdef ADD_SCALAR_ORDERING
G2O_REGISTER_OPTIMIZATION_ALGORITHM(gn_fix3_2_scalar, new CSparseSolverCreator(OptimizationAlgorithmProperty("gn_fix3_2_scalar", "Gauss-Newton: Cholesky solver using CSparse (fixed blocksize, scalar ordering)", "CSparse", true, 3, 2)));
G2O_REGISTER_OPTIMIZATION_ALGORITHM(gn_fix6_3_scalar, new CSparseSolverCreator(OptimizationAlgorithmProperty("gn_fix6_3_scalar", "Gauss-Newton: Cholesky solver using CSparse (fixed blocksize, scalar ordering)", "CSparse", true, 6, 3)));
G2O_REGISTER_OPTIMIZATION_ALGORITHM(gn_fix7_3_scalar, new CSparseSolverCreator(OptimizationAlgorithmProperty("gn_fix7_3_scalar", "Gauss-Newton: Cholesky solver using CSparse (fixed blocksize, scalar ordering)", "CSparse", true, 7, 3)));
G2O_REGISTER_OPTIMIZATION_ALGORITHM(lm_fix3_2_scalar, new CSparseSolverCreator(OptimizationAlgorithmProperty("lm_fix3_2_scalar", "Levenberg: Cholesky solver using CSparse (fixed blocksize, scalar ordering)", "CSparse", true, 3, 2)));
G2O_REGISTER_OPTIMIZATION_ALGORITHM(lm_fix6_3_scalar, new CSparseSolverCreator(OptimizationAlgorithmProperty("lm_fix6_3_scalar", "Levenberg: Cholesky solver using CSparse (fixed blocksize, scalar ordering)", "CSparse", true, 6, 3)));
G2O_REGISTER_OPTIMIZATION_ALGORITHM(lm_fix7_3_scalar,new CSparseSolverCreator(OptimizationAlgorithmProperty("lm_fix7_3_scalar", "Levenberg: Cholesky solver using CSparse (fixed blocksize, scalar ordering)", "CSparse", true, 7, 3)));
#endif
G2O_REGISTER_OPTIMIZATION_ALGORITHM(dl_var, new CSparseSolverCreator(OptimizationAlgorithmProperty("dl_var", "Dogleg: Cholesky solver using CSparse (variable blocksize)", "CSparse", false, Eigen::Dynamic, Eigen::Dynamic)));
}
