ORB-SLAM2学习笔记——epnp求解（代码好长阿）

epnp求解

1、理论部分

2、代码部分（作者牛pi）

//对pnp求解后值进行保存
PnPsolver::PnPsolver(const Frame &F, const vector<MapPoint*> &vpMapPointMatches):
    pws(0), us(0), alphas(0), pcs(0), maximum_number_of_correspondences(0), number_of_correspondences(0), mnInliersi(0),
    mnIterations(0), mnBestInliers(0), N(0)
{
    mvpMapPointMatches = vpMapPointMatches;
    mvP2D.reserve(F.mvpMapPoints.size());
    mvSigma2.reserve(F.mvpMapPoints.size());
    mvP3Dw.reserve(F.mvpMapPoints.size());
    mvKeyPointIndices.reserve(F.mvpMapPoints.size());
    mvAllIndices.reserve(F.mvpMapPoints.size());

    int idx=0;
    //遍历所有匹配点
    for(size_t i=0, iend=vpMapPointMatches.size(); i<iend; i++)
    {
      //提取匹配信息
        MapPoint* pMP = vpMapPointMatches[i];
      //存在匹配
        if(pMP)
        {
            //匹配为好的（内点之间的匹配就是好的）
            if(!pMP->isBad())
            {
                //提取关键点
                const cv::KeyPoint &kp = F.mvKeysUn[i];
                //对应的像素存入mvp2D中
                mvP2D.push_back(kp.pt);
                //
                mvSigma2.push_back(F.mvLevelSigma2[kp.octave]);
                //得到与世界坐标中的位姿关系
                cv::Mat Pos = pMP->GetWorldPos();
                //导入世界坐标到mvP3D中
                mvP3Dw.push_back(cv::Point3f(Pos.at<float>(0),Pos.at<float>(1), Pos.at<float>(2)));
                //导入关键点编号
                mvKeyPointIndices.push_back(i);
                //导入2d点的编号
                // mvAllIndices为所有参与PnP的2D点的索引
                mvAllIndices.push_back(idx);               

                idx++;
            }
        }
    }

    // Set camera calibration parameters
    //相机外参
    fu = F.fx;
    fv = F.fy;
    uc = F.cx;
    vc = F.cy;

    SetRansacParameters();
}
//计算完成，释放内存
PnPsolver::~PnPsolver()
{
  delete [] pws;
  delete [] us;
  delete [] alphas;
  delete [] pcs;
}

//概率 最小内点数量 最大迭代次数 最小置位 系数（用于计算最小内点） 阈值
void PnPsolver::SetRansacParameters(double probability, int minInliers, int maxIterations, int minSet, float epsilon, float th2)
{
    mRansacProb = probability;
    mRansacMinInliers = minInliers;
    mRansacMaxIts = maxIterations;
    mRansacEpsilon = epsilon;
    mRansacMinSet = minSet;
    //匹配点的数量
    N = mvP2D.size(); // number of correspondences
    //标志位（布尔类型）
    mvbInliersi.resize(N);

    // Adjust Parameters according to number of correspondences
    //根据相关点的数量进行调整参数
    //初始化最小内点数量
    int nMinInliers = N*mRansacEpsilon;
    //更新内点数
    if(nMinInliers<mRansacMinInliers)
        nMinInliers=mRansacMinInliers;
    //内点数过少会被重置
    if(nMinInliers<minSet)
        nMinInliers=minSet;
    mRansacMinInliers = nMinInliers;
    //更新系数 mRansacMinInliers > nMinInliers
    //传入的内点比我们要求的多
    if(mRansacEpsilon<(float)mRansacMinInliers/N)
        mRansacEpsilon=(float)mRansacMinInliers/N;

    // Set RANSAC iterations according to probability, epsilon, and max iterations
    //根据各个参数调整迭代次数
    int nIterations;
    //内点数等于匹配点数 迭代一次就行
    if(mRansacMinInliers==N)
        nIterations=1;
    else
        //ceil：大于或等于的最小整数 pow（x， y） x的y次方
        nIterations = ceil(log(1-mRansacProb)/log(1-pow(mRansacEpsilon,3)));
    //
    mRansacMaxIts = max(1,min(nIterations,mRansacMaxIts));
    //最大误差
    mvMaxError.resize(mvSigma2.size());
    //给允许的最大误差赋值
    for(size_t i=0; i<mvSigma2.size(); i++)
        mvMaxError[i] = mvSigma2[i]*th2;
}
//
cv::Mat PnPsolver::find(vector<bool> &vbInliers, int &nInliers)
{
    bool bFlag;
    //iterate（最大迭代次数， 标志位， 内点标志位， 内点数量）
    return iterate(mRansacMaxIts,bFlag,vbInliers,nInliers);    
}
//iterate（迭代次数， 标志位（是否要迭代），（内点标志位）， （内点数量） ）
cv::Mat PnPsolver::iterate(int nIterations, bool &bNoMore, vector<bool> &vbInliers, int &nInliers)
{
  //初始化
    bNoMore = false;
    vbInliers.clear();
    nInliers=0;
  //
    set_maximum_number_of_correspondences(mRansacMinSet);
  //匹配点小于内点 结束迭代
    if(N<mRansacMinInliers)
    {
        bNoMore = true;
        return cv::Mat();
    }
    // mvAllIndices为所有参与PnP的2D点的索引
    // vAvailableIndices为每次从mvAllIndices中随机挑选mRansacMinSet组3D-2D对应点进行一次RANSAC
    vector<size_t> vAvailableIndices;
    //当前迭代次数
    int nCurrentIterations = 0;
    //迭代次数小于最大迭代次数， 当前迭代次数小于要求次数 继续迭代
    while(mnIterations<mRansacMaxIts || nCurrentIterations<nIterations)
    {
        nCurrentIterations++;
        mnIterations++;
        //重置相关性
        reset_correspondences();
        //把序号存好
        vAvailableIndices = mvAllIndices;

        // Get min set of points
        for(short i = 0; i < mRansacMinSet; ++i)
        {
            int randi = DUtils::Random::RandomInt(0, vAvailableIndices.size()-1);

            int idx = vAvailableIndices[randi];

            add_correspondence(mvP3Dw[idx].x,mvP3Dw[idx].y,mvP3Dw[idx].z,mvP2D[idx].x,mvP2D[idx].y);

            vAvailableIndices[randi] = vAvailableIndices.back();
            vAvailableIndices.pop_back();
        }
        //计算位姿
        // Compute camera pose
        compute_pose(mRi, mti);
        //检查内点
        // Check inliers
        CheckInliers();
        //当前内点数目 >= 要求的最小内点数目（根据匹配点数决定的）
        if(mnInliersi>=mRansacMinInliers)
        {
            // If it is the best solution so far, save it
            //如果比最优内点数目还多
            if(mnInliersi>mnBestInliers)
            { 
                //替换最优点
                //内点布尔类型
                mvbBestInliers = mvbInliersi;
                //数量
                mnBestInliers = mnInliersi;
                //定义 旋转矩阵 平移向量 还有 T 
                cv::Mat Rcw(3,3,CV_64F,mRi);
                cv::Mat tcw(3,1,CV_64F,mti);
                Rcw.convertTo(Rcw,CV_32F);
                tcw.convertTo(tcw,CV_32F);
                mBestTcw = cv::Mat::eye(4,4,CV_32F);
                Rcw.copyTo(mBestTcw.rowRange(0,3).colRange(0,3));
                tcw.copyTo(mBestTcw.rowRange(0,3).col(3));
            }
            //
            if(Refine())
            {
                nInliers = mnRefinedInliers;
                //所有标志位置于false
                vbInliers = vector<bool>(mvpMapPointMatches.size(),false);
                //遍历所有点 如果为重新定义的点 标志位true
                for(int i=0; i<N; i++)
                {
                    if(mvbRefinedInliers[i])
                        vbInliers[mvKeyPointIndices[i]] = true;
                }
                //得到T 
                return mRefinedTcw.clone();
            }

        }
    }
    //当前迭代次数大于要求最大迭代次数
    if(mnIterations>=mRansacMaxIts)
    {   
        bNoMore=true;
        //内殿不够
        if(mnBestInliers>=mRansacMinInliers)
        {
            //最好内点数替换当前内点数
            nInliers=mnBestInliers;
            //当前内点标识 false
            vbInliers = vector<bool>(mvpMapPointMatches.size(),false);
            //遍历所有最好内殿 标志为true
            for(int i=0; i<N; i++)
            {
                if(mvbBestInliers[i])
                    vbInliers[mvKeyPointIndices[i]] = true;
            }
            //返回 T
            return mBestTcw.clone();
        }
    }

    return cv::Mat();
}

bool PnPsolver::Refine()
{
    vector<int> vIndices;
    vIndices.reserve(mvbBestInliers.size());

    for(size_t i=0; i<mvbBestInliers.size(); i++)
    {
        if(mvbBestInliers[i])
        {
            vIndices.push_back(i);
        }
    }

    set_maximum_number_of_correspondences(vIndices.size());

    reset_correspondences();

    for(size_t i=0; i<vIndices.size(); i++)
    {
        int idx = vIndices[i];
        add_correspondence(mvP3Dw[idx].x,mvP3Dw[idx].y,mvP3Dw[idx].z,mvP2D[idx].x,mvP2D[idx].y);
    }

    // Compute camera pose
    compute_pose(mRi, mti);

    // Check inliers
    CheckInliers();

    mnRefinedInliers =mnInliersi;
    mvbRefinedInliers = mvbInliersi;

    if(mnInliersi>mRansacMinInliers)
    {
        cv::Mat Rcw(3,3,CV_64F,mRi);
        cv::Mat tcw(3,1,CV_64F,mti);
        Rcw.convertTo(Rcw,CV_32F);
        tcw.convertTo(tcw,CV_32F);
        mRefinedTcw = cv::Mat::eye(4,4,CV_32F);
        Rcw.copyTo(mRefinedTcw.rowRange(0,3).colRange(0,3));
        tcw.copyTo(mRefinedTcw.rowRange(0,3).col(3));
        return true;
    }

    return false;
}

//检查内点是否合格  用误差检查
void PnPsolver::CheckInliers()
{
    mnInliersi=0;

    for(int i=0; i<N; i++)
    {
        cv::Point3f P3Dw = mvP3Dw[i];
        cv::Point2f P2D = mvP2D[i];
        //求相机平面坐标
        float Xc = mRi[0][0]*P3Dw.x+mRi[0][1]*P3Dw.y+mRi[0][2]*P3Dw.z+mti[0];
        float Yc = mRi[1][0]*P3Dw.x+mRi[1][1]*P3Dw.y+mRi[1][2]*P3Dw.z+mti[1];
        float invZc = 1/(mRi[2][0]*P3Dw.x+mRi[2][1]*P3Dw.y+mRi[2][2]*P3Dw.z+mti[2]);
        //求像素坐标
        double ue = uc + fu * Xc * invZc;
        double ve = vc + fv * Yc * invZc;
        //求误差
        float distX = P2D.x-ue;
        float distY = P2D.y-ve;

        float error2 = distX*distX+distY*distY;
        //误差小于要求最大误差
        if(error2<mvMaxError[i])
        {
            //当前的内点为好点
            mvbInliersi[i]=true;
            mnInliersi++;
        }
        //否则判定为坏点
        else
        {
            mvbInliersi[i]=false;
        }
    }
}

//给一些相关系数赋值
// number_of_correspondences为RANSAC每次PnP求解时时3D点和2D点匹配对数
// RANSAC需要很多次，maximum_number_of_correspondences为匹配对数最大值
// 这个变量用于决定pws us alphas pcs容器的大小，因此只能逐渐变大不能减小

void PnPsolver::set_maximum_number_of_correspondences(int n)
{
  //如果maximum_number_of_correspondences之前设置的过小，则重新设置，并重新初始化pws us alphas pcs的大小
  if (maximum_number_of_correspondences < n) {
    if (pws != 0) delete [] pws;
    if (us != 0) delete [] us;
    if (alphas != 0) delete [] alphas;
    if (pcs != 0) delete [] pcs;

    maximum_number_of_correspondences = n;
    pws = new double[3 * maximum_number_of_correspondences];
    us = new double[2 * maximum_number_of_correspondences];
    alphas = new double[4 * maximum_number_of_correspondences];
    pcs = new double[3 * maximum_number_of_correspondences];
  }
}
//重置匹配点的数目
void PnPsolver::reset_correspondences(void)
{
  number_of_correspondences = 0;
}
//添加匹配点对参数进行修改
void PnPsolver::add_correspondence(double X, double Y, double Z, double u, double v)
{
  pws[3 * number_of_correspondences    ] = X;
  pws[3 * number_of_correspondences + 1] = Y;
  pws[3 * number_of_correspondences + 2] = Z;

  us[2 * number_of_correspondences    ] = u;
  us[2 * number_of_correspondences + 1] = v;

  number_of_correspondences++;
}
//筛选控制点
void PnPsolver::choose_control_points(void)
{
  // Take C0 as the reference points centroid:
  // 步骤1：第一个控制点：参与PnP计算的参考3D点的几何中心
  cws[0][0] = cws[0][1] = cws[0][2] = 0;
  //求所有参考帧3D点的坐标的和
  for(int i = 0; i < number_of_correspondences; i++)
    for(int j = 0; j < 3; j++)
      cws[0][j] += pws[3 * i + j];
  //求3D点的几何中心
  for(int j = 0; j < 3; j++)
    cws[0][j] /= number_of_correspondences;


  // Take C1, C2, and C3 from PCA on the reference points:
  // 步骤2：计算其它三个控制点，C1, C2, C3通过PCA分解得到
  // 将所有的3D参考点写成矩阵，(number_of_correspondences *　３)的矩阵
  CvMat * PW0 = cvCreateMat(number_of_correspondences, 3, CV_64F);
  //
  double pw0tpw0[3 * 3], dc[3], uct[3 * 3];
  CvMat PW0tPW0 = cvMat(3, 3, CV_64F, pw0tpw0);
  CvMat DC      = cvMat(3, 1, CV_64F, dc);
  CvMat UCt     = cvMat(3, 3, CV_64F, uct);
  // 步骤2.1：将存在pws中的参考3D点减去第一个控制点的坐标（相当于把第一个控制点作为原点）, 并存入PW0
  for(int i = 0; i < number_of_correspondences; i++)
    for(int j = 0; j < 3; j++)
      PW0->data.db[3 * i + j] = pws[3 * i + j] - cws[0][j];
  // 步骤2.2：利用SVD分解P'P可以获得P的主分量
  // 类似于齐次线性最小二乘求解的过程，
  // PW0的转置乘以PW0
  cvMulTransposed(PW0, &PW0tPW0, 1);
  //PW0的转置乘以PW0进行SVD分解
  cvSVD(&PW0tPW0, &DC, &UCt, 0, CV_SVD_MODIFY_A | CV_SVD_U_T);

  cvReleaseMat(&PW0);
  // 步骤2.3：得到C1, C2, C3三个3D控制点，最后加上之前减掉的第一个控制点这个偏移量
  for(int i = 1; i < 4; i++) {
    double k = sqrt(dc[i - 1] / number_of_correspondences);
    for(int j = 0; j < 3; j++)
      cws[i][j] = cws[0][j] + k * uct[3 * (i - 1) + j];
  }
}//得到四个控制点

// 求解四个控制点的系数alphas
// (a2 a3 a4)' = inverse(cws2-cws1 cws3-cws1 cws4-cws1)*(pws-cws1)，a1 = 1-a2-a3-a4
// 每一个3D控制点，都有一组alphas与之对应
// cws1 cws2 cws3 cws4为四个控制点的坐标
// pws为3D参考点的坐标
void PnPsolver::compute_barycentric_coordinates(void)
{
  double cc[3 * 3], cc_inv[3 * 3];
  CvMat CC     = cvMat(3, 3, CV_64F, cc);
  CvMat CC_inv = cvMat(3, 3, CV_64F, cc_inv);

  for(int i = 0; i < 3; i++)
    for(int j = 1; j < 4; j++)
      cc[3 * i + j - 1] = cws[j][i] - cws[0][i];

  cvInvert(&CC, &CC_inv, CV_SVD);
  double * ci = cc_inv;
  for(int i = 0; i < number_of_correspondences; i++) {
    double * pi = pws + 3 * i;
    double * a = alphas + 4 * i;

    for(int j = 0; j < 3; j++)
      a[1 + j] =
	ci[3 * j    ] * (pi[0] - cws[0][0]) +
	ci[3 * j + 1] * (pi[1] - cws[0][1]) +
	ci[3 * j + 2] * (pi[2] - cws[0][2]);
    a[0] = 1.0f - a[1] - a[2] - a[3];
  }
}