graph.h

Go to the documentation of this file.

#ifndef GRAPH_H
#define GRAPH_H

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <image_geometry/pinhole_camera_model.h>
#include <nav_msgs/Odometry.h>

#include <cv.h>
#include <highgui.h>

#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/types/slam3d/edge_se3.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/core/optimization_algorithm_levenberg.h>

#include <boost/thread.hpp>
#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>

#include "frame.h"
#include "loop_closing.h"
#include "stereo_slam/GraphPoses.h"

using namespace std;
using namespace boost;
namespace fs  = boost::filesystem;

namespace slam
{

class LoopClosing;

class Graph
{

public:

  Graph(LoopClosing* loop_closing);

  void init();

  void run();

  void addFrameToQueue(Frame frame);

  void addEdge(int i, int j, tf::Transform edge, int sigma);

  void update();

  void findClosestVertices(int vertex_id, int window_center, int window, int best_n, vector<int> &neighbors);

  void getFrameVertices(int frame_id, vector<int> &vertices);

  int getVertexFrameId(int id);

  int getLastVertexFrameId();

  tf::Transform getVertexPose(int vertex_id, bool lock = true);

  bool getFramePose(int frame_id, tf::Transform& frame_pose);

  tf::Transform getVertexPoseRelativeToCamera(int id);

  tf::Transform getVertexCameraPose(int id, bool lock = true);

  void saveGraph();

  inline void setCamera2Odom(const tf::Transform& camera2odom){camera2odom_ = camera2odom;}

  inline void setCameraMatrix(const cv::Mat& camera_matrix){camera_matrix_ = camera_matrix;}

  inline cv::Mat getCameraMatrix() const {return camera_matrix_;}

  inline void setCameraModel(const image_geometry::PinholeCameraModel& camera_model){camera_model_ = camera_model;}

  inline image_geometry::PinholeCameraModel getCameraModel() const {return camera_model_;}

  inline int getFrameNum() const {return frame_id_+1;}

protected:

  tf::Transform correctClusterPose(tf::Transform initial_pose);

  vector< vector<int> > createComb(vector<int> cluster_ids);

  bool checkNewFrameInQueue();

  void processNewFrame();

  int addVertex(tf::Transform pose);

  void saveFrame(Frame frame, bool draw_clusters = false);

  void publishCameraPose(tf::Transform camera_pose);

  void publishGraph();

private:

  g2o::SparseOptimizer graph_optimizer_; 

  list<Frame> frame_queue_; 

  int frame_id_; 

  vector< pair< int,int > > cluster_frame_relation_; 

  vector<tf::Transform> local_cluster_poses_; 

  vector<tf::Transform> initial_cluster_pose_history_; 

  vector<double> frame_stamps_; //> Stores the frame timestamps

  mutex mutex_graph_; 

  mutex mutex_frame_queue_; 

  tf::Transform camera2odom_; 

  LoopClosing* loop_closing_; 

  cv::Mat camera_matrix_; 

  image_geometry::PinholeCameraModel camera_model_; 

  ros::Publisher pose_pub_; 

  ros::Publisher graph_pub_; 
};

} // namespace

#endif // GRAPH_H