graph.h
Go to the documentation of this file.
00001 
00006 #ifndef GRAPH_H
00007 #define GRAPH_H
00008 
00009 #include <ros/ros.h>
00010 #include <tf/transform_broadcaster.h>
00011 #include <image_geometry/pinhole_camera_model.h>
00012 #include <nav_msgs/Odometry.h>
00013 
00014 #include <cv.h>
00015 #include <highgui.h>
00016 
00017 #include <g2o/core/sparse_optimizer.h>
00018 #include <g2o/core/block_solver.h>
00019 #include <g2o/types/slam3d/edge_se3.h>
00020 #include <g2o/solvers/cholmod/linear_solver_cholmod.h>
00021 #include <g2o/core/optimization_algorithm_levenberg.h>
00022 
00023 #include <boost/thread.hpp>
00024 #include <boost/filesystem.hpp>
00025 #include <boost/lexical_cast.hpp>
00026 
00027 #include "frame.h"
00028 #include "loop_closing.h"
00029 #include "stereo_slam/GraphPoses.h"
00030 
00031 using namespace std;
00032 using namespace boost;
00033 namespace fs  = boost::filesystem;
00034 
00035 namespace slam
00036 {
00037 
00038 class LoopClosing;
00039 
00040 class Graph
00041 {
00042 
00043 public:
00044 
00048   Graph(LoopClosing* loop_closing);
00049 
00052   void init();
00053 
00056   void run();
00057 
00061   void addFrameToQueue(Frame frame);
00062 
00069   void addEdge(int i, int j, tf::Transform edge, int sigma);
00070 
00073   void update();
00074 
00082   void findClosestVertices(int vertex_id, int window_center, int window, int best_n, vector<int> &neighbors);
00083 
00088   void getFrameVertices(int frame_id, vector<int> &vertices);
00089 
00094   int getVertexFrameId(int id);
00095 
00099   int getLastVertexFrameId();
00100 
00106   tf::Transform getVertexPose(int vertex_id, bool lock = true);
00107 
00113   bool getFramePose(int frame_id, tf::Transform& frame_pose);
00114 
00119   tf::Transform getVertexPoseRelativeToCamera(int id);
00120 
00126   tf::Transform getVertexCameraPose(int id, bool lock = true);
00127 
00130   void saveGraph();
00131 
00135   inline void setCamera2Odom(const tf::Transform& camera2odom){camera2odom_ = camera2odom;}
00136 
00140   inline void setCameraMatrix(const cv::Mat& camera_matrix){camera_matrix_ = camera_matrix;}
00141 
00144   inline cv::Mat getCameraMatrix() const {return camera_matrix_;}
00145 
00149   inline void setCameraModel(const image_geometry::PinholeCameraModel& camera_model){camera_model_ = camera_model;}
00150 
00153   inline image_geometry::PinholeCameraModel getCameraModel() const {return camera_model_;}
00154 
00157   inline int getFrameNum() const {return frame_id_+1;}
00158 
00159 protected:
00160 
00165   tf::Transform correctClusterPose(tf::Transform initial_pose);
00166 
00171   vector< vector<int> > createComb(vector<int> cluster_ids);
00172 
00176   bool checkNewFrameInQueue();
00177 
00180   void processNewFrame();
00181 
00186   int addVertex(tf::Transform pose);
00187 
00192   void saveFrame(Frame frame, bool draw_clusters = false);
00193 
00197   void publishCameraPose(tf::Transform camera_pose);
00198 
00201   void publishGraph();
00202 
00203 private:
00204 
00205   g2o::SparseOptimizer graph_optimizer_; 
00206 
00207   list<Frame> frame_queue_; 
00208 
00209   int frame_id_; 
00210 
00211   vector< pair< int,int > > cluster_frame_relation_; 
00212 
00213   vector<tf::Transform> local_cluster_poses_; 
00214 
00215   vector<tf::Transform> initial_cluster_pose_history_; 
00216 
00217   vector<double> frame_stamps_; //> Stores the frame timestamps
00218 
00219   mutex mutex_graph_; 
00220 
00221   mutex mutex_frame_queue_; 
00222 
00223   tf::Transform camera2odom_; 
00224 
00225   LoopClosing* loop_closing_; 
00226 
00227   cv::Mat camera_matrix_; 
00228 
00229   image_geometry::PinholeCameraModel camera_model_; 
00230 
00231   ros::Publisher pose_pub_; 
00232 
00233   ros::Publisher graph_pub_; 
00234 };
00235 
00236 } // namespace
00237 
00238 #endif // GRAPH_H


stereo_slam
Author(s): Pep Lluis Negre
autogenerated on Thu Jun 6 2019 21:40:57