TwoViewReconstruction.h

#pragma once
#include "Base/Headers/Buffer.hpp"
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <OrbSLAM/Thirdparty/Sophus/sophus/se3.hpp>
#include <Eigen/Core>
#include "Base/Headers/Buffer.hpp"
namespace NDEVR
{

    class TwoViewReconstruction
    {
        typedef std::pair<uint04, uint04> Match;    
 
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        TwoViewReconstruction(const Eigen::Matrix3f& k, float sigma = 1.0, uint04 iterations = 200U);

        bool Reconstruct(const PrimitiveAlignedBuffer<cv::KeyPoint, 32>& vKeys1, const PrimitiveAlignedBuffer<cv::KeyPoint, 32>& vKeys2, const Buffer<uint04>& vMatches12,
            Sophus::SE3f& T21, Buffer<cv::Point3f>& vP3D, Buffer<bool>& vbTriangulated);
 
    private:
 
        void FindHomography(Buffer<bool>& vbMatchesInliers, float& score, Eigen::Matrix3f& H21);
        void FindFundamental(Buffer<bool>& vbInliers, float& score, Eigen::Matrix3f& F21);
 
        Eigen::Matrix3f ComputeH21(const Buffer<cv::Point2f>& vP1, const Buffer<cv::Point2f>& vP2);
        Eigen::Matrix3f ComputeF21(const Buffer<cv::Point2f>& vP1, const Buffer<cv::Point2f>& vP2);
 
        float CheckHomography(const Eigen::Matrix3f& H21, const Eigen::Matrix3f& H12, Buffer<bool>& vbMatchesInliers, float sigma);
 
        float CheckFundamental(const Eigen::Matrix3f& F21, Buffer<bool>& vbMatchesInliers, float sigma);
 
        bool ReconstructF(Buffer<bool>& vbMatchesInliers, Eigen::Matrix3f& F21, Eigen::Matrix3f& K,
            Sophus::SE3f& T21, Buffer<cv::Point3f>& vP3D, Buffer<bool>& vbTriangulated, float minParallax, uint04 minTriangulated);
 
        bool ReconstructH(Buffer<bool>& vbMatchesInliers, Eigen::Matrix3f& H21, Eigen::Matrix3f& K,
            Sophus::SE3f& T21, Buffer<cv::Point3f>& vP3D, Buffer<bool>& vbTriangulated, float minParallax, uint04 minTriangulated);
 
        void Normalize(const PrimitiveAlignedBuffer<cv::KeyPoint, 32>& vKeys, Buffer<cv::Point2f>& vNormalizedPoints, Eigen::Matrix3f& T);
 
 
        uint04 CheckRT(const Eigen::Matrix3f& R, const Eigen::Vector3f& t, const PrimitiveAlignedBuffer<cv::KeyPoint, 32>& vKeys1, const PrimitiveAlignedBuffer<cv::KeyPoint, 32>& vKeys2,
            const Buffer<Match>& vMatches12, Buffer<bool>& vbMatchesInliers,
            const Eigen::Matrix3f& K, Buffer<cv::Point3f>& vP3D, float th2, Buffer<bool>& vbGood, float& parallax);
 
        void DecomposeE(const Eigen::Matrix3f& E, Eigen::Matrix3f& R1, Eigen::Matrix3f& R2, Eigen::Vector3f& t);
 
 
        // Keypoints from Reference Frame (Frame 1)
        PrimitiveAlignedBuffer<cv::KeyPoint, 32> mvKeys1;
 
        // Keypoints from Current Frame (Frame 2)
        PrimitiveAlignedBuffer<cv::KeyPoint, 32> mvKeys2;
 
        // Current Matches from Reference to Current
        Buffer<Match> mvMatches12;
        Buffer<bool> mvbMatched1;
 
        // Calibration
        Eigen::Matrix3f mK;
 
        // Standard Deviation and Variance
        float mSigma, mSigma2;
 
        // Ransac max iterations
        uint04 mMaxIterations;
 
        // Ransac sets
        Buffer<Buffer<uint04> > mvSets;
 
    };
 
}