loop_closing.h

Go to the documentation of this file.

#ifndef LOOP_CLOSING_H
#define LOOP_CLOSING_H

#include <ros/ros.h>

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

#include <libhaloc/lc.h>

#include "constants.h"
#include "cluster.h"
#include "graph.h"

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

namespace slam
{

class Graph;

class LoopClosing
{

public:

  LoopClosing();

  inline void setGraph(Graph *graph){graph_ = graph;}

  void run();

  void addClusterToQueue(Cluster cluster);

  void finalize();

protected:

  bool checkNewClusterInQueue();

  void processNewCluster();

  void searchByProximity();

  void searchByHash();

  bool closeLoopWithCluster(Cluster candidate);

  void getCandidates(int cluster_id, vector< pair<int,float> >& candidates);

  Cluster readCluster(int id);

  void drawLoopClosure(vector<int> cand_kfs,
                       vector<int> cand_matchings,
                       vector<int> inliers,
                       vector<int> definitive_inliers_per_pair,
                       vector< vector<int> > definitive_cluster_pairs,
                       vector<cv::Point2f> matched_query_kp_l,
                       vector<cv::Point2f> matched_cand_kp_l);

private:

  Cluster c_cluster_; 

  list<Cluster> cluster_queue_; 

  mutex mutex_cluster_queue_; 

  haloc::Hash hash_; 

  vector< pair<int, vector<float> > > hash_table_;  

  vector< pair<int, int > > cluster_lc_found_; 

  int num_loop_closures_; 

  string execution_dir_; 

  string loop_closures_dir_; 

  Graph* graph_; 

  ros::Publisher pub_num_keyframes_; 

  ros::Publisher pub_num_lc_; 

  ros::Publisher pub_queue_; 

  ros::Publisher pub_lc_matchings_; 

  image_geometry::PinholeCameraModel camera_model_; 

};

} // namespace

#endif // LOOP_CLOSING_H