Go to the documentation of this file.00001
00024 #ifndef RGBDTOOLS_KEYFRAME_GRAPH_SOLVER_G2O_H
00025 #define RGBDTOOLS_KEYFRAME_GRAPH_SOLVER_G2O_H
00026
00027
00028 #include "g2o/solvers/csparse/g2o_csparse_api.h"
00029 #include "g2o/types/slam3d/edge_se3_pointxyz.h"
00030 #include "g2o/types/slam3d/vertex_pointxyz.h"
00031 #include "g2o/types/slam3d/edge_se3.h"
00032 #include "g2o/types/slam3d/g2o_types_slam3d_api.h"
00033 #include "g2o/core/sparse_optimizer.h"
00034 #include "g2o/core/block_solver.h"
00035 #include "g2o/core/factory.h"
00036 #include "g2o/core/optimization_algorithm_factory.h"
00037 #include "g2o/core/optimization_algorithm_gauss_newton.h"
00038 #include "g2o/solvers/csparse/linear_solver_csparse.h"
00039 #include "rgbdtools/map_util.h"
00040 #include "g2o/types/sba/types_sba.h"
00041 #include "g2o/core/optimization_algorithm_levenberg.h"
00042 #include "g2o/types/sba/types_six_dof_expmap.h"
00043 #include "rgbdtools/graph/keyframe_graph_solver.h"
00044
00045 namespace rgbdtools {
00046
00050 class KeyframeGraphSolverG2O: public KeyframeGraphSolver
00051 {
00052 public:
00053
00056 KeyframeGraphSolverG2O();
00057
00060 ~KeyframeGraphSolverG2O();
00061
00067 void solve(KeyframeVector& keyframes,
00068 const KeyframeAssociationVector& associations);
00069
00070 void solve(KeyframeVector& keyframes,
00071 const KeyframeAssociationVector& associations,
00072 AffineTransformVector& path);
00073
00074 private:
00075
00076 int vertexIdx;
00077 g2o::SparseOptimizer optimizer;
00078 g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
00079 g2o::BlockSolver_6_3 * solver_ptr;
00080
00083 void addVertex(const AffineTransform& vertex_pose,
00084 int vertex_idx);
00085
00088 void addEdge(int from_idx, int to_idx,
00089 const AffineTransform& relative_pose,
00090 const Eigen::Matrix<double,6,6>& information_matrix);
00091
00094 void optimizeGraph();
00095
00100
00101
00102 void getOptimizedPoses(AffineTransformVector& poses_);
00103 };
00104
00105 }
00106
00107 #endif // RGBDTOOLS_KEYFRAME_GRAPH_SOLVER_G2O_H