Modal (aka tripod) solver rework

Several major things are done in this commit:

- First of all, logic of modal solver was changed.
  We do not rely on only minimizer to take care of
  guessing rotation for frame, but we're using
  analytical rotation computation for point clouds
  to obtain initial rotation.

  Then this rotation is being refined using Ceres
  minimizer and now instead of minimizing average
  distance between points of point of two clouds,
  minimization of reprojection error of point
  cloud onto frame happens.

  This gives quite a bit of precision improvement.

- Second bigger improvement here is using bundle
  adjustment for a result of first step when we're
  only estimating rotation between neighbor images
  and reprojecting markers.

  This averages error across the image sequence
  avoiding error accumulation. Also, this will
  tweak bundles themselves a bit for better match.

- And last bigger improvement here is support of
  camera intrinsics refirenment.

  This allowed to significantly improve solution
  for real-life footage and results after such
  refining are much more usable than it were before.

Thanks to Keir for the help and code review.

8 files changed, 373 insertions, 57 deletions

diff --git a/extern/libmv/CMakeLists.txt b/extern/libmv/CMakeLists.txt
index 1c289cd2778..241e8bf2cd1 100644
--- a/extern/libmv/CMakeLists.txt
+++ b/extern/libmv/CMakeLists.txt
@@ -99,6 +99,7 @@ set(SRC
 	libmv/multiview/homography.h
 	libmv/multiview/homography_parameterization.h
 	libmv/multiview/nviewtriangulation.h
+	libmv/multiview/panography.h
 	libmv/multiview/projection.h
 	libmv/multiview/resection.h
 	libmv/multiview/triangulation.h
diff --git a/extern/libmv/files.txt b/extern/libmv/files.txt
index 22c5226adbe..8eead23ea08 100644
--- a/extern/libmv/files.txt
+++ b/extern/libmv/files.txt
@@ -21,6 +21,7 @@ libmv/multiview/homography.cc
 libmv/multiview/homography.h
 libmv/multiview/homography_parameterization.h
 libmv/multiview/nviewtriangulation.h
+libmv/multiview/panography.h
 libmv/multiview/projection.cc
 libmv/multiview/projection.h
 libmv/multiview/resection.h
@@ -142,25 +143,6 @@ third_party/glog/src/windows/glog/vlog_is_on.h
 third_party/glog/src/windows/port.cc
 third_party/glog/src/windows/port.h
 third_party/glog/src/windows/preprocess.sh
-third_party/ldl/CMakeLists.txt
-third_party/ldl/Doc/ChangeLog
-third_party/ldl/Doc/lesser.txt
-third_party/ldl/Include/ldl.h
-third_party/ldl/README.libmv
-third_party/ldl/README.txt
-third_party/ldl/Source/ldl.c
 third_party/msinttypes/inttypes.h
 third_party/msinttypes/README.libmv
 third_party/msinttypes/stdint.h
-third_party/ssba/COPYING.TXT
-third_party/ssba/Geometry/v3d_cameramatrix.h
-third_party/ssba/Geometry/v3d_distortion.h
-third_party/ssba/Geometry/v3d_metricbundle.cpp
-third_party/ssba/Geometry/v3d_metricbundle.h
-third_party/ssba/Math/v3d_linear.h
-third_party/ssba/Math/v3d_linear_utils.h
-third_party/ssba/Math/v3d_mathutilities.h
-third_party/ssba/Math/v3d_optimization.cpp
-third_party/ssba/Math/v3d_optimization.h
-third_party/ssba/README.libmv
-third_party/ssba/README.TXT
diff --git a/extern/libmv/libmv-capi.cpp b/extern/libmv/libmv-capi.cpp
index 4d6b35b91f8..74224eb5368 100644
--- a/extern/libmv/libmv-capi.cpp
+++ b/extern/libmv/libmv-capi.cpp
@@ -444,7 +444,8 @@ int libmv_refineParametersAreValid(int parameters) {
 
 static void libmv_solveRefineIntrinsics(libmv::Tracks *tracks, libmv::CameraIntrinsics *intrinsics,
 			libmv::EuclideanReconstruction *reconstruction, int refine_intrinsics,
-			reconstruct_progress_update_cb progress_update_callback, void *callback_customdata)
+			reconstruct_progress_update_cb progress_update_callback, void *callback_customdata,
+			int bundle_constraints = libmv::BUNDLE_NO_CONSTRAINTS)
 {
 	/* only a few combinations are supported but trust the caller */
 	int libmv_refine_flags = 0;
@@ -465,7 +466,7 @@ static void libmv_solveRefineIntrinsics(libmv::Tracks *tracks, libmv::CameraIntr
 	progress_update_callback(callback_customdata, 1.0, "Refining solution");
 
 	libmv::EuclideanBundleCommonIntrinsics(*(libmv::Tracks *)tracks, libmv_refine_flags,
-		reconstruction, intrinsics);
+		reconstruction, intrinsics, bundle_constraints);
 }
 
 static void cameraIntrinsicsFromOptions(libmv::CameraIntrinsics *camera_intrinsics,
@@ -568,6 +569,7 @@ libmv_Reconstruction *libmv_solveReconstruction(libmv_Tracks *libmv_tracks,
 
 struct libmv_Reconstruction *libmv_solveModal(struct libmv_Tracks *libmv_tracks,
 			libmv_cameraIntrinsicsOptions *libmv_camera_intrinsics_options,
+			libmv_reconstructionOptions *libmv_reconstruction_options,
 			reconstruct_progress_update_cb progress_update_callback,
 			void *callback_customdata)
 {
@@ -582,13 +584,28 @@ struct libmv_Reconstruction *libmv_solveModal(struct libmv_Tracks *libmv_tracks,
 
 	cameraIntrinsicsFromOptions(camera_intrinsics, libmv_camera_intrinsics_options);
 
-	/* Invert the camera intrinsics */
+	/* Invert the camera intrinsics. */
 	libmv::Tracks normalized_tracks = getNormalizedTracks(tracks, camera_intrinsics);
 
-	/* actual reconstruction */
+	/* Actual reconstruction. */
 	libmv::ModalSolver(normalized_tracks, reconstruction, &update_callback);
 
-	/* finish reconstruction */
+	libmv::CameraIntrinsics empty_intrinsics;
+	libmv::EuclideanBundleCommonIntrinsics(normalized_tracks,
+	                                       libmv::BUNDLE_NO_INTRINSICS,
+	                                       reconstruction,
+	                                       &empty_intrinsics,
+	                                       libmv::BUNDLE_NO_TRANSLATION);
+
+	/* Refinement. */
+	if (libmv_reconstruction_options->refine_intrinsics) {
+		libmv_solveRefineIntrinsics((libmv::Tracks *)tracks, camera_intrinsics, reconstruction,
+			libmv_reconstruction_options->refine_intrinsics,
+			progress_update_callback, callback_customdata,
+			libmv::BUNDLE_NO_TRANSLATION);
+	}
+
+	/* Finish reconstruction. */
 	finishReconstruction(tracks, camera_intrinsics, libmv_reconstruction,
 	                     progress_update_callback, callback_customdata);
 
diff --git a/extern/libmv/libmv-capi.h b/extern/libmv/libmv-capi.h
index 588df8de440..af0d656146b 100644
--- a/extern/libmv/libmv-capi.h
+++ b/extern/libmv/libmv-capi.h
@@ -110,6 +110,7 @@ struct libmv_Reconstruction *libmv_solveReconstruction(struct libmv_Tracks *libm
 			void *callback_customdata);
 struct libmv_Reconstruction *libmv_solveModal(struct libmv_Tracks *libmv_tracks,
 			libmv_cameraIntrinsicsOptions *libmv_camera_intrinsics_options,
+			libmv_reconstructionOptions *libmv_reconstruction_options,
 			reconstruct_progress_update_cb progress_update_callback,
 			void *callback_customdata);
 int libmv_reporojectionPointForTrack(struct libmv_Reconstruction *libmv_reconstruction, int track, double pos[3]);
diff --git a/extern/libmv/libmv/multiview/panography.h b/extern/libmv/libmv/multiview/panography.h
new file mode 100644
index 00000000000..6f01beb0ffd
--- /dev/null
+++ b/extern/libmv/libmv/multiview/panography.h
@@ -0,0 +1,181 @@
+// Copyright (c) 2009 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.
+//
+
+#ifndef LIBMV_MULTIVIEW_PANOGRAPHY_H
+#define LIBMV_MULTIVIEW_PANOGRAPHY_H
+
+#include "libmv/numeric/numeric.h"
+#include "libmv/numeric/poly.h"
+#include "libmv/base/vector.h"
+
+namespace libmv {
+
+static bool Build_Minimal2Point_PolynomialFactor(
+                                          const Mat & x1, const Mat & x2,
+                                          double * P) //P must be a double[4]
+{
+  assert(2 == x1.rows());
+  assert(2 == x1.cols());
+  assert(x1.rows() == x2.rows());
+  assert(x1.cols() == x2.cols());
+
+  // Setup the variable of the input problem:
+  Vec xx1 = (x1.col(0)).transpose();
+  Vec yx1 = (x1.col(1)).transpose();
+
+  double a12 = xx1.dot(yx1);
+  Vec xx2 = (x2.col(0)).transpose();
+  Vec yx2 = (x2.col(1)).transpose();
+  double b12 = xx2.dot(yx2);
+
+  double a1  = xx1.squaredNorm();
+  double a2  = yx1.squaredNorm();
+
+  double b1  = xx2.squaredNorm();
+  double b2  = yx2.squaredNorm();
+
+  // Build the 3rd degre polynomial in F^2.
+  //
+  //   f^6 * p + f^4 * q + f^2* r + s = 0;
+  //
+  // Coefficients in ascending powers of alpha, i.e. P[N]*x^N.
+  // Run panography_coeffs.py to get the below coefficients.
+  P[0] = b1*b2*a12*a12-a1*a2*b12*b12;
+  P[1] = -2*a1*a2*b12+2*a12*b1*b2+b1*a12*a12+b2*a12*a12-a1*b12*b12-a2*b12*b12;
+  P[2] = b1*b2-a1*a2-2*a1*b12-2*a2*b12+2*a12*b1+2*a12*b2+a12*a12-b12*b12;
+  P[3] = b1+b2-2*b12-a1-a2+2*a12;
+
+  // If P[3] equal to 0 we get ill conditionned data
+  return (P[3] != 0.0);
+}
+
+// This implements a minimal solution (2 points) for panoramic stitching:
+//
+//   http://www.cs.ubc.ca/~mbrown/minimal/minimal.html
+//
+//   [1] M. Brown and R. Hartley and D. Nister. Minimal Solutions for Panoramic
+//       Stitching. CVPR07.
+//
+// The 2-point algorithm solves for the rotation of the camera with a single
+// focal length (4 degrees of freedom).
+//
+// Compute from 1 to 3 possible focal lenght for 2 point correspondences.
+// Suppose that the cameras share the same optical center and focal lengths:
+//
+//   Image 1 => H*x = x'  =>  Image 2
+//   x (u1j)                  x' (u2j)
+//   a (u11)                  a' (u21)
+//   b (u12)                  b' (u22)
+//
+// The return values are 1 to 3 possible values for the focal lengths such
+// that:
+//
+//       [f 0 0]
+//   K = [0 f 0]
+//       [0 0 1]
+//
+static void F_FromCorrespondance_2points(const Mat &x1, const Mat &x2,
+                                         vector<double> *fs) {
+
+  // Build Polynomial factor to get squared focal value.
+  double P[4];
+  Build_Minimal2Point_PolynomialFactor(x1, x2, &P[0]);
+
+  // Solve it by using F = f^2 and a Cubic polynomial solver
+  //
+  //   F^3 * p + F^2 * q + F^1 * r + s = 0
+  //
+  double roots[3];
+  int num_roots = SolveCubicPolynomial(P, roots);
+  for (int i = 0; i < num_roots; ++i)  {
+    if (roots[i] > 0.0)  {
+      fs->push_back(sqrt(roots[i]));
+    }
+  }
+}
+
+// Compute the 3x3 rotation matrix that fits two 3D point clouds in the least
+// square sense. The method is from:
+//
+//   K. Arun,T. Huand and D. Blostein. Least-squares fitting of 2 3-D point
+//   sets.  IEEE Transactions on Pattern Analysis and Machine Intelligence,
+//   9:698-700, 1987.
+//
+// Given the calibration matrices K1, K2 solve for the rotation from
+// corresponding image rays.
+//
+//   R = min || X2 - R * x1 ||.
+//
+// In case of panography, which is for a camera that shares the same camera
+// center,
+//
+//   H = K2 * R * K1.inverse();
+//
+// For the full explanation, see Section 8, Solving for Rotation from [1].
+//
+// Parameters:
+//
+//   x1 : Point cloud A (3D coords)
+//   x2 : Point cloud B (3D coords)
+//
+//        [f 0 0]
+//   K1 = [0 f 0]
+//        [0 0 1]
+//
+//   K2 (the same form as K1, but may have different f)
+//
+// Returns: A rotation matrix that minimizes
+//
+//   R = arg min || X2 - R * x1 ||
+//
+static void GetR_FixedCameraCenter(const Mat &x1, const Mat &x2,
+                                   const double focal,
+                                   Mat3 *R)  {
+  assert(3 == x1.rows());
+  assert(2 <= x1.cols());
+  assert(x1.rows() == x2.rows());
+  assert(x1.cols() == x2.cols());
+
+  // Build simplified K matrix
+  Mat3 K( Mat3::Identity() * 1.0/focal );
+  K(2,2)= 1.0;
+
+  // Build the correlation matrix; equation (22) in [1].
+  Mat3 C = Mat3::Zero();
+  for(int i = 0; i < x1.cols(); ++i) {
+    Mat r1i = (K * x1.col(i)).normalized();
+    Mat r2i = (K * x2.col(i)).normalized();
+    C += r2i * r1i.transpose();
+  }
+
+  // Solve for rotation. Equations (24) and (25) in [1].
+  Eigen::JacobiSVD<Mat> svd(C, Eigen::ComputeThinU | Eigen::ComputeThinV);
+  Mat3 scale = Mat3::Identity();
+  scale(2,2) = ((svd.matrixU() * svd.matrixV().transpose()).determinant() > 0.0)
+             ?  1.0
+             : -1.0;
+
+  (*R) = svd.matrixU() * scale * svd.matrixV().transpose();
+}
+
+}  // namespace libmv
+
+#endif // LIBMV_MULTIVIEW_PANOGRAPHY_H
diff --git a/extern/libmv/libmv/simple_pipeline/bundle.cc b/extern/libmv/libmv/simple_pipeline/bundle.cc
index 34117cbbbd7..3e36a78c9df 100644
--- a/extern/libmv/libmv/simple_pipeline/bundle.cc
+++ b/extern/libmv/libmv/simple_pipeline/bundle.cc
@@ -233,7 +233,8 @@ void EuclideanBundle(const Tracks &tracks,
 void EuclideanBundleCommonIntrinsics(const Tracks &tracks,
                                      int bundle_intrinsics,
                                      EuclideanReconstruction *reconstruction,
-                                     CameraIntrinsics *intrinsics) {
+                                     CameraIntrinsics *intrinsics,
+                                     int bundle_constraints) {
   LG << "Original intrinsics: " << *intrinsics;
   vector<Marker> markers = tracks.AllMarkers();
 
@@ -270,7 +271,7 @@ void EuclideanBundleCommonIntrinsics(const Tracks &tracks,
     }
 
     problem.AddResidualBlock(new ceres::AutoDiffCostFunction<
-        OpenCVReprojectionError, 2, 8, 9 /* 3 */, 3, 3>(
+        OpenCVReprojectionError, 2, 8, 9, 3, 3>(
             new OpenCVReprojectionError(
                 marker.x,
                 marker.y)),
@@ -284,6 +285,10 @@ void EuclideanBundleCommonIntrinsics(const Tracks &tracks,
     problem.SetParameterization(&camera->R(0, 0),
                                 &rotation_parameterization);
 
+    if (bundle_constraints & BUNDLE_NO_TRANSLATION) {
+      problem.SetParameterBlockConstant(&camera->t(0));
+    }
+
     num_residuals++;
   }
   LG << "Number of residuals: " << num_residuals;
diff --git a/extern/libmv/libmv/simple_pipeline/bundle.h b/extern/libmv/libmv/simple_pipeline/bundle.h
index 55657cb2d92..573bcf4cc21 100644
--- a/extern/libmv/libmv/simple_pipeline/bundle.h
+++ b/extern/libmv/libmv/simple_pipeline/bundle.h
@@ -67,6 +67,11 @@ void EuclideanBundle(const Tracks &tracks,
     BUNDLE_FOCAL_LENGTH | BUNDLE_PRINCIPAL_POINT
     BUNDLE_FOCAL_LENGTH | BUNDLE_PRINCIPAL_POINT | BUNDLE_RADIAL
     BUNDLE_FOCAL_LENGTH | BUNDLE_PRINCIPAL_POINT | BUNDLE_RADIAL | BUNDLE_TANGENTIAL
+    BUNDLE_RADIAL
+
+    Constraints denotes which blocks to keep constant during bundling.
+    For example it is useful to keep camera translations constant
+    when bundling tripod motions.
 
     \note This assumes an outlier-free set of markers.
 
@@ -83,10 +88,15 @@ enum BundleIntrinsics {
   BUNDLE_TANGENTIAL_P2 = 32,
   BUNDLE_TANGENTIAL = 48,
 };
+enum BundleConstraints {
+	BUNDLE_NO_CONSTRAINTS = 0,
+	BUNDLE_NO_TRANSLATION = 1,
+};
 void EuclideanBundleCommonIntrinsics(const Tracks &tracks,
                                      int bundle_intrinsics,
                                      EuclideanReconstruction *reconstruction,
-                                     CameraIntrinsics *intrinsics);
+                                     CameraIntrinsics *intrinsics,
+                                     int bundle_constraints = BUNDLE_NO_CONSTRAINTS);
 
 /*!
     Refine camera poses and 3D coordinates using bundle adjustment.
diff --git a/extern/libmv/libmv/simple_pipeline/modal_solver.cc b/extern/libmv/libmv/simple_pipeline/modal_solver.cc
index bb49b30dbce..aa2fa9a8a9d 100644
--- a/extern/libmv/libmv/simple_pipeline/modal_solver.cc
+++ b/extern/libmv/libmv/simple_pipeline/modal_solver.cc
@@ -18,11 +18,14 @@
 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 // IN THE SOFTWARE.
 
+#include "libmv/simple_pipeline/modal_solver.h"
+
 #include <cstdio>
 
+#include "ceres/ceres.h"
+#include "ceres/rotation.h"
 #include "libmv/logging/logging.h"
-#include "libmv/simple_pipeline/modal_solver.h"
-#include "libmv/simple_pipeline/rigid_registration.h"
+#include "libmv/multiview/panography.h"
 
 #ifdef _MSC_VER
 #  define snprintf _snprintf
@@ -30,7 +33,8 @@
 
 namespace libmv {
 
-static void ProjectMarkerOnSphere(Marker &marker, Vec3 &X) {
+namespace {
+void ProjectMarkerOnSphere(Marker &marker, Vec3 &X) {
   X(0) = marker.x;
   X(1) = marker.y;
   X(2) = 1.0;
@@ -38,18 +42,59 @@ static void ProjectMarkerOnSphere(Marker &marker, Vec3 &X) {
   X *= 5.0 / X.norm();
 }
 
-static void ModalSolverLogProress(ProgressUpdateCallback *update_callback,
-    double progress)
+void ModalSolverLogProress(ProgressUpdateCallback *update_callback,
+                           double progress)
 {
   if (update_callback) {
     char message[256];
 
-    snprintf(message, sizeof(message), "Solving progress %d%%", (int)(progress * 100));
+    snprintf(message, sizeof(message), "Solving progress %d%%",
+             (int)(progress * 100));
 
     update_callback->invoke(progress, message);
   }
 }
 
+struct ModalReprojectionError {
+  ModalReprojectionError(Vec2 marker, Vec3 bundle)
+    : marker(marker), bundle(bundle) { }
+
+  template <typename T>
+  bool operator()(const T* quaternion,   // Rotation quaternion
+                  T* residuals) const {
+
+    T R[9];
+    ceres::QuaternionToRotation(quaternion, R);
+
+    // 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));
+
+    // Compute projective coordinates: x = RX.
+    T x[3];
+    x[0] = R[0]*X[0] + R[3]*X[1] + R[6]*X[2];
+    x[1] = R[1]*X[0] + R[4]*X[1] + R[7]*X[2];
+    x[2] = R[2]*X[0] + R[5]*X[1] + R[8]*X[2];
+
+    // Compute normalized coordinates: x /= x[2].
+    T xn = x[0] / x[2];
+    T yn = x[1] / x[2];
+
+    // The error is the difference between reprojected
+    // and observed marker position.
+    residuals[0] = xn - T(marker(0));
+    residuals[1] = yn - T(marker(1));
+
+    return true;
+  }
+
+  Vec2 marker;
+  Vec3 bundle;
+};
+}  // namespace
+
 void ModalSolver(Tracks &tracks,
                  EuclideanReconstruction *reconstruction,
                  ProgressUpdateCallback *update_callback) {
@@ -59,58 +104,132 @@ void ModalSolver(Tracks &tracks,
   LG << "Max image: " << max_image;
   LG << "Max track: " << max_track;
 
-  Mat3 R = Mat3::Identity();
+  // For minimization we're using quaternions.
+  Vec3 zero_rotation = Vec3::Zero();
+  Vec4 quaternion;
+  ceres::AngleAxisToQuaternion(&zero_rotation(0), &quaternion(0));
 
   for (int image = 0; image <= max_image; ++image) {
     vector<Marker> all_markers = tracks.MarkersInImage(image);
 
     ModalSolverLogProress(update_callback, (float) image / max_image);
 
-    // Skip empty frames without doing anything
+    // Skip empty images without doing anything.
     if (all_markers.size() == 0) {
-      LG << "Skipping frame: " << image;
+      LG << "Skipping image: " << image;
       continue;
     }
 
-    vector<Vec3> points, reference_points;
+    // STEP 1: Estimate rotation analytically.
+    Mat3 current_R;
+    ceres::QuaternionToRotation(&quaternion(0), &current_R(0, 0));
+
+    // Construct point cloud for current and previous images,
+    // using markers appear at current image for which we know
+    // 3D positions.
+    Mat x1, x2;
+    for (int i = 0; i < all_markers.size(); ++i) {
+      Marker &marker = all_markers[i];
+      EuclideanPoint *point = reconstruction->PointForTrack(marker.track);
+      if (point) {
+        Vec3 X;
+        ProjectMarkerOnSphere(marker, X);
 
-    // Cnstruct pairs of markers from current and previous image,
-    // to reproject them and find rigid transformation between
-    // previous and current image
-    for (int track = 0; track <= max_track; ++track) {
-      EuclideanPoint *point = reconstruction->PointForTrack(track);
+        int last_column = x1.cols();
+        x1.conservativeResize(3, last_column + 1);
+        x2.conservativeResize(3, last_column + 1);
 
-      if (point) {
-        Marker marker = tracks.MarkerInImageForTrack(image, track);
+        x1.col(last_column) = current_R * point->X;
+        x2.col(last_column) = X;
+      }
+    }
 
-        if (marker.image == image) {
-          Vec3 X;
+    if (x1.cols() >= 2) {
+      Mat3 delta_R;
 
-          LG << "Use track " << track << " for rigid registration between image " <<
-            image - 1 << " and " << image;
+      // Compute delta rotation matrix for two point clouds.
+      // Could be a bit confusing at first glance, but order
+      // of clouds is indeed so.
+      GetR_FixedCameraCenter(x2, x1, 1.0, &delta_R);
 
-          ProjectMarkerOnSphere(marker, X);
+      // Convert delta rotation form matrix to final image
+      // rotation stored in a quaternion
+      Vec3 delta_angle_axis;
+      ceres::RotationMatrixToAngleAxis(&delta_R(0, 0), &delta_angle_axis(0));
 
-          points.push_back(point->X);
-          reference_points.push_back(X);
-        }
+      Vec3 current_angle_axis;
+      ceres::QuaternionToAngleAxis(&quaternion(0), &current_angle_axis(0));
+
+      Vec3 angle_axis = current_angle_axis + delta_angle_axis;
+
+      ceres::AngleAxisToQuaternion(&angle_axis(0), &quaternion(0));
+
+      LG << "Analytically computed quaternion "
+         << quaternion.transpose();
+    }
+
+    // STEP 2: Refine rotation with Ceres.
+    ceres::Problem problem;
+
+    ceres::LocalParameterization* quaternion_parameterization =
+        new ceres::QuaternionParameterization;
+
+    int num_residuals = 0;
+    for (int i = 0; i < all_markers.size(); ++i) {
+      Marker &marker = all_markers[i];
+      EuclideanPoint *point = reconstruction->PointForTrack(marker.track);
+
+      if (point) {
+        problem.AddResidualBlock(new ceres::AutoDiffCostFunction<
+            ModalReprojectionError,
+            2, /* num_residuals */
+            4>(new ModalReprojectionError(Vec2(marker.x, marker.y),
+                                          point->X)),
+            NULL,
+            &quaternion(0));
+        num_residuals++;
+
+        problem.SetParameterization(&quaternion(0),
+                                    quaternion_parameterization);
       }
     }
 
-    if (points.size()) {
-      // Find rigid delta transformation to current image
-      RigidRegistration(reference_points, points, R);
+    LG << "Number of residuals: " << num_residuals;
+
+    if (num_residuals) {
+      // Configure the solve.
+      ceres::Solver::Options solver_options;
+      solver_options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
+      solver_options.max_num_iterations = 50;
+      solver_options.update_state_every_iteration = true;
+      solver_options.gradient_tolerance = 1e-36;
+      solver_options.parameter_tolerance = 1e-36;
+      solver_options.function_tolerance = 1e-36;
+
+      // Run the solve.
+      ceres::Solver::Summary summary;
+      ceres::Solve(solver_options, &problem, &summary);
+
+      LG << "Summary:\n" << summary.FullReport();
+      LG << "Refined quaternion " << quaternion.transpose();
     }
 
+    // Convert quaternion to rotation matrix.
+    Mat3 R;
+    ceres::QuaternionToRotation(&quaternion(0), &R(0, 0));
     reconstruction->InsertCamera(image, R, Vec3::Zero());
 
-    // Review if there's new tracks for which position might be reconstructed
+    // STEP 3: reproject all new markers appeared at image
+
+    // Check if there're new markers appeared on current image
+    // and reproject them on sphere to obtain 3D position/
     for (int track = 0; track <= max_track; ++track) {
       if (!reconstruction->PointForTrack(track)) {
         Marker marker = tracks.MarkerInImageForTrack(image, track);
 
         if (marker.image == image) {
-          // New track appeared on this image, project it's position onto sphere
+          // New track appeared on this image,
+          // project it's position onto sphere.
 
           LG << "Projecting track " << track << " at image " << image;