RANSAC(随机采样一致性)算法在多目三维空间点重构中的应用demo(c++代码实现)

RANSAC(随机采样一致性) 算法与最小二乘算法不同之处在于它假设数据样本中包含局外点即outliers，其余为局内点即inliers。该算法通过迭代一定的次数，每次随机选取若干样本构造一个模型，然后验证其余样本是否为局内点。以此找到一个相对合理的最优模型。在计算机视觉方面广泛应用。具体可参考网上其余博主对RANSAC算法层面上的讲述和理解。

1 问题：笔者在做多目三维人体关节点重构时遇到一个这样的问题：在某些视角视角下，关节点的二维检测是不准确的，错误的检测在三角化是就会导致重构结果偏差较大。例如下图的例子（Shelf数据集中的某一帧）：

绿色箭头所指的这个关节点在0 1 2 4视角出现，但是0视角下该关节点的二维检测显然是不准确的，三角化后重构结果也会不准确。

2 使用RANSAC算法排除较差的二维检测

现在我们把不同视角的二维检测当做若干的样本，每次随机选择一定数量(这里为2)的视角，重构出一个三维点，将改点投影到其余视角，通过投影误差与一个阈值的比较，判定新的视角是否为inliers，是的话将其加入到重构列表里，否则不加。通过若干次随机采样的迭代后，从中选择inliers数量最多的那次作为bestModel，最后使用inliers重构最后的三维点。这么说起来，其实ransac算法还是很好理解的。

3 用c++来实现ransac三角化算法

代码实现使用了一位大神写的 RANSAC通用框架，在此基础上，我们只需要定义一个待解决问题的模型就行了，以下代码包括一个TriangulationRansacModel和测试程序，代码仓库地址，如果有帮助，可以加个星。

TriangulationModel.hpp

#pragma once

#include <opencv2/opencv.hpp>
#include "AbstractModel.hpp"

// triangulation using SVD
void TriangulateWithConf(const std::vector<cv::Point3f>& pointsOnEachCamera,
                         const std::vector<cv::Mat>& cameraMatrices,
                         cv::Mat& reconstructedPoint) {
  const auto numberCameras = (int)cameraMatrices.size();
  cv::Mat A = cv::Mat::zeros(numberCameras * 2, 4, CV_32F);
  for (auto i = 0; i < numberCameras; i++) {
    A.row(i * 2) = (pointsOnEachCamera[i].x * cameraMatrices[i].row(2) -
                    cameraMatrices[i].row(0)) *
                   pointsOnEachCamera[i].z;

    A.row(i * 2 + 1) = (pointsOnEachCamera[i].y * cameraMatrices[i].row(2) -
                        cameraMatrices[i].row(1)) *
                       pointsOnEachCamera[i].z;
  }
  // Solve x for Ax = 0 --> SVD on A
  cv::SVD::solveZ(A, reconstructedPoint);
  reconstructedPoint /= reconstructedPoint.at<float>(3);
}

// per view sample
class SingleView : public GRANSAC::AbstractParameter {
 public:
  SingleView(GRANSAC::VPFloat x, GRANSAC::VPFloat y, GRANSAC::VPFloat conf,
             cv::Mat& ProjMatrix)
      : m_ProjMatrix(ProjMatrix) {
    m_Point2D = cv::Point3f(x, y, conf);
  };

  cv::Point3f m_Point2D;
  cv::Mat& m_ProjMatrix;
};

// our model
class TriangulationRansacModel : public GRANSAC::AbstractModel<2> {
 protected:
  // Parametric form
  cv::Mat m_ReconstructedPoint;

  // calculate reprojection error
  virtual GRANSAC::VPFloat ComputeDistanceMeasure(
      std::shared_ptr<GRANSAC::AbstractParameter> Param) override {
    auto singleView = std::dynamic_pointer_cast<SingleView>(Param);
    if (singleView == nullptr)
      throw std::runtime_error(
          "TriangulationRansacModel::ComputeDistanceMeasure() - Passed "
          "parameter are not of type Point2D.");

    cv::Mat projPoint2D = singleView->m_ProjMatrix * m_ReconstructedPoint;
    projPoint2D = projPoint2D / projPoint2D.at<float>(2);
    cv::Point3f& point2D = singleView->m_Point2D;
    GRANSAC::VPFloat Dist = sqrt(pow(point2D.x - projPoint2D.at<float>(0), 2) +
                                 pow(point2D.y - projPoint2D.at<float>(1), 2));

    return Dist;
  };

 public:
  TriangulationRansacModel(
      const std::vector<std::shared_ptr<GRANSAC::AbstractParameter>>&
          InputParams) {
    Initialize(InputParams);
  };

  // estimate 3d point from two views
  virtual void Initialize(
      const std::vector<std::shared_ptr<GRANSAC::AbstractParameter>>&
          InputParams) override {
    if (InputParams.size() != 2)
      throw std::runtime_error(
          "TriangulationRansacModel - Number of input parameters does not "
          "match minimum number required for this model.");

    // Check for AbstractParamter types
    auto singleView1 = std::dynamic_pointer_cast<SingleView>(InputParams[0]);
    auto singleView2 = std::dynamic_pointer_cast<SingleView>(InputParams[1]);
    if (singleView1 == nullptr || singleView2 == nullptr)
      throw std::runtime_error(
          "TriangulationRansacModel - InputParams type mismatch. It is not a "
          "Point2D.");

    std::copy(InputParams.begin(), InputParams.end(), m_MinModelParams.begin());

    // reconstruct 3d point
    std::vector<cv::Point3f> pointsOnEachCamera{singleView1->m_Point2D,
                                                singleView2->m_Point2D};
    std::vector<cv::Mat> cameraMatrices{singleView1->m_ProjMatrix,
                                        singleView2->m_ProjMatrix};
    TriangulateWithConf(pointsOnEachCamera, cameraMatrices,
                        m_ReconstructedPoint);
  };

  virtual std::pair<GRANSAC::VPFloat,
                    std::vector<std::shared_ptr<GRANSAC::AbstractParameter>>>
  Evaluate(const std::vector<std::shared_ptr<GRANSAC::AbstractParameter>>&
               EvaluateParams,
           GRANSAC::VPFloat Threshold) {
    std::vector<std::shared_ptr<GRANSAC::AbstractParameter>> Inliers;
    int nTotalParams = EvaluateParams.size();
    int nInliers = 0;

    for (auto& Param : EvaluateParams) {
      if (ComputeDistanceMeasure(Param) < Threshold) {
        Inliers.push_back(Param);
        nInliers++;
      }
    }

    GRANSAC::VPFloat InlierFraction =
        GRANSAC::VPFloat(nInliers) /
        GRANSAC::VPFloat(nTotalParams);  // This is the inlier fraction

    return std::make_pair(InlierFraction, Inliers);
  };

  // get final reconstructed 3d point using best inliers
  cv::Mat GetBestReconstructed3DPoint(
      const std::vector<std::shared_ptr<GRANSAC::AbstractParameter>>&
          BestInliers) {
    std::vector<cv::Point3f> pointsOnEachCamera;
    std::vector<cv::Mat> cameraMatrices;

    for (auto& Inlier : BestInliers) {
      auto sv = std::dynamic_pointer_cast<SingleView>(Inlier);
      pointsOnEachCamera.emplace_back(sv->m_Point2D);
      cameraMatrices.emplace_back(sv->m_ProjMatrix);
    }
    cv::Mat reconstructed3DPoint;
    TriangulateWithConf(pointsOnEachCamera, cameraMatrices,
                        reconstructed3DPoint);
    return reconstructed3DPoint;
  }
};

TriangulationRansacSample.cpp

#include <cmath>
#include <iostream>
#include <opencv2/opencv.hpp>

#include "GRANSAC.hpp"
#include "TriangulationModel.hpp"

int main(int argc, char* arcv[]) {
  // load projection matrix
  unsigned int nCameraViews = 5;
  std::vector<cv::Mat> Ps;

  Ps.resize(nCameraViews);
  cv::FileStorage psread("/home/shuai/Desktop/Shelf_Ps.xml",
                         cv::FileStorage::READ);
  for (unsigned int i = 0; i < nCameraViews; i++) {
    cv::Mat m;
    psread["p_" + std::to_string(i)] >> m;
    Ps.at(i) = m;
  }
  psread.release();

  // prepare candidate view data
  std::vector<std::shared_ptr<GRANSAC::AbstractParameter>> CandViews;

  CandViews.emplace_back(
      std::make_shared<SingleView>(577, 768, 0.732, Ps.at(0)));
  CandViews.emplace_back(
      std::make_shared<SingleView>(933, 666, 1.06, Ps.at(1)));
  CandViews.emplace_back(
      std::make_shared<SingleView>(828, 467, 1.072, Ps.at(2)));
  CandViews.emplace_back(
      std::make_shared<SingleView>(389, 475, 1.009, Ps.at(4)));

  // RANSAC Triangulation
  GRANSAC::RANSAC<TriangulationRansacModel, 2> Estimator;
  Estimator.Initialize(15, 10);  // Threshold, iterations
  int64_t start = cv::getTickCount();
  Estimator.Estimate(CandViews);
  int64_t end = cv::getTickCount();
  std::cout << "RANSAC Took: "
            << GRANSAC::VPFloat(end - start) /
                   GRANSAC::VPFloat(cv::getTickFrequency()) * 1000.0
            << " ms." << std::endl;

  // other informations to output
  auto& BestInliers = Estimator.GetBestInliers();
  std::cout << std::endl;
  std::cout << "Best Inliers: " << std::endl;
  for (auto& Inlier : BestInliers) {
    auto sv = std::dynamic_pointer_cast<SingleView>(Inlier);
    std::cout << sv->m_Point2D << std::endl;
  }

  auto bestModel = Estimator.GetBestModel();
  auto reconstruct3DPoint =
      std::dynamic_pointer_cast<TriangulationRansacModel>(bestModel)
          ->GetBestReconstructed3DPoint(BestInliers);

  std::cout << std::endl;
  std::cout << "Reconstructed 3D Point: " << std::endl
            << reconstruct3DPoint << std::endl;

  std::cout << std::endl;
  return 0;
}

4 结果

结果中，只选择了三个视角进行重构，其中0视角的(577, 768)被忽略。