sparse_stereo.hpp

Go to the documentation of this file.

#ifndef __VISUAL_ODOMETRY_STEREO_HPP__
#define __VISUAL_ODOMETRY_STEREO_HPP__

#include <stereo/config.h>
#include <stereo/sparse_stereo_types.h>
#include <frame_helper/CalibrationCv.h>
#include <base/Time.hpp>
#include <base/Eigen.hpp>
#include <base/samples/DistanceImage.hpp>

#ifdef OPENCV_HAS_GPUMAT_IN_CORE
#include <opencv2/core/gpumat.hpp>
#else
#include <opencv2/gpu/gpu.hpp>
#endif

namespace stereo
{

struct FeatureInfo
{
  base::Time detectorTime;
  base::Time descriptorTime;

  std::vector<cv::KeyPoint> keypoints;
  cv::Mat descriptors;
};

class StereoFeatures
{
public:
    StereoFeatures();

    void setCalibration( const frame_helper::StereoCalibration &calib );

    void setConfiguration( const FeatureConfiguration &config);

    void setDetectorConfiguration( const DetectorConfiguration &detector_config );

    void setDistanceImages( 
            const base::samples::DistanceImage *left, 
            const base::samples::DistanceImage *right );

    void processFramePair( const cv::Mat &left_image, const cv::Mat &right_image, StereoFeatureArray *stereo_features = NULL );
     
    StereoFeatureArray& getStereoFeatures() { return stereoFeatures; }

    FeatureInfo& getFeatureInfoLeft() { return leftFeatures; }

    FeatureInfo& getFeatureInfoRight() { return rightFeatures; }

    void calculateInterFrameCorrespondences( const StereoFeatureArray& frame1, const StereoFeatureArray& frame2, int filterMethod );
    void calculateInterFrameCorrespondences( 
            const cv::Mat& feat1, const std::vector<cv::KeyPoint>& keyp1, const std::vector<Eigen::Vector3d>& points1,
            const cv::Mat& feat2, const std::vector<cv::KeyPoint>& keyp2, const std::vector<Eigen::Vector3d>& points2, 
            int filterMethod );

    std::vector<std::pair<long,long> > getInterFrameCorrespondences() { return correspondences; }

    base::Affine3d getInterFrameCorrespondenceTransform() { return correspondenceTransform; }

    const cv::Mat& getDebugImage() { return debugImage; }

    cv::Mat getInterFrameDebugImage( const cv::Mat& debug1, const StereoFeatureArray& frame1, const cv::Mat& debug2, const StereoFeatureArray& frame2 , std::vector<std::pair<long,long> > *correspondence = NULL);

public:
    // use threading parameter: 0 for no threads, 1 for 2 threads (one per image), 2 for 8 threads (4 per image).
    void findFeatures( const cv::Mat &left_image, const cv::Mat &right_image, int use_threading = 1, int crop_left = 0, int crop_right = 0); 
    bool getPutativeStereoCorrespondences();
    bool refineFeatureCorrespondences();
    void calculateDepthInformationBetweenCorrespondences(StereoFeatureArray *stereo_features = NULL);

    void crossCheckMatching( const cv::Mat& descriptors1, const cv::Mat& descriptors2, 
            std::vector<cv::DMatch>& filteredMatches12, int knn = 1, float distanceFactor = 2.0 );

    cv::Mat getHomography() { return homography;}

protected:
    void initDetector( size_t lastNumFeatures );
    void findFeatures2( const cv::Mat &image, FeatureInfo& info, bool left_frame = true, int crop_left = 0, int crop_right = 0 );
    void findFeatures_threading( const cv::Mat &image, FeatureInfo& info, bool left_frame = true, int crop_left = 0, int crop_right = 0);

    void crossCheckMatching( std::vector<std::vector<cv::DMatch> > matches12, std::vector<std::vector<cv::DMatch> > matches21, std::vector<cv::DMatch>& filteredMatches12, int knn = 1, float distanceFactor = 2.0);

    frame_helper::StereoCalibrationCv calib;
    FeatureConfiguration config;
    DetectorConfiguration detectorParams;

    FeatureInfo leftFeatures, rightFeatures;
    FeatureInfo leftPutativeMatches, rightPutativeMatches;
    FeatureInfo leftMatches, rightMatches;

    StereoFeatureArray stereoFeatures;
    std::vector<std::pair<long,long> > correspondences;
    base::Affine3d correspondenceTransform;

    cv::Ptr<cv::FeatureDetector> detector;
    cv::Ptr<cv::DescriptorExtractor> descriptorExtractor;
    cv::Ptr<cv::DescriptorMatcher> descriptorMatcher;
 
    cv::Mat homography;

    cv::Mat debugImage;
    int debugRightOffset;
    const base::samples::DistanceImage *dist_left, *dist_right;
    bool use_gpu_detector;
    cv::gpu::GpuMat descriptors_gpu_left;
    cv::gpu::GpuMat descriptors_gpu_right;
};

}

#endif