forked from logzhan/ORB-SLAM3-UESTC
258 lines
9.4 KiB
C
258 lines
9.4 KiB
C
|
/**
|
||
|
|
* 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
|