Assorted camera tracker improvements
authorSergey Sharybin <sergey.vfx@gmail.com>
Mon, 14 Nov 2011 06:41:23 +0000 (06:41 +0000)
committerSergey Sharybin <sergey.vfx@gmail.com>
Mon, 14 Nov 2011 06:41:23 +0000 (06:41 +0000)
- Add support for refining the camera's intrinsic parameters
  during a solve. Currently, refining supports only the following
  combinations of intrinsic parameters:

    f
    f, cx, cy
    f, cx, cy, k1, k2
    f, k1
    f, k1, k2

  This is not the same as autocalibration, since the user must
  still make a reasonable initial guess about the focal length and
  other parameters, whereas true autocalibration would eliminate
  the need for the user specify intrinsic parameters at all.

  However, the solver works well with only rough guesses for the
  focal length, so perhaps full autocalibation is not that
  important.

  Adding support for the last two combinations, (f, k1) and (f,
  k1, k2) required changes to the library libmv depends on for
  bundle adjustment, SSBA. These changes should get ported
  upstream not just to libmv but to SSBA as well.

- Improved the region of convergence for bundle adjustment by
  increasing the number of Levenberg-Marquardt iterations from 50
  to 500. This way, the solver is able to crawl out of the bad
  local minima it gets stuck in when changing from, for example,
  bundling k1 and k2 to just k1 and resetting k2 to 0.

- Add several new region tracker implementations. A region tracker
  is a libmv concept, which refers to tracking a template image
  pattern through frames. The impact to end users is that tracking
  should "just work better". I am reserving a more detailed
  writeup, and maybe a paper, for later.

- Other libmv tweaks, such as detecting that a tracker is headed
  outside of the image bounds.

This includes several changes made directly to the libmv extern
code rather expecting to get those changes through normal libmv
channels, because I, the libmv BDFL, decided it was faster to work
on libmv directly in Blender, then later reverse-port the libmv
changes from Blender back into libmv trunk. The interesting part
is that I added a full Levenberg-Marquardt loop to the region
tracking code, which should lead to a more stable solutions. I
also added a hacky implementation of "Efficient Second-Order
Minimization" for tracking, which works nicely. A more detailed
quantitative evaluation will follow.

Original patch by Keir, cleaned a bit by myself.

24 files changed:
extern/libmv/CMakeLists.txt
extern/libmv/libmv-capi.cpp
extern/libmv/libmv-capi.h
extern/libmv/libmv/simple_pipeline/bundle.cc
extern/libmv/libmv/simple_pipeline/bundle.h
extern/libmv/libmv/simple_pipeline/camera_intrinsics.cc
extern/libmv/libmv/simple_pipeline/camera_intrinsics.h
extern/libmv/libmv/simple_pipeline/initialize_reconstruction.cc
extern/libmv/libmv/simple_pipeline/pipeline.cc
extern/libmv/libmv/tracking/esm_region_tracker.cc [new file with mode: 0644]
extern/libmv/libmv/tracking/esm_region_tracker.h [new file with mode: 0644]
extern/libmv/libmv/tracking/klt_region_tracker.cc
extern/libmv/libmv/tracking/lmicklt_region_tracker.cc [new file with mode: 0644]
extern/libmv/libmv/tracking/lmicklt_region_tracker.h [new file with mode: 0644]
extern/libmv/libmv/tracking/pyramid_region_tracker.cc
extern/libmv/libmv/tracking/trklt_region_tracker.cc
extern/libmv/third_party/ssba/Geometry/v3d_metricbundle.cpp
extern/libmv/third_party/ssba/Geometry/v3d_metricbundle.h
release/scripts/startup/bl_ui/space_clip.py
source/blender/blenkernel/BKE_tracking.h
source/blender/blenkernel/intern/tracking.c
source/blender/editors/space_clip/tracking_ops.c
source/blender/makesdna/DNA_tracking_types.h
source/blender/makesrna/intern/rna_tracking.c

index 333791e28eb9ca541b4145a67566de5ed42a731f..cd1f572197ce988c33e3b86d9d1f9eb440e4cf50 100644 (file)
@@ -52,8 +52,10 @@ set(SRC
        libmv/image/array_nd.cc
        libmv/tracking/pyramid_region_tracker.cc
        libmv/tracking/sad.cc
+       libmv/tracking/esm_region_tracker.cc
        libmv/tracking/trklt_region_tracker.cc
        libmv/tracking/klt_region_tracker.cc
+       libmv/tracking/lmicklt_region_tracker.cc
        libmv/tracking/retrack_region_tracker.cc
        libmv/multiview/projection.cc
        libmv/multiview/conditioning.cc
@@ -99,8 +101,10 @@ set(SRC
        libmv/tracking/retrack_region_tracker.h
        libmv/tracking/sad.h
        libmv/tracking/pyramid_region_tracker.h
+       libmv/tracking/esm_region_tracker.h
        libmv/tracking/trklt_region_tracker.h
        libmv/tracking/klt_region_tracker.h
+       libmv/tracking/lmicklt_region_tracker.h
        libmv/base/id_generator.h
        libmv/base/vector.h
        libmv/base/scoped_ptr.h
index 2e007bb47b21399a578266180671e514e510ba6f..4d17aa6f5386044cfb68bd676dff58d0aba6fcb6 100644 (file)
 #include "glog/logging.h"
 #include "Math/v3d_optimization.h"
 
+#include "libmv/tracking/esm_region_tracker.h"
 #include "libmv/tracking/klt_region_tracker.h"
 #include "libmv/tracking/trklt_region_tracker.h"
+#include "libmv/tracking/lmicklt_region_tracker.h"
 #include "libmv/tracking/pyramid_region_tracker.h"
 
 #include "libmv/tracking/sad.h"
@@ -47,6 +49,7 @@
 #include "libmv/simple_pipeline/camera_intrinsics.h"
 
 #include <stdlib.h>
+#include <assert.h>
 
 #ifdef DUMP_FAILURE
 #  include <png.h>
@@ -59,7 +62,7 @@
 #define DEFAULT_WINDOW_HALFSIZE        5
 
 typedef struct libmv_RegionTracker {
-       libmv::TrkltRegionTracker *trklt_region_tracker;
+       libmv::EsmRegionTracker *klt_region_tracker;
        libmv::RegionTracker *region_tracker;
 } libmv_RegionTracker;
 
@@ -112,17 +115,17 @@ void libmv_setLoggingVerbosity(int verbosity)
 
 libmv_RegionTracker *libmv_regionTrackerNew(int max_iterations, int pyramid_level)
 {
-       libmv::TrkltRegionTracker *trklt_region_tracker = new libmv::TrkltRegionTracker;
+       libmv::EsmRegionTracker *klt_region_tracker = new libmv::EsmRegionTracker;
 
-       trklt_region_tracker->half_window_size = DEFAULT_WINDOW_HALFSIZE;
-       trklt_region_tracker->max_iterations = max_iterations;
-       trklt_region_tracker->min_determinant = 1e-4;
+       klt_region_tracker->half_window_size = DEFAULT_WINDOW_HALFSIZE;
+       klt_region_tracker->max_iterations = max_iterations;
+       klt_region_tracker->min_determinant = 1e-4;
 
        libmv::PyramidRegionTracker *region_tracker =
-               new libmv::PyramidRegionTracker(trklt_region_tracker, pyramid_level);
+               new libmv::PyramidRegionTracker(klt_region_tracker, pyramid_level);
 
        libmv_RegionTracker *configured_region_tracker = new libmv_RegionTracker;
-       configured_region_tracker->trklt_region_tracker = trklt_region_tracker;
+       configured_region_tracker->klt_region_tracker = klt_region_tracker;
        configured_region_tracker->region_tracker = region_tracker;
 
        return configured_region_tracker;
@@ -271,13 +274,13 @@ int libmv_regionTrackerTrack(libmv_RegionTracker *libmv_tracker, const float *im
                         double x1, double y1, double *x2, double *y2)
 {
        libmv::RegionTracker *region_tracker;
-       libmv::TrkltRegionTracker *trklt_region_tracker;
+       libmv::EsmRegionTracker *klt_region_tracker;
        libmv::FloatImage old_patch, new_patch;
 
-       trklt_region_tracker = libmv_tracker->trklt_region_tracker;
+       klt_region_tracker = libmv_tracker->klt_region_tracker;
        region_tracker = libmv_tracker->region_tracker;
 
-       trklt_region_tracker->half_window_size = half_window_size;
+       klt_region_tracker->half_window_size = half_window_size;
 
        floatBufToImage(ima1, width, height, &old_patch);
        floatBufToImage(ima2, width, height, &new_patch);
@@ -353,8 +356,24 @@ void libmv_tracksDestroy(libmv_Tracks *libmv_tracks)
 
 /* ************ Reconstruction solver ************ */
 
+int libmv_refineParametersAreValid(int parameters) {
+       return (parameters == (LIBMV_REFINE_FOCAL_LENGTH))         ||
+              (parameters == (LIBMV_REFINE_FOCAL_LENGTH           |
+                              LIBMV_REFINE_PRINCIPAL_POINT))      ||
+              (parameters == (LIBMV_REFINE_FOCAL_LENGTH           |
+                              LIBMV_REFINE_PRINCIPAL_POINT        |
+                              LIBMV_REFINE_RADIAL_DISTORTION_K1   |
+                              LIBMV_REFINE_RADIAL_DISTORTION_K2)) ||
+              (parameters == (LIBMV_REFINE_FOCAL_LENGTH           |
+                              LIBMV_REFINE_RADIAL_DISTORTION_K1   |
+                              LIBMV_REFINE_RADIAL_DISTORTION_K2)) ||
+              (parameters == (LIBMV_REFINE_FOCAL_LENGTH           |
+                              LIBMV_REFINE_RADIAL_DISTORTION_K1));
+}
+
+
 libmv_Reconstruction *libmv_solveReconstruction(libmv_Tracks *tracks, int keyframe1, int keyframe2,
-                       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)
 {
        /* Invert the camera intrinsics. */
        libmv::vector<libmv::Marker> markers = ((libmv::Tracks*)tracks)->AllMarkers();
@@ -378,13 +397,34 @@ libmv_Reconstruction *libmv_solveReconstruction(libmv_Tracks *tracks, int keyfra
 
        libmv::Tracks normalized_tracks(markers);
 
+       // printf("frames to init from: %d, %d\n", keyframe1, keyframe2);
        libmv::vector<libmv::Marker> keyframe_markers =
                normalized_tracks.MarkersForTracksInBothImages(keyframe1, keyframe2);
+       // printf("number of markers for init: %d\n", keyframe_markers.size());
 
        libmv::EuclideanReconstructTwoFrames(keyframe_markers, reconstruction);
        libmv::EuclideanBundle(normalized_tracks, reconstruction);
+
        libmv::EuclideanCompleteReconstruction(normalized_tracks, reconstruction);
 
+       if (refine_intrinsics) {
+               /* only a few combinations are supported but trust the caller */
+               int libmv_refine_flags = 0;
+               if (refine_intrinsics & LIBMV_REFINE_FOCAL_LENGTH) {
+                       libmv_refine_flags |= libmv::BUNDLE_FOCAL_LENGTH;
+               }
+               if (refine_intrinsics & LIBMV_REFINE_PRINCIPAL_POINT) {
+                       libmv_refine_flags |= libmv::BUNDLE_PRINCIPAL_POINT;
+               }
+               if (refine_intrinsics & LIBMV_REFINE_RADIAL_DISTORTION_K1) {
+                       libmv_refine_flags |= libmv::BUNDLE_RADIAL_K1;
+               }
+               if (refine_intrinsics & LIBMV_REFINE_RADIAL_DISTORTION_K2) {
+                       libmv_refine_flags |= libmv::BUNDLE_RADIAL_K2;
+               }
+               libmv::EuclideanBundleCommonIntrinsics(*(libmv::Tracks *)tracks, libmv_refine_flags, reconstruction, intrinsics);
+       }
+
        libmv_reconstruction->tracks = *(libmv::Tracks *)tracks;
        libmv_reconstruction->error = libmv::EuclideanReprojectionError(*(libmv::Tracks *)tracks, *reconstruction, *intrinsics);
 
@@ -608,6 +648,10 @@ void libmv_destroyFeatures(struct libmv_Features *libmv_features)
 
 /* ************ camera intrinsics ************ */
 
+struct libmv_CameraIntrinsics *libmv_ReconstructionExtractIntrinsics(struct libmv_Reconstruction *libmv_Reconstruction) {
+  return (struct libmv_CameraIntrinsics *)&libmv_Reconstruction->intrinsics;
+}
+
 struct libmv_CameraIntrinsics *libmv_CameraIntrinsicsNew(double focal_length, double principal_x, double principal_y,
                        double k1, double k2, double k3, int width, int height)
 {
@@ -654,6 +698,16 @@ void libmv_CameraIntrinsicsUpdate(struct libmv_CameraIntrinsics *libmvIntrinsics
                intrinsics->SetImageSize(width, height);
 }
 
+void libmv_CameraIntrinsicsExtract(struct libmv_CameraIntrinsics *libmvIntrinsics, double *focal_length,
+                       double *principal_x, double *principal_y, double *k1, double *k2, double *k3, int *width, int *height) {
+       libmv::CameraIntrinsics *intrinsics= (libmv::CameraIntrinsics *) libmvIntrinsics;
+       *focal_length = intrinsics->focal_length();
+       *principal_x = intrinsics->principal_point_x();
+       *principal_y = intrinsics->principal_point_y();
+       *k1 = intrinsics->k1();
+       *k2 = intrinsics->k2();
+}
+
 void libmv_CameraIntrinsicsUndistortByte(struct libmv_CameraIntrinsics *libmvIntrinsics,
                        unsigned char *src, unsigned char *dst, int width, int height, float overscan, int channels)
 {
index b71a66b73a683fa6204d6fac98fefb6056c5aa2d..d378afc5ea8ebb96da1df7ae386dcd614390b9c1 100644 (file)
@@ -61,8 +61,14 @@ void libmv_tracksInsert(struct libmv_Tracks *libmv_tracks, int image, int track,
 void libmv_tracksDestroy(struct libmv_Tracks *libmv_tracks);
 
 /* Reconstruction solver */
+#define LIBMV_REFINE_FOCAL_LENGTH      (1<<0)
+#define LIBMV_REFINE_PRINCIPAL_POINT   (1<<1)
+#define LIBMV_REFINE_RADIAL_DISTORTION_K1 (1<<2)
+#define LIBMV_REFINE_RADIAL_DISTORTION_K2 (1<<4)
+int libmv_refineParametersAreValid(int parameters);
+
 struct libmv_Reconstruction *libmv_solveReconstruction(struct libmv_Tracks *tracks, int keyframe1, int keyframe2,
-                       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);
 int libmv_reporojectionPointForTrack(struct libmv_Reconstruction *libmv_reconstruction, int track, double pos[3]);
 double libmv_reporojectionErrorForTrack(struct libmv_Reconstruction *libmv_reconstruction, int track);
 double libmv_reporojectionErrorForImage(struct libmv_Reconstruction *libmv_reconstruction, int image);
@@ -80,18 +86,21 @@ void libmv_getFeature(struct libmv_Features *libmv_features, int number, double
 void libmv_destroyFeatures(struct libmv_Features *libmv_features);
 
 /* camera intrinsics */
+struct libmv_CameraIntrinsics *libmv_ReconstructionExtractIntrinsics(struct libmv_Reconstruction *libmv_Reconstruction);
+
 struct libmv_CameraIntrinsics *libmv_CameraIntrinsicsNew(double focal_length, double principal_x, double principal_y,
                        double k1, double k2, double k3, int width, int height);
 
 struct libmv_CameraIntrinsics *libmv_CameraIntrinsicsCopy(struct libmv_CameraIntrinsics *libmvIntrinsics);
 
-struct libmv_CameraIntrinsics *libmv_CameraIntrinsicsCopy(struct libmv_CameraIntrinsics *libmvIntrinsics);
-
 void libmv_CameraIntrinsicsDestroy(struct libmv_CameraIntrinsics *libmvIntrinsics);
 
 void libmv_CameraIntrinsicsUpdate(struct libmv_CameraIntrinsics *libmvIntrinsics, double focal_length,
                        double principal_x, double principal_y, double k1, double k2, double k3, int width, int height);
 
+void libmv_CameraIntrinsicsExtract(struct libmv_CameraIntrinsics *libmvIntrinsics, double *focal_length,
+                       double *principal_x, double *principal_y, double *k1, double *k2, double *k3, int *width, int *height);
+
 void libmv_CameraIntrinsicsUndistortByte(struct libmv_CameraIntrinsics *libmvIntrinsics,
                        unsigned char *src, unsigned char *dst, int width, int height, float overscan, int channels);
 
index cb8822dcf44db82ba5014046be59cefa28a6d8f8..d382cd5a4fc5edd42357d88d0db715e7514342ee 100644 (file)
@@ -27,6 +27,8 @@
 #include "libmv/multiview/fundamental.h"
 #include "libmv/multiview/projection.h"
 #include "libmv/numeric/numeric.h"
+#include "libmv/simple_pipeline/camera_intrinsics.h"
+#include "libmv/simple_pipeline/bundle.h"
 #include "libmv/simple_pipeline/reconstruction.h"
 #include "libmv/simple_pipeline/tracks.h"
 #include "third_party/ssba/Geometry/v3d_cameramatrix.h"
@@ -38,6 +40,18 @@ namespace libmv {
 
 void EuclideanBundle(const Tracks &tracks,
                      EuclideanReconstruction *reconstruction) {
+  CameraIntrinsics intrinsics;
+  EuclideanBundleCommonIntrinsics(tracks,
+                                  BUNDLE_NO_INTRINSICS,
+                                  reconstruction,
+                                  &intrinsics);
+}
+
+void EuclideanBundleCommonIntrinsics(const Tracks &tracks,
+                                     int bundle_intrinsics,
+                                     EuclideanReconstruction *reconstruction,
+                                     CameraIntrinsics *intrinsics) {
+  LG << "Original intrinsics: " << *intrinsics;
   vector<Marker> markers = tracks.AllMarkers();
 
   // "index" in this context is the index that V3D's optimizer will see. The
@@ -69,18 +83,20 @@ void EuclideanBundle(const Tracks &tracks,
     }
   }
 
-  // Make a V3D identity matrix, needed in a few places for K, since this
-  // assumes a calibrated setup.
-  V3D::Matrix3x3d identity3x3;
-  identity3x3[0][0] = 1.0;
-  identity3x3[0][1] = 0.0;
-  identity3x3[0][2] = 0.0;
-  identity3x3[1][0] = 0.0;
-  identity3x3[1][1] = 1.0;
-  identity3x3[1][2] = 0.0;
-  identity3x3[2][0] = 0.0;
-  identity3x3[2][1] = 0.0;
-  identity3x3[2][2] = 1.0;
+  // Convert libmv's K matrix to V3d's K matrix.
+  V3D::Matrix3x3d v3d_K;
+  for (int i = 0; i < 3; ++i) {
+    for (int j = 0; j < 3; ++j) {
+      v3d_K[i][j] = intrinsics->K()(i, j);
+    }
+  }
+
+  // Convert libmv's distortion to v3d distortion.
+  V3D::StdDistortionFunction v3d_distortion;
+  v3d_distortion.k1 = intrinsics->k1();
+  v3d_distortion.k2 = intrinsics->k2();
+  v3d_distortion.p1 = intrinsics->p1();
+  v3d_distortion.p2 = intrinsics->p2();
 
   // Convert libmv's cameras to V3D's cameras.
   std::vector<V3D::CameraMatrix> v3d_cameras(index_to_camera.size());
@@ -98,7 +114,7 @@ void EuclideanBundle(const Tracks &tracks,
       }
       t[i] = t_libmv(i);
     }
-    v3d_cameras[k].setIntrinsic(identity3x3);
+    v3d_cameras[k].setIntrinsic(v3d_K);
     v3d_cameras[k].setRotation(R);
     v3d_cameras[k].setTranslation(t);
   }
@@ -134,27 +150,81 @@ void EuclideanBundle(const Tracks &tracks,
   }
   LG << "Number of residuals: " << num_residuals;
   
-  // This is calibrated reconstruction, so use zero distortion.
-  V3D::StdDistortionFunction v3d_distortion;
-  v3d_distortion.k1 = 0;
-  v3d_distortion.k2 = 0;
-  v3d_distortion.p1 = 0;
-  v3d_distortion.p2 = 0;
+  // Convert from libmv's specification for which intrinsics to bundle to V3D's.
+  int v3d_bundle_intrinsics;
+  if (bundle_intrinsics == BUNDLE_NO_INTRINSICS) {
+    LG << "Bundling only camera positions.";
+    v3d_bundle_intrinsics = V3D::FULL_BUNDLE_METRIC;
+  } else if (bundle_intrinsics == BUNDLE_FOCAL_LENGTH) {
+    LG << "Bundling f.";
+    v3d_bundle_intrinsics = V3D::FULL_BUNDLE_FOCAL_LENGTH;
+  } else if (bundle_intrinsics == (BUNDLE_FOCAL_LENGTH |
+                                   BUNDLE_PRINCIPAL_POINT)) {
+    LG << "Bundling f, px, py.";
+    v3d_bundle_intrinsics = V3D::FULL_BUNDLE_FOCAL_LENGTH_PP;
+  } else if (bundle_intrinsics == (BUNDLE_FOCAL_LENGTH |
+                                   BUNDLE_PRINCIPAL_POINT |
+                                   BUNDLE_RADIAL)) {
+    LG << "Bundling f, px, py, k1, k2.";
+    v3d_bundle_intrinsics = V3D::FULL_BUNDLE_RADIAL;
+  } else if (bundle_intrinsics == (BUNDLE_FOCAL_LENGTH |
+                                   BUNDLE_PRINCIPAL_POINT |
+                                   BUNDLE_RADIAL |
+                                   BUNDLE_TANGENTIAL)) {
+    LG << "Bundling f, px, py, k1, k2, p1, p2.";
+    v3d_bundle_intrinsics = V3D::FULL_BUNDLE_RADIAL_TANGENTIAL;
+  } else if (bundle_intrinsics == (BUNDLE_FOCAL_LENGTH |
+                                   BUNDLE_RADIAL |
+                                   BUNDLE_TANGENTIAL)) {
+    LG << "Bundling f, px, py, k1, k2, p1, p2.";
+    v3d_bundle_intrinsics = V3D::FULL_BUNDLE_RADIAL_TANGENTIAL;
+  } else if (bundle_intrinsics == (BUNDLE_FOCAL_LENGTH |
+                                   BUNDLE_RADIAL)) {
+    LG << "Bundling f, k1, k2.";
+    v3d_bundle_intrinsics = V3D::FULL_BUNDLE_FOCAL_AND_RADIAL;
+  } else if (bundle_intrinsics == (BUNDLE_FOCAL_LENGTH |
+                                   BUNDLE_RADIAL_K1)) {
+    LG << "Bundling f, k1.";
+    v3d_bundle_intrinsics = V3D::FULL_BUNDLE_FOCAL_AND_RADIAL_K1;
+  } else {
+    LOG(FATAL) << "Unsupported bundle combination.";
+  }
+
+  // Ignore any outliers; assume supervised tracking.
+  double v3d_inlier_threshold = 500000.0;
 
   // Finally, run the bundle adjustment.
-  double const inlierThreshold = 500000.0;
-  V3D::CommonInternalsMetricBundleOptimizer opt(V3D::FULL_BUNDLE_METRIC,
-                                                inlierThreshold,
-                                                identity3x3,
+  V3D::CommonInternalsMetricBundleOptimizer opt(v3d_bundle_intrinsics,
+                                                v3d_inlier_threshold,
+                                                v3d_K,
                                                 v3d_distortion,
                                                 v3d_cameras,
                                                 v3d_points,
                                                 v3d_measurements,
                                                 v3d_camera_for_measurement,
                                                 v3d_point_for_measurement);
-  opt.maxIterations = 50;
+  opt.maxIterations = 500;
   opt.minimize();
-  LG << "Bundle status: " << opt.status;
+  if (opt.status == V3D::LEVENBERG_OPTIMIZER_TIMEOUT) {
+    LG << "Bundle status: Timed out.";
+  } else if (opt.status == V3D::LEVENBERG_OPTIMIZER_SMALL_UPDATE) {
+    LG << "Bundle status: Small update.";
+  } else if (opt.status == V3D::LEVENBERG_OPTIMIZER_CONVERGED) {
+    LG << "Bundle status: Converged.";
+  }
+
+  // Convert V3D's K matrix back to libmv's K matrix.
+  Mat3 K;
+  for (int i = 0; i < 3; ++i) {
+    for (int j = 0; j < 3; ++j) {
+      K(i, j) = v3d_K[i][j];
+    }
+  }
+  intrinsics->SetK(K);
+
+  // Convert V3D's distortion back to libmv's distortion.
+  intrinsics->SetRadialDistortion(v3d_distortion.k1, v3d_distortion.k2, 0.0);
+  intrinsics->SetTangentialDistortion(v3d_distortion.p1, v3d_distortion.p2);
 
   // Convert V3D's cameras back to libmv's cameras.
   for (int k = 0; k < num_cameras; k++) {
@@ -173,6 +243,7 @@ void EuclideanBundle(const Tracks &tracks,
       index_to_point[k]->X(i) = v3d_points[k][i];
     }
   }
+  LG << "Final intrinsics: " << *intrinsics;
 }
 
 void ProjectiveBundle(const Tracks & /*tracks*/,
index c7fb2a79607ea29a2c0693c36a585e68cd66dbdd..55657cb2d927dbdea48e3584b2f487488b79b125 100644 (file)
@@ -23,6 +23,7 @@
 
 namespace libmv {
 
+class CameraIntrinsics;
 class EuclideanReconstruction;
 class ProjectiveReconstruction;
 class Tracks;
@@ -47,6 +48,46 @@ class Tracks;
 void EuclideanBundle(const Tracks &tracks,
                      EuclideanReconstruction *reconstruction);
 
+/*!
+    Refine camera poses and 3D coordinates using bundle adjustment.
+
+    This routine adjusts all cameras positions, points, and the camera
+    intrinsics (assumed common across all images) in \a *reconstruction. This
+    assumes a full observation for reconstructed tracks; this implies that if
+    there is a reconstructed 3D point (a bundle) for a track, then all markers
+    for that track will be included in the minimization. \a tracks should
+    contain markers used in the initial reconstruction.
+
+    The cameras, bundles, and intrinsics are refined in-place.
+
+    The only supported combinations of bundle parameters are:
+
+    BUNDLE_NO_INTRINSICS
+    BUNDLE_FOCAL_LENGTH
+    BUNDLE_FOCAL_LENGTH | BUNDLE_PRINCIPAL_POINT
+    BUNDLE_FOCAL_LENGTH | BUNDLE_PRINCIPAL_POINT | BUNDLE_RADIAL
+    BUNDLE_FOCAL_LENGTH | BUNDLE_PRINCIPAL_POINT | BUNDLE_RADIAL | BUNDLE_TANGENTIAL
+
+    \note This assumes an outlier-free set of markers.
+
+    \sa EuclideanResect, EuclideanIntersect, EuclideanReconstructTwoFrames
+*/
+enum BundleIntrinsics {
+  BUNDLE_NO_INTRINSICS = 0,
+  BUNDLE_FOCAL_LENGTH = 1,
+  BUNDLE_PRINCIPAL_POINT = 2,
+  BUNDLE_RADIAL_K1 = 4,
+  BUNDLE_RADIAL_K2 = 8,
+  BUNDLE_RADIAL = 12,
+  BUNDLE_TANGENTIAL_P1 = 16,
+  BUNDLE_TANGENTIAL_P2 = 32,
+  BUNDLE_TANGENTIAL = 48,
+};
+void EuclideanBundleCommonIntrinsics(const Tracks &tracks,
+                                     int bundle_intrinsics,
+                                     EuclideanReconstruction *reconstruction,
+                                     CameraIntrinsics *intrinsics);
+
 /*!
     Refine camera poses and 3D coordinates using bundle adjustment.
 
index 366129dd3d225b6ec077415a27654a29fd554090..917f80e6926b42a446c9687a7c51b9b1af2fb9bd 100644 (file)
@@ -348,4 +348,26 @@ void CameraIntrinsics::Undistort(const unsigned char* src, unsigned char* dst, i
   //else assert("channels must be between 1 and 4");
 }
 
+std::ostream& operator <<(std::ostream &os,
+                          const CameraIntrinsics &intrinsics) {
+  if (intrinsics.focal_length_x() == intrinsics.focal_length_x()) {
+    os << "f=" << intrinsics.focal_length();
+  } else {
+    os <<  "fx=" << intrinsics.focal_length_x()
+       << " fy=" << intrinsics.focal_length_y();
+  }
+  os << " cx=" << intrinsics.principal_point_x()
+     << " cy=" << intrinsics.principal_point_y()
+     << " w=" << intrinsics.image_width()
+     << " h=" << intrinsics.image_height();
+
+  if (intrinsics.k1() != 0.0) { os << " k1=" << intrinsics.k1(); }
+  if (intrinsics.k2() != 0.0) { os << " k2=" << intrinsics.k2(); }
+  if (intrinsics.k3() != 0.0) { os << " k3=" << intrinsics.k3(); }
+  if (intrinsics.p1() != 0.0) { os << " p1=" << intrinsics.p1(); }
+  if (intrinsics.p2() != 0.0) { os << " p2=" << intrinsics.p2(); }
+
+  return os;
+}
+
 }  // namespace libmv
index f4bf903c36cea8def03213e608350d56eb3d41d8..e0556674ad5e1b35e1ebd9ff688a6cf444b5a441 100644 (file)
 #ifndef LIBMV_SIMPLE_PIPELINE_CAMERA_INTRINSICS_H_
 #define LIBMV_SIMPLE_PIPELINE_CAMERA_INTRINSICS_H_
 
+#include <iostream>
+#include <string>
+
 #include <Eigen/Core>
-typedef Eigen::Matrix<double, 3, 3> Mat3;
 
 namespace libmv {
 
+typedef Eigen::Matrix<double, 3, 3> Mat3;
+
 struct Grid;
 
 class CameraIntrinsics {
@@ -35,7 +39,6 @@ class CameraIntrinsics {
   ~CameraIntrinsics();
 
   const Mat3 &K()                 const { return K_;            }
-  // FIXME(MatthiasF): these should be CamelCase methods
   double      focal_length()      const { return K_(0, 0);      }
   double      focal_length_x()    const { return K_(0, 0);      }
   double      focal_length_y()    const { return K_(1, 1);      }
@@ -55,8 +58,10 @@ class CameraIntrinsics {
   /// Set both x and y focal length in pixels.
   void SetFocalLength(double focal_x, double focal_y);
 
+  /// Set principal point in pixels.
   void SetPrincipalPoint(double cx, double cy);
 
+  /// Set the image size in pixels.
   void SetImageSize(int width, int height);
 
   void SetRadialDistortion(double k1, double k2, double k3 = 0);
@@ -92,6 +97,7 @@ class CameraIntrinsics {
   */
   void Distort(const float* src, float* dst,
                int width, int height, double overscan, int channels);
+
   /*!
       Distort an image using the current camera instrinsics
 
@@ -102,6 +108,7 @@ class CameraIntrinsics {
   */
   void Distort(const unsigned char* src, unsigned char* dst,
                int width, int height, double overscan, int channels);
+
   /*!
       Undistort an image using the current camera instrinsics
 
@@ -112,6 +119,7 @@ class CameraIntrinsics {
   */
   void Undistort(const float* src, float* dst,
                  int width, int height, double overscan, int channels);
+
   /*!
       Undistort an image using the current camera instrinsics
 
@@ -147,6 +155,10 @@ class CameraIntrinsics {
   struct Grid *undistort_;
 };
 
+/// A human-readable representation of the camera intrinsic parameters.
+std::ostream& operator <<(std::ostream &os,
+                          const CameraIntrinsics &intrinsics);
+
 }  // namespace libmv
 
 #endif  // LIBMV_SIMPLE_PIPELINE_CAMERA_INTRINSICS_H_
index 0597f09f72825ad972a44a873308d9bb4d80da18..77fe2a530c40f615868f05578dc5121116b4fd69 100644 (file)
@@ -67,6 +67,7 @@ void GetImagesInMarkers(const vector<Marker> &markers,
 bool EuclideanReconstructTwoFrames(const vector<Marker> &markers,
                                    EuclideanReconstruction *reconstruction) {
   if (markers.size() < 16) {
+    LG << "Not enough markers to initialize from two frames: " << markers.size();
     return false;
   }
 
@@ -105,6 +106,7 @@ bool EuclideanReconstructTwoFrames(const vector<Marker> &markers,
                                             K, x1.col(0),
                                             K, x2.col(0),
                                             &R, &t)) {
+    LG << "Failed to compute R and t from E and K.";
     return false;
   }
 
index 818c24cb5e7fdb29ab614490f1488836edb76341..9512a41c00f33cd6d1a53fbfc96d73aca015be2e 100644 (file)
@@ -262,7 +262,6 @@ double InternalReprojectionError(const Tracks &image_tracks,
            ex,
            ey,
            sqrt(ex*ex + ey*ey));
-    //LG << line;
     total_error += sqrt(ex*ex + ey*ey);
   }
   LG << "Skipped " << num_skipped << " markers.";
diff --git a/extern/libmv/libmv/tracking/esm_region_tracker.cc b/extern/libmv/libmv/tracking/esm_region_tracker.cc
new file mode 100644 (file)
index 0000000..2e5c255
--- /dev/null
@@ -0,0 +1,288 @@
+// Copyright (c) 2007, 2008, 2009, 2011 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.
+
+#include "libmv/tracking/esm_region_tracker.h"
+
+#include "libmv/logging/logging.h"
+#include "libmv/image/image.h"
+#include "libmv/image/convolve.h"
+#include "libmv/image/sample.h"
+#include "libmv/numeric/numeric.h"
+
+namespace libmv {
+
+// TODO(keir): Reduce duplication between here and the other region trackers.
+bool RegionIsInBounds(const FloatImage &image1,
+                      double x, double y,
+                      int half_window_size) {
+  // Check the minimum coordinates.
+  int min_x = floor(x) - half_window_size - 1;
+  int min_y = floor(y) - half_window_size - 1;
+  if (min_x < 0.0 ||
+      min_y < 0.0) {
+    return false;
+  }
+
+  // Check the maximum coordinates.
+  int max_x = ceil(x) + half_window_size + 1;
+  int max_y = ceil(y) + half_window_size + 1;
+  if (max_x > image1.cols() ||
+      max_y > image1.rows()) {
+    return false;
+  }
+
+  // Ok, we're good.
+  return true;
+}
+
+// Sample a region centered at x,y in image with size extending by half_width
+// from x,y. Channels specifies the number of channels to sample from.
+void SamplePattern(const FloatImage &image,
+                   double x, double y,
+                   int half_width,
+                   int channels,
+                   FloatImage *sampled) {
+  sampled->Resize(2 * half_width + 1, 2 * half_width + 1, channels);
+  for (int r = -half_width; r <= half_width; ++r) {
+    for (int c = -half_width; c <= half_width; ++c) {
+      for (int i = 0; i < channels; ++i) {
+        (*sampled)(r + half_width, c + half_width, i) =
+            SampleLinear(image, y + r, x + c, i);
+      }
+    }
+  }
+}
+
+// Estimate "reasonable" error by computing autocorrelation for a small shift.
+// TODO(keir): Add a facility for 
+double EstimateReasonableError(const FloatImage &image,
+                               double x, double y,
+                               int half_width) {
+  double error = 0.0;
+  for (int r = -half_width; r <= half_width; ++r) {
+    for (int c = -half_width; c <= half_width; ++c) {
+      double s = SampleLinear(image, y + r, x + c, 0);
+      double e1 = SampleLinear(image, y + r + 0.5, x + c, 0) - s;
+      double e2 = SampleLinear(image, y + r, x + c + 0.5, 0) - s;
+      error += e1*e1 + e2*e2;
+    }
+  }
+  // XXX hack
+  return error / 2.0 * 16.0;
+}
+
+// This is implemented from "Lukas and Kanade 20 years on: Part 1. Page 42,
+// figure 14: the Levenberg-Marquardt-Inverse Compositional Algorithm".
+bool EsmRegionTracker::Track(const FloatImage &image1,
+                             const FloatImage &image2,
+                             double  x1, double  y1,
+                             double *x2, double *y2) const {
+  if (!RegionIsInBounds(image1, x1, y1, half_window_size)) {
+    LG << "Fell out of image1's window with x1=" << x1 << ", y1=" << y1
+       << ", hw=" << half_window_size << ".";
+    return false;
+  }
+  
+  int width = 2 * half_window_size + 1;
+
+  // TODO(keir): Avoid recomputing gradients for e.g. the pyramid tracker.
+  Array3Df image_and_gradient1;
+  Array3Df image_and_gradient2;
+  BlurredImageAndDerivativesChannels(image1, sigma, &image_and_gradient1);
+  BlurredImageAndDerivativesChannels(image2, sigma, &image_and_gradient2);
+
+  // Step -1: Resample the template (image1) since it is not pixel aligned.
+  //
+  // Take a sample of the gradient of the pattern area of image1 at the
+  // subpixel position x1, x2. This is reused for each iteration, so
+  // precomputing it saves time.
+  Array3Df image_and_gradient1_sampled;
+  SamplePattern(image_and_gradient1, x1, y1, half_window_size, 3,
+                &image_and_gradient1_sampled);
+
+  // Step 0: Initialize delta = 0.01.
+  // 
+  // Ignored for my "normal" LM loop.
+
+  double reasonable_error =
+      EstimateReasonableError(image1, x1, y1, half_window_size);
+
+  // Step 1: Warp I with W(x, p) to compute I(W(x; p).
+  //
+  // Use two images for accepting / rejecting updates.
+  // XXX is this necessary?
+  int current_image = 0, new_image = 1;
+  Array3Df image_and_gradient2_sampled[2];
+  SamplePattern(image_and_gradient2, *x2, *y2, half_window_size, 3,
+                &image_and_gradient2_sampled[current_image]);
+
+  // Step 2: Compute the squared error I - J.
+  double error = 0;
+  for (int r = 0; r < width; ++r) {
+    for (int c = 0; c < width; ++c) {
+      double e = image_and_gradient1_sampled(r, c, 0) -
+                 image_and_gradient2_sampled[current_image](r, c, 0);
+      error += e*e;
+    }
+  }
+
+  // Step 3: Evaluate the gradient of the template.
+  //
+  // This is done above when sampling the template (step -1).
+
+  // Step 4: Evaluate the jacobian dw/dp at (x; 0).
+  //
+  // The jacobian between dx,dy and the warp is constant 2x2 identity, so it
+  // doesn't have to get computed. The Gauss-Newton Hessian matrix computation
+  // below would normally have to include a multiply by the jacobian.
+
+  // Step 5: Compute the steepest descent images of the template.
+  //
+  // Since the jacobian of the position w.r.t. the sampled template position is
+  // the identity, the steepest descent images are the same as the gradient.
+
+  // Step 6: Compute the Gauss-Newton Hessian for the template (image1).
+  //
+  // This could get rolled into the previous loop, but split it for now for
+  // clarity.
+  Mat2 H_image1 = Mat2::Zero();
+  for (int r = 0; r < width; ++r) {
+    for (int c = 0; c < width; ++c) {
+      Vec2 g(image_and_gradient1_sampled(r, c, 1),
+             image_and_gradient1_sampled(r, c, 2));
+      H_image1 += g * g.transpose();
+    }
+  }
+
+  double tau = 1e-4, eps1, eps2, eps3;
+  eps1 = eps2 = eps3 = 1e-15;
+
+  double mu = tau * std::max(H_image1(0, 0), H_image1(1, 1));
+  double nu = M_E;
+
+  int i;
+  for (i = 0; i < max_iterations; ++i) {
+    // Check that the entire image patch is within the bounds of the images.
+    if (!RegionIsInBounds(image2, *x2, *y2, half_window_size)) {
+      LG << "Fell out of image2's window with x2=" << *x2 << ", y2=" << *y2
+         << ", hw=" << half_window_size << ".";
+      return false;
+    }
+
+    Mat2 H = Mat2::Zero();
+    for (int r = 0; r < width; ++r) {
+      for (int c = 0; c < width; ++c) {
+        Vec2 g1(image_and_gradient1_sampled(r, c, 1),
+                image_and_gradient1_sampled(r, c, 2));
+        Vec2 g2(image_and_gradient2_sampled[current_image](r, c, 1),
+                image_and_gradient2_sampled[current_image](r, c, 2));
+        Vec2 g = g1 + g2; // Should be / 2.0, but do that outside the loop.
+        H += g * g.transpose();
+      }
+    }
+    H /= 4.0;
+
+    // Step 7: Compute z
+    Vec2 z = Vec2::Zero();
+    for (int r = 0; r < width; ++r) {
+      for (int c = 0; c < width; ++c) {
+        double e = image_and_gradient2_sampled[current_image](r, c, 0) -
+                   image_and_gradient1_sampled(r, c, 0);
+        z(0) += image_and_gradient1_sampled(r, c, 1) * e;
+        z(1) += image_and_gradient1_sampled(r, c, 2) * e;
+        z(0) += image_and_gradient2_sampled[current_image](r, c, 1) * e;
+        z(1) += image_and_gradient2_sampled[current_image](r, c, 2) * e;
+      }
+    }
+    z /= 2.0;
+
+    // Step 8: Compute Hlm and (dx,dy)
+    Mat2 diag  = H.diagonal().asDiagonal();
+    diag *= mu;
+    Mat2 Hlm = H + diag;
+    Vec2 d = Hlm.lu().solve(z);
+
+    // TODO(keir): Use the usual LM termination and update criterion instead of
+    // this hacky version from the LK 20 years on paper.
+    LG << "x=" << *x2 << ", y=" << *y2 << ", dx=" << d[0] << ", dy=" << d[1]
+       << ", mu=" << mu << ", nu=" << nu;
+
+    // Step 9: Update the warp; W(x; p) <-- W(x;p) compose W(x, dp)^-1
+    double new_x2 = *x2 - d[0];
+    double new_y2 = *y2 - d[1];
+
+    // Step 9.1: Sample the image at the new position.
+    SamplePattern(image_and_gradient2, new_x2, new_y2, half_window_size, 3,
+                  &image_and_gradient2_sampled[new_image]);
+
+    // Step 9.2: Compute the new error.
+    // TODO(keir): Eliminate duplication with above code.
+    double new_error = 0;
+    for (int r = 0; r < width; ++r) {
+      for (int c = 0; c < width; ++c) {
+        double e = image_and_gradient1_sampled(r, c, 0) -
+                   image_and_gradient2_sampled[new_image](r, c, 0);
+        new_error += e*e;
+      }
+    }
+    //LG << "Old error: " << error << ", new error: " << new_error;
+
+    double rho = (error - new_error) / (d.transpose() * (mu * d + z));
+
+    // Step 10: Accept or reject step.
+    if (rho <= 0) {
+      // This was a bad step, so don't update.
+      mu *= nu;
+
+      // The standard Levenberg-Marquardt update multiplies nu by 2, but
+      // instead chose e. I chose a constant greater than 2 since in typical
+      // tracking examples, I saw that once updates started failing they should
+      // ramp towards steepest descent faster. This has no basis in theory, but
+      // appears to lead to faster tracking overall.
+      nu *= M_E;
+      LG << "Error increased, so reject update.";
+    } else {
+      // The step was good, so update.
+      *x2 = new_x2;
+      *y2 = new_y2;
+      std::swap(new_image, current_image);
+      error = new_error;
+
+      mu *= std::max(1/3., 1 - pow(2*rho - 1, 3));
+      nu = M_E;  // See above for why to use e.
+    }
+
+    // If the step was accepted, then check for termination.
+    if (d.squaredNorm() < min_update_squared_distance) {
+      if (new_error > reasonable_error) {
+        LG << "Update size shrank but reasonable error ("
+           << reasonable_error << ") not achieved; failing.";
+        return true; // XXX
+      }
+      LG << "Successful track in " << (i + 1) << " iterations.";
+      return true;
+    }
+  }
+  // Getting here means we hit max iterations, so tracking failed.
+  LG << "Too many iterations; max is set to " << max_iterations << ".";
+  return false;
+}
+
+}  // namespace libmv
diff --git a/extern/libmv/libmv/tracking/esm_region_tracker.h b/extern/libmv/libmv/tracking/esm_region_tracker.h
new file mode 100644 (file)
index 0000000..c634172
--- /dev/null
@@ -0,0 +1,61 @@
+// Copyright (c) 2007, 2008, 2009, 2011 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_REGION_TRACKING_LMICKLT_REGION_TRACKER_H_
+#define LIBMV_REGION_TRACKING_LMICKLT_REGION_TRACKER_H_
+
+#include "libmv/image/image.h"
+#include "libmv/tracking/region_tracker.h"
+
+namespace libmv {
+
+/*!
+    A translation-only Efficient Second-order Minimization ("ESM") tracker
+
+    Based on the paper "Real-time image-based trackiing of planes using
+    Efficient Second-order Minimization"
+*/
+struct EsmRegionTracker : public RegionTracker {
+  EsmRegionTracker()
+      : half_window_size(4),
+        max_iterations(16),
+        min_determinant(1e-6),
+        min_update_squared_distance(1e-4),
+        sigma(0.9) {}
+  
+  virtual ~EsmRegionTracker() {}
+
+  // Tracker interface.
+  virtual bool Track(const FloatImage &image1,
+                     const FloatImage &image2,
+                     double  x1, double  y1,
+                     double *x2, double *y2) const;
+
+  // No point in creating getters or setters.
+  int half_window_size;
+  int max_iterations;
+  double min_determinant;
+  double min_update_squared_distance;
+  double sigma;
+};
+
+}  // namespace libmv
+
+#endif  // LIBMV_REGION_TRACKING_LMICKLT_REGION_TRACKER_H_
index 299077be155622b6685b7e4d8ec63138bfd7f2a4..78ce0be603c3f84f705b180d568093ac86987d37 100644 (file)
@@ -63,24 +63,26 @@ static void ComputeTrackingEquation(const Array3Df &image_and_gradient1,
   }
 }
 
-// Solve the tracking equation
-//
-//   [gxx gxy] [dx] = [ex]
-//   [gxy gyy] [dy] = [ey]
-//
-// for dx and dy.  Borrowed from Stan Birchfield's KLT implementation.
-static bool SolveTrackingEquation(float gxx, float gxy, float gyy,
-                                  float ex, float ey,
-                                  float min_determinant,
-                                  float *dx, float *dy) {
-  float det = gxx * gyy - gxy * gxy;
-  if (det < min_determinant) {
-    *dx = 0;
-    *dy = 0;
+bool RegionIsInBounds(const FloatImage &image1,
+                      double x, double y,
+                      int half_window_size) {
+  // Check the minimum coordinates.
+  int min_x = floor(x) - half_window_size - 1;
+  int min_y = floor(y) - half_window_size - 1;
+  if (min_x < 0.0 ||
+      min_y < 0.0) {
+    return false;
+  }
+
+  // Check the maximum coordinates.
+  int max_x = ceil(x) + half_window_size + 1;
+  int max_y = ceil(y) + half_window_size + 1;
+  if (max_x > image1.cols() ||
+      max_y > image1.rows()) {
     return false;
   }
-  *dx = (gyy * ex - gxy * ey) / det;
-  *dy = (gxx * ey - gxy * ex) / det;
+
+  // Ok, we're good.
   return true;
 }
 
@@ -88,6 +90,12 @@ bool KltRegionTracker::Track(const FloatImage &image1,
                              const FloatImage &image2,
                              double  x1, double  y1,
                              double *x2, double *y2) const {
+  if (!RegionIsInBounds(image1, x1, y1, half_window_size)) {
+    LG << "Fell out of image1's window with x1=" << x1 << ", y1=" << y1
+       << ", hw=" << half_window_size << ".";
+    return false;
+  }
+
   Array3Df image_and_gradient1;
   Array3Df image_and_gradient2;
   BlurredImageAndDerivativesChannels(image1, sigma, &image_and_gradient1);
@@ -96,6 +104,13 @@ bool KltRegionTracker::Track(const FloatImage &image1,
   int i;
   float dx = 0, dy = 0;
   for (i = 0; i < max_iterations; ++i) {
+    // Check that the entire image patch is within the bounds of the images.
+    if (!RegionIsInBounds(image2, *x2, *y2, half_window_size)) {
+      LG << "Fell out of image2's window with x2=" << *x2 << ", y2=" << *y2
+         << ", hw=" << half_window_size << ".";
+      return false;
+    }
+
     // Compute gradient matrix and error vector.
     float gxx, gxy, gyy, ex, ey;
     ComputeTrackingEquation(image_and_gradient1,
@@ -105,19 +120,32 @@ bool KltRegionTracker::Track(const FloatImage &image1,
                             half_window_size,
                             &gxx, &gxy, &gyy, &ex, &ey);
 
-    // Solve the linear system for the best update to x2 and y2.
-    if (!SolveTrackingEquation(gxx, gxy, gyy, ex, ey, min_determinant,
-                               &dx, &dy)) {
-      // The determinant, which indicates the trackiness of the point, is too
-      // small, so fail out.
-      LG << "Determinant too small; failing tracking.";
-      return false;
-    }
+    // Solve the tracking equation
+    //
+    //   [gxx gxy] [dx] = [ex]
+    //   [gxy gyy] [dy] = [ey]
+    //
+    // for dx and dy.  Borrowed from Stan Birchfield's KLT implementation.
+    float determinant = gxx * gyy - gxy * gxy;
+    dx = (gyy * ex - gxy * ey) / determinant;
+    dy = (gxx * ey - gxy * ex) / determinant;
 
     // Update the position with the solved displacement.
     *x2 += dx;
     *y2 += dy;
 
+    // Check for the quality of the solution, but not until having already
+    // updated the position with our best estimate. The reason to do the update
+    // anyway is that the user already knows the position is bad, so we may as
+    // well try our best.
+    if (determinant < min_determinant) {
+      // The determinant, which indicates the trackiness of the point, is too
+      // small, so fail out.
+      LG << "Determinant " << determinant << " is too small; failing tracking.";
+      return false;
+    }
+    LG << "x=" << *x2 << ", y=" << *y2 << ", dx=" << dx << ", dy=" << dy << ", det=" << determinant;
+
     // If the update is small, then we probably found the target.
     if (dx * dx + dy * dy < min_update_squared_distance) {
       LG << "Successful track in " << i << " iterations.";
@@ -125,7 +153,7 @@ bool KltRegionTracker::Track(const FloatImage &image1,
     }
   }
   // Getting here means we hit max iterations, so tracking failed.
-  LG << "Too many iterations.";
+  LG << "Too many iterations; max is set to " << max_iterations << ".";
   return false;
 }
 
diff --git a/extern/libmv/libmv/tracking/lmicklt_region_tracker.cc b/extern/libmv/libmv/tracking/lmicklt_region_tracker.cc
new file mode 100644 (file)
index 0000000..5ac96e6
--- /dev/null
@@ -0,0 +1,265 @@
+// Copyright (c) 2007, 2008, 2009, 2011 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.
+
+#include "libmv/tracking/lmicklt_region_tracker.h"
+
+#include "libmv/logging/logging.h"
+#include "libmv/image/image.h"
+#include "libmv/image/convolve.h"
+#include "libmv/image/sample.h"
+#include "libmv/numeric/numeric.h"
+
+namespace libmv {
+
+// TODO(keir): Reduce duplication between here and the other region trackers.
+bool RegionIsInBounds(const FloatImage &image1,
+                      double x, double y,
+                      int half_window_size) {
+  // Check the minimum coordinates.
+  int min_x = floor(x) - half_window_size - 1;
+  int min_y = floor(y) - half_window_size - 1;
+  if (min_x < 0.0 ||
+      min_y < 0.0) {
+    return false;
+  }
+
+  // Check the maximum coordinates.
+  int max_x = ceil(x) + half_window_size + 1;
+  int max_y = ceil(y) + half_window_size + 1;
+  if (max_x > image1.cols() ||
+      max_y > image1.rows()) {
+    return false;
+  }
+
+  // Ok, we're good.
+  return true;
+}
+
+// Sample a region centered at x,y in image with size extending by half_width
+// from x,y. Channels specifies the number of channels to sample from.
+void SamplePattern(const FloatImage &image,
+                   double x, double y,
+                   int half_width,
+                   int channels,
+                   FloatImage *sampled) {
+  sampled->Resize(2 * half_width + 1, 2 * half_width + 1, channels);
+  for (int r = -half_width; r <= half_width; ++r) {
+    for (int c = -half_width; c <= half_width; ++c) {
+      for (int i = 0; i < channels; ++i) {
+        (*sampled)(r + half_width, c + half_width, i) =
+            SampleLinear(image, y + r, x + c, i);
+      }
+    }
+  }
+}
+
+// Estimate "reasonable" error by computing autocorrelation for a small shift.
+double EstimateReasonableError(const FloatImage &image,
+                               double x, double y,
+                               int half_width) {
+  double error = 0.0;
+  for (int r = -half_width; r <= half_width; ++r) {
+    for (int c = -half_width; c <= half_width; ++c) {
+      double s = SampleLinear(image, y + r, x + c, 0);
+      double e1 = SampleLinear(image, y + r + 0.5, x + c, 0) - s;
+      double e2 = SampleLinear(image, y + r, x + c + 0.5, 0) - s;
+      error += e1*e1 + e2*e2;
+    }
+  }
+  return error / 2.0 * 8.0;
+}
+
+// This is implemented from "Lukas and Kanade 20 years on: Part 1. Page 42,
+// figure 14: the Levenberg-Marquardt-Inverse Compositional Algorithm".
+bool LmickltRegionTracker::Track(const FloatImage &image1,
+                             const FloatImage &image2,
+                             double  x1, double  y1,
+                             double *x2, double *y2) const {
+  if (!RegionIsInBounds(image1, x1, y1, half_window_size)) {
+    LG << "Fell out of image1's window with x1=" << x1 << ", y1=" << y1
+       << ", hw=" << half_window_size << ".";
+    return false;
+  }
+  
+  int width = 2 * half_window_size + 1;
+
+  // TODO(keir): Avoid recomputing gradients for e.g. the pyramid tracker.
+  Array3Df image_and_gradient1;
+  Array3Df image_and_gradient2;
+  BlurredImageAndDerivativesChannels(image1, sigma, &image_and_gradient1);
+
+  // TODO(keir): Avoid computing the derivative of image2.
+  BlurredImageAndDerivativesChannels(image2, sigma, &image_and_gradient2);
+
+  // Step -1: Resample the template (image1) since it is not pixel aligned.
+  //
+  // Take a sample of the gradient of the pattern area of image1 at the
+  // subpixel position x1, x2. This is reused for each iteration, so
+  // precomputing it saves time.
+  Array3Df image_and_gradient1_sampled;
+  SamplePattern(image_and_gradient1, x1, y1, half_window_size, 3,
+                &image_and_gradient1_sampled);
+
+  // Step 0: Initialize delta = 0.01.
+  // 
+  // Ignored for my "normal" LM loop.
+
+  double reasonable_error =
+      EstimateReasonableError(image1, x1, y1, half_window_size);
+
+  // Step 1: Warp I with W(x, p) to compute I(W(x; p).
+  //
+  // Use two images for accepting / rejecting updates.
+  int current_image = 0, new_image = 1;
+  Array3Df image2_sampled[2];
+  SamplePattern(image_and_gradient2, *x2, *y2, half_window_size, 1,
+                &image2_sampled[current_image]);
+
+  // Step 2: Compute the squared error I - J.
+  double error = 0;
+  for (int r = 0; r < width; ++r) {
+    for (int c = 0; c < width; ++c) {
+      double e = image_and_gradient1_sampled(r, c, 0) -
+                 image2_sampled[current_image](r, c, 0);
+      error += e*e;
+    }
+  }
+
+  // Step 3: Evaluate the gradient of the template.
+  //
+  // This is done above when sampling the template (step -1).
+
+  // Step 4: Evaluate the jacobian dw/dp at (x; 0).
+  //
+  // The jacobian between dx,dy and the warp is constant 2x2 identity, so it
+  // doesn't have to get computed. The Gauss-Newton Hessian matrix computation
+  // below would normally have to include a multiply by the jacobian.
+
+  // Step 5: Compute the steepest descent images of the template.
+  //
+  // Since the jacobian of the position w.r.t. the sampled template position is
+  // the identity, the steepest descent images are the same as the gradient.
+
+  // Step 6: Compute the Gauss-Newton Hessian for the template (image1).
+  //
+  // This could get rolled into the previous loop, but split it for now for
+  // clarity.
+  Mat2 H = Mat2::Zero();
+  for (int r = 0; r < width; ++r) {
+    for (int c = 0; c < width; ++c) {
+      Vec2 g(image_and_gradient1_sampled(r, c, 1),
+             image_and_gradient1_sampled(r, c, 2));
+      H += g * g.transpose();
+    }
+  }
+
+  double tau = 1e-3, eps1, eps2, eps3;
+  eps1 = eps2 = eps3 = 1e-15;
+
+  double mu = tau * std::max(H(0, 0), H(1, 1));
+  double nu = 2.0;
+
+  int i;
+  for (i = 0; i < max_iterations; ++i) {
+    // Check that the entire image patch is within the bounds of the images.
+    if (!RegionIsInBounds(image2, *x2, *y2, half_window_size)) {
+      LG << "Fell out of image2's window with x2=" << *x2 << ", y2=" << *y2
+         << ", hw=" << half_window_size << ".";
+      return false;
+    }
+
+    // Step 7: Compute z
+    Vec2 z = Vec2::Zero();
+    for (int r = 0; r < width; ++r) {
+      for (int c = 0; c < width; ++c) {
+        double e = image2_sampled[current_image](r, c, 0) -
+                   image_and_gradient1_sampled(r, c, 0);
+        z(0) += image_and_gradient1_sampled(r, c, 1) * e;
+        z(1) += image_and_gradient1_sampled(r, c, 2) * e;
+      }
+    }
+
+    // Step 8: Compute Hlm and (dx,dy)
+    Mat2 diag  = H.diagonal().asDiagonal();
+    diag *= mu;
+    Mat2 Hlm = H + diag;
+    Vec2 d = Hlm.lu().solve(z);
+
+    // TODO(keir): Use the usual LM termination and update criterion instead of
+    // this hacky version from the LK 20 years on paper.
+    LG << "x=" << *x2 << ", y=" << *y2 << ", dx=" << d[0] << ", dy=" << d[1]
+       << ", mu=" << mu << ", nu=" << nu;
+
+    // Step 9: Update the warp; W(x; p) <-- W(x;p) compose W(x, dp)^-1
+    double new_x2 = *x2 - d[0];
+    double new_y2 = *y2 - d[1];
+
+    // Step 9.1: Sample the image at the new position.
+    SamplePattern(image_and_gradient2, new_x2, new_y2, half_window_size, 1,
+                  &image2_sampled[new_image]);
+
+    // Step 9.2: Compute the new error.
+    // TODO(keir): Eliminate duplication with above code.
+    double new_error = 0;
+    for (int r = 0; r < width; ++r) {
+      for (int c = 0; c < width; ++c) {
+        double e = image_and_gradient1_sampled(r, c, 0) -
+                   image2_sampled[new_image](r, c, 0);
+        new_error += e*e;
+      }
+    }
+    LG << "Old error: " << error << ", new error: " << new_error;
+
+    // If the step was accepted, then check for termination.
+    if (d.squaredNorm() < min_update_squared_distance) {
+      if (new_error > reasonable_error) {
+        LG << "Update size shrank but reasonable error ("
+           << reasonable_error << ") not achieved; failing.";
+        return false;
+      }
+      LG << "Successful track in " << i << " iterations.";
+      return true;
+    }
+
+    double rho = (error - new_error) / (d.transpose() * (mu * d + z));
+
+    // Step 10: Accept or reject step.
+    if (rho <= 0) {
+      // This was a bad step, so don't update.
+      mu *= nu;
+      nu *= 2;
+      LG << "Error increased, so reject update.";
+    } else {
+      // The step was good, so update.
+      *x2 = new_x2;
+      *y2 = new_y2;
+      std::swap(new_image, current_image);
+      error = new_error;
+
+      mu *= std::max(1/3., 1 - pow(2*rho - 1, 3));
+      nu = 2;
+    }
+  }
+  // Getting here means we hit max iterations, so tracking failed.
+  LG << "Too many iterations; max is set to " << max_iterations << ".";
+  return false;
+}
+
+}  // namespace libmv
diff --git a/extern/libmv/libmv/tracking/lmicklt_region_tracker.h b/extern/libmv/libmv/tracking/lmicklt_region_tracker.h
new file mode 100644 (file)
index 0000000..744f2df
--- /dev/null
@@ -0,0 +1,62 @@
+// Copyright (c) 2007, 2008, 2009, 2011 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_REGION_TRACKING_LMICKLT_REGION_TRACKER_H_
+#define LIBMV_REGION_TRACKING_LMICKLT_REGION_TRACKER_H_
+
+#include "libmv/image/image.h"
+#include "libmv/tracking/region_tracker.h"
+
+namespace libmv {
+
+/*!
+    Levenberg-Marquardt inverse compositional region tracking
+
+    This tracker implements the Levenberg-Marquardt inverse compositional
+    region tracking algorithm as described in the paper "Lucas and Kanade 20
+    years on: A unifying framework."
+*/
+struct LmickltRegionTracker : public RegionTracker {
+  LmickltRegionTracker()
+      : half_window_size(4),
+        max_iterations(16),
+        min_determinant(1e-6),
+        min_update_squared_distance(1e-6),
+        sigma(0.9) {}
+  
+  virtual ~LmickltRegionTracker() {}
+
+  // Tracker interface.
+  virtual bool Track(const FloatImage &image1,
+                     const FloatImage &image2,
+                     double  x1, double  y1,
+                     double *x2, double *y2) const;
+
+  // No point in creating getters or setters.
+  int half_window_size;
+  int max_iterations;
+  double min_determinant;
+  double min_update_squared_distance;
+  double sigma;
+};
+
+}  // namespace libmv
+
+#endif  // LIBMV_REGION_TRACKING_LMICKLT_REGION_TRACKER_H_
index 58f42b26d7c6fe7b9346c073799cbd7272257210..c177f9c5a836e8b8e1b3c6629a4b798d656f0756 100644 (file)
@@ -63,15 +63,32 @@ bool PyramidRegionTracker::Track(const FloatImage &image1,
     *x2 *= 2;
     *y2 *= 2;
 
+    // Save the previous best guess for this level, since tracking within this
+    // level might fail.
+    double x2_new = *x2;
+    double y2_new = *y2;
+
     // Track the point on this level with the base tracker.
-    bool succeeded = tracker_->Track(pyramid1[i], pyramid2[i], xx, yy, x2, y2);
+    LG << "Tracking on level " << i;
+    bool succeeded = tracker_->Track(pyramid1[i], pyramid2[i], xx, yy,
+                                     &x2_new, &y2_new);
+
+    if (!succeeded) {
+      if (i == 0) {
+        // Only fail on the highest-resolution level, because a failure on a
+        // coarse level does not mean failure at a lower level (consider
+        // out-of-bounds conditions).
+        LG << "Finest level of pyramid tracking failed; failing.";
+        return false;
+      }
 
-    if (i == 0 && !succeeded) {
-      // Only fail on the highest-resolution level, because a failure on a
-      // coarse level does not mean failure at a lower level (consider
-      // out-of-bounds conditions).
-      LG << "Finest level of pyramid tracking failed; failing.";
-      return false;
+      LG << "Failed to track at level " << i << "; restoring guess.";
+    } else {
+      // Only save the update if the track for this level succeeded. This is a
+      // bit of a hack; the jury remains out on whether this is better than
+      // re-using the previous failed-attempt.
+      *x2 = x2_new;
+      *y2 = y2_new;
     }
   }
   return true;
index 94319fcb7d3392460be4e4343c4c037d85e50d52..7e51787ebacca00d8c562737d6859661e5285686 100644 (file)
@@ -28,6 +28,8 @@
 
 namespace libmv {
 
+// TODO(keir): Switch this to use the smarter LM loop like in ESM.
+
 // Computes U and e from the Ud = e equation (number 14) from the paper.
 static void ComputeTrackingEquation(const Array3Df &image_and_gradient1,
                                     const Array3Df &image_and_gradient2,
@@ -79,10 +81,39 @@ static void ComputeTrackingEquation(const Array3Df &image_and_gradient1,
   *e = (A + lambda*Mat2f::Identity())*Di*(V - W) + 0.5*(S - R);
 }
 
+bool RegionIsInBounds(const FloatImage &image1,
+                      double x, double y,
+                      int half_window_size) {
+  // Check the minimum coordinates.
+  int min_x = floor(x) - half_window_size - 1;
+  int min_y = floor(y) - half_window_size - 1;
+  if (min_x < 0.0 ||
+      min_y < 0.0) {
+    return false;
+  }
+
+  // Check the maximum coordinates.
+  int max_x = ceil(x) + half_window_size + 1;
+  int max_y = ceil(y) + half_window_size + 1;
+  if (max_x > image1.cols() ||
+      max_y > image1.rows()) {
+    return false;
+  }
+
+  // Ok, we're good.
+  return true;
+}
+
 bool TrkltRegionTracker::Track(const FloatImage &image1,
                                const FloatImage &image2,
                                double  x1, double  y1,
                                double *x2, double *y2) const {
+  if (!RegionIsInBounds(image1, x1, y1, half_window_size)) {
+    LG << "Fell out of image1's window with x1=" << x1 << ", y1=" << y1
+       << ", hw=" << half_window_size << ".";
+    return false;
+  }
+
   Array3Df image_and_gradient1;
   Array3Df image_and_gradient2;
   BlurredImageAndDerivativesChannels(image1, sigma, &image_and_gradient1);
@@ -91,6 +122,13 @@ bool TrkltRegionTracker::Track(const FloatImage &image1,
   int i;
   Vec2f d = Vec2f::Zero();
   for (i = 0; i < max_iterations; ++i) {
+    // Check that the entire image patch is within the bounds of the images.
+    if (!RegionIsInBounds(image2, *x2, *y2, half_window_size)) {
+      LG << "Fell out of image2's window with x2=" << *x2 << ", y2=" << *y2
+         << ", hw=" << half_window_size << ".";
+      return false;
+    }
+
     // Compute gradient matrix and error vector.
     Mat2f U;
     Vec2f e;
@@ -120,6 +158,8 @@ bool TrkltRegionTracker::Track(const FloatImage &image1,
       LG << "Determinant " << determinant << " is too small; failing tracking.";
       return false;
     }
+    LG << "x=" << *x2 << ", y=" << *y2 << ", dx=" << d[0] << ", dy=" << d[1] << ", det=" << determinant;
+
 
     // If the update is small, then we probably found the target.
     if (d.squaredNorm() < min_update_squared_distance) {
index 1c1f0cb2627ebafba2c93534478a375179749244..7eb2dfac5d931be6aee5214105fd7817bb7c025f 100644 (file)
@@ -158,6 +158,31 @@ namespace V3D
 
       switch (_mode)
       {
+         case FULL_BUNDLE_FOCAL_AND_RADIAL_K1:
+         {
+            // Focal length.
+            Ck[0][0] = xd[0];
+            Ck[1][0] = xd[1];
+
+            // For radial, k1 only.
+            Matrix2x2d dxd_dk1k2 = _distortion.derivativeWrtRadialParameters(xu);
+            Matrix2x2d d_dk1k2 = dp_dxd * dxd_dk1k2;
+            Ck[0][1] = d_dk1k2[0][0];
+            Ck[1][1] = d_dk1k2[1][0];
+            break;
+         }
+         case FULL_BUNDLE_FOCAL_AND_RADIAL:
+         {
+            // Focal length.
+            Ck[0][0] = xd[0];
+            Ck[1][0] = xd[1];
+
+            // Radial k1 and k2.
+            Matrix2x2d dxd_dk1k2 = _distortion.derivativeWrtRadialParameters(xu);
+            Matrix2x2d d_dk1k2 = dp_dxd * dxd_dk1k2;
+            copyMatrixSlice(d_dk1k2, 0, 0, 2, 2, Ck, 0, 1);
+            break;
+         }
          case FULL_BUNDLE_RADIAL_TANGENTIAL:
          {
             Matrix2x2d dxd_dp1p2 = _distortion.derivativeWrtTangentialParameters(xu);
@@ -194,6 +219,21 @@ namespace V3D
    {
       switch (_mode)
       {
+         case FULL_BUNDLE_FOCAL_AND_RADIAL_K1:
+         {
+            _K[0][0] += deltaC[0];
+            _K[1][1] = _cachedAspectRatio * _K[0][0];
+            _distortion.k1 += deltaC[1];
+            break;
+         }
+         case FULL_BUNDLE_FOCAL_AND_RADIAL:
+         {
+            _K[0][0] += deltaC[0];
+            _K[1][1] = _cachedAspectRatio * _K[0][0];
+            _distortion.k1 += deltaC[1];
+            _distortion.k2 += deltaC[2];
+            break;
+         }
          case FULL_BUNDLE_RADIAL_TANGENTIAL:
          {
             _distortion.p1 += deltaC[5];
index 076a9e6434662e8d671d2700a22fab1fed854325..339e174ed9e44d8270cbfbf18581363bd114c680 100644 (file)
@@ -166,7 +166,9 @@ namespace V3D
       FULL_BUNDLE_FOCAL_LENGTH = 1,      // f
       FULL_BUNDLE_FOCAL_LENGTH_PP = 2,   // f, cx, cy
       FULL_BUNDLE_RADIAL = 3,            // f, cx, cy, k1, k2
-      FULL_BUNDLE_RADIAL_TANGENTIAL = 4  // f, cx, cy, k1, k2, p1, p2
+      FULL_BUNDLE_RADIAL_TANGENTIAL = 4, // f, cx, cy, k1, k2, p1, p2
+      FULL_BUNDLE_FOCAL_AND_RADIAL_K1 = 5,      // f, k1
+      FULL_BUNDLE_FOCAL_AND_RADIAL = 6,      // f, k1, k2
    };
 
    struct CommonInternalsMetricBundleOptimizer : public MetricBundleOptimizerBase
@@ -175,11 +177,13 @@ namespace V3D
          {
             switch (mode)
             {
-               case FULL_BUNDLE_METRIC:            return 0;
-               case FULL_BUNDLE_FOCAL_LENGTH:      return 1;
-               case FULL_BUNDLE_FOCAL_LENGTH_PP:   return 3;
-               case FULL_BUNDLE_RADIAL:            return 5;
-               case FULL_BUNDLE_RADIAL_TANGENTIAL: return 7;
+               case FULL_BUNDLE_METRIC:              return 0;
+               case FULL_BUNDLE_FOCAL_LENGTH:        return 1;
+               case FULL_BUNDLE_FOCAL_LENGTH_PP:     return 3;
+               case FULL_BUNDLE_RADIAL:              return 5;
+               case FULL_BUNDLE_RADIAL_TANGENTIAL:   return 7;
+               case FULL_BUNDLE_FOCAL_AND_RADIAL_K1: return 2;
+               case FULL_BUNDLE_FOCAL_AND_RADIAL:    return 3;
             }
             return 0;
          }
@@ -266,11 +270,13 @@ namespace V3D
          {
             switch (mode)
             {
-               case FULL_BUNDLE_METRIC:            return 0;
-               case FULL_BUNDLE_FOCAL_LENGTH:      return 1;
-               case FULL_BUNDLE_FOCAL_LENGTH_PP:   return 3;
-               case FULL_BUNDLE_RADIAL:            return 5;
-               case FULL_BUNDLE_RADIAL_TANGENTIAL: return 7;
+               case FULL_BUNDLE_METRIC:              return 0;
+               case FULL_BUNDLE_FOCAL_LENGTH:        return 1;
+               case FULL_BUNDLE_FOCAL_LENGTH_PP:     return 3;
+               case FULL_BUNDLE_RADIAL:              return 5;
+               case FULL_BUNDLE_RADIAL_TANGENTIAL:   return 7;
+               case FULL_BUNDLE_FOCAL_AND_RADIAL_K1: return 2;
+               case FULL_BUNDLE_FOCAL_AND_RADIAL:    return 3;
             }
             return 0;
          }
index 55fc641f5ae0b97a1a54b271edc4b0967cc23735..d14d345094253ef2ec1ae3180a160eacb3773872 100644 (file)
@@ -161,6 +161,10 @@ class CLIP_PT_tools_solving(Panel):
         col.prop(settings, "keyframe_a")
         col.prop(settings, "keyframe_b")
 
+        col = layout.column(align=True)
+        col.label(text="Refine:")
+        col.prop(settings, "refine_intrinsics", text="")
+
 
 class CLIP_PT_tools_cleanup(Panel):
     bl_space_type = 'CLIP_EDITOR'
index a8fb2c836def10877c8063a6d5ad2f0b0ff24584..af852d49d8224cbe23298efa9bf46acf25ef324c 100644 (file)
@@ -91,6 +91,8 @@ void BKE_tracking_sync_user(struct MovieClipUser *user, struct MovieTrackingCont
 int BKE_tracking_next(struct MovieTrackingContext *context);
 
 /* Camera solving */
+int BKE_tracking_can_solve(struct MovieTracking *tracking, char *error_msg, int error_size);
+
 float BKE_tracking_solve_reconstruction(struct MovieTracking *tracking, int width, int height);
 
 struct MovieReconstructedCamera *BKE_tracking_get_reconstructed_camera(struct MovieTracking *tracking, int framenr);
index d65840720cd5db85583a80b17bf198e77f663502..4989e8f1b550711d322ad35d21de706cb7caf9a4 100644 (file)
@@ -47,6 +47,7 @@
 #include "BLI_listbase.h"
 #include "BLI_ghash.h"
 #include "BLI_path_util.h"
+#include "BLI_string.h"
 
 #include "BKE_global.h"
 #include "BKE_tracking.h"
@@ -1260,7 +1261,28 @@ static struct libmv_Tracks *create_libmv_tracks(MovieTracking *tracking, int wid
        return tracks;
 }
 
-static int retrieve_libmv_reconstruct(MovieTracking *tracking, struct libmv_Reconstruction *libmv_reconstruction)
+static void retrieve_libmv_reconstruct_intrinscis(MovieTracking *tracking, struct libmv_Reconstruction *libmv_reconstruction)
+{
+       struct libmv_CameraIntrinsics *libmv_intrinsics = libmv_ReconstructionExtractIntrinsics(libmv_reconstruction);
+
+       float aspy= 1.0f/tracking->camera.pixel_aspect;
+
+       double focal_length, principal_x, principal_y, k1, k2, k3;
+       int width, height;
+
+       libmv_CameraIntrinsicsExtract(libmv_intrinsics, &focal_length, &principal_x, &principal_y,
+                       &k1, &k2, &k3, &width, &height);
+
+       tracking->camera.focal= focal_length;
+       tracking->camera.principal[0]= principal_x;
+
+       /* todo: verify divide by aspy is correct */
+       tracking->camera.principal[1]= principal_y / aspy;
+       tracking->camera.k1= k1;
+       tracking->camera.k2= k2;
+}
+
+static int retrieve_libmv_reconstruct_tracks(MovieTracking *tracking, struct libmv_Reconstruction *libmv_reconstruction)
 {
        int tracknr= 0;
        int sfra= INT_MAX, efra= INT_MIN, a, origin_set= 0;
@@ -1373,7 +1395,68 @@ static int retrieve_libmv_reconstruct(MovieTracking *tracking, struct libmv_Reco
        return ok;
 }
 
+static int retrieve_libmv_reconstruct(MovieTracking *tracking, struct libmv_Reconstruction *libmv_reconstruction)
+{
+       /* take the intrinscis back from libmv */
+       retrieve_libmv_reconstruct_intrinscis(tracking, libmv_reconstruction);
+
+       return retrieve_libmv_reconstruct_tracks(tracking, libmv_reconstruction);
+}
+
+static int get_refine_intrinsics_flags(MovieTracking *tracking)
+{
+       int refine= tracking->settings.refine_camera_intrinsics;
+       int flags= 0;
+
+       if(refine&REFINE_FOCAL_LENGTH)
+               flags|= LIBMV_REFINE_FOCAL_LENGTH;
+
+       if(refine&REFINE_PRINCIPAL_POINT)
+               flags|= LIBMV_REFINE_PRINCIPAL_POINT;
+
+       if(refine&REFINE_RADIAL_DISTORTION_K1)
+               flags|= REFINE_RADIAL_DISTORTION_K1;
+
+       if(refine&REFINE_RADIAL_DISTORTION_K2)
+               flags|= REFINE_RADIAL_DISTORTION_K2;
+
+       return flags;
+}
+
+static int count_tracks_on_both_keyframes(MovieTracking *tracking)
+{
+       int tot= 0;
+       int frame1= tracking->settings.keyframe1, frame2= tracking->settings.keyframe2;
+       MovieTrackingTrack *track;
+
+       track= tracking->tracks.first;
+       while(track) {
+               if(BKE_tracking_has_marker(track, frame1))
+                       if(BKE_tracking_has_marker(track, frame2))
+                               tot++;
+
+               track= track->next;
+       }
+
+       return tot;
+}
+#endif
+
+int BKE_tracking_can_solve(MovieTracking *tracking, char *error_msg, int error_size)
+{
+#if WITH_LIBMV
+       if(count_tracks_on_both_keyframes(tracking)<8) {
+               BLI_strncpy(error_msg, "At least 8 tracks on both of keyframes are needed for reconstruction", error_size);
+               return 0;
+       }
+
+       return 1;
+#else
+       BLI_strncpy(error_msg, "Blender is compiled without motion tracking library", error_size);
+
+       return 0;
 #endif
+}
 
 float BKE_tracking_solve_reconstruction(MovieTracking *tracking, int width, int height)
 {
@@ -1384,6 +1467,7 @@ float BKE_tracking_solve_reconstruction(MovieTracking *tracking, int width, int
                struct libmv_Tracks *tracks= create_libmv_tracks(tracking, width, height*aspy);
                struct libmv_Reconstruction *reconstruction = libmv_solveReconstruction(tracks,
                        tracking->settings.keyframe1, tracking->settings.keyframe2,
+                       get_refine_intrinsics_flags(tracking),
                        camera->focal,
                        camera->principal[0], camera->principal[1]*aspy,
                        camera->k1, camera->k2, camera->k3);
index e9006f5b1e94e699b035cead114111da4a6861f9..1b08a9aee4c61362bee827616f99a66f7fd012e4 100644 (file)
@@ -1504,24 +1504,6 @@ void CLIP_OT_track_markers(wmOperatorType *ot)
 
 /********************** solve camera operator *********************/
 
-static int check_solve_track_count(MovieTracking *tracking)
-{
-       int tot= 0;
-       int frame1= tracking->settings.keyframe1, frame2= tracking->settings.keyframe2;
-       MovieTrackingTrack *track;
-
-       track= tracking->tracks.first;
-       while(track) {
-               if(BKE_tracking_has_marker(track, frame1))
-                       if(BKE_tracking_has_marker(track, frame2))
-                               tot++;
-
-               track= track->next;
-       }
-
-       return tot>=8;
-}
-
 static int solve_camera_exec(bContext *C, wmOperator *op)
 {
        SpaceClip *sc= CTX_wm_space_clip(C);
@@ -1530,9 +1512,11 @@ static int solve_camera_exec(bContext *C, wmOperator *op)
        MovieTracking *tracking= &clip->tracking;
        int width, height;
        float error;
+       char error_msg[255];
+
+       if(!BKE_tracking_can_solve(tracking, error_msg, sizeof(error_msg))) {
+               BKE_report(op->reports, RPT_ERROR, error_msg);
 
-       if(!check_solve_track_count(tracking)) {
-               BKE_report(op->reports, RPT_ERROR, "At least 8 tracks on both of keyframes are needed for reconstruction");
                return OPERATOR_CANCELLED;
        }
 
index b359ea3544d52046a7be16a16e844aaded994da0..e1aff048626ac2c79157c0bc29432742c41814c0 100644 (file)
@@ -123,6 +123,11 @@ typedef struct MovieTrackingSettings {
        /* ** reconstruction settings ** */
        int keyframe1, keyframe2;       /* two keyframes for reconstrution initialization */
 
+       /* ** which camera intrinsics to refine. uses on the REFINE_* flags */
+       short refine_camera_intrinsics;
+
+       char pad2[6];
+
        /* ** tool settings ** */
 
        /* set scale */
@@ -203,6 +208,12 @@ enum {
 #define TRACKING_SPEED_QUARTER         4
 #define TRACKING_SPEED_DOUBLE          5
 
+/* MovieTrackingSettings->refine_camera_intrinsics */
+#define REFINE_FOCAL_LENGTH                    (1<<0)
+#define REFINE_PRINCIPAL_POINT         (1<<1)
+#define REFINE_RADIAL_DISTORTION_K1    (1<<2)
+#define REFINE_RADIAL_DISTORTION_K2    (1<<4)
+
 /* MovieTrackingStrabilization->flag */
 #define TRACKING_2D_STABILIZATION      (1<<0)
 #define TRACKING_AUTOSCALE                     (1<<1)
index 2c6384c75d82d2c69fd610db008661cbcc6c1ab0..3e783d06dc49889c5ea164f9868e23f65b21bb1f 100644 (file)
@@ -229,6 +229,23 @@ static void rna_def_trackingSettings(BlenderRNA *brna)
                {0, NULL, 0, NULL, NULL}
        };
 
+       static EnumPropertyItem refine_items[] = {
+               {0, "NONE", 0, "Nothing", "Do not refine camera intrinsics"},
+               {REFINE_FOCAL_LENGTH, "FOCAL_LENGTH", 0, "Focal Length", "Refine focal length"},
+               {REFINE_FOCAL_LENGTH|
+                REFINE_PRINCIPAL_POINT, "FOCAL_LENGTH_PRINCIPAL_POINT", 0, "Focal Length, Principal Point", "Refine focal length and principal point"},
+               {REFINE_FOCAL_LENGTH|
+                REFINE_PRINCIPAL_POINT|
+                REFINE_RADIAL_DISTORTION_K1|
+                REFINE_RADIAL_DISTORTION_K2,
+                "FOCAL_LENGTH_PRINCIPAL_POINT_RADIAL_K1_K2", 0, "Focal Length, Principal Point, K1, K2", "Refine focal length, principal point and radial distortion K1 and K2"},
+               {REFINE_FOCAL_LENGTH|
+                REFINE_RADIAL_DISTORTION_K1|
+                REFINE_RADIAL_DISTORTION_K2, "FOCAL_LENGTH_RADIAL_K1_K2", 0, "Focal length, K1. K2", "Refine focal length and radial distortion K1 and K2"},
+               {REFINE_FOCAL_LENGTH|REFINE_RADIAL_DISTORTION_K1, "FOCAL_LENGTH_RADIAL_K1", 0, "Focal length, K1", "Refine focal length and radial distortion K1"},
+               {0, NULL, 0, NULL, NULL}
+       };
+
        srna= RNA_def_struct(brna, "MovieTrackingSettings", NULL);
        RNA_def_struct_ui_text(srna, "Movie tracking settings", "Match moving settings");
 
@@ -271,6 +288,13 @@ static void rna_def_trackingSettings(BlenderRNA *brna)
        RNA_def_property_int_sdna(prop, NULL, "keyframe2");
        RNA_def_property_ui_text(prop, "Keyframe B", "Second keyframe used for reconstruction initialization");
 
+       /* intrinsics refinement during bundle adjustment */
+       prop= RNA_def_property(srna, "refine_intrinsics", PROP_ENUM, PROP_NONE);
+       RNA_def_property_enum_sdna(prop, NULL, "refine_camera_intrinsics");
+       RNA_def_property_clear_flag(prop, PROP_ANIMATABLE);
+       RNA_def_property_enum_items(prop, refine_items);
+       RNA_def_property_ui_text(prop, "Refine", "Refine intrinsics during camera solving");
+
        /* tool settings */
 
        /* distance */