//!Set parameters for icp optimizer
void optimizerSetup(g2o::SparseOptimizer& optimizer){
//optimizer.setMethod(g2o::SparseOptimizer::LevenbergMarquardt);
//g2o::Optimizato
optimizer.setVerbose(false);
// variable-size block solver
g2o::BlockSolverX::LinearSolverType * linearSolver
= new g2o::LinearSolverCholmod<g2o ::BlockSolverX::PoseMatrixType>();
g2o::BlockSolverX * solver_ptr
= new g2o::BlockSolverX(linearSolver);
g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr);
optimizer.setAlgorithm(solver);
//optimizer.setSolver(solver_ptr);
g2o::ParameterCamera* cameraParams = new g2o::ParameterCamera();
//FIXME From Parameter server or cam calibration
cameraParams->setKcam(521,521,319.5,239.5);
g2o::SE3Quat offset; // identity
cameraParams->setOffset(offset);
cameraParams->setId(0);
optimizer.addParameter(cameraParams);
}
LocalMapImpl(const dvo::core::RgbdImagePyramid::Ptr& keyframe, const dvo::core::AffineTransformd& keyframe_pose) :
keyframe_(keyframe),
keyframe_vertex_(0),
previous_vertex_(0),
current_vertex_(0),
max_vertex_id_(1),
max_edge_id_(1)
{
// g2o setup
graph_.setAlgorithm(
new g2o::OptimizationAlgorithmLevenberg(
new BlockSolver(
new LinearSolver()
)
)
);
graph_.setVerbose(false);
keyframe_vertex_ = addFrameVertex(ros::Time(keyframe->timestamp()));
keyframe_vertex_->setFixed(true);
keyframe_vertex_->setEstimate(toIsometry(keyframe_pose));
}