案例5：OpenMVG估计本质矩阵并执行光束法平差(BA)

对估计的3D点、相机内外参执行BA

#include "openMVG/cameras/Camera_Pinhole.hpp"
#include "openMVG/cameras/Camera_Pinhole_Radial.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 "openMVG/sfm/sfm_data.hpp"
#include "openMVG/sfm/sfm_data_BA.hpp"
#include "openMVG/sfm/sfm_data_BA_ceres.hpp"
#include "openMVG/sfm/sfm_data_io.hpp"

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

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

using namespace openMVG;
using namespace openMVG::image;
using namespace openMVG::matching;
using namespace openMVG::cameras;
using namespace openMVG::geometry;
using namespace openMVG::sfm;
using namespace std;

bool readIntrinsic(const std::string & fileName, Mat3 & K);

int main() {
		//..............
		//以上代码省略，估计本质矩阵的代码在专栏里面有了
    std::cout << "Which BA do you want ?\n"
      << "\t 1: Refine [X],[f,ppx,ppy,R|t] (individual cameras)\n"
      << "\t 2: Refine [X],[R|t], shared [f, ppx, ppy]\n"
      << "\t 3: Refine [X],[R|t], shared brown K3 distortion model [f,ppx,ppy,k1,k2,k3]\n" << std::endl;
    int iBAType = -1;
    std::cin >> iBAType;
    const bool bSharedIntrinsic = (iBAType == 2 || iBAType == 3) ? true : false;

    // 初始化SFM场景
    SfM_Data tiny_scene;
    tiny_scene.views[0].reset(new View("", 0, bSharedIntrinsic ? 0 : 1, 0, imageL.Width(), imageL.Height()));
    tiny_scene.views[1].reset(new View("", 1, bSharedIntrinsic ? 0 : 1, 1, imageR.Width(), imageR.Height()));
    // 是否共享相机内参数
    switch (iBAType)
    {
      case 1: // Each view use it's own pinhole camera intrinsic
        tiny_scene.intrinsics[0].reset(new Pinhole_Intrinsic(imageL.Width(), imageL.Height(), K(0, 0), K(0, 2), K(1, 2)));
        tiny_scene.intrinsics[1].reset(new Pinhole_Intrinsic(imageR.Width(), imageR.Height(), K(0, 0), K(0, 2), K(1, 2)));
        break;
      case 2: // Shared pinhole camera intrinsic
        tiny_scene.intrinsics[0].reset(new Pinhole_Intrinsic(imageL.Width(), imageL.Height(), K(0, 0), K(0, 2), K(1, 2)));
        break;
      case 3: // Shared pinhole camera intrinsic with radial K3 distortion
        tiny_scene.intrinsics[0].reset(new Pinhole_Intrinsic_Radial_K3(imageL.Width(), imageL.Height(), K(0, 0), K(0, 2), K(1, 2)));
        break;
      default:
        std::cerr << "Invalid input number" << std::endl;
        return EXIT_FAILURE;
    }

    // 设置两视图位姿
    const Pose3 pose0 = tiny_scene.poses[tiny_scene.views[0]->id_pose] = Pose3(Mat3::Identity(), Vec3::Zero());
    const Pose3 pose1 = tiny_scene.poses[tiny_scene.views[1]->id_pose] = relativePose_info.relativePose;

    // 执行三角化并增加路标点（1个3D点以及对应的两个投影点）
    Landmarks & landmarks = tiny_scene.structure;
    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_];
      // Point triangulation
      Vec3 X;
      const ETriangulationMethod triangulation_method = ETriangulationMethod::DEFAULT;
      if (Triangulate2View
      (
        pose0.rotation(), pose0.translation(), (*tiny_scene.intrinsics[0])(LL.coords().cast<double>()),
        pose1.rotation(), pose1.translation(), (*tiny_scene.intrinsics[1])(RR.coords().cast<double>()),
        X,
        triangulation_method
      ))
      {
        // Add a new landmark (3D point with it's 2d observations)
        Landmark landmark;
        landmark.obs[tiny_scene.views[0]->id_view] = Observation(LL.coords().cast<double>(), vec_PutativeMatches[inlier_idx].i_);
        landmark.obs[tiny_scene.views[1]->id_view] = Observation(RR.coords().cast<double>(), vec_PutativeMatches[inlier_idx].j_);
        landmark.X = X;
        landmarks.insert({landmarks.size(), landmark});
      }
    }
    Save(tiny_scene, "EssentialGeometry_start.ply", ESfM_Data(ALL));

    //D. 执行BA

    Bundle_Adjustment_Ceres bundle_adjustment_obj;
    bundle_adjustment_obj.Adjust(tiny_scene,
      Optimize_Options(
        Intrinsic_Parameter_Type::ADJUST_ALL,
        Extrinsic_Parameter_Type::ADJUST_ALL,
        Structure_Parameter_Type::ADJUST_ALL));

    Save(tiny_scene, "EssentialGeometry_refined.ply", ESfM_Data(ALL));
  }
  return EXIT_SUCCESS;
}

bool readIntrinsic(const std::string & fileName, Mat3 & K)
{
  // Load the K matrix
  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;
}