[multiview] 2 View triangulation openMVG#1623

Enable the usage of the new generic 2view triangulation routine in:
- the sequential SfM pipeline,
- the ComputeStructureFromKnowPoses (epipolar filtered, or robust
triangulation of tracks).

src/openMVG/multiview/triangulation.hpp

@@ -10,28 +10,21 @@
 #define OPENMVG_MULTIVIEW_TRIANGULATION_HPP
 
 #include "openMVG/numeric/eigen_alias_definition.hpp"
+#include "openMVG/multiview/triangulation_method.hpp"
 
 namespace openMVG
 {
 
-enum class ETriangulationMethod : unsigned char
-{
-  DIRECT_LINEAR_TRANSFORM, // DLT
-  L1_ANGULAR,
-  LINFINITY_ANGULAR,
-  INVERSE_DEPTH_WEIGHTED_MIDPOINT
-};
-
 /**
-* @brief Generic triangulation rountine (Aggregate all the solver in one place).
+* @brief Generic triangulation routine (Aggregate all the 2view triangulation solvers in one place).
 * @param R0 First Camera rotation matrix
 * @param t0 First Camera translation vector
 * @param x0 bearing vector of the landmark observation in the first camera
 * @param R1 Second Camera rotation matrix
 * @param t1 Second Camera translation vector
 * @param x1 bearing vector of the landmark observation in the second camera
 * @param[out] X_euclidean Euclidean triangulated point
-* @return true if the point pass the adequacy test, false otherwise
+* @return true if the point pass the adequacy or cheirality test (depending of the solver), false otherwise
 * (invalid 3d point or method that does not exist)
 **/
 bool Triangulate2View
@@ -43,7 +36,7 @@ bool Triangulate2View
   const Vec3 &t1,
   const Vec3 &bearing1,
   Vec3 &X,
-  ETriangulationMethod etri_method = ETriangulationMethod::INVERSE_DEPTH_WEIGHTED_MIDPOINT
+  ETriangulationMethod etri_method = ETriangulationMethod::DEFAULT
 );
 
 /**

src/openMVG/multiview/triangulation_method.hpp

@@ -0,0 +1,26 @@
+// This file is part of OpenMVG, an Open Multiple View Geometry C++ library.
+
+// Copyright (c) 2019 Pierre MOULON.
+
+// This Source Code Form is subject to the terms of the Mozilla Public
+// License, v. 2.0. If a copy of the MPL was not distributed with this
+// file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef OPENMVG_MULTIVIEW_TRIANGULATION_TYPE_HPP
+#define OPENMVG_MULTIVIEW_TRIANGULATION_TYPE_HPP
+
+namespace openMVG
+{
+/// Define various enumerate for each available two view triangulation solver
+enum class ETriangulationMethod : unsigned char
+{
+  DIRECT_LINEAR_TRANSFORM, // DLT
+  L1_ANGULAR,
+  LINFINITY_ANGULAR,
+  INVERSE_DEPTH_WEIGHTED_MIDPOINT,
+  DEFAULT = INVERSE_DEPTH_WEIGHTED_MIDPOINT
+};
+
+}  // namespace openMVG
+
+#endif //OPENMVG_MULTIVIEW_TRIANGULATION_TYPE_HPP

src/openMVG/sfm/pipelines/sequential/sequential_SfM.cpp

@@ -582,8 +582,18 @@ bool SequentialSfMReconstructionEngine::MakeInitialPair3D(const Pair & current_p
         x2 = features_provider_->feats_per_view[J][j].coords().cast<double>();
 
       Vec3 X;
-      TriangulateDLT(Pose_I.asMatrix(), (*cam_I)(cam_I->get_ud_pixel(x1)),
-                     Pose_J.asMatrix(), (*cam_J)(cam_J->get_ud_pixel(x2)), &X);
+      Triangulate2View
+      (
+        Pose_I.rotation(),
+        Pose_I.translation(),
+        (*cam_I)(cam_I->get_ud_pixel(x1)),
+        Pose_J.rotation(),
+        Pose_J.translation(),
+        (*cam_J)(cam_J->get_ud_pixel(x2)),
+        X,
+        triangulation_method_
+      );
+
       Observations obs;
       obs[view_I->id_view] = Observation(x1, i);
       obs[view_J->id_view] = Observation(x2, j);
@@ -1125,9 +1135,18 @@ bool SequentialSfMReconstructionEngine::Resection(const uint32_t viewIndex)
                 xI_ud = cam_I->get_ud_pixel(xI),
                 xJ_ud = cam_J->get_ud_pixel(xJ);
               Vec3 X = Vec3::Zero();
-              TriangulateDLT(pose_I.asMatrix(), (*cam_I)(xI_ud),
-                             pose_J.asMatrix(), (*cam_J)(xJ_ud),
-                             &X);
+
+              Triangulate2View
+              (
+                pose_I.rotation(),
+                pose_I.translation(),
+                (*cam_I)(xI_ud),
+                pose_J.rotation(),
+                pose_J.translation(),
+                (*cam_J)(xJ_ud),
+                X,
+                triangulation_method_
+              );
               // Check triangulation result
               const double angle = AngleBetweenRay(
                 pose_I, cam_I, pose_J, cam_J, xI_ud, xJ_ud);

src/openMVG/sfm/pipelines/sequential/sequential_SfM.hpp

@@ -15,6 +15,7 @@
 
 #include "openMVG/sfm/pipelines/sfm_engine.hpp"
 #include "openMVG/cameras/cameras.hpp"
+#include "openMVG/multiview/triangulation_method.hpp"
 #include "openMVG/tracks/tracks.hpp"
 
 namespace htmlDocument { class htmlDocumentStream; }
@@ -71,6 +72,12 @@ class SequentialSfMReconstructionEngine : public ReconstructionEngine
     cam_type_ = camType;
   }
 
+  /// Configure the 2view triangulation method used by the SfM engine
+  void SetTriangulationMethod(const ETriangulationMethod method)
+  {
+    triangulation_method_ = method;
+  }
+
 protected:
 
 
@@ -116,6 +123,8 @@ class SequentialSfMReconstructionEngine : public ReconstructionEngine
   Hash_Map<IndexT, double> map_ACThreshold_; // Per camera confidence (A contrario estimated threshold error)
 
   std::set<uint32_t> set_remaining_view_id_;     // Remaining camera index that can be used for resection
+
+  ETriangulationMethod triangulation_method_ = ETriangulationMethod::DEFAULT;
 };
 
 } // namespace sfm

src/openMVG/sfm/pipelines/structure_from_known_poses/structure_estimator.cpp

@@ -70,13 +70,15 @@ SfM_Data_Structure_Estimation_From_Known_Poses::SfM_Data_Structure_Estimation_Fr
 void SfM_Data_Structure_Estimation_From_Known_Poses::run(
   SfM_Data & sfm_data,
   const Pair_Set & pairs,
-  const std::shared_ptr<Regions_Provider> & regions_provider)
+  const std::shared_ptr<Regions_Provider> & regions_provider,
+  const ETriangulationMethod triangulation_method
+)
 {
   sfm_data.structure.clear();
 
   match(sfm_data, pairs, regions_provider);
   filter(sfm_data, pairs, regions_provider);
-  triangulate(sfm_data, regions_provider);
+  triangulate(sfm_data, regions_provider, triangulation_method);
 }
 
 /// Use guided matching to find corresponding 2-view correspondences
@@ -294,10 +296,11 @@ void SfM_Data_Structure_Estimation_From_Known_Poses::filter(
   matching::PairWiseMatches().swap(putatives_matches);
 }
 
-/// Init & triangulate landmark observations from validated 3-view correspondences
+/// Init & triangulate landmark observations from the fused validated 3-view correspondences
 void SfM_Data_Structure_Estimation_From_Known_Poses::triangulate(
   SfM_Data & sfm_data,
-  const std::shared_ptr<Regions_Provider> & regions_provider)
+  const std::shared_ptr<Regions_Provider> & regions_provider,
+  const ETriangulationMethod triangulation_method)
 {
   openMVG::tracks::STLMAPTracks map_tracksCommon;
   openMVG::tracks::TracksBuilder tracksBuilder;
@@ -309,7 +312,14 @@ void SfM_Data_Structure_Estimation_From_Known_Poses::triangulate(
   // Generate new Structure tracks
   sfm_data.structure.clear();
 
-  SfM_Data_Structure_Computation_Robust structure_estimator(max_reprojection_error_);
+  const IndexT min_required_inliers = 3;
+  const IndexT min_sample_index = 2;
+
+  SfM_Data_Structure_Computation_Robust structure_estimator(
+    max_reprojection_error_,
+    min_required_inliers,
+    min_sample_index,
+    triangulation_method);
   C_Progress_display my_progress_bar( map_tracksCommon.size(), std::cout,
     "Tracks to structure conversion:\n" );
   // Fill sfm_data with the computed tracks (no 3D yet)

src/openMVG/sfm/pipelines/structure_from_known_poses/structure_estimator.hpp

@@ -12,6 +12,7 @@
 #include <memory>
 
 #include "openMVG/matching/indMatch.hpp"
+#include "openMVG/multiview/triangulation_method.hpp"
 
 namespace openMVG { namespace sfm { struct Regions_Provider; } }
 namespace openMVG { namespace sfm { struct SfM_Data; } }
@@ -33,7 +34,8 @@ class SfM_Data_Structure_Estimation_From_Known_Poses
   void run(
     SfM_Data & sfm_data,
     const Pair_Set & pairs,
-    const std::shared_ptr<Regions_Provider> & regions_provider);
+    const std::shared_ptr<Regions_Provider> & regions_provider,
+    const ETriangulationMethod triangulation_method);
 
 private:
 
@@ -49,10 +51,11 @@ class SfM_Data_Structure_Estimation_From_Known_Poses
     const Pair_Set & pairs,
     const std::shared_ptr<Regions_Provider> & regions_provider);
 
-  /// Init & triangulate landmark observations from validated 3-view correspondences
+  /// Init & triangulate landmark observations from the fused validated 3-view correspondences
   void triangulate(
     SfM_Data & sfm_data,
-    const std::shared_ptr<Regions_Provider> & regions_provider);
+    const std::shared_ptr<Regions_Provider> & regions_provider,
+    const ETriangulationMethod triangulation_method);
 
 private:
   //--

src/openMVG/sfm/sfm_data_triangulation.cpp

@@ -13,6 +13,7 @@
 
 #include "openMVG/geometry/pose3.hpp"
 #include "openMVG/multiview/triangulation_nview.hpp"
+#include "openMVG/multiview/triangulation.hpp"
 #include "openMVG/robust_estimation/rand_sampling.hpp"
 #include "openMVG/sfm/sfm_data.hpp"
 #include "openMVG/sfm/sfm_landmark.hpp"
@@ -46,26 +47,29 @@ bool track_triangulation
 (
   const SfM_Data & sfm_data,
   const Observations & obs,
-  Vec3 & X
+  Vec3 & X,
+  const ETriangulationMethod & etri_method = ETriangulationMethod::DEFAULT
 )
 {
   if (obs.size() >= 2)
   {
     std::vector<Vec3> bearing;
     std::vector<Mat34> poses;
+    std::vector<Pose3> poses_;
     bearing.reserve(obs.size());
     poses.reserve(obs.size());
     for (const auto& observation : obs)
     {
       const View * view = sfm_data.views.at(observation.first).get();
       if (!sfm_data.IsPoseAndIntrinsicDefined(view))
-        continue;
+        return false;
       const IntrinsicBase * cam = sfm_data.GetIntrinsics().at(view->id_intrinsic).get();
       const Pose3 pose = sfm_data.GetPoseOrDie(view);
       bearing.emplace_back((*cam)(cam->get_ud_pixel(observation.second.x)));
       poses.emplace_back(pose.asMatrix());
+      poses_.emplace_back(pose);
     }
-    if (bearing.size() >= 2)
+    if (bearing.size() > 2)
     {
       const Eigen::Map<const Mat3X> bearing_matrix(bearing[0].data(), 3, bearing.size());
       Vec4 Xhomogeneous;
@@ -79,6 +83,20 @@ bool track_triangulation
         return true;
       }
     }
+    else
+    {
+      return Triangulate2View
+      (
+        poses_.front().rotation(),
+        poses_.front().translation(),
+        bearing.front(),
+        poses_.back().rotation(),
+        poses_.back().translation(),
+        bearing.back(),
+        X,
+        etri_method
+      );
+    }
   }
   return false;
 }
@@ -210,12 +228,14 @@ SfM_Data_Structure_Computation_Robust::SfM_Data_Structure_Computation_Robust
   const double max_reprojection_error,
   const IndexT min_required_inliers,
   const IndexT min_sample_index,
+  const ETriangulationMethod etri_method,
   bool bConsoleVerbose
 ):
   SfM_Data_Structure_Computation_Basis(bConsoleVerbose),
   max_reprojection_error_(max_reprojection_error),
   min_required_inliers_(min_required_inliers),
-  min_sample_index_(min_sample_index)
+  min_sample_index_(min_sample_index),
+  etri_method_(etri_method)
 {
 }
 
@@ -329,7 +349,7 @@ const
   {
     // Generate the 3D point hypothesis by triangulating all the observations
     Vec3 X;
-    if (track_triangulation(sfm_data, obs, X) &&
+    if (track_triangulation(sfm_data, obs, X, etri_method_) &&
         track_check_predicate(obs, sfm_data, X, predicate_binding))
     {
       landmark.X = X;
@@ -363,7 +383,7 @@ const
     // Hypothesis generation
     const auto minimal_sample = ObservationsSampler(obs, samples);
 
-    if (!track_triangulation(sfm_data, minimal_sample, X))
+    if (!track_triangulation(sfm_data, minimal_sample, X, etri_method_))
       continue;
 
     // Test validity of the hypothesis

src/openMVG/sfm/sfm_data_triangulation.hpp

@@ -12,6 +12,7 @@
 #include <set>
 
 #include "openMVG/numeric/eigen_alias_definition.hpp"
+#include "openMVG/multiview/triangulation_method.hpp"
 #include "openMVG/sfm/sfm_landmark.hpp"
 #include "openMVG/types.hpp"
 
@@ -54,6 +55,7 @@ struct SfM_Data_Structure_Computation_Robust: public SfM_Data_Structure_Computat
     const double max_reprojection_error = 4.0, // pixels
     const IndexT min_required_inliers = 3,
     const IndexT min_sample_index = 3,
+    const ETriangulationMethod etri_method = ETriangulationMethod::DEFAULT,
     bool bConsoleVerbose = false
   );
 
@@ -77,6 +79,7 @@ struct SfM_Data_Structure_Computation_Robust: public SfM_Data_Structure_Computat
   double max_reprojection_error_;
   const IndexT min_required_inliers_;
   const IndexT min_sample_index_;
+  const ETriangulationMethod etri_method_;
 
 };
 

src/software/SfM/main_ComputeStructureFromKnownPoses.cpp

@@ -63,6 +63,7 @@ int main(int argc, char **argv)
   std::string sOutFile = "";
   double dMax_reprojection_error = 4.0;
   unsigned int ui_max_cache_size = 0;
+  int triangulation_method = static_cast<int>(ETriangulationMethod::DEFAULT);
 
   cmd.add( make_option('i', sSfM_Data_Filename, "input_file") );
   cmd.add( make_option('m', sMatchesDir, "match_dir") );
@@ -73,6 +74,7 @@ int main(int argc, char **argv)
   cmd.add( make_option('r', dMax_reprojection_error, "residual_threshold"));
   cmd.add( make_option('c', ui_max_cache_size, "cache_size") );
   cmd.add( make_switch('d', "direct_triangulation"));
+  cmd.add( make_option('t', triangulation_method, "triangulation_method"));
 
   try {
     if (argc == 1) throw std::string("Invalid command line parameter.");
@@ -93,6 +95,11 @@ int main(int argc, char **argv)
     << " [Provided Matches -> Robust triangulation of the match file (activated by -d)]\n"
     << "\t[-d|--direct_triangulation] Robustly triangulate the tracks computed from the file given by [--f|--match_file]\n"
     << "\t[-f|--match_file] path to a matches file (loaded pair indexes will be used)\n"
+    << "\t[-t|--triangulation_method] triangulation method (default=" << triangulation_method << "):\n"
+    << "\t\t" << static_cast<int>(ETriangulationMethod::DIRECT_LINEAR_TRANSFORM) << ": DIRECT_LINEAR_TRANSFORM\n"
+    << "\t\t" << static_cast<int>(ETriangulationMethod::L1_ANGULAR) << ": L1_ANGULAR\n"
+    << "\t\t" << static_cast<int>(ETriangulationMethod::LINFINITY_ANGULAR) << ": DIRECT_LINEAR_TRANSFORM\n"
+    << "\t\t" << static_cast<int>(ETriangulationMethod::INVERSE_DEPTH_WEIGHTED_MIDPOINT) << ": INVERSE_DEPTH_WEIGHTED_MIDPOINT\n"
 
     << "\n[Optional]\n"
     << "[-b|--bundle_adjustment] (switch) perform a bundle adjustment on the scene (OFF by default)\n"
@@ -249,6 +256,7 @@ int main(int argc, char **argv)
         max_reprojection_error,
         min_track_length,
         min_track_length,
+        static_cast<ETriangulationMethod>(triangulation_method),
         console_verbose);
       structure_estimator.triangulate(sfm_data);
     }
@@ -308,7 +316,8 @@ int main(int argc, char **argv)
     // Compute Structure from known camera poses
     //------------------------------------------
     SfM_Data_Structure_Estimation_From_Known_Poses structure_estimator(dMax_reprojection_error);
-    structure_estimator.run(sfm_data, pairs, regions_provider);
+    structure_estimator.run(sfm_data, pairs, regions_provider,
+      static_cast<ETriangulationMethod>(triangulation_method));
     std::cout << "\nStructure estimation took (s): " << timer.elapsed() << "." << std::endl;
 
   }