motion_estimation_pairwise_ransac.h

Go to the documentation of this file.

#ifndef RGBDTOOLS_MOTION_ESTIMATION_PAIRWISE_RANSAC_H
#define RGBDTOOLS_MOTION_ESTIMATION_PAIRWISE_RANSAC_H

#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/registration/transformation_estimation_svd.h>

#include "rgbdtools/types.h"
#include "rgbdtools/registration/motion_estimation.h"
#include "rgbdtools/features/orb_detector.h"

namespace rgbdtools {

class MotionEstimationPairwiseRANSAC: public MotionEstimation
{
  public:

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    MotionEstimationPairwiseRANSAC();
    
    virtual ~MotionEstimationPairwiseRANSAC();

    bool getMotionEstimationImpl(
      RGBDFrame& frame,
      const AffineTransform& prediction,
      AffineTransform& motion);

    void setFeatureDetector(FeatureDetectorPtr detector) {detector_ = detector;}
    
  private:

    bool initialized_;

    RGBDFrame * prev_frame_;

    FeatureDetectorPtr detector_;
       
    AffineTransform f2b_; 

    // parameters
    
    int sac_min_inliers_;
    double sac_max_eucl_dist_sq_;
    bool sac_reestimate_tf_;
    int ransac_max_iterations_;
    double ransac_confidence_;
    double log_one_minus_ransac_confidence_;
    bool matcher_use_desc_ratio_test_;
    double matcher_max_desc_ratio_;
    double matcher_max_desc_dist_; 
    
    int pairwiseMatchingRANSAC(
      const RGBDFrame& frame_q, const RGBDFrame& frame_t,
      DMatchVector& best_inlier_matches,
      Eigen::Matrix4f& best_transformation);
     
    void getCandidateMatches(
      const RGBDFrame& frame_q, const RGBDFrame& frame_t, 
      cv::DescriptorMatcher& matcher,
      DMatchVector& candidate_matches);
      
    void getRandomIndices(
      int k, int n, IntVector& output)
    {
      while ((int)output.size() < k)
      {
        int random_number = rand() % n;
        bool duplicate = false;    

        for (unsigned int i = 0; i < output.size(); ++i)
        {
          if (output[i] == random_number)
          {
            duplicate = true;
            break;
          }
        }

        if (!duplicate)
          output.push_back(random_number);
      }
    }

    void get3RandomIndices(
      int n, std::set<int>& mask, IntVector& output)
    {
      int key;
      
      while(true)
      {
        output.clear();
        getRandomIndices(3, n, output);
        
        // calculate a key based on the 3 random indices
        key = output[0] * n * n + output[1] * n + output[2];
               
        //printf("%d %d %d\n", output[0], output[1], output[2]);
        
        // value not present in mask
        if (mask.find(key) == mask.end())
          break;
      }

      mask.insert(key);
    }
    
    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;
    }
};

} // namespace rgbdtools

#endif // RGBDTOOLS_MOTION_ESTIMATION_PAIRWISE_RANSAC_H