00001 00024 #ifndef CCNY_RGBD_KEYFRAME_GRAPH_SOLVER_G2O_H 00025 #define CCNY_RGBD_KEYFRAME_GRAPH_SOLVER_G2O_H 00026 00027 #include <ros/ros.h> 00028 #include <tf/transform_datatypes.h> 00029 00030 #include <g2o/core/graph_optimizer_sparse.h> 00031 #include <g2o/core/block_solver.h> 00032 #include <g2o/solvers/cholmod/linear_solver_cholmod.h> 00033 #include <g2o/types/slam3d/types_six_dof_quat.h> 00034 00035 #include "ccny_rgbd/mapping/keyframe_graph_solver.h" 00036 00037 namespace ccny_rgbd { 00038 00042 class KeyframeGraphSolverG2O: public KeyframeGraphSolver 00043 { 00044 public: 00045 00050 KeyframeGraphSolverG2O(const ros::NodeHandle& nh, 00051 const ros::NodeHandle& nh_private); 00052 00055 ~KeyframeGraphSolverG2O(); 00056 00062 void solve(KeyframeVector& keyframes, 00063 KeyframeAssociationVector& associations); 00064 00065 private: 00066 00067 int vertexIdx; 00068 g2o::SparseOptimizer optimizer; 00069 g2o::BlockSolverX::LinearSolverType * linearSolver; 00070 g2o::BlockSolverX * solver_ptr; 00071 00074 void addVertex(const Eigen::Matrix4f& vertex_pose, 00075 int vertex_idx); 00076 00079 void addEdge(int from_idx, int to_idx, 00080 const Eigen::Matrix4f& relative_pose, 00081 const Eigen::Matrix<double,6,6>& information_matrix); 00082 00085 void optimizeGraph(); 00086 00091 void updatePoses(KeyframeVector& keyframes); 00092 }; 00093 00094 } //namespace ccny_rgbd 00095 00096 #endif // CCNY_RGBD_KEYFRAME_GRAPH_SOLVER_G2O_H
