Camera Tracking: allow fallback to reprojection resection by user demand

This fixes some "regressions" introduced in rev50781 which lead to much
worse solution in some cases. Now it's possible to bring old behavior back.

Perhaps it's more like temporal solution for time being smarter solution is
found. But finding such a solution isn't so fast, so let's bring manual
control over reprojection usage.

But anyway, imo it's now nice to have a structure which could be used to
pass different settings to the solver.

9 files changed, 93 insertions, 26 deletions

diff --git a/extern/libmv/libmv-capi.cpp b/extern/libmv/libmv-capi.cpp
index 0f73234f49c..a15927f881d 100644
--- a/extern/libmv/libmv-capi.cpp
+++ b/extern/libmv/libmv-capi.cpp
@@ -550,7 +550,8 @@ static void libmv_solveRefineIntrinsics(libmv::Tracks *tracks, libmv::CameraIntr
 }
 
 libmv_Reconstruction *libmv_solveReconstruction(libmv_Tracks *tracks, int keyframe1, int keyframe2,
-			int refine_intrinsics, double focal_length, double principal_x, double principal_y, double k1, double k2, double k3,
+			int refine_intrinsics, double focal_length, double principal_x, double principal_y,
+			double k1, double k2, double k3, struct libmv_reconstructionOptions *options,
 			reconstruct_progress_update_cb progress_update_callback, void *callback_customdata)
 {
 	/* Invert the camera intrinsics. */
@@ -558,6 +559,7 @@ libmv_Reconstruction *libmv_solveReconstruction(libmv_Tracks *tracks, int keyfra
 	libmv_Reconstruction *libmv_reconstruction = new libmv_Reconstruction();
 	libmv::EuclideanReconstruction *reconstruction = &libmv_reconstruction->reconstruction;
 	libmv::CameraIntrinsics *intrinsics = &libmv_reconstruction->intrinsics;
+	libmv::ReconstructionOptions reconstruction_options;
 
 	ReconstructUpdateCallback update_callback =
 		ReconstructUpdateCallback(progress_update_callback, callback_customdata);
@@ -566,6 +568,9 @@ libmv_Reconstruction *libmv_solveReconstruction(libmv_Tracks *tracks, int keyfra
 	intrinsics->SetPrincipalPoint(principal_x, principal_y);
 	intrinsics->SetRadialDistortion(k1, k2, k3);
 
+	reconstruction_options.success_threshold = options->success_threshold;
+	reconstruction_options.use_fallback_reconstruction = options->use_fallback_reconstruction;
+
 	for (int i = 0; i < markers.size(); ++i) {
 		intrinsics->InvertIntrinsics(markers[i].x,
 			markers[i].y,
@@ -584,7 +589,8 @@ libmv_Reconstruction *libmv_solveReconstruction(libmv_Tracks *tracks, int keyfra
 
 	libmv::EuclideanReconstructTwoFrames(keyframe_markers, reconstruction);
 	libmv::EuclideanBundle(normalized_tracks, reconstruction);
-	libmv::EuclideanCompleteReconstruction(normalized_tracks, reconstruction, &update_callback);
+	libmv::EuclideanCompleteReconstruction(reconstruction_options, normalized_tracks,
+	                                       reconstruction, &update_callback);
 
 	if (refine_intrinsics) {
 		libmv_solveRefineIntrinsics((libmv::Tracks *)tracks, intrinsics, reconstruction,
diff --git a/extern/libmv/libmv-capi.h b/extern/libmv/libmv-capi.h
index 5253ba76a21..e5885e7addf 100644
--- a/extern/libmv/libmv-capi.h
+++ b/extern/libmv/libmv-capi.h
@@ -91,13 +91,20 @@ void libmv_tracksDestroy(struct libmv_Tracks *libmv_tracks);
 #define LIBMV_REFINE_RADIAL_DISTORTION_K1 (1<<2)
 #define LIBMV_REFINE_RADIAL_DISTORTION_K2 (1<<4)
 
+/* TODO: make keyframes/distortion model a part of options? */
+struct libmv_reconstructionOptions {
+	double success_threshold;
+	int use_fallback_reconstruction;
+};
+
 typedef void (*reconstruct_progress_update_cb) (void *customdata, double progress, const char *message);
 
 int libmv_refineParametersAreValid(int parameters);
 
 struct libmv_Reconstruction *libmv_solveReconstruction(struct libmv_Tracks *tracks, int keyframe1, int keyframe2,
 			int refine_intrinsics, double focal_length, double principal_x, double principal_y, double k1, double k2, double k3,
-			reconstruct_progress_update_cb progress_update_callback, void *callback_customdata);
+			struct libmv_reconstructionOptions *options, reconstruct_progress_update_cb progress_update_callback,
+			void *callback_customdata);
 struct libmv_Reconstruction *libmv_solveModal(struct libmv_Tracks *tracks, double focal_length,
 			double principal_x, double principal_y, double k1, double k2, double k3,
 			reconstruct_progress_update_cb progress_update_callback, void *callback_customdata);
diff --git a/extern/libmv/libmv/multiview/euclidean_resection.cc b/extern/libmv/libmv/multiview/euclidean_resection.cc
index 6cad4cac049..2605bf04622 100644
--- a/extern/libmv/libmv/multiview/euclidean_resection.cc
+++ b/extern/libmv/libmv/multiview/euclidean_resection.cc
@@ -37,13 +37,14 @@ typedef unsigned int uint;
 bool EuclideanResection(const Mat2X &x_camera, 
                         const Mat3X &X_world,
                         Mat3 *R, Vec3 *t,
-                        ResectionMethod method) {
+                        ResectionMethod method,
+                        double success_threshold) {
   switch (method) {
     case RESECTION_ANSAR_DANIILIDIS:
       EuclideanResectionAnsarDaniilidis(x_camera, X_world, R, t);
       break;
     case RESECTION_EPNP:
-      return EuclideanResectionEPnP(x_camera, X_world, R, t);      
+      return EuclideanResectionEPnP(x_camera, X_world, R, t, success_threshold);
       break;
     default:
       LOG(FATAL) << "Unknown resection method.";
@@ -435,8 +436,9 @@ static void ComputePointsCoordinatesInCameraFrame(
 }
 
 bool EuclideanResectionEPnP(const Mat2X &x_camera,
-                            const Mat3X &X_world, 
-                            Mat3 *R, Vec3 *t) {
+                            const Mat3X &X_world,
+                            Mat3 *R, Vec3 *t,
+                            double success_threshold) {
   CHECK(x_camera.cols() == X_world.cols());
   CHECK(x_camera.cols() > 3);
   size_t num_points = X_world.cols();
@@ -544,7 +546,12 @@ bool EuclideanResectionEPnP(const Mat2X &x_camera,
   //
   // TODO(keir): Decide if setting this to infinity, effectively disabling the
   // check, is the right approach. So far this seems the case.
-  double kSuccessThreshold = std::numeric_limits<double>::max();
+  //
+  // TODO(sergey): Made it an option for now, in some cases it makes sense to
+  // still fallback to reprojection solution (see bug [#32765] from Blender bug tracker)
+
+  // double kSuccessThreshold = std::numeric_limits<double>::max();
+  double kSuccessThreshold = success_threshold;
 
   // Find the first possible solution for R, t corresponding to:
   // Betas          = [b00 b01 b11 b02 b12 b22 b03 b13 b23 b33]
diff --git a/extern/libmv/libmv/multiview/euclidean_resection.h b/extern/libmv/libmv/multiview/euclidean_resection.h
index f20513d3fbe..b0428ec61fd 100644
--- a/extern/libmv/libmv/multiview/euclidean_resection.h
+++ b/extern/libmv/libmv/multiview/euclidean_resection.h
@@ -45,11 +45,14 @@ enum ResectionMethod {
  * \param R         Solution for the camera rotation matrix
  * \param t         Solution for the camera translation vector
  * \param method    The resection method to use.
+ * \param success_threshold  Threshold of an error which is still considered a success
+ *                           (currently used by EPnP algorithm only)
  */
 bool EuclideanResection(const Mat2X &x_camera, 
                         const Mat3X &X_world,
                         Mat3 *R, Vec3 *t,
-                        ResectionMethod method = RESECTION_EPNP);
+                        ResectionMethod method = RESECTION_EPNP,
+                        double success_threshold = 1e-3);
 
 /**
  * Computes the extrinsic parameters, R and t for a calibrated camera
@@ -110,6 +113,7 @@ void EuclideanResectionAnsarDaniilidis(const Mat2X &x_camera,
  * \param X_world 3D points in the world coordinate system
  * \param R       Solution for the camera rotation matrix
  * \param t       Solution for the camera translation vector
+ * \param success_threshold  Threshold of an error which is still considered a success
  *
  * This is the algorithm described in:
  * "{EP$n$P: An Accurate $O(n)$ Solution to the P$n$P Problem", by V. Lepetit
@@ -118,7 +122,8 @@ void EuclideanResectionAnsarDaniilidis(const Mat2X &x_camera,
  */
 bool EuclideanResectionEPnP(const Mat2X &x_camera,
                             const Mat3X &X_world, 
-                            Mat3 *R, Vec3 *t);
+                            Mat3 *R, Vec3 *t,
+                            double success_threshold = 1e-3);
 
 } // namespace euclidean_resection
 } // namespace libmv
diff --git a/extern/libmv/libmv/simple_pipeline/pipeline.cc b/extern/libmv/libmv/simple_pipeline/pipeline.cc
index 4773a70d69b..efceda5c455 100644
--- a/extern/libmv/libmv/simple_pipeline/pipeline.cc
+++ b/extern/libmv/libmv/simple_pipeline/pipeline.cc
@@ -50,9 +50,10 @@ struct EuclideanPipelineRoutines {
     EuclideanBundle(tracks, reconstruction);
   }
 
-  static bool Resect(const vector<Marker> &markers,
+  static bool Resect(const ReconstructionOptions &options,
+                     const vector<Marker> &markers,
                      EuclideanReconstruction *reconstruction, bool final_pass) {
-    return EuclideanResect(markers, reconstruction, final_pass);
+    return EuclideanResect(options, markers, reconstruction, final_pass);
   }
 
   static bool Intersect(const vector<Marker> &markers,
@@ -88,7 +89,8 @@ struct ProjectivePipelineRoutines {
     ProjectiveBundle(tracks, reconstruction);
   }
 
-  static bool Resect(const vector<Marker> &markers,
+  static bool Resect(const ReconstructionOptions &options,
+                     const vector<Marker> &markers,
                      ProjectiveReconstruction *reconstruction, bool final_pass) {
     return ProjectiveResect(markers, reconstruction);
   }
@@ -136,6 +138,7 @@ static void CompleteReconstructionLogProress(ProgressUpdateCallback *update_call
 
 template<typename PipelineRoutines>
 void InternalCompleteReconstruction(
+    const ReconstructionOptions &options,
     const Tracks &tracks,
     typename PipelineRoutines::Reconstruction *reconstruction,
     ProgressUpdateCallback *update_callback = NULL) {
@@ -204,7 +207,7 @@ void InternalCompleteReconstruction(
       if (reconstructed_markers.size() >= 5) {
         CompleteReconstructionLogProress(update_callback,
                                          (double)tot_resects/(max_image));
-        if (PipelineRoutines::Resect(reconstructed_markers, reconstruction, false)) {
+        if (PipelineRoutines::Resect(options, reconstructed_markers, reconstruction, false)) {
           num_resects++;
           tot_resects++;
           LG << "Ran Resect() for image " << image;
@@ -240,7 +243,7 @@ void InternalCompleteReconstruction(
     if (reconstructed_markers.size() >= 5) {
       CompleteReconstructionLogProress(update_callback,
                                        (double)tot_resects/(max_image));
-      if (PipelineRoutines::Resect(reconstructed_markers, reconstruction, true)) {
+      if (PipelineRoutines::Resect(options, reconstructed_markers, reconstruction, true)) {
         num_resects++;
         LG << "Ran final Resect() for image " << image;
       } else {
@@ -325,17 +328,21 @@ double ProjectiveReprojectionError(
                                                                intrinsics);
 }
 
-void EuclideanCompleteReconstruction(const Tracks &tracks,
+void EuclideanCompleteReconstruction(const ReconstructionOptions &options,
+                                     const Tracks &tracks,
                                      EuclideanReconstruction *reconstruction,
                                      ProgressUpdateCallback *update_callback) {
-  InternalCompleteReconstruction<EuclideanPipelineRoutines>(tracks,
+  InternalCompleteReconstruction<EuclideanPipelineRoutines>(options,
+                                                            tracks,
                                                             reconstruction,
                                                             update_callback);
 }
 
-void ProjectiveCompleteReconstruction(const Tracks &tracks,
+void ProjectiveCompleteReconstruction(const ReconstructionOptions &options,
+                                      const Tracks &tracks,
                                       ProjectiveReconstruction *reconstruction) {
-  InternalCompleteReconstruction<ProjectivePipelineRoutines>(tracks,
+  InternalCompleteReconstruction<ProjectivePipelineRoutines>(options,
+                                                             tracks,
                                                              reconstruction);
 }
 
diff --git a/extern/libmv/libmv/simple_pipeline/pipeline.h b/extern/libmv/libmv/simple_pipeline/pipeline.h
index e940b57bc0d..11c11297d78 100644
--- a/extern/libmv/libmv/simple_pipeline/pipeline.h
+++ b/extern/libmv/libmv/simple_pipeline/pipeline.h
@@ -39,6 +39,9 @@ namespace libmv {
     repeated until all points and cameras are estimated. Periodically, bundle
     adjustment is run to ensure a quality reconstruction.
 
+    \a options are used to define some specific befaviours based on settings
+    see documentation for ReconstructionOptions
+
     \a tracks should contain markers used in the reconstruction.
     \a reconstruction should contain at least some 3D points or some estimated
     cameras. The minimum number of cameras is two (with no 3D points) and the
@@ -46,7 +49,8 @@ namespace libmv {
 
     \sa EuclideanResect, EuclideanIntersect, EuclideanBundle
 */
-void EuclideanCompleteReconstruction(const Tracks &tracks,
+void EuclideanCompleteReconstruction(const ReconstructionOptions &options,
+                                     const Tracks &tracks,
                                      EuclideanReconstruction *reconstruction,
                                      ProgressUpdateCallback *update_callback = NULL);
 
@@ -63,6 +67,9 @@ void EuclideanCompleteReconstruction(const Tracks &tracks,
     repeated until all points and cameras are estimated. Periodically, bundle
     adjustment is run to ensure a quality reconstruction.
 
+    \a options are used to define some specific befaviours based on settings
+    see documentation for ReconstructionOptions
+
     \a tracks should contain markers used in the reconstruction.
     \a reconstruction should contain at least some 3D points or some estimated
     cameras. The minimum number of cameras is two (with no 3D points) and the
@@ -70,7 +77,8 @@ void EuclideanCompleteReconstruction(const Tracks &tracks,
 
     \sa ProjectiveResect, ProjectiveIntersect, ProjectiveBundle
 */
-void ProjectiveCompleteReconstruction(const Tracks &tracks,
+void ProjectiveCompleteReconstruction(const ReconstructionOptions &options,
+                                      const Tracks &tracks,
                                       ProjectiveReconstruction *reconstruction);
 
 
diff --git a/extern/libmv/libmv/simple_pipeline/reconstruction.h b/extern/libmv/libmv/simple_pipeline/reconstruction.h
index 947a0636476..71789e3a245 100644
--- a/extern/libmv/libmv/simple_pipeline/reconstruction.h
+++ b/extern/libmv/libmv/simple_pipeline/reconstruction.h
@@ -26,6 +26,17 @@
 
 namespace libmv {
 
+struct ReconstructionOptions {
+	// threshold value of reconstruction error which is still considered successful
+	// if reconstruction error bigger than this value, fallback reconstruction
+	// algorithm would be used (if enabled)
+	double success_threshold;
+
+	// use fallback reconstruction algorithm in cases main reconstruction algorithm
+	// failed to reconstruct
+	bool use_fallback_reconstruction;
+};
+
 /*!
     A EuclideanCamera is the location and rotation of the camera viewing \a image.
 
diff --git a/extern/libmv/libmv/simple_pipeline/resect.cc b/extern/libmv/libmv/simple_pipeline/resect.cc
index 8737182dd43..4c9ca6d8677 100644
--- a/extern/libmv/libmv/simple_pipeline/resect.cc
+++ b/extern/libmv/libmv/simple_pipeline/resect.cc
@@ -90,7 +90,8 @@ struct EuclideanResectCostFunction {
 
 }  // namespace
 
-bool EuclideanResect(const vector<Marker> &markers,
+bool EuclideanResect(const ReconstructionOptions &options,
+                     const vector<Marker> &markers,
                      EuclideanReconstruction *reconstruction, bool final_pass) {
   if (markers.size() < 5) {
     return false;
@@ -104,13 +105,24 @@ bool EuclideanResect(const vector<Marker> &markers,
 
   Mat3 R;
   Vec3 t;
-  if (0 || !euclidean_resection::EuclideanResection(points_2d, points_3d, &R, &t)) {
+
+  double success_threshold = std::numeric_limits<double>::max();
+
+  if(options.use_fallback_reconstruction)
+    success_threshold = options.success_threshold;
+
+  if (0 || !euclidean_resection::EuclideanResection(points_2d, points_3d, &R, &t,
+                                                    euclidean_resection::RESECTION_EPNP,
+                                                    success_threshold))
+  {
     // printf("Resection for image %d failed\n", markers[0].image);
     LG << "Resection for image " << markers[0].image << " failed;"
        << " trying fallback projective resection.";
 
-    LG << "No fallback; failing resection for " << markers[0].image;
-    return false;
+    if (!options.use_fallback_reconstruction) {
+        LG << "No fallback; failing resection for " << markers[0].image;
+        return false;
+    }
 
     if (!final_pass) return false;
     // Euclidean resection failed. Fall back to projective resection, which is
diff --git a/extern/libmv/libmv/simple_pipeline/resect.h b/extern/libmv/libmv/simple_pipeline/resect.h
index f8b5b9f68ee..1691e7ee245 100644
--- a/extern/libmv/libmv/simple_pipeline/resect.h
+++ b/extern/libmv/libmv/simple_pipeline/resect.h
@@ -35,6 +35,9 @@ namespace libmv {
     reconstruction object, and solves for the pose and orientation of the
     camera for that frame.
 
+    \a options are used to define some specific befaviours based on settings
+    see documentation for ReconstructionOptions
+
     \a markers should contain \l Marker markers \endlink belonging to tracks
     visible in the one frame to be resectioned. Each of the tracks associated
     with the markers must have a corresponding reconstructed 3D position in the
@@ -51,7 +54,8 @@ namespace libmv {
 
     \sa EuclideanIntersect, EuclideanReconstructTwoFrames
 */
-bool EuclideanResect(const vector<Marker> &markers,
+bool EuclideanResect(const ReconstructionOptions &options,
+                     const vector<Marker> &markers,
                      EuclideanReconstruction *reconstruction, bool final_pass);
 
 /*!