g2o_solver.hpp

Go to the documentation of this file.

/*********************************************************************
*
*  Copyright (c) 2017, Saurav Agarwal 
*  All rights reserved.
*
*********************************************************************/
 
 
#ifndef KARTO_G2OSolver_H
#define KARTO_G2OSolver_H
 
#include <karto_sdk/Mapper.h>
#include "g2o/core/sparse_optimizer.h"
 
namespace solver_plugins
{
 
class G2OSolver : public karto::ScanSolver
{
  public:
 
    G2OSolver();
    
    virtual ~G2OSolver();
 
  public:
    
    virtual void Clear();
    
    virtual void Compute();
    
    virtual const karto::ScanSolver::IdPoseVector& GetCorrections() const;
 
    virtual void AddNode(karto::Vertex<karto::LocalizedRangeScan>* pVertex);
    
    virtual void AddConstraint(karto::Edge<karto::LocalizedRangeScan>* pEdge);
 
    void getGraph(std::vector<Eigen::Vector2d>& nodes); // std::vector<std::pair<Eigen::Vector2d, Eigen::Vector2d> > &edges);
 
    void useRobustKernel(bool flag)
    {
        useRobustKernel_ = flag;
    }
 
    virtual void ModifyNode(const int& unique_id, const Eigen::Vector3d& pose); // change a node's pose
 
  private:
    
    karto::ScanSolver::IdPoseVector corrections_;
 
    g2o::SparseOptimizer optimizer_;
 
    int latestNodeID_; // ID of the latest added node, this is used internally in AddHeadingConstraint
 
    bool useRobustKernel_;
 
};
 
}
 
#endif // KARTO_G2OSolver_H