Sim3Solver.h

 
 
#ifndef SIM3SOLVER_H
#define SIM3SOLVER_H
 
#include <opencv2/opencv.hpp>
#include "Base/Headers/Buffer.hpp"
 
#include "OrbSLAM/Headers/KeyFrame.h"
 
 
 
namespace NDEVR
{

class Sim3Solver
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    Sim3Solver(KeyFrame* pKF1, KeyFrame* pKF2, const Buffer<MapPoint*> &vpMatched12, const bool bFixScale = true,
               const Buffer<KeyFrame*> vpKeyFrameMatchedMP = Buffer<KeyFrame*>());

    void setRansacParameters(fltp04 probability = 0.99f, uint04 minInliers = 6 , uint04 maxIterations = 300);

    Eigen::Matrix4f find(Buffer<bool> &vbInliers12, uint04& nInliers);

    Eigen::Matrix4f iterate(uint04 nIterations, bool &bNoMore, Buffer<bool> &vbInliers, uint04& nInliers);
    Eigen::Matrix4f iterate(uint04 nIterations, bool &bNoMore, Buffer<bool> &vbInliers, uint04& nInliers, bool &bConverge);

    Eigen::Matrix4f GetEstimatedTransformation();
    Eigen::Matrix3f GetEstimatedRotation();
    Eigen::Vector3f GetEstimatedTranslation();
    float GetEstimatedScale();
 
protected:
 
    void ComputeCentroid(Eigen::Matrix3f &P, Eigen::Matrix3f &Pr, Eigen::Vector3f &C);
 
    void ComputeSim3(Eigen::Matrix3f &P1, Eigen::Matrix3f &P2);
 
    void CheckInliers();
 
    void Project(const Buffer<Eigen::Vector3f> &vP3Dw, Buffer<Eigen::Vector2f> &vP2D, Eigen::Matrix4f Tcw, GeometricCamera* pCamera);
    void FromCameraToImage(const Buffer<Eigen::Vector3f> &vP3Dc, Buffer<Eigen::Vector2f> &vP2D, GeometricCamera* pCamera);
 
 
protected:
 
    // KeyFrames and matches
    KeyFrame* mpKF1 = nullptr;
    KeyFrame* mpKF2 = nullptr;
 
    Buffer<Eigen::Vector3f> mvX3Dc1;
    Buffer<Eigen::Vector3f> mvX3Dc2;
    Buffer<MapPoint*> mvpMapPoints1;
    Buffer<MapPoint*> mvpMapPoints2;
    Buffer<MapPoint*> mvpMatches12;
    Buffer<uint04> mvnIndices1;
    Buffer<uint04> mvSigmaSquare1;
    Buffer<uint04> mvSigmaSquare2;
    Buffer<fltp04> mvnMaxError1;
    Buffer<fltp04> mvnMaxError2;
 
    uint04 N = 0;
    uint04 mN1 = 0;
 
    // Current Estimation
    Eigen::Matrix3f mR12i;
    Eigen::Vector3f mt12i;
    float ms12i = 0.0f;
    Eigen::Matrix4f mT12i;
    Eigen::Matrix4f mT21i;
    Buffer<bool> mvbInliersi;
    uint04 mnInliersi = 0;
 
    // Current Ransac State
    uint04 mnIterations = 0;
    Buffer<bool> mvbBestInliers;
    uint04 mnBestInliers = 0;
    Eigen::Matrix4f mBestT12;
    Eigen::Matrix3f mBestRotation;
    Eigen::Vector3f mBestTranslation;
    float mBestScale = 0.0f;
 
    // Scale is fixed to 1 in the stereo/RGBD case
    bool mbFixScale = false;
 
    // Indices for random selection
    Buffer<uint04> mvAllIndices;
 
    // Projections
    Buffer<Eigen::Vector2f> mvP1im1;
    Buffer<Eigen::Vector2f> mvP2im2;
 
    // RANSAC probability
    fltp04 mRansacProb = 0.0f;
 
    // RANSAC min inliers
    uint04 mRansacMinInliers = 0;
 
    // RANSAC max iterations
    uint04 mRansacMaxIts = 0;
 
    // Threshold inlier/outlier. e = dist(Pi,T_ij*Pj)^2 < 5.991*mSigma2
    float mTh = 0.0f;
    float mSigma2 = 0.0f;
 
    // Calibration
    //cv::Mat mK1;
    //cv::Mat mK2;
 
    GeometricCamera* pCamera1 = nullptr;
    GeometricCamera* pCamera2 = nullptr;
 
};
 
} //namespace ORB_SLAM
 
#endif // SIM3SOLVER_H