Weighted tracks

Added a weight slider to track which defines
how much particular track affects in a final
reconstruction. This weight is for sure
animateable.

Currently it affects on BA step only which in
most cases will work just fine.

The usecase of this slider is to have it set
to 1.0 most of the time where the track is
good, but blend it's weight down to 0 when
tracker looses the track. This will prevent
camera from jump.

Tutorial is to be done by Sebastian.

7 files changed, 68 insertions, 42 deletions

diff --git a/extern/libmv/libmv-capi.cc b/extern/libmv/libmv-capi.cc
index 91a3b845815..0585bd3e8ac 100644
--- a/extern/libmv/libmv-capi.cc
+++ b/extern/libmv/libmv-capi.cc
@@ -393,9 +393,9 @@ void libmv_tracksDestroy(struct libmv_Tracks *libmv_tracks)
 	LIBMV_OBJECT_DELETE(libmv_tracks, Tracks);
 }
 
-void libmv_tracksInsert(struct libmv_Tracks *libmv_tracks, int image, int track, double x, double y)
+void libmv_tracksInsert(struct libmv_Tracks *libmv_tracks, int image, int track, double x, double y, double weight)
 {
-	((libmv::Tracks*) libmv_tracks)->Insert(image, track, x, y);
+	((libmv::Tracks*) libmv_tracks)->Insert(image, track, x, y, weight);
 }
 
 /* ************ Reconstruction ************ */
diff --git a/extern/libmv/libmv-capi.h b/extern/libmv/libmv-capi.h
index 9541f411ba0..13cc3ae8499 100644
--- a/extern/libmv/libmv-capi.h
+++ b/extern/libmv/libmv-capi.h
@@ -73,7 +73,7 @@ void libmv_samplePlanarPatch(const float *image, int width, int height,
 /* Tracks */
 struct libmv_Tracks *libmv_tracksNew(void);
 void libmv_tracksDestroy(struct libmv_Tracks *libmv_tracks);
-void libmv_tracksInsert(struct libmv_Tracks *libmv_tracks, int image, int track, double x, double y);
+void libmv_tracksInsert(struct libmv_Tracks *libmv_tracks, int image, int track, double x, double y, double weight);
 
 /* Reconstruction */
 #define LIBMV_REFINE_FOCAL_LENGTH          (1 << 0)
diff --git a/extern/libmv/libmv-capi_stub.cc b/extern/libmv/libmv-capi_stub.cc
index e6d3753961b..bda9605b422 100644
--- a/extern/libmv/libmv-capi_stub.cc
+++ b/extern/libmv/libmv-capi_stub.cc
@@ -85,7 +85,7 @@ struct libmv_Tracks *libmv_tracksNew(void)
 }
 
 void libmv_tracksInsert(struct libmv_Tracks * /*libmv_tracks*/, int /*image*/,
-                        int /*track*/, double /*x*/, double /*y*/)
+                        int /*track*/, double /*x*/, double /*y*/, double /*weight*/)
 {
 }
 
diff --git a/extern/libmv/libmv/simple_pipeline/bundle.cc b/extern/libmv/libmv/simple_pipeline/bundle.cc
index c778c11b3f6..e7887256892 100644
--- a/extern/libmv/libmv/simple_pipeline/bundle.cc
+++ b/extern/libmv/libmv/simple_pipeline/bundle.cc
@@ -60,8 +60,11 @@ namespace {
 //
 // This functor uses a radial distortion model.
 struct OpenCVReprojectionError {
-  OpenCVReprojectionError(const double observed_x, const double observed_y)
-      : observed_x(observed_x), observed_y(observed_y) {}
+  OpenCVReprojectionError(const double observed_x,
+                          const double observed_y,
+                          const double weight)
+      : observed_x_(observed_x), observed_y_(observed_y),
+        weight_(weight) {}
 
   template <typename T>
   bool operator()(const T* const intrinsics,
@@ -112,13 +115,14 @@ struct OpenCVReprojectionError {
                                           &predicted_y);
 
     // The error is the difference between the predicted and observed position.
-    residuals[0] = predicted_x - T(observed_x);
-    residuals[1] = predicted_y - T(observed_y);
+    residuals[0] = (predicted_x - T(observed_x_)) * weight_;
+    residuals[1] = (predicted_y - T(observed_y_)) * weight_;
     return true;
   }
 
-  const double observed_x;
-  const double observed_y;
+  const double observed_x_;
+  const double observed_y_;
+  const double weight_;
 };
 
 // Print a message to the log which camera intrinsics are gonna to be optimixed.
@@ -378,25 +382,31 @@ void EuclideanBundleCommonIntrinsics(const Tracks &tracks,
     // 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)),
-        NULL,
-        ceres_intrinsics,
-        current_camera_R_t,
-        &point->X(0));
-
-    // We lock the first camera to better deal with scene orientation ambiguity.
-    if (!have_locked_camera) {
-      problem.SetParameterBlockConstant(current_camera_R_t);
-      have_locked_camera = true;
-    }
+    // Skip residual block for markers which does have absolutely
+    // no affect on the final solution.
+    // This way ceres is not gonna to go crazy.
+    if (marker.weight != 0.0) {
+      problem.AddResidualBlock(new ceres::AutoDiffCostFunction<
+          OpenCVReprojectionError, 2, 8, 6, 3>(
+              new OpenCVReprojectionError(
+                  marker.x,
+                  marker.y,
+                  marker.weight)),
+          NULL,
+          ceres_intrinsics,
+          current_camera_R_t,
+          &point->X(0));
+
+      // We lock the first camera to better deal with scene orientation ambiguity.
+      if (!have_locked_camera) {
+        problem.SetParameterBlockConstant(current_camera_R_t);
+        have_locked_camera = true;
+      }
 
-    if (bundle_constraints & BUNDLE_NO_TRANSLATION) {
-      problem.SetParameterization(current_camera_R_t,
+      if (bundle_constraints & BUNDLE_NO_TRANSLATION) {
+        problem.SetParameterization(current_camera_R_t,
                                   constant_translation_parameterization);
+      }
     }
 
     num_residuals++;
diff --git a/extern/libmv/libmv/simple_pipeline/modal_solver.cc b/extern/libmv/libmv/simple_pipeline/modal_solver.cc
index 90dfde15660..caccce68cbe 100644
--- a/extern/libmv/libmv/simple_pipeline/modal_solver.cc
+++ b/extern/libmv/libmv/simple_pipeline/modal_solver.cc
@@ -57,8 +57,10 @@ void ModalSolverLogProress(ProgressUpdateCallback *update_callback,
 struct ModalReprojectionError {
   ModalReprojectionError(double observed_x,
                          double observed_y,
+                         const double weight,
                          const Vec3 &bundle)
-    : observed_x(observed_x), observed_y(observed_y), bundle(bundle) { }
+    : observed_x_(observed_x), observed_y_(observed_y),
+      weight_(weight), bundle_(bundle) { }
 
   template <typename T>
   bool operator()(const T* quaternion,   // Rotation quaternion
@@ -68,9 +70,9 @@ struct ModalReprojectionError {
 
     // Convert bundle position from double to T.
     T X[3];
-    X[0] = T(bundle(0));
-    X[1] = T(bundle(1));
-    X[2] = T(bundle(2));
+    X[0] = T(bundle_(0));
+    X[1] = T(bundle_(1));
+    X[2] = T(bundle_(2));
 
     // Compute projective coordinates: x = RX.
     T x[3];
@@ -84,15 +86,16 @@ struct ModalReprojectionError {
 
     // The error is the difference between reprojected
     // and observed marker position.
-    residuals[0] = xn - T(observed_x);
-    residuals[1] = yn - T(observed_y);
+    residuals[0] = xn - T(observed_x_);
+    residuals[1] = yn - T(observed_y_);
 
     return true;
   }
 
-  double observed_x;
-  double observed_y;
-  Vec3 bundle;
+  double observed_x_;
+  double observed_y_;
+  double weight_;
+  Vec3 bundle_;
 };
 }  // namespace
 
@@ -180,11 +183,13 @@ void ModalSolver(const Tracks &tracks,
       Marker &marker = all_markers[i];
       EuclideanPoint *point = reconstruction->PointForTrack(marker.track);
 
-      if (point) {
+      if (point && marker.weight != 0.0) {
         problem.AddResidualBlock(new ceres::AutoDiffCostFunction<
             ModalReprojectionError,
             2, /* num_residuals */
-            4>(new ModalReprojectionError(marker.x, marker.y,
+            4>(new ModalReprojectionError(marker.x,
+                                          marker.y,
+                                          marker.weight,
                                           point->X)),
             NULL,
             &quaternion(0));
diff --git a/extern/libmv/libmv/simple_pipeline/tracks.cc b/extern/libmv/libmv/simple_pipeline/tracks.cc
index f9e50d20af9..d5d009708ba 100644
--- a/extern/libmv/libmv/simple_pipeline/tracks.cc
+++ b/extern/libmv/libmv/simple_pipeline/tracks.cc
@@ -34,7 +34,7 @@ Tracks::Tracks(const Tracks &other) {
 
 Tracks::Tracks(const vector<Marker> &markers) : markers_(markers) {}
 
-void Tracks::Insert(int image, int track, double x, double y) {
+void Tracks::Insert(int image, int track, double x, double y, double weight) {
   // TODO(keir): Wow, this is quadratic for repeated insertions. Fix this by
   // adding a smarter data structure like a set<>.
   for (int i = 0; i < markers_.size(); ++i) {
@@ -45,7 +45,7 @@ void Tracks::Insert(int image, int track, double x, double y) {
       return;
     }
   }
-  Marker marker = { image, track, x, y };
+  Marker marker = { image, track, x, y, weight };
   markers_.push_back(marker);
 }
 
@@ -122,7 +122,7 @@ Marker Tracks::MarkerInImageForTrack(int image, int track) const {
       return markers_[i];
     }
   }
-  Marker null = { -1, -1, -1, -1 };
+  Marker null = { -1, -1, -1, -1, 0.0 };
   return null;
 }
 
diff --git a/extern/libmv/libmv/simple_pipeline/tracks.h b/extern/libmv/libmv/simple_pipeline/tracks.h
index f9af3ada45b..e2f8cf6b459 100644
--- a/extern/libmv/libmv/simple_pipeline/tracks.h
+++ b/extern/libmv/libmv/simple_pipeline/tracks.h
@@ -33,14 +33,20 @@ namespace libmv {
     in the image identified by \a image. All markers for to the same target
     form a track identified by a common \a track number.
 
+    \a weight is used by bundle adjustment and weight means how much the
+    track affects on a final solution.
+
     \note Markers are typically aggregated with the help of the \l Tracks class.
 
     \sa Tracks
 */
+// TODO(sergey): Consider using comment for every member separately
+//               instead of having one giantic comment block.
 struct Marker {
   int image;
   int track;
   double x, y;
+  double weight;
 };
 
 /*!
@@ -72,9 +78,14 @@ class Tracks {
       \a image and \a track are the keys used to retrieve the markers with the
       other methods in this class.
 
+      \a weight is used by bundle adjustment and weight means how much the
+      track affects on a final solution.
+
       \note To get an identifier for a new track, use \l MaxTrack() + 1.
   */
-  void Insert(int image, int track, double x, double y);
+  // TODO(sergey): Consider using InsetWeightedMarker istead of using
+  //               stupid default value?
+  void Insert(int image, int track, double x, double y, double weight = 1.0);
 
   /// Returns all the markers.
   vector<Marker> AllMarkers() const;