bool SparseOptimizerIncremental::initSolver(int dimension, int batchEveryN)
{
//cerr << __PRETTY_FUNCTION__ << endl;
slamDimension = dimension;
if (dimension == 3) {
setAlgorithm(createSolver("fix3_2_cholmod"));
OptimizationAlgorithmGaussNewton* gaussNewton = dynamic_cast<OptimizationAlgorithmGaussNewton*>(solver());
assert(gaussNewton);
BlockSolver<BlockSolverTraits<3, 2> >* bs = dynamic_cast<BlockSolver<BlockSolverTraits<3, 2> >*>(gaussNewton->solver());
assert(bs && "Unable to get internal block solver");
LinearSolverCholmodOnline<Matrix3d>* s = dynamic_cast<LinearSolverCholmodOnline<Matrix3d>*>(bs->linearSolver());
bs->setAdditionalVectorSpace(300);
bs->setSchur(false);
_solverInterface = s;
_underlyingSolver = bs;
} else {
setAlgorithm(createSolver("fix6_3_cholmod"));
OptimizationAlgorithmGaussNewton* gaussNewton = dynamic_cast<OptimizationAlgorithmGaussNewton*>(solver());
assert(gaussNewton);
BlockSolver<BlockSolverTraits<6, 3> >* bs = dynamic_cast<BlockSolver<BlockSolverTraits<6, 3> >*>(gaussNewton->solver());
assert(bs && "Unable to get internal block solver");
LinearSolverCholmodOnline<Matrix<double, 6, 6> >* s = dynamic_cast<LinearSolverCholmodOnline<Matrix<double, 6, 6> >*>(bs->linearSolver());
bs->setAdditionalVectorSpace(600);
bs->setSchur(false);
_solverInterface = s;
_underlyingSolver = bs;
}
_solverInterface->cmember = &_cmember;
_solverInterface->batchEveryN = batchEveryN;
if (! solver()) {
cerr << "Error allocating solver. Allocating CHOLMOD solver failed!" << endl;
return false;
}
return true;
}
bool SparseOptimizerIncremental::initSolver(int dimension, int batchEveryN)
{
//cerr << __PRETTY_FUNCTION__ << endl;
slamDimension = dimension;
if (dimension == 3) {
setSolver(createSolver(this, "fix3_2_cholmod"));
BlockSolver<BlockSolverTraits<3, 2> >* bs = dynamic_cast<BlockSolver<BlockSolverTraits<3, 2> >*>(solver());
LinearSolverCholmodOnline<Matrix3d>* s = dynamic_cast<LinearSolverCholmodOnline<Matrix3d>*>(bs->linearSolver());
_solverInterface = s;
} else {
setSolver(createSolver(this, "fix6_3_cholmod"));
BlockSolver<BlockSolverTraits<6, 3> >* bs = dynamic_cast<BlockSolver<BlockSolverTraits<6, 3> >*>(solver());
LinearSolverCholmodOnline<Matrix<double, 6, 6> >* s = dynamic_cast<LinearSolverCholmodOnline<Matrix<double, 6, 6> >*>(bs->linearSolver());
_solverInterface = s;
}
_solverInterface->cmember = &_cmember;
_solverInterface->batchEveryN = batchEveryN;
solver()->setSchur(false);
if (! solver()) {
cerr << "Error allocating solver. Allocating CHOLMOD solver failed!" << endl;
return false;
}
return true;
}