[Multiview] Add lambda twist p3p solver

docs/sphinx/rst/bibliography.rst

@@ -99,6 +99,9 @@ Bibliography
 .. [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
+
 .. [Ceres] **Ceres Solver**
     Sameer Agarwal and Keir Mierle and Others, http://ceres-solver.org
 

docs/sphinx/rst/openMVG/multiview/multiview.rst

@@ -121,11 +121,12 @@ It consists in estimating the camera parameters of the right camera that minimiz
 
    Residual error.
 
-openMVG provides 3 different solvers for this problem:
+openMVG provides 4 different solvers for this problem:
 
 * 6pt Direct Linear Transform [HZ]_,
-* 3pt with intrinsic EPnP [Ke]_,  
+* 3pt with intrinsic P3P [Ke]_,  
 * 3pt with intrinsic P3P [Kneip]_.
+* 3pt with intrinsic P3P [Nordberg]_,
 
 Kernel concept
 ---------------------

src/openMVG/multiview/solver_resection.hpp

@@ -17,6 +17,7 @@ enum class SolverType
   DLT_6POINTS = 0,
   P3P_KE_CVPR17 = 1,
   P3P_KNEIP_CVPR11 = 2,
+  P3P_NORDBERG_ECCV18 = 3
 };
 
 }  // namespace resection

src/openMVG/multiview/solver_resection_kernel_test.cpp

@@ -102,7 +102,6 @@ TEST(P3P_Ke_CVPR17, Multiview) {
     const Mat bearing_vectors = (d._K[0].inverse() * x.colwise().homogeneous()).colwise().normalized();
     const Mat X = d._X;
     openMVG::euclidean_resection::PoseResectionKernel_P3P_Ke kernel(bearing_vectors, X);
-
     std::vector<Mat34> Ps;
     kernel.Fit({0,1,2}, &Ps); // 3 points sample are required, lets take the first three
 
@@ -127,6 +126,49 @@ TEST(P3P_Ke_CVPR17, Multiview) {
   }
 }
 
+
+TEST(P3P_Nordberg_ECCV18, Multiview) {
+
+  const int nViews = 3;
+  const int nbPoints = 3;
+  const NViewDataSet d =
+    NRealisticCamerasRing(nViews, nbPoints,
+                          nViewDatasetConfigurator(1,1,0,0,5,0)); // Suppose a camera with Unit matrix as K
+
+
+
+  // Solve the problem and check that fitted value are good enough
+  for (int nResectionCameraIndex = 0; nResectionCameraIndex < nViews; ++nResectionCameraIndex)
+  {
+    const Mat x = d._x[nResectionCameraIndex];
+    const Mat bearing_vectors = (d._K[0].inverse() * x.colwise().homogeneous()).colwise().normalized();
+    const Mat X = d._X;
+    openMVG::euclidean_resection::PoseResectionKernel_P3P_Persson kernel(bearing_vectors, X);
+
+    std::vector<Mat34> Ps;
+    kernel.Fit({0,1,2}, &Ps); // 3 points sample are required, lets take the first three
+
+    bool bFound = false;
+    size_t index = -1;
+    for (size_t i = 0; i < Ps.size(); ++i)  {
+      Mat34 GT_ProjectionMatrix = d.P(nResectionCameraIndex).array()
+      / d.P(nResectionCameraIndex).norm();
+      Mat34 COMPUTED_ProjectionMatrix = Ps[i].array() / Ps[i].norm();
+      if ( NormLInfinity(GT_ProjectionMatrix - COMPUTED_ProjectionMatrix) < 1e-8 )
+      {
+        bFound = true;
+        index = i;
+      }
+    }
+    EXPECT_TRUE(bFound);
+
+    // Check that for the found matrix the residual is small
+    for (Mat::Index i = 0; i < x.cols(); ++i) {
+      EXPECT_NEAR(0.0, kernel.Error(i, Ps[index]), 1e-8);
+    }
+  }
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */

src/openMVG/multiview/solver_resection_p3p.hpp

@@ -11,5 +11,6 @@
 
 #include "openMVG/multiview/solver_resection_p3p_ke.hpp"
 #include "openMVG/multiview/solver_resection_p3p_kneip.hpp"
+#include "openMVG/multiview/solver_resection_p3p_nordberg.hpp"
 
 #endif // OPENMVG_MULTIVIEW_RESECTION_P3P_HPP

src/openMVG/multiview/solver_resection_p3p_ke.cpp

@@ -30,7 +30,7 @@ namespace euclidean_resection {
 *
 * @param[in] bearing_vectors 3x3 matrix with UNITARY feature vectors (each column is a vector)
 * @param[in] X_observations  3x3 matrix with corresponding 3D world points (each column is a point)
-* @param[in] rotation_translation_solutions vector that will contain the solutions (up to 4 solutions)
+* @param[out] rotation_translation_solutions vector that will contain the solutions (up to 4 solutions)
 *
 * @return true if at least one solution is found, false if no solution was found
 *