PnPsolver.h

/** 
* This file is part of PYSLAM 
*
* Copyright (C) 2016-present Luigi Freda <luigi dot freda at gmail dot com> 
*
* PYSLAM 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.
*
* PYSLAM 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 PYSLAM. If not, see <http://www.gnu.org/licenses/>.
*/
/**
* This file is part of ORB-SLAM2.
* This file is a modified version of EPnP <http://cvlab.epfl.ch/EPnP/index.php>, see FreeBSD license below.
*
* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
* For more information see <https://github.com/raulmur/ORB_SLAM2>
*
* ORB-SLAM2 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-SLAM2 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-SLAM2. If not, see <http://www.gnu.org/licenses/>.
*/

/**
* Copyright (c) 2009, V. Lepetit, EPFL
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
*    list of conditions and the following disclaimer.
* 2. 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.
*
* 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 THE COPYRIGHT OWNER OR 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.
*
* The views and conclusions contained in the software and documentation are those
* of the authors and should not be interpreted as representing official policies,
*   either expressed or implied, of the FreeBSD Project
*/

#pragma once

#include <Eigen/Core>
#include <Eigen/Dense>

#include <opencv2/core/core.hpp>
#include <opencv2/core/types_c.h>

namespace utils
{

class PnPsolverInput {
public: 
  // 2D Points data
  std::vector<Eigen::Vector2f> mvP2D; // image projection
  std::vector<float> mvSigma2;

  // 3D Points data
  std::vector<Eigen::Vector3f> mvP3Dw; // 3d world coordinates

  // Camera data 
  float fx, fy, cx, cy;
};


class PinHoleCamera {
public:
  PinHoleCamera() : fx(0), fy(0), cx(0), cy(0) {}
  PinHoleCamera(double fx, double fy, double cx, double cy) : fx(fx), fy(fy), cx(cx), cy(cy) {}

  void setParameters(double fx, double fy, double cx, double cy) {
    this->fx = fx;
    this->fy = fy;
    this->cx = cx;
    this->cy = cy;
  }

  template <typename T2>
  cv::Point3f unproject(const T2 &p) const {
    if constexpr (std::is_same<T2, cv::Point2f>::value || std::is_same<T2, cv::Point2d>::value) {
      return cv::Point3f((p.x - cx) / fx, (p.y - cy) / fy, 1.0);
    }
    else if constexpr (std::is_same<T2, Eigen::Vector2f>::value || std::is_same<T2, Eigen::Vector2d>::value) {
      return cv::Point3f((p.x() - cx) / fx, (p.y() - cy) / fy, 1.0);
    } else {
      static_assert(std::is_same<T2, cv::Point2f>::value || std::is_same<T2, cv::Point2d>::value ||
                    std::is_same<T2, Eigen::Vector2f>::value || std::is_same<T2, Eigen::Vector2d>::value,
                    "Unsupported type for unproject");
    }
  }

  template <typename T3>
  cv::Point2f project(const T3 &p) const {
    if constexpr (std::is_same<T3, cv::Point3f>::value || std::is_same<T3, cv::Point3d>::value) {
      return cv::Point2f(fx * p.x / p.z + cx, fy * p.y / p.z + cy);
    }
    else if constexpr (std::is_same<T3, Eigen::Vector3f>::value || std::is_same<T3, Eigen::Vector3d>::value) {
      return cv::Point2f(fx * p.x() / p.z() + cx, fy * p.y() / p.z() + cy);
    } else {
      static_assert(std::is_same<T3, cv::Point3f>::value || std::is_same<T3, cv::Point3d>::value ||
                    std::is_same<T3, Eigen::Vector3f>::value || std::is_same<T3, Eigen::Vector3d>::value,
                    "Unsupported type for project");
    }
  }

public: 
  double fx, fy, cx, cy;
};


class PnPsolver {
 public:
 explicit PnPsolver(const PnPsolverInput& input);

  ~PnPsolver();

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

  cv::Mat find(std::vector<uint8_t> &vbInliers, int &nInliers);

  cv::Mat iterate(int nIterations, bool &bNoMore, std::vector<uint8_t> &vbInliers, int &nInliers);

 private:

  void CheckInliers();
  bool Refine();

  // Functions from the original EPnP code
  void set_maximum_number_of_correspondences(const int n);
  void reset_correspondences(void);
  void add_correspondence(const double X, const double Y, const double Z,
              const double u, const double v);

  double compute_pose(double R[3][3], double T[3]);

  void relative_error(double & rot_err, double & transl_err,
              const double Rtrue[3][3], const double ttrue[3],
              const double Rest[3][3],  const double test[3]);

  void print_pose(const double R[3][3], const double t[3]);
  double reprojection_error(const double R[3][3], const double t[3]);

  void choose_control_points(void);
  void compute_barycentric_coordinates(void);
  void fill_M(cv::Mat &M, const int row, const double * alphas, const double u, const double v);
  void compute_ccs(const double * betas, const double * ut);
  void compute_pcs(void);

  void solve_for_sign(void);

  void find_betas_approx_1(const cv::Mat & L_6x10, const cv::Mat & Rho, double * betas);
  void find_betas_approx_2(const cv::Mat & L_6x10, const cv::Mat & Rho, double * betas);
  void find_betas_approx_3(const cv::Mat & L_6x10, const cv::Mat & Rho, double * betas);
  void qr_solve(cv::Mat & A, cv::Mat & b, cv::Mat & X);

  double dot(const double * v1, const double * v2);
  double dist2(const double * p1, const double * p2);

  void compute_rho(double * rho);
  void compute_L_6x10(const double * ut, double * l_6x10);

  void gauss_newton(const cv::Mat & L_6x10, const cv::Mat & Rho, double current_betas[4]);
  void compute_A_and_b_gauss_newton(const double * l_6x10, const double * rho,
				    double cb[4], cv::Mat & A, cv::Mat & b);

  double compute_R_and_t(const double * ut, const double * betas,
			 double R[3][3], double t[3]);

  void estimate_R_and_t(double R[3][3], double t[3]);

  void copy_R_and_t(const double R_dst[3][3], const double t_dst[3],
		    double R_src[3][3], double t_src[3]);

  void mat_to_quat(const double R[3][3], double q[4]);


  double uc, vc, fu, fv;

  double * pws, * us, * alphas, * pcs;
  int maximum_number_of_correspondences;
  int number_of_correspondences;

  double cws[4][3], ccs[4][3];
  double cws_determinant;

  // 2D Points
  std::vector<cv::Point2f> mvP2D;
  std::vector<float> mvSigma2;

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

  // Index in Frame
  std::vector<size_t> mvKeyPointIndices;

  // Current Estimation
  double mRi[3][3];
  double mti[3];
  cv::Mat mTcwi;
  std::vector<bool> mvbInliersi;
  int mnInliersi;

  // Current Ransac State
  int mnIterations;
  std::vector<bool> mvbBestInliers;
  int mnBestInliers;
  cv::Mat mBestTcw;

  // Refined
  cv::Mat mRefinedTcw;
  std::vector<bool> mvbRefinedInliers;
  int mnRefinedInliers;

  // Number of Correspondences
  int N;

  // Indices for random selection [0 .. N-1]
  std::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)
  std::vector<float> mvMaxError;

};

} //namespace