MLPnPsolver.h

#pragma once
#include "GraphOptimization/Headers/DLLInfo.h"
#include "OrbSLAM/Headers/MapPoint.h"
#include "OrbSLAM/Headers/Frame.h"
 
#include <Eigen/Dense>
#include <Eigen/SparseCore>
 
namespace NDEVR{
    class MLPnPsolver
    {
    public:
        MLPnPsolver(const Frame &F, const Buffer<MapPoint*> &vpMapPointMatches);

        void SetRansacParameters(g_type probability = 0.99, uint04 minInliers = 8, uint04 maxIterations = 300, uint04 minSet = 6, float epsilon = 0.4,  float th2 = 5.991f);

        bool iterate(uint04 nIterations, bool &bNoMore, Buffer<bool> &vbInliers, uint04 &nInliers, Eigen::Matrix4f &Tout);
 
        //Type definitions needed by the original code

        typedef Eigen::Vector3<g_type> bearingVector_t;

        typedef Buffer<bearingVector_t> bearingVectors_t;

        typedef Eigen::Matrix2<g_type> cov2_mat_t;

        typedef Eigen::Matrix3<g_type> cov3_mat_t;

        typedef Buffer<cov3_mat_t> cov3_mats_t;

        typedef Eigen::Vector3<g_type> point_t;

        typedef Buffer<point_t> points_t;

        typedef Eigen::Vector4<g_type> point4_t;

        typedef Buffer<point4_t> points4_t;

        typedef Eigen::Vector3<g_type> rodrigues_t;

        typedef Eigen::Matrix3<g_type> rotation_t;

        typedef Eigen::Matrix<g_type,3,4> transformation_t;

        typedef Eigen::Vector3<g_type> translation_t;
 
 
 
    private:
        void CheckInliers();
        bool Refine();
 
        //Functions from de original MLPnP code
 
        /*
         * Computes the camera pose given 3D points coordinates (in the camera reference
         * system), the camera rays and (optionally) the covariance matrix of those camera rays.
         * Result is stored in solution
         */
        void computePose(
                const bearingVectors_t & f,
                const points_t & p,
                const cov3_mats_t & covMats,
                const Buffer<uint04>& indices,
                transformation_t & result);
 
        void mlpnp_gn(Eigen::VectorX<g_type>& x,
                      const points_t& pts,
                      const Buffer<Eigen::MatrixX<g_type>>& nullspaces,
                      const Eigen::SparseMatrix<g_type> Kll,
                      bool use_cov);
 
        void mlpnp_residuals_and_jacs(
                const Eigen::VectorX<g_type>& x,
                const points_t& pts,
                const Buffer<Eigen::MatrixX<g_type>>& nullspaces,
                Eigen::VectorX<g_type>& r,
                Eigen::MatrixX<g_type>& fjac,
                bool getJacs);
 
        void mlpnpJacs(
            const point_t& pt,
            const Eigen::Vector3<g_type>& nullspace_r,
            const Eigen::Vector3<g_type>& nullspace_s,
            const rodrigues_t& w,
            const translation_t& t,
            Eigen::MatrixX<g_type>& jacs);
 
        //Auxiliar methods

        Eigen::Matrix3<g_type> rodrigues2rot(const Eigen::Vector3<g_type>& omega);

        Eigen::Vector3<g_type> rot2rodrigues(const Eigen::Matrix3<g_type>& R);
 
        //----------------------------------------------------
        //Fields of the solver
        //----------------------------------------------------
        Buffer<MapPoint*> mvpMapPointMatches;
 
        // 2D Points
        Buffer<cv::Point2f> mvP2D;
        //Substitued by bearing Buffers
        bearingVectors_t mvBearingVecs;
 
        Buffer<float> mvSigma2;
 
        // 3D Points
        //vector<cv::Point3f> mvP3Dw;
        points_t mvP3Dw;
 
        // Index in Frame
        Buffer<uint04> mvKeyPointIndices;
 
        // Current Estimation
        g_type mRi[3][3];
        g_type mti[3];
        Eigen::Matrix4f mTcwi;
        Buffer<bool> mvbInliersi;
        uint04 mnInliersi = 0;
 
        // Current Ransac State
        uint04 mnIterations = 0;
        Buffer<bool> mvbBestInliers;
        uint04 mnBestInliers = 0;
        Eigen::Matrix4f mBestTcw;
 
        // Refined
        Eigen::Matrix4f mRefinedTcw;
        Buffer<bool> mvbRefinedInliers;
        uint04 mnRefinedInliers = 0;
 
        // Number of Correspondences
        uint04 N = 0;
 
        // Indices for random selection [0 .. N-1]
        Buffer<uint04> mvAllIndices;
 
        // RANSAC probability
        g_type mRansacProb;
 
        // RANSAC min inliers
        uint04 mRansacMinInliers = 0;
 
        // RANSAC max iterations
        uint04 mRansacMaxIts = 0;
 
        // RANSAC expected inliers/total ratio
        float mRansacEpsilon = 0.0f;
 
        // RANSAC Minimun Set used at each iteration
        uint04 mRansacMinSet = 0;
 
        // Max square error associated with scale level. Max error = th*th*sigma(level)*sigma(level)
        Buffer<float> mvMaxError;
 
        GeometricCamera* mpCamera = nullptr;
    };
 
}