ROS2-ORB-SLAM3/include/MLPnPsolver.h

/**
* This file is part of ORB-SLAM3
*
* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
*
* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public
* License as published by the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even
* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along with ORB-SLAM3.
* If not, see <http://www.gnu.org/licenses/>.
*/

/******************************************************************************
* Author:   Steffen Urban                                              *
* Contact:  urbste@gmail.com                                          *
* License:  Copyright (c) 2016 Steffen Urban, ANU. All rights reserved.      *
*                                                                            *
* Redistribution and use in source and binary forms, with or without         *
* modification, are permitted provided that the following conditions         *
* are met:                                                                   *
* * Redistributions of source code must retain the above copyright           *
*   notice, this list of conditions and the following disclaimer.            *
* * Redistributions in binary form must reproduce the above copyright        *
*   notice, this list of conditions and the following disclaimer in the      *
*   documentation and/or other materials provided with the distribution.     *
* * Neither the name of ANU nor the names of its contributors may be         *
*   used to endorse or promote products derived from this software without   *
*   specific prior written permission.                                       *
*                                                                            *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"*
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE  *
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *
* ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE        *
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL *
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR *
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT         *
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY  *
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF     *
* SUCH DAMAGE.                                                               *
******************************************************************************/

#ifndef ORB_SLAM3_MLPNPSOLVER_H
#define ORB_SLAM3_MLPNPSOLVER_H

#include "MapPoint.h"
#include "Frame.h"

#include<Eigen/Dense>
#include<Eigen/Sparse>

namespace ORB_SLAM3{
    class MLPnPsolver {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        MLPnPsolver(const Frame &F, const vector<MapPoint*> &vpMapPointMatches);

        ~MLPnPsolver();

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

        //Find metod is necessary?

        bool iterate(int nIterations, bool &bNoMore, vector<bool> &vbInliers, int &nInliers, Eigen::Matrix4f &Tout);

        //Type definitions needed by the original code

        /** A 3-vector of unit length used to describe landmark observations/bearings
         *  in camera frames (always expressed in camera frames)
         */
        typedef Eigen::Vector3d bearingVector_t;

        /** An array of bearing-vectors */
        typedef std::vector<bearingVector_t, Eigen::aligned_allocator<bearingVector_t> >
                bearingVectors_t;

        /** A 2-matrix containing the 2D covariance information of a bearing vector
        */
        typedef Eigen::Matrix2d cov2_mat_t;

        /** A 3-matrix containing the 3D covariance information of a bearing vector */
        typedef Eigen::Matrix3d cov3_mat_t;

        /** An array of 3D covariance matrices */
        typedef std::vector<cov3_mat_t, Eigen::aligned_allocator<cov3_mat_t> >
                cov3_mats_t;

        /** A 3-vector describing a point in 3D-space */
        typedef Eigen::Vector3d point_t;

        /** An array of 3D-points */
        typedef std::vector<point_t, Eigen::aligned_allocator<point_t> >
                points_t;

        /** A homogeneous 3-vector describing a point in 3D-space */
        typedef Eigen::Vector4d point4_t;

        /** An array of homogeneous 3D-points */
        typedef std::vector<point4_t, Eigen::aligned_allocator<point4_t> >
                points4_t;

        /** A 3-vector containing the rodrigues parameters of a rotation matrix */
        typedef Eigen::Vector3d rodrigues_t;

        /** A rotation matrix */
        typedef Eigen::Matrix3d rotation_t;

        /** A 3x4 transformation matrix containing rotation \f$ \mathbf{R} \f$ and
         *  translation \f$ \mathbf{t} \f$ as follows:
         *  \f$ \left( \begin{array}{cc} \mathbf{R} & \mathbf{t} \end{array} \right) \f$
         */
        typedef Eigen::Matrix<double,3,4> transformation_t;

        /** A 3-vector describing a translation/camera position */
        typedef Eigen::Vector3d 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 std::vector<int>& indices,
                transformation_t & result);

        void mlpnp_gn(Eigen::VectorXd& x,
                      const points_t& pts,
                      const std::vector<Eigen::MatrixXd>& nullspaces,
                      const Eigen::SparseMatrix<double> Kll,
                      bool use_cov);

        void mlpnp_residuals_and_jacs(
                const Eigen::VectorXd& x,
                const points_t& pts,
                const std::vector<Eigen::MatrixXd>& nullspaces,
                Eigen::VectorXd& r,
                Eigen::MatrixXd& fjac,
                bool getJacs);

        void mlpnpJacs(
            const point_t& pt,
            const Eigen::Vector3d& nullspace_r,
            const Eigen::Vector3d& nullspace_s,
            const rodrigues_t& w,
            const translation_t& t,
            Eigen::MatrixXd& jacs);

        //Auxiliar methods

        /**
        * \brief Compute a rotation matrix from Rodrigues axis angle.
        *
        * \param[in] omega The Rodrigues-parameters of a rotation.
        * \return The 3x3 rotation matrix.
        */
        Eigen::Matrix3d rodrigues2rot(const Eigen::Vector3d & omega);

        /**
        * \brief Compute the Rodrigues-parameters of a rotation matrix.
        *
        * \param[in] R The 3x3 rotation matrix.
        * \return The Rodrigues-parameters.
        */
        Eigen::Vector3d rot2rodrigues(const Eigen::Matrix3d & R);

        //----------------------------------------------------
        //Fields of the solver
        //----------------------------------------------------
        vector<MapPoint*> mvpMapPointMatches;

        // 2D Points
        vector<cv::Point2f> mvP2D;
        //Substitued by bearing vectors
        bearingVectors_t mvBearingVecs;

        vector<float> mvSigma2;

        // 3D Points
        //vector<cv::Point3f> mvP3Dw;
        points_t mvP3Dw;

        // Index in Frame
        vector<size_t> mvKeyPointIndices;

        // Current Estimation
        double mRi[3][3];
        double mti[3];
        Eigen::Matrix4f mTcwi;
        vector<bool> mvbInliersi;
        int mnInliersi;

        // Current Ransac State
        int mnIterations;
        vector<bool> mvbBestInliers;
        int mnBestInliers;
        Eigen::Matrix4f mBestTcw;

        // Refined
        Eigen::Matrix4f mRefinedTcw;
        vector<bool> mvbRefinedInliers;
        int mnRefinedInliers;

        // Number of Correspondences
        int N;

        // Indices for random selection [0 .. N-1]
        vector<size_t> mvAllIndices;

        // RANSAC probability
        double mRansacProb;

        // RANSAC min inliers
        int mRansacMinInliers;

        // RANSAC max iterations
        int mRansacMaxIts;

        // RANSAC expected inliers/total ratio
        float mRansacEpsilon;

        // RANSAC Threshold inlier/outlier. Max error e = dist(P1,T_12*P2)^2
        float mRansacTh;

        // RANSAC Minimun Set used at each iteration
        int mRansacMinSet;

        // Max square error associated with scale level. Max error = th*th*sigma(level)*sigma(level)
        vector<float> mvMaxError;

        GeometricCamera* mpCamera;
    };

}




#endif //ORB_SLAM3_MLPNPSOLVER_H