[multiview] 2 View triangulation openMVG#1623

- Add a generic function to access to all the solver in a common
interface.
- Add the related unit test

docs/sphinx/rst/bibliography.rst

@@ -4,7 +4,7 @@
 Bibliography
 ============
 
-.. [Ke] **An Efficient Algebraic Solution to the Perspective-Three-Point Proble.**
+.. [Ke] **An Efficient Algebraic Solution to the Perspective-Three-Point Problem.**
     Ke, T.; Roumeliotis, S.
     CVPR 2017
 
@@ -45,12 +45,12 @@ Bibliography
     Pierre Moulon, Pascal Monasse and Renaud Marlet.
     In ICCV, 2013.
 
-.. [TracksCVMP12] **Unordered feature tracking made fast and easy**
+.. [TracksCVMP12] **Unordered feature tracking made fast and easy.**
     Pierre Moulon and Pascal Monasse, CVMP 2012.
 
 .. [KVLD12] **Virtual Line Descriptor and Semi-Local Graph Matching Method for Reliable Feature Correspondence.**
     Zhe LIU and Renaud MARLET, BMVC 2012.
-    
+
 .. [Moulon13] **Global Multiple-View Color Consistency.**
     Pierre Moulon, Bruno Duisit and Pascal Monasse, CVMP 2013.
 
@@ -87,31 +87,36 @@ Bibliography
     Moisan, Lionel, Pierre Moulon, and Pascal Monasse.
     Image Processing On Line 10 (2012)
 
-.. [VLFEAT]  **VLFeat: An Open and Portable Library of Computer Vision Algorithms**
+.. [VLFEAT]  **VLFeat: An Open and Portable Library of Computer Vision Algorithms.**
     A. Vedaldi and B. Fulkerson, 2008. http://www.vlfeat.org/, http://www.vlfeat.org/overview/sift.html.
 
 .. [OlssonDuality] **Outlier Removal Using Duality.**
     Carl Olsson, Anders Eriksson and Richard Hartley, Richard. CVPR 2010.
 
-.. [CASCADEHASHING] **Fast and Accurate Image Matching with Cascade Hashing for 3D Reconstruction**
+.. [CASCADEHASHING] **Fast and Accurate Image Matching with Cascade Hashing for 3D Reconstruction.**
    Jian Cheng, Cong Leng, Jiaxiang Wu, Hainan Cui, Hanqing Lu. CVPR 2014.
-   
-.. [Magnus] **Two-View Orthographic Epipolar Geometry: Minimal and Optimal Solvers**
+
+.. [Magnus] **Two-View Orthographic Epipolar Geometry: Minimal and Optimal Solvers.**
    Magnus Oskarsson. In Journal of Mathematical Imaging and Vision, 2017.
 
-.. [Nordberg] **"Lambda Twist: An Accurate Fast Robust Perspective Three Point (P3P) Solver**
-   Persson, Mikael; Nordberg, Klas. ECCV 2018
+.. [Nordberg] **Lambda Twist: An Accurate Fast Robust Perspective Three Point (P3P) Solver.**
+   Persson, Mikael; Nordberg, Klas. ECCV 2018.
 
-.. [Ceres] **Ceres Solver**
-    Sameer Agarwal and Keir Mierle and Others, http://ceres-solver.org
+.. [Lee_ICCV19] **Closed-Form Optimal Triangulation Based on Angular Errors.**
+   S.H. Lee, J. Civera. ICCV 2019.
 
-.. [CppUnitLite] **CppUnitLite** http://c2.com/cgi/wiki?CppUnitLite
+.. [Lee_BMVC19] **Triangulation: Why Optimize?**
+   S.H. Lee, J. Civera. BMVC 2019.
+
+.. [Ceres] **Ceres Solver.**
+    Sameer Agarwal and Keir Mierle and Others, http://ceres-solver.org
 
-.. [Eigen] **Eigen** http://eigen.tuxfamily.org/index.php?title=Main_Page
+.. [CppUnitLite] **CppUnitLite.** http://c2.com/cgi/wiki?CppUnitLite
 
-.. [LEMON] **Lemon** http://lemon.cs.elte.hu/trac/lemon
+.. [Eigen] **Eigen.** http://eigen.tuxfamily.org/index.php?title=Main_Page
 
-.. [STLplus] **STLplus** http://stlplus.sourceforge.net/
+.. [LEMON] **Lemon.** http://lemon.cs.elte.hu/trac/lemon
 
-.. [LP] **Linear Programming** http://en.wikipedia.org/wiki/Linear_programming
+.. [STLplus] **STLplus.** http://stlplus.sourceforge.net/
 
+.. [LP] **Linear Programming.** http://en.wikipedia.org/wiki/Linear_programming

src/openMVG/multiview/triangulation.cpp

@@ -31,7 +31,7 @@ void TriangulateDLT
   design.row(2) = x2[0] * P2.row(2) - x2[2] * P2.row(0);
   design.row(3) = x2[1] * P2.row(2) - x2[2] * P2.row(1);
 
-  Eigen::JacobiSVD<Mat4> svd( design, Eigen::ComputeFullV );
+  const Eigen::JacobiSVD<Mat4> svd( design, Eigen::ComputeFullV );
   ( *X_homogeneous ) = svd.matrixV().col( 3 );
 }
 
@@ -49,6 +49,27 @@ void TriangulateDLT
   (*X_euclidean) = X_homogeneous.hnormalized();
 }
 
+bool TriangulateDLT
+(
+  const Mat3 &R0,
+  const Vec3 &t0,
+  const Vec3 &x0,
+  const Mat3 &R1,
+  const Vec3 &t1,
+  const Vec3 &x1,
+  Vec3 *X
+)
+{
+  Mat34 P0, P1;
+  P0.block<3,3>(0,0) = R0;
+  P1.block<3,3>(0,0) = R1;
+  P0.block<3,1>(0,3) = t0;
+  P1.block<3,1>(0,3) = t1;
+  TriangulateDLT(P0, x0, P1, x1, X);
+  return x0.dot(R0 * (*X + R0.transpose() * t0)) > 0.0 &&
+         x1.dot(R1 * (*X + R1.transpose() * t1)) > 0.0;
+}
+
 // Helper function
 // Compute Relative motion between two absolute poses parameterized by Rt
 // Rotate one bearing vector according to the relative motion
@@ -78,7 +99,7 @@ inline bool Compute3DPoint(
   const Vec3 &t,
   const Mat3 &R1,
   const Vec3 &t1,
-  Vec3 * X) 
+  Vec3 *X)
 {
   const Vec3 z = mprime1.cross(mprime0);
   const double z_squared = z.squaredNorm();
@@ -117,14 +138,14 @@ bool TriangulateL1Angular
   // pre compute n0 and n1 cf. 5. Lemma 2
   const Vec3 n0 = Rx0.cross(t).normalized();
   const Vec3 n1 = x1.cross(t).normalized();
-  
+
   if(Rx0.normalized().cross(t).squaredNorm() <= x1.normalized().cross(t).squaredNorm())
   {
     // Eq. (12)
     mprime0 = Rx0 - Rx0.dot(n1) * n1;
     mprime1 = x1;
-  } 
-  else 
+  }
+  else
   {
     // Eq. (13)
     mprime0 = Rx0;
@@ -182,9 +203,9 @@ bool TriangulateIDWMidpoint(
   const double p_norm = Rx0.cross(x1).norm();
   const double q_norm = Rx0.cross(t).norm();
   const double r_norm = x1.cross(t).norm();
-  
+
   // Eq. (10)
-  const auto xprime1 = ( q_norm / (q_norm + r_norm) ) 
+  const auto xprime1 = ( q_norm / (q_norm + r_norm) )
     * ( t + (r_norm / p_norm) * (Rx0 + x1) );
 
   // relative to absolute
@@ -194,7 +215,7 @@ bool TriangulateIDWMidpoint(
   const Vec3 lambda0_Rx0 = (r_norm / p_norm) * Rx0;
   const Vec3 lambda1_x1 = (q_norm / p_norm) * x1;
 
-  // Eq. (9) - test adequation 
+  // Eq. (9) - test adequation
   return (t + lambda0_Rx0 - lambda1_x1).squaredNorm()
     <
     std::min(std::min(
@@ -203,4 +224,36 @@ bool TriangulateIDWMidpoint(
       (t - lambda0_Rx0 + lambda1_x1).squaredNorm());
 }
 
+bool Triangulate2View
+(
+  const Mat3 &R0,
+  const Vec3 &t0,
+  const Vec3 &bearing0,
+  const Mat3 &R1,
+  const Vec3 &t1,
+  const Vec3 &bearing1,
+  Vec3 &X,
+  ETriangulationMethod etri_method
+)
+{
+  switch (etri_method)
+  {
+    case ETriangulationMethod::DIRECT_LINEAR_TRANSFORM:
+      return TriangulateDLT(R0, t0, bearing0, R1, t1, bearing1, &X);
+    break;
+    case ETriangulationMethod::L1_ANGULAR:
+      return TriangulateL1Angular(R0, t0, bearing0, R1, t1, bearing1, &X);
+    break;
+    case ETriangulationMethod::LINFINITY_ANGULAR:
+      return TriangulateLInfinityAngular(R0, t0, bearing0, R1, t1, bearing1, &X);
+    break;
+    case ETriangulationMethod::INVERSE_DEPTH_WEIGHTED_MIDPOINT:
+      return TriangulateIDWMidpoint(R0, t0, bearing0, R1, t1, bearing1, &X);
+    break;
+    default:
+      return false;
+  }
+  return false;
+}
+
 }  // namespace openMVG

src/openMVG/multiview/triangulation.hpp

@@ -14,6 +14,38 @@
 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).
+* @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
+* (invalid 3d point or method that does not exist)
+**/
+bool Triangulate2View
+(
+  const Mat3 &R0,
+  const Vec3 &t0,
+  const Vec3 &bearing0,
+  const Mat3 &R1,
+  const Vec3 &t1,
+  const Vec3 &bearing1,
+  Vec3 &X,
+  ETriangulationMethod etri_method = ETriangulationMethod::INVERSE_DEPTH_WEIGHTED_MIDPOINT
+);
+
 /**
 * @brief Linear DLT triangulation
 * @param P1 First camera projection matrix
@@ -44,13 +76,38 @@ void TriangulateDLT
 * @ref Multiple View Geometry - Richard Hartley, Andrew Zisserman - second edition
 */
 void TriangulateDLT
-( const Mat34 &P1,
+(
+  const Mat34 &P1,
   const Vec3 &x1,
   const Mat34 &P2,
   const Vec3 &x2,
   Vec3 *X_euclidean
 );
 
+/**
+* @brief Linear DLT triangulation
+* @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 cheirality test, false otherwise
+* @see HZ 12.2 pag.312
+* @ref Multiple View Geometry - Richard Hartley, Andrew Zisserman - second edition
+*/
+bool TriangulateDLT
+(
+  const Mat3 &R0,
+  const Vec3 &t0,
+  const Vec3 &x0,
+  const Mat3 &R1,
+  const Vec3 &t1,
+  const Vec3 &x1,
+  Vec3 *X_euclidean
+);
+
 /**
 * @brief Optimal L1 Angular triangulation
 * @brief Minimize the L1 norm of angular errors
@@ -124,8 +181,6 @@ bool TriangulateIDWMidpoint(
   Vec3 *X_euclidean
 );
 
-
-
 } // namespace openMVG
 
 #endif  // OPENMVG_MULTIVIEW_TRIANGULATION_HPP

src/openMVG/multiview/triangulation_test.cpp

@@ -1,24 +1,3 @@
-
-// Copyright (c) 2010 libmv authors.
-//
-// Permission is hereby granted, free of charge, to any person obtaining a copy
-// of this software and associated documentation files (the "Software"), to
-// deal in the Software without restriction, including without limitation the
-// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
-// sell copies of the Software, and to permit persons to whom the Software is
-// furnished to do so, subject to the following conditions:
-//
-// The above copyright notice and this permission notice shall be included in
-// all copies or substantial portions of the Software.
-//
-// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
-// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
-// IN THE SOFTWARE.
-
 // This file is part of OpenMVG, an Open Multiple View Geometry C++ library.
 
 // Copyright (c) 2012, 2013 Pierre MOULON.
@@ -43,15 +22,18 @@ TEST(Triangulation, TriangulateDLT) {
 
   for (int i = 0; i < d._X.cols(); ++i) {
     const Vec3 X_gt = d._X.col(i);
-    Vec3 X_estimated;
+    Vec3 X_estimated = Vec3::Random();
     const Vec2 x1 = d._x[0].col(i);
     const Vec2 x2 = d._x[1].col(i);
     TriangulateDLT(d.P(0), x1.homogeneous(), d.P(1), x2.homogeneous(), &X_estimated);
     EXPECT_NEAR(0, DistanceLInfinity(X_estimated, X_gt), 1e-8);
+    X_estimated = Vec3::Random();
+    TriangulateDLT(d._R[0], d._t[0], d._K[0].inverse() * x1.homogeneous(), d._R[1], d._t[1], d._K[1].inverse() * x2.homogeneous(), &X_estimated);
+    EXPECT_NEAR(0, DistanceLInfinity(X_estimated, X_gt), 1e-8);
   }
 }
 
-TEST(Triangulation, TriangulateL1Angular) 
+TEST(Triangulation, TriangulateL1Angular)
 {
   const NViewDataSet d = NRealisticCamerasRing(2, 12);
   for (int i = 0; i < d._X.cols(); ++i) {
@@ -65,7 +47,7 @@ TEST(Triangulation, TriangulateL1Angular)
   }
 }
 
-  TEST(Triangulation, TriangulateLInfinityAngular) 
+  TEST(Triangulation, TriangulateLInfinityAngular)
 {
   const NViewDataSet d = NRealisticCamerasRing(2, 12);
   for (int i = 0; i < d._X.cols(); ++i) {
@@ -93,6 +75,35 @@ TEST(Triangulation, TriangulateIDWMidpoint)
   }
 }
 
+TEST(Triangulation, Factoring)
+{
+  const NViewDataSet d = NRealisticCamerasRing(2, 12);
+  const std::vector<ETriangulationMethod> triangulation_methods =
+    {
+      ETriangulationMethod::DIRECT_LINEAR_TRANSFORM,
+      ETriangulationMethod::L1_ANGULAR,
+      ETriangulationMethod::LINFINITY_ANGULAR,
+      ETriangulationMethod::INVERSE_DEPTH_WEIGHTED_MIDPOINT
+    };
+  for (const auto& method_it : triangulation_methods)
+  {
+    for (int i = 0; i < d._X.cols(); ++i)
+    {
+      const Vec3 X_gt = d._X.col(i);
+      Vec3 X_estimated = Vec3::Random();
+      const Vec2 x1 = d._x[0].col(i);
+      const Vec2 x2 = d._x[1].col(i);
+      const bool cheir_ok =
+        Triangulate2View(
+          d._R[0], d._t[0], d._K[0].inverse() * x1.homogeneous(),
+          d._R[1], d._t[1], d._K[1].inverse() * x2.homogeneous(),
+          X_estimated,
+          method_it);
+      EXPECT_TRUE(cheir_ok);
+      EXPECT_NEAR(0, DistanceLInfinity(X_estimated, X_gt), 1e-8);
+    }
+  }
+}
 
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}