keyframe_graph_solver_g2o.h

Go to the documentation of this file.

#ifndef RGBDTOOLS_KEYFRAME_GRAPH_SOLVER_G2O_H
#define RGBDTOOLS_KEYFRAME_GRAPH_SOLVER_G2O_H

//#include <g2o/core/graph_optimizer_sparse.h>
#include "g2o/solvers/csparse/g2o_csparse_api.h"
#include "g2o/types/slam3d/edge_se3_pointxyz.h"
#include "g2o/types/slam3d/vertex_pointxyz.h"
#include "g2o/types/slam3d/edge_se3.h"
#include "g2o/types/slam3d/g2o_types_slam3d_api.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/factory.h"
#include "g2o/core/optimization_algorithm_factory.h"
#include "g2o/core/optimization_algorithm_gauss_newton.h"
#include "g2o/solvers/csparse/linear_solver_csparse.h"
#include "rgbdtools/map_util.h"
#include "g2o/types/sba/types_sba.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/types/sba/types_six_dof_expmap.h"
#include "rgbdtools/graph/keyframe_graph_solver.h"

namespace rgbdtools {

class KeyframeGraphSolverG2O: public KeyframeGraphSolver
{
  public:

    KeyframeGraphSolverG2O();
    
    ~KeyframeGraphSolverG2O();
 
    void solve(KeyframeVector& keyframes,
               const KeyframeAssociationVector& associations);

    void solve(KeyframeVector& keyframes,
               const KeyframeAssociationVector& associations,
               AffineTransformVector& path);

  private:

    int vertexIdx;
    g2o::SparseOptimizer optimizer;
    g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
    g2o::BlockSolver_6_3 * solver_ptr;
        
    void addVertex(const AffineTransform& vertex_pose,
                   int vertex_idx);
    
    void addEdge(int from_idx,  int to_idx,
                 const AffineTransform& relative_pose,
                 const Eigen::Matrix<double,6,6>& information_matrix);
    
    void optimizeGraph();
    
    //void updatePoses(KeyframeVector& keyframes);
    
    void getOptimizedPoses(AffineTransformVector& poses_);
};

} // namespace rgbdtools

#endif // RGBDTOOLS_KEYFRAME_GRAPH_SOLVER_G2O_H