keyframe_graph_detector.h

Go to the documentation of this file.

#ifndef RGBDTOOLS_KEYFRAME_GRAPH_DETECTOR_H
#define RGBDTOOLS_KEYFRAME_GRAPH_DETECTOR_H

#include <boost/thread/mutex.hpp>
#include <set>
#include <pcl/registration/transformation_estimation_svd.h>
#include <opencv2/nonfree/features2d.hpp>
#include <aruco/aruco.h>

#include "rgbdtools/types.h"
#include "rgbdtools/rgbd_keyframe.h"
#include "rgbdtools/map_util.h"
#include "rgbdtools/graph/keyframe_association.h"

namespace rgbdtools {

class KeyframeGraphDetector
{
  public:

    enum CandidateGenerationMethod
    {
      CANDIDATE_GENERATION_BRUTE_FORCE,
      CANDIDATE_GENERATION_SURF_TREE
    };

    enum PairwiseMatchingMethod
    {
      PAIRWISE_MATCHING_BFSAC,
      PAIRWISE_MATCHING_RANSAC
    };
    
    enum PairwiseMatcherIndex
    {
      PAIRWISE_MATCHER_LINEAR,
      PAIRWISE_MATCHER_KDTREE
    };
    
    KeyframeGraphDetector();
    
    virtual ~KeyframeGraphDetector();

    void setMaxIterations(bool ransac_max_iterations);
    void setVerbose(bool verbose);
    void setNCandidates(int n_candidates);
    void setKNearestNeighbors(int k_nearest_neighbors);
    void setNKeypoints(int n_keypoints);
    void setOutputPath(const std::string& output_path);
    void setSACSaveResults(bool sac_save_results);
    void setSACReestimateTf(bool sac_reestimate_tf);
    void setCandidateGenerationMethod(CandidateGenerationMethod candidate_method);
    void setPairwiseMatchingMethod(PairwiseMatchingMethod pairwsie_matching_method);
    void setPairwiseMatcherIndex(PairwiseMatcherIndex pairwsie_matcher_index);
    void setMapping(bool mapping);
    void setSacMinInliers(int min);
    void setMatcherUseDescRatioTest(bool matcher_use_desc_ratio_test);
    void setMatcherMaxDescRatio(double matcher_max_desc_ratio);
    void setMatcherMaxDescDist(double matcher_max_desc_dist);
 
    const cv::Mat getAssociationMatrix() const { return association_matrix_; }
    const cv::Mat getCandidateMatrix() const { return candidate_matrix_; }
    const cv::Mat getCorrespondenceMatrix() const { return correspondence_matrix_; }
    const cv::Mat getMatchMatrix() const { return match_matrix_; }
    const std::vector<std::vector<aruco::Marker> > getMarkers() const { return found_markers; }

    void generateKeyframeAssociations(
      KeyframeVector& keyframes,
      KeyframeAssociationVector& associations);
    void generateKeyframeAssociations_Iterative(
      KeyframeVector& keyframes,
      KeyframeAssociationVector& associations);

    // --------------------------------------------
    void prepareFeaturesForRANSAC_mine(KeyframeVector& keyframes);
    void prepareFeaturesForRANSAC_Iterative(u_int kf_idx, KeyframeVector& keyframes);
    void prepareFeaturesForRANSAC(KeyframeVector& keyframes);
    void buildMatchMatrixSurfTree_Iterative(unsigned int kf_idx,
      const KeyframeVector& keyframes);
    void buildAssociationMatrix(
      const KeyframeVector& keyframes,
      KeyframeAssociationVector& associations);
    void buildAssociationMatrix_Iterative(
      const KeyframeVector& keyframes,
      KeyframeAssociationVector& associations);


    bool verbose_;
    bool mapping_;
    int ransac_max_iterations_;
    
    int sac_min_inliers_;
    
    bool matcher_use_desc_ratio_test_;
    
    double matcher_max_desc_ratio_;
    
    double matcher_max_desc_dist_;
    
    double sac_max_eucl_dist_sq_;
    
    bool sac_reestimate_tf_;
    
    double ransac_sufficient_inlier_ratio_;
    
    double ransac_confidence_;
    double log_one_minus_ransac_confidence_;
    
    bool sac_save_results_;
    
    std::string output_path_;
    
    double n_candidates_;
    
    int k_nearest_neighbors_;    
        
    int n_keypoints_;
    
    double init_surf_threshold_;

    int n_matches_accept;
    
    CandidateGenerationMethod candidate_method_;
              
    PairwiseMatchingMethod pairwise_matching_method_;
    
    PairwiseMatcherIndex pairwise_matcher_index_;
    
    // Aruco
    float marker_size; //length of the side in meters
    std::vector<std::vector<aruco::Marker> > found_markers;

    u_int n_original_poses;
    //------------------------------------------
    
    cv::Mat association_matrix_;
    
    cv::Mat candidate_matrix_;
    
    cv::Mat correspondence_matrix_;
    
    cv::Mat match_matrix_;  
    
    std::vector<cv::FlannBasedMatcher> matchers_;
    
    // ------------
    
    void buildCandidateMatrix(const KeyframeVector& keyframes);
    
    void buildMatchMatrixSurfTree(const KeyframeVector& keyframes);
    void buildCandidateMatrixSurfTree_Iterative(int v);

    void buildCandidateMatrixSurfTree();
          
    void buildCorrespondenceMatrix(
      const KeyframeVector& keyframes,
      KeyframeAssociationVector& associations);
    void buildCorrespondenceMatrix_mine(
      const KeyframeVector& keyframes,
      KeyframeAssociationVector& associations);
    void buildCorrespondenceMatrix_onlyLandmarks(
      const KeyframeVector& keyframes,
      KeyframeAssociationVector& associations);
    void buildCorrespondenceMatrix_mine_Iterative(u_int kf_idx_q,
                                                  const KeyframeVector& keyframes,
                                                  KeyframeAssociationVector& associations);
    void buildCorrespondenceMatrix_rgb_Iterative(u_int kf_idx_q,
                                                 const KeyframeVector& keyframes,
                                                 KeyframeAssociationVector& associations);
    int pairwiseMatching(
      int kf_idx_q, int kf_idx_t,
      const KeyframeVector& keyframes,
      DMatchVector& best_inlier_matches,
      Eigen::Matrix4f& best_transformation);
    
    int pairwiseMatchingBFSAC(
      int kf_idx_q, int kf_idx_t,
      const KeyframeVector& keyframes,
      DMatchVector& best_inlier_matches,
      Eigen::Matrix4f& best_transformation);
    
    int pairwiseMatchingRANSAC(
      int kf_idx_q, int kf_idx_t,
      const KeyframeVector& keyframes,
      DMatchVector& best_inlier_matches,
      Eigen::Matrix4f& best_transformation);

    int pairwiseMatchingRANSAC(
      int kf_idx_q, int kf_idx_t,
      const KeyframeVector& keyframes,
      DMatchVector& best_inlier_matches,
      Eigen::Matrix4f& best_transformation,
      std::vector<cv::DMatch> matches);
    
    void getCandidateMatches(
      const RGBDFrame& frame_q, const RGBDFrame& frame_t,
      cv::FlannBasedMatcher& matcher,
      DMatchVector& candidate_matches);

    void prepareMatchers(
      const KeyframeVector& keyframes);
    private:
};

} // namespace rgbdtools

#endif // RGBDTOOLS_KEYFRAME_GRAPH_DETECTOR_H