graph_manager.h

Go to the documentation of this file.

/* This file is part of RGBDSLAM.
 * 
 * RGBDSLAM is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * RGBDSLAM is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with RGBDSLAM.  If not, see <http://www.gnu.org/licenses/>.
 */


/*
 * graph_manager.h
 *
 *  Created on: 19.01.2011
 *      Author: hess
 */

#ifndef GRAPH_MANAGER_H_
#define GRAPH_MANAGER_H_

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include "node.h"
#include "ColorOctomapServer.h"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <map>
#include <QObject>
#include <QString>
#include <QMatrix4x4>
#include <QList>
#include <QMap>
#include <QMutex>

#include <iostream>
#include <sstream>
#include <string>
#include <ctime>
#include <memory> //for auto_ptr
#include <utility>
#include "parameter_server.h"
// #define DO_LOOP_CLOSING
// DO_FEATURE_OPTIMIZATION is set in CMakeLists.txt
#ifdef DO_FEATURE_OPTIMIZATION
#include "landmark.h"
#endif

//#include "g2o/core/graph_optimizer_sparse.h"
#include "g2o/core/sparse_optimizer.h"

//#include "g2o/types/slam3d/camera_parameters.h"
#include "g2o/types/slam3d/parameter_camera.h"
#include "g2o/types/slam2d/vertex_se2.h"

#include "g2o/core/hyper_dijkstra.h"
#include "g2o/core/robust_kernel_impl.h"

namespace tf {
class TransformListener;
}

//typedef g2o::HyperGraph::VertexSet::iterator Vset_it;
typedef std::map<int, Node* >::iterator graph_it;
typedef std::set<g2o::HyperGraph::Edge*> EdgeSet;
typedef g2o::HyperGraph::EdgeSet::iterator EdgeSet_it;
typedef std::map<int, Node* >::iterator graph_it;
//#define ROSCONSOLE_SEVERITY_INFO
/*
class GraphNode {
  GraphNode() : backend_node(NULL), frontend_node(NULL), index(-1) {}

  g2o::VertexSE3* backend_node;
  Node* frontend_node;
  int index;
}
*/

class GraphManager : public QObject {
    Q_OBJECT
    Q_SIGNALS:
      void newTransformationMatrix(QString);
      void sendFinished();
      void setGUIInfo(QString message);
      void setGUIStatus(QString message);
      void setPointCloud(pointcloud_type * pc, QMatrix4x4 transformation);
      void setFeatures(const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >*);
      void updateTransforms(QList<QMatrix4x4>* transformations);
      void setGUIInfo2(QString message);
      void setGraphEdges(const QList<QPair<int, int> >* edge_list);
      void deleteLastNode();
      void resetGLViewer();
      void setGraph(const g2o::OptimizableGraph*);
      void iamBusy(int id, const char* message, int max);
      void progress(int id, const char* message, int val);
      void renderableOctomap(Renderable* r);
    
    public Q_SLOTS:
      void reset();
      void deleteLastFrame(); 
      void clearPointCloud(pointcloud_type const * pc);
      void clearPointClouds();
      void reducePointCloud(pointcloud_type const * pc);
      double optimizeGraph(double iter = -1, bool nonthreaded=false);
      void printEdgeErrors(QString);
      unsigned int pruneEdgesWithErrorAbove(float);
      void sanityCheck(float);
      void toggleMapping(bool);
      void filterNodesByPosition(float x, float y, float z);

      //The following SLOT methods are in graph_mgr_io.cpp:
      void sendAllClouds(bool threaded_if_available=true);
      void saveBagfileAsync(QString filename);
      void saveBagfile(QString filename);
      void saveIndividualClouds(QString file_basename, bool threaded=true);
      void saveAllClouds(QString filename, bool threaded=true);
      void saveAllFeatures(QString filename, bool threaded = true);
      void saveOctomap(QString filename, bool threaded = true);
      void saveTrajectory(QString filebasename, bool with_ground_truth = false);
      void saveG2OGraph(QString filename);
      void occupancyFilterClouds();

    public:
      GraphManager();
      ~GraphManager();

    bool addNode(Node* newNode); 

    void addOdometry(ros::Time timestamp, tf::TransformListener* listener);
    bool nodeComparisons(Node* newNode, 
                         QMatrix4x4& curr_motion_estimate,
                         bool& edge_to_keyframe);

    void firstNode(Node* new_node);

    g2o::HyperGraph::VertexSet camera_vertices;
    g2o::HyperGraph::EdgeSet cam_cam_edges_;
    //g2o::HyperGraph::EdgeSet dummy_edges_; 
    g2o::HyperGraph::EdgeSet odometry_edges_;
    g2o::HyperGraph::EdgeSet current_match_edges_;

#ifdef DO_FEATURE_OPTIMIZATION
    g2o::HyperGraph::EdgeSet cam_lm_edges;
    g2o::HyperGraph::VertexSet landmark_vertices;
#endif

    void drawFeatureFlow(cv::Mat& canvas,
                         cv::Scalar line_color = cv::Scalar(255,0,0,0), 
                         cv::Scalar circle_color = cv::Scalar(0,0,255,0)); 

    //Juergen: overloading function to draw features in different image
    void drawFeatureFlow(cv::Mat& canvas_flow, cv::Mat& canvas_features,
                         cv::Scalar line_color = cv::Scalar(255,0,0,0), 
                         cv::Scalar circle_color = cv::Scalar(0,0,255,0)); 

    bool isBusy();
    
    void deleteFeatureInformation();
    void writeOctomap(QString filename) const;
    void setOptimizerVerbose(bool verbose);

    void setEmpiricalCovariances();
protected:
        
    void resetGraph();
    double optimizeGraphImpl(double max_iter);

    bool updateCloudOrigin(Node* node);
    void createOptimizer(std::string backend, g2o::SparseOptimizer* optimizer = NULL);
    MatchingResult curr_best_result_; 

    tf::StampedTransform computeFixedToBaseTransform(Node* node, bool invert);
    QList<int> getPotentialEdgeTargetsWithDijkstra(const Node* new_node, int sequential_targets, int geodesic_targets, int sampled_targets = 5, int predecessor_id = -1, bool include_predecessor = false);
    
    //std::vector<int> getPotentialEdgeTargetsFeatures(const Node* new_node, int max_targets);

    void localizationUpdate(Node* new_node, QMatrix4x4 motion_estimate);
#ifdef DO_FEATURE_OPTIMIZATION
    std::vector<Landmark> landmarks;
    void updateLandmarks(const MatchingResult& match_result, Node* old_node, Node* new_node);
    int next_landmark_id;
    // merge lm_2 into lm_1
    void mergeLandmarks(Landmark *lm_1, Landmark *lm_2);
    void printLandmarkStatistic();
    void updateProjectionEdges();
    void updateLandmarkInGraph(Landmark* lm);
    void removeFeaturesFromGraph();
#endif


#ifdef DO_LOOP_CLOSING
    void createSearchTree(); // use all images in the GraphManager
    void createSearchTree(const std::vector<int>& node_ids); // as stored in GraphManager::graph_
    // find nodes with similar images, returns sorted list of images with their score (more is better)
    void getNeighbours(int node_id, uint neighbour_cnt, std::vector<std::pair<int,float> >& neighbours);

    cv::flann::Index *tree;
    cv::Mat all_descriptors;

    std::set<std::pair<int,int> > tested_pairs;
    std::set<std::pair<int,int> > matched_pairs;

    void loopClosingTest();
    uint descriptor_length;
    uint total_descriptor_count;
    int *descriptor_to_node;
#endif
    
    bool addEdgeToG2O(const LoadedEdge3D& edge, Node* n1, Node* n2, bool good_edge, bool set_estimate, QMatrix4x4& motion_estimate);
    bool addOdometryEdgeToG2O(const LoadedEdge3D& edge, Node* n1, Node* n2, QMatrix4x4& motion_estimate);

    //Delete a camera frame. Be careful, this might split the graph!
    void deleteCameraFrame(int id);

    void broadcastTransform(const tf::StampedTransform& computed_motion);

    void broadcastLatestTransform(const ros::TimerEvent& event) const;

    tf::StampedTransform stampedTransformInWorldFrame(const Node* node, 
                                                      const tf::Transform& computed_motion) const;

    void addKeyframe(int id);

    int last_added_cam_vertex_id(){
      return graph_[graph_.size()-1]->vertex_id_;
    }

    int nodeId2VertexId(int node_id){
     assert(graph_.find(node_id) != graph_.end());
     return graph_[node_id]->vertex_id_;
    }
    QList<QMatrix4x4>* getAllPosesAsMatrixList() const;
    QList<QPair<int, int> >* getGraphEdges(); 

    // MEMBER VARIABLES
    QList<QPair<int, int> > current_edges_;
    //QMutex current_edges_lock_;
    //QList<QMatrix4x4> current_poses_;

    //void mergeAllClouds(pointcloud_type & merge);
    double geodesicDiscount(g2o::HyperDijkstra& hypdij, const MatchingResult& mr);
    void setEmpiricalCovariancesForEdgeSet(EdgeSet& edges);
    
    g2o::SparseOptimizer* optimizer_;

    ros::Publisher marker_pub_; 
    ros::Publisher ransac_marker_pub_;
    ros::Publisher whole_cloud_pub_;
    ros::Publisher batch_cloud_pub_;
    ros::Publisher online_cloud_pub_;
    
    ros::Timer timer_;
    mutable tf::TransformBroadcaster br_;
    tf::Transform computed_motion_; 
    tf::Transform  init_base_pose_;
    tf::StampedTransform latest_transform_cache_;//base_frame -> optical_frame 

    typedef std::pair<int, Node*> GraphNodeType;
    //QMap<int, Node* > graph_;
    std::map<int, Node* > graph_;
    bool reset_request_;
    unsigned int marker_id_;
    bool batch_processing_runs_;
    bool process_node_runs_;
    bool localization_only_;
    QMutex optimizer_mutex_;
    QMutex optimization_mutex_;
    //cv::FlannBasedMatcher global_flann_matcher;
    QList<int> keyframe_ids_;//Keyframes are added, if no previous keyframe was matched
    //NEW std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > feature_coords_;  
    //NEW cv::Mat feature_descriptors_;         
    unsigned int loop_closures_edges, sequential_edges;
    unsigned int next_seq_id;
    unsigned int next_vertex_id;
    std::string current_backend_;
    int earliest_loop_closure_node_;
    ColorOctomapServer co_server_;
    
    //The following methods are defined in graph_mgr_io.cpp:
    void sendAllCloudsImpl();
    void saveAllCloudsToFile(QString filename);
    void saveAllFeaturesToFile(QString filename);
    void saveIndividualCloudsToFile(QString filename);
    void saveOctomapImpl(QString filename);
    void renderToOctomap(Node* node);
    void pointCloud2MeshFile(QString filename, pointcloud_type full_cloud);
    void savePlyFile(QString filename, pointcloud_normal_type& full_cloud);

    //RVIZ visualization stuff (also in graph_mgr_io.cpp)
    void visualizeGraphEdges() const;
    void visualizeGraphNodes() const;
    void visualizeGraphIds() const;
    void visualizeFeatureFlow3D(unsigned int marker_id = 0, bool draw_outlier = true);
    
    g2o::RobustKernelHuber robust_kernel_;
    //g2o::RobustKernelDCS robust_kernel_;
    g2o::SparseOptimizer::Vertex* calibration_vertex_;
    //g2o::SparseOptimizer::Vertex* odom_calibration_vertex_;


    //Odometry variables needed
    unsigned int last_odom_time_;

};

void publishCloud(Node* node, ros::Time timestamp, ros::Publisher pub);


#endif /* GRAPH_MANAGER_H_ */