案例4：OpenMVG估计本质矩阵、三角化、计算重投影误差

1. 估计本质矩阵(Ransac方法)
2. 将估计本质矩阵的模型内点进行三角化
3. 计算3D点云的重投影误差

#include "openMVG/cameras/Camera_Pinhole.hpp"
#include "openMVG/features/feature.hpp"
#include "openMVG/features/sift/SIFT_Anatomy_Image_Describer.hpp"
#include "openMVG/features/svg_features.hpp"
#include "openMVG/geometry/pose3.hpp"
#include "openMVG/image/image_io.hpp"
#include "openMVG/image/image_concat.hpp"
#include "openMVG/matching/indMatchDecoratorXY.hpp"
#include "openMVG/matching/regions_matcher.hpp"
#include "openMVG/matching/svg_matches.hpp"
#include "openMVG/multiview/triangulation.hpp"
#include "openMVG/numeric/eigen_alias_definition.hpp"
#include "openMVG/sfm/pipelines/sfm_robust_model_estimation.hpp"

#include "third_party/stlplus3/filesystemSimplified/file_system.hpp"

#include <iostream>
#include <string>
#include <utility>

using namespace openMVG;
using namespace openMVG::matching;
using namespace openMVG::image;
using namespace openMVG::cameras;
using namespace openMVG::geometry;
using namespace std;
//读取相机内参数文件
bool readIntrinsic(const std::string & fileName, Mat3 & K);
//导出3d点和相机位姿到ply文件
bool exportToPly(const std::vector<Vec3> & vec_points,
  const std::vector<Vec3> & vec_camPos,
  const std::string & sFileName);

int main() {
 
  //....前面省略了特征匹配部分，见专栏
  {
    Mat3 K;
    //读取相机内参数
    if (!readIntrinsic(stlplus::create_filespec(sInputDir,"K","txt"), K))
    {
      std::cerr << "Cannot read intrinsic parameters." << std::endl;
      return EXIT_FAILURE;
    }

    const Pinhole_Intrinsic
      camL(imageL.Width(), imageL.Height(), K(0,0), K(0,2), K(1,2)),
      camR(imageR.Width(), imageR.Height(), K(0,0), K(0,2), K(1,2));

    //A. 匹配点矩阵
    Mat xL(2, vec_PutativeMatches.size());
    Mat xR(2, vec_PutativeMatches.size());
    for (size_t k = 0; k < vec_PutativeMatches.size(); ++k)  {
      const PointFeature & imaL = featsL[vec_PutativeMatches[k].i_];
      const PointFeature & imaR = featsR[vec_PutativeMatches[k].j_];
      xL.col(k) = imaL.coords().cast<double>();
      xR.col(k) = imaR.coords().cast<double>();
    }

    //B. 本质矩阵估计，robustRelativePose结构体中已经存储了
    const std::pair<size_t, size_t> size_imaL(imageL.Width(), imageL.Height());
    const std::pair<size_t, size_t> size_imaR(imageR.Width(), imageR.Height());
    sfm::RelativePose_Info relativePose_info;
    if (!sfm::robustRelativePose(&camL, &camR, xL, xR, relativePose_info, size_imaL, size_imaR, 256))
    {
      std::cerr << " /!\\ Robust relative pose estimation failure."
        << std::endl;
      return EXIT_FAILURE;
    }

    std::cout << "\nFound an Essential matrix:\n"
      << "\tprecision: " << relativePose_info.found_residual_precision << " pixels\n"
      << "\t#inliers: " << relativePose_info.vec_inliers.size() << "\n"
      << "\t#matches: " << vec_PutativeMatches.size()
      << std::endl;

    //可视化本质矩阵使用的模型内点
    const bool bVertical = true;
    InlierMatches2SVG
    (
      jpg_filenameL,
      {imageL.Width(), imageL.Height()},
      regionsL->GetRegionsPositions(),
      jpg_filenameR,
      {imageR.Width(), imageR.Height()},
      regionsR->GetRegionsPositions(),
      vec_PutativeMatches,
      relativePose_info.vec_inliers,
      "04_ACRansacEssential.svg",
      bVertical
    );

    //C. 三角化
    std::vector<Vec3> vec_3DPoints;

    // 设置相机内参数矩阵和位姿
    const Pinhole_Intrinsic intrinsic0(imageL.Width(), imageL.Height(), K(0, 0), K(0, 2), K(1, 2));
    const Pinhole_Intrinsic intrinsic1(imageR.Width(), imageR.Height(), K(0, 0), K(0, 2), K(1, 2));

    const Pose3 pose0 = Pose3(Mat3::Identity(), Vec3::Zero());
    const Pose3 pose1 = relativePose_info.relativePose;

    // 将估计本质矩阵所用的模型内点进行三角化得到3D点
    std::vector<double> vec_residuals;
    vec_residuals.reserve(relativePose_info.vec_inliers.size() * 4);
    for (const auto inlier_idx : relativePose_info.vec_inliers)  {
      const SIOPointFeature & LL = regionsL->Features()[vec_PutativeMatches[inlier_idx].i_];
      const SIOPointFeature & RR = regionsR->Features()[vec_PutativeMatches[inlier_idx].j_];
      // 存储三角化的点
      Vec3 X;
      const ETriangulationMethod triangulation_method = ETriangulationMethod::DEFAULT;
      if (Triangulate2View
      (
        pose0.rotation(), pose0.translation(), intrinsic0(LL.coords().cast<double>()),
        pose1.rotation(), pose1.translation(), intrinsic1(RR.coords().cast<double>()),
        X,
        triangulation_method
      ))
      {
      	 //计算每个3D点的重投影误差
        const Vec2 residual0 = intrinsic0.residual(pose0(X), LL.coords().cast<double>());
        const Vec2 residual1 = intrinsic1.residual(pose1(X), RR.coords().cast<double>());
        vec_residuals.emplace_back(std::abs(residual0(0)));
        vec_residuals.emplace_back(std::abs(residual0(1)));
        vec_residuals.emplace_back(std::abs(residual1(0)));
        vec_residuals.emplace_back(std::abs(residual1(1)));
        vec_3DPoints.emplace_back(X);
      }
    }

    //计算重投影误差的最大值、最小值，均值...
    float dMin, dMax, dMean, dMedian;
    minMaxMeanMedian<float>(vec_residuals.cbegin(), vec_residuals.cend(),
      dMin, dMax, dMean, dMedian);

    std::cout << std::endl
      << "Triangulation residuals statistics:" << "\n"
      << "\t-- Residual min:\t" << dMin << "\n"
      << "\t-- Residual median:\t" << dMedian << "\n"
      << "\t-- Residual max:\t " << dMax << "\n"
      << "\t-- Residual mean:\t " << dMean << std::endl;

      //将相机位姿和3D点云导出为PLY格式
      std::vector<Vec3> vec_camPos;
      vec_camPos.push_back( pose0.center() );
      vec_camPos.push_back( pose1.center() );
      exportToPly(vec_3DPoints, vec_camPos, "EssentialGeometry.ply");
  }
  return EXIT_SUCCESS;
}

//相机内参数矩阵读取实现，3*3矩阵读取
bool readIntrinsic(const std::string & fileName, Mat3 & K)
{
  ifstream in;
  in.open( fileName.c_str(), ifstream::in);
  if (in)  {
    for (int j=0; j < 3; ++j)
      for (int i=0; i < 3; ++i)
        in >> K(j,i);
  }
  else  {
    std::cerr << std::endl
      << "Invalid input K.txt file" << std::endl;
    return false;
  }
  return true;
}

// 将相机位姿和3D点云导出为PLY格式，具体实现：
bool exportToPly(const std::vector<Vec3> & vec_points,
  const std::vector<Vec3> & vec_camPos,
  const std::string & sFileName)
{
  std::ofstream outfile;
  outfile.open(sFileName.c_str(), std::ios_base::out);
	
	//ply格式文件头
  outfile << "ply"
    << '\n' << "format ascii 1.0"
    << '\n' << "element vertex " << vec_points.size()+vec_camPos.size()
    << '\n' << "property float x"
    << '\n' << "property float y"
    << '\n' << "property float z"
    << '\n' << "property uchar red"
    << '\n' << "property uchar green"
    << '\n' << "property uchar blue"
    << '\n' << "end_header" << std::endl;
	//3D点和位姿点的颜色信息不同
  for (size_t i=0; i < vec_points.size(); ++i)  {
      outfile << vec_points[i].transpose()
      << " 255 255 255" << "\n";
  }

  for (size_t i=0; i < vec_camPos.size(); ++i)  {
    outfile << vec_camPos[i].transpose()
      << " 0 255 0" << "\n";
  }
  outfile.flush();
  const bool bOk = outfile.good();
  outfile.close();
  return bOk;
}