Implement separate BA step for tracks which have constant zero weight

This is needed to minimize their reprojection error over the footage.
Without this extra step positions of such tracks were calculated by
algebraic intersection code only, which doesn't give best precision.

1 files changed, 100 insertions, 2 deletions

diff --git a/extern/libmv/libmv/simple_pipeline/bundle.cc b/extern/libmv/libmv/simple_pipeline/bundle.cc
index 9835248e7d5..09523340ed7 100644
--- a/extern/libmv/libmv/simple_pipeline/bundle.cc
+++ b/extern/libmv/libmv/simple_pipeline/bundle.cc
@@ -321,6 +321,78 @@ void EuclideanBundlerPerformEvaluation(const Tracks &tracks,
     }
 }
 
+// This is an utility function to only bundle 3D position of
+// given markers list.
+//
+// Main purpose of this function is to adjust positions of tracks
+// which does have constant zero weight and so far only were using
+// algebraic intersection to obtain their 3D positions.
+//
+// At this point we only need to bundle points positions, cameras
+// are to be totally still here.
+void EuclideanBundlePointsOnly(const vector<Marker> &markers,
+                               vector<Vec6> &all_cameras_R_t,
+                               double ceres_intrinsics[8],
+                               EuclideanReconstruction *reconstruction) {
+  ceres::Problem::Options problem_options;
+  ceres::Problem problem(problem_options);
+  int num_residuals = 0;
+  for (int i = 0; i < markers.size(); ++i) {
+    const Marker &marker = markers[i];
+    EuclideanCamera *camera = reconstruction->CameraForImage(marker.image);
+    EuclideanPoint *point = reconstruction->PointForTrack(marker.track);
+    if (camera == NULL || point == NULL) {
+      continue;
+    }
+
+    // Rotation of camera denoted in angle axis followed with
+    // camera translaiton.
+    double *current_camera_R_t = &all_cameras_R_t[camera->image](0);
+
+    problem.AddResidualBlock(new ceres::AutoDiffCostFunction<
+        OpenCVReprojectionError, 2, 8, 6, 3>(
+            new OpenCVReprojectionError(
+                marker.x,
+                marker.y,
+                1.0)),
+        NULL,
+        ceres_intrinsics,
+        current_camera_R_t,
+        &point->X(0));
+
+    problem.SetParameterBlockConstant(current_camera_R_t);
+    num_residuals++;
+  }
+
+  LG << "Number of residuals: " << num_residuals;
+  if (!num_residuals) {
+    LG << "Skipping running minimizer with zero residuals";
+    return;
+  }
+
+  problem.SetParameterBlockConstant(ceres_intrinsics);
+
+  // Configure the solver.
+  ceres::Solver::Options options;
+  options.use_nonmonotonic_steps = true;
+  options.preconditioner_type = ceres::SCHUR_JACOBI;
+  options.linear_solver_type = ceres::ITERATIVE_SCHUR;
+  options.use_inner_iterations = true;
+  options.max_num_iterations = 100;
+
+#ifdef _OPENMP
+  options.num_threads = omp_get_max_threads();
+  options.num_linear_solver_threads = omp_get_max_threads();
+#endif
+
+  // Solve!
+  ceres::Solver::Summary summary;
+  ceres::Solve(options, &problem, &summary);
+
+  LG << "Final report:\n" << summary.FullReport();
+
+}
+
 }  // namespace
 
 void EuclideanBundle(const Tracks &tracks,
@@ -343,8 +415,8 @@ void EuclideanBundleCommonIntrinsics(const Tracks &tracks,
   LG << "Original intrinsics: " << *intrinsics;
   vector<Marker> markers = tracks.AllMarkers();
 
-  ceres::Problem::Options problem_options;
-  ceres::Problem problem(problem_options);
+  // N-th element denotes whether track N is a constant zero-weigthed track.
+  vector<bool> zero_weight_tracks_flags(tracks.MaxTrack(), true);
 
   // Residual blocks with 10 parameters are unwieldly with Ceres, so pack the
   // intrinsics into a single block and rely on local parameterizations to
@@ -375,6 +447,8 @@ void EuclideanBundleCommonIntrinsics(const Tracks &tracks,
   }
 
   // Add residual blocks to the problem.
+  ceres::Problem::Options problem_options;
+  ceres::Problem problem(problem_options);
   int num_residuals = 0;
   bool have_locked_camera = false;
   for (int i = 0; i < markers.size(); ++i) {
@@ -414,6 +488,8 @@ void EuclideanBundleCommonIntrinsics(const Tracks &tracks,
         problem.SetParameterization(current_camera_R_t,
                                     constant_translation_parameterization);
       }
+
+      zero_weight_tracks_flags[marker.track] = false;
     }
 
     num_residuals++;
@@ -492,6 +568,28 @@ void EuclideanBundleCommonIntrinsics(const Tracks &tracks,
     EuclideanBundlerPerformEvaluation(tracks, reconstruction, &all_cameras_R_t,
                                       &problem, evaluation);
   }
+
+  // Separate step to adjust positions of tracks which are
+  // constant zero-weighted.
+  vector<Marker> zero_weight_markers;
+  for (int track = 0; track < tracks.MaxTrack(); ++track) {
+    if (zero_weight_tracks_flags[track]) {
+      vector<Marker> current_markers = tracks.MarkersForTrack(track);
+      zero_weight_markers.reserve(zero_weight_markers.size() +
+                                  current_markers.size());
+      for (int i = 0; i < current_markers.size(); ++i) {
+        zero_weight_markers.push_back(current_markers[i]);
+      }
+    }
+  }
+
+  if (zero_weight_markers.size()) {
+    LG << "Refining position of constant zero-weighted tracks";
+    EuclideanBundlePointsOnly(zero_weight_markers,
+                              all_cameras_R_t,
+                              ceres_intrinsics,
+                              reconstruction);
+  }
 }
 
 void ProjectiveBundle(const Tracks & /*tracks*/,