keyframe_graph_detector.h

Go to the documentation of this file.

#ifndef CCNY_RGBD_KEYFRAME_GRAPH_DETECTOR_H
#define CCNY_RGBD_KEYFRAME_GRAPH_DETECTOR_H

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

#include "ccny_rgbd/types.h"
#include "ccny_rgbd/structures/rgbd_keyframe.h"
#include "ccny_rgbd/structures/keyframe_association.h"

namespace ccny_rgbd {

class KeyframeGraphDetector
{
  public:

    KeyframeGraphDetector(const ros::NodeHandle& nh, 
                          const ros::NodeHandle& nh_private);
    
    virtual ~KeyframeGraphDetector();

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

   protected:
  
    ros::NodeHandle nh_;          
    ros::NodeHandle nh_private_;  

   private:

    int max_ransac_iterations_;
    
    bool save_ransac_results_;
    
    std::string ransac_results_path_;
    
    double n_ransac_candidates_;
    
    int k_nearest_neighbors_;    
    
    int min_ransac_inliers_;
    
    double max_corresp_dist_desc_;
    
    double max_corresp_dist_eucl_;
    
    double max_corresp_dist_eucl_sq_;
    
    int n_keypoints_;

    void prepareFeaturesForRANSAC(KeyframeVector& keyframes);

    void visualOdometryAssociations(
      KeyframeVector& keyframes,
      KeyframeAssociationVector& associations);
    
    void treeAssociations(
      KeyframeVector& keyframes,
      KeyframeAssociationVector& associations);   

    void manualBruteForceAssociations(
      KeyframeVector& keyframes,
      KeyframeAssociationVector& associations);

    double distEuclideanSq(const PointFeature& a, const PointFeature& b)
    {
      float dx = a.x - b.x;
      float dy = a.y - b.y;
      float dz = a.z - b.z;
      return dx*dx + dy*dy + dz*dz;
    }

    void getRandomIndices(int k, int n, IntVector& output);

    void trainMatcher(const KeyframeVector& keyframes,
                      cv::FlannBasedMatcher& matcher);
    
    void pairwiseMatchingRANSAC(
      RGBDFrame& frame_a, RGBDFrame& frame_b,
      double max_eucl_dist_sq, 
      double max_desc_dist,
      double sufficient_inlier_ratio,
      std::vector<cv::DMatch>& all_matches,
      std::vector<cv::DMatch>& best_inlier_matches,
      Eigen::Matrix4f& best_transformation);
};

} // namespace ccny_rgbd

#endif // CCNY_RGBD_KEYFRAME_GRAPH_DETECTOR_H