Exemplo n.º 1
0
//!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);
}
Exemplo n.º 2
0
  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));
  }