Opencv 3.4 中的EPnp位姿估计算法

这里只介绍opencv3.4 中Epnp算法的头文件，具体内容可以参考论文《EPnP: An Accurate O(n) Solution to the PnP Problem》+opencv3.4 源代码。

源代码+参考文论+部分ppt已经传到csdn。

#ifndef epnp_h
#define epnp_h

#include "precomp.hpp"
#include "opencv2/core/core_c.h"

namespace cv
{

class epnp {
 public:
  /*  max 注释
  *   函数功能：ePnP算法的初始化构造函数，ePnP求解最少需要4对点，而且是相机内参已知
  *             
  *   参数：
  *   [in]    cameraMatrix            相机内参
  *   [in]    opoints                 参考点在世界坐标系中的点，至少4个点-------float or double
  *   [in]    ipoints                 参考点在相机图像上的投影点坐标，至少4个点-------float or double
  *    返回值：
  *           
  */
  epnp(const cv::Mat& cameraMatrix, const cv::Mat& opoints, const cv::Mat& ipoints);
  ~epnp();

  // 此函数没有实现---没有使用
  void add_correspondence(const double X, const double Y, const double Z,
              const double u, const double v);

  /*  max 注释
  *   函数功能：ePnP算法的初始化构造函数，ePnP求解最少需要4对点，而且是相机内参已知
  *
  *   参数：
  *   [out]    R                 旋转矩阵3x3，世界坐标系到相机坐标系
  *   [out]    t                 平移向量，世界坐标系到相机坐标系
  *    返回值：
  *
  */
  void compute_pose(cv::Mat& R, cv::Mat& t);



 private:
  epnp(const epnp &); // copy disabled
  epnp& operator=(const epnp &); // assign disabled

  // 初始化相机内参
  template <typename T>
  void init_camera_parameters(const cv::Mat& cameraMatrix)
  {
    uc = cameraMatrix.at<T> (0, 2);
    vc = cameraMatrix.at<T> (1, 2);
    fu = cameraMatrix.at<T> (0, 0);
    fv = cameraMatrix.at<T> (1, 1);
  }


  // 将输入的世界坐标系的参考点和图像坐标系中的点转化到成员变量存储
  template <typename OpointType, typename IpointType>
  void init_points(const cv::Mat& opoints, const cv::Mat& ipoints)
  {
      for(int i = 0; i < number_of_correspondences; i++)
      {
          pws[3 * i    ] = opoints.at<OpointType>(i).x;
          pws[3 * i + 1] = opoints.at<OpointType>(i).y;
          pws[3 * i + 2] = opoints.at<OpointType>(i).z;

          us[2 * i    ] = ipoints.at<IpointType>(i).x*fu + uc;
          us[2 * i + 1] = ipoints.at<IpointType>(i).y*fv + vc;
      }
  }


  // 计算世界坐标系的参考点在相机图像坐标系下的投影误差
  double reprojection_error(const double R[3][3], const double t[3]);

  // 选择参考坐标系中的控制点，第一个点为质心，第2-4个点PCA中的3个主成分方向上的点
  void choose_control_points(void);

  // 计算出论文中的alpha， 灰度质心齐次坐标
  void compute_barycentric_coordinates(void);

  // 填充M矩阵，MX=0;
  void fill_M(CvMat * M, const int row, const double * alphas, const double u, const double v);


  // 根据计算的betas值和对应的特征向量，计算相机坐标系中的控制点
  void compute_ccs(const double * betas, const double * ut);
  // 通过相机坐标系下的控制点，计算所有参考点在相机坐标系下的坐标；
  void compute_pcs(void);
  // 检测参考点在相机坐标系下的z轴坐标是否大于零，大于零不做修改。如果小于零，需要按照相机光心（原点）中心对称一下。
  void solve_for_sign(void);


  // 假设N=4，就是有4组特征向量的组合来表示解
  void find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho, double * betas);
  // 假设N=2，就是有2组特征向量的组合来表示解
  void find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho, double * betas);
  // 假设N=3，就是有3组特征向量的组合来表示解
  void find_betas_approx_3(const CvMat * L_6x10, const CvMat * Rho, double * betas);
  void qr_solve(CvMat * A, CvMat * b, CvMat * X);

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


  // 求beta时的rho向量，L*beta=rho
  void compute_rho(double * rho);

  // 求beta时的L矩阵（就是论文中N=4时，由最小的四个奇异值所对应的右特征向量所组成) l_6x10 为输出的L矩阵
  void compute_L_6x10(const double * ut, double * l_6x10);

  // 高斯牛顿迭代求解，只迭代5次。
  void gauss_newton(const CvMat * L_6x10, const CvMat * Rho, double current_betas[4]);
  // 高斯牛顿中的梯度
  void compute_A_and_b_gauss_newton(const double * l_6x10, const double * rho,
                    const double cb[4], CvMat * A, CvMat * b);

  // 计算相机坐标系和世界坐标系之间的变换
  double compute_R_and_t(const double * ut, const double * betas,
             double R[3][3], double t[3]);

  // horn 的绝对定向算法
  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]);


  double uc, vc, fu, fv;                              // 相机内参

  std::vector<double> pws, us, alphas, pcs;          // pws 参考点在世界坐标系中的坐标； us 对应世界点在图像坐标系上的投影；  alphas 与论文中的一样；pcs 参考点在相机坐标系中的坐标
  int number_of_correspondences;                    // 对应点的个数

  double cws[4][3], ccs[4][3];                       //cws 参考坐标系中4个3D参考点；     ccs 相机坐标系中4个3D 参考点
  int max_nr;
  double * A1, * A2;                                // 高斯牛顿使用的量
};

}

#endif