diff options
Diffstat (limited to 'extern')
18 files changed, 1112 insertions, 107 deletions
diff --git a/extern/libmv/CMakeLists.txt b/extern/libmv/CMakeLists.txt index 333791e28eb..cd1f572197c 100644 --- a/extern/libmv/CMakeLists.txt +++ b/extern/libmv/CMakeLists.txt @@ -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 diff --git a/extern/libmv/libmv-capi.cpp b/extern/libmv/libmv-capi.cpp index 2e007bb47b2..9d6cfd5d17a 100644 --- a/extern/libmv/libmv-capi.cpp +++ b/extern/libmv/libmv-capi.cpp @@ -33,8 +33,10 @@ #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> @@ -56,13 +59,6 @@ # define snprintf _snprintf #endif -#define DEFAULT_WINDOW_HALFSIZE 5 - -typedef struct libmv_RegionTracker { - libmv::TrkltRegionTracker *trklt_region_tracker; - libmv::RegionTracker *region_tracker; -} libmv_RegionTracker; - typedef struct libmv_Reconstruction { libmv::EuclideanReconstruction reconstruction; @@ -110,22 +106,18 @@ void libmv_setLoggingVerbosity(int verbosity) /* ************ RegionTracker ************ */ -libmv_RegionTracker *libmv_regionTrackerNew(int max_iterations, int pyramid_level) +libmv_RegionTracker *libmv_regionTrackerNew(int max_iterations, int pyramid_level, int half_window_size) { - 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 = half_window_size; + 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); - - libmv_RegionTracker *configured_region_tracker = new libmv_RegionTracker; - configured_region_tracker->trklt_region_tracker = trklt_region_tracker; - configured_region_tracker->region_tracker = region_tracker; + new libmv::PyramidRegionTracker(klt_region_tracker, pyramid_level); - return configured_region_tracker; + return (libmv_RegionTracker *)region_tracker; } static void floatBufToImage(const float *buf, int width, int height, libmv::FloatImage *image) @@ -267,18 +259,11 @@ static void saveBytesImage(char *prefix, unsigned char *data, int width, int hei #endif int libmv_regionTrackerTrack(libmv_RegionTracker *libmv_tracker, const float *ima1, const float *ima2, - int width, int height, int half_window_size, - double x1, double y1, double *x2, double *y2) + int width, int height, double x1, double y1, double *x2, double *y2) { - libmv::RegionTracker *region_tracker; - libmv::TrkltRegionTracker *trklt_region_tracker; + libmv::RegionTracker *region_tracker = (libmv::RegionTracker *)libmv_tracker; libmv::FloatImage old_patch, new_patch; - trklt_region_tracker = libmv_tracker->trklt_region_tracker; - region_tracker = libmv_tracker->region_tracker; - - trklt_region_tracker->half_window_size = half_window_size; - floatBufToImage(ima1, width, height, &old_patch); floatBufToImage(ima2, width, height, &new_patch); @@ -301,8 +286,9 @@ int libmv_regionTrackerTrack(libmv_RegionTracker *libmv_tracker, const float *im void libmv_regionTrackerDestroy(libmv_RegionTracker *libmv_tracker) { - delete libmv_tracker->region_tracker; - delete libmv_tracker; + libmv::RegionTracker *region_tracker= (libmv::RegionTracker *)libmv_tracker; + + delete region_tracker; } /* ************ Tracks ************ */ @@ -353,8 +339,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 +380,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 +631,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 +681,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) { diff --git a/extern/libmv/libmv-capi.h b/extern/libmv/libmv-capi.h index b71a66b73a6..8252a11739b 100644 --- a/extern/libmv/libmv-capi.h +++ b/extern/libmv/libmv-capi.h @@ -43,10 +43,9 @@ void libmv_startDebugLogging(void); void libmv_setLoggingVerbosity(int verbosity); /* RegionTracker */ -struct libmv_RegionTracker *libmv_regionTrackerNew(int max_iterations, int pyramid_level); +struct libmv_RegionTracker *libmv_regionTrackerNew(int max_iterations, int pyramid_level, int half_window_size); int libmv_regionTrackerTrack(struct libmv_RegionTracker *libmv_tracker, const float *ima1, const float *ima2, - int width, int height, int half_window_size, - double x1, double y1, double *x2, double *y2); + int width, int height, double x1, double y1, double *x2, double *y2); void libmv_regionTrackerDestroy(struct libmv_RegionTracker *libmv_tracker); /* SAD Tracker */ @@ -61,8 +60,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 +85,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); diff --git a/extern/libmv/libmv/simple_pipeline/bundle.cc b/extern/libmv/libmv/simple_pipeline/bundle.cc index cb8822dcf44..d382cd5a4fc 100644 --- a/extern/libmv/libmv/simple_pipeline/bundle.cc +++ b/extern/libmv/libmv/simple_pipeline/bundle.cc @@ -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*/, diff --git a/extern/libmv/libmv/simple_pipeline/bundle.h b/extern/libmv/libmv/simple_pipeline/bundle.h index c7fb2a79607..55657cb2d92 100644 --- a/extern/libmv/libmv/simple_pipeline/bundle.h +++ b/extern/libmv/libmv/simple_pipeline/bundle.h @@ -23,6 +23,7 @@ namespace libmv { +class CameraIntrinsics; class EuclideanReconstruction; class ProjectiveReconstruction; class Tracks; @@ -50,6 +51,46 @@ void EuclideanBundle(const Tracks &tracks, /*! 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. + This routine adjusts all cameras and points 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 diff --git a/extern/libmv/libmv/simple_pipeline/camera_intrinsics.cc b/extern/libmv/libmv/simple_pipeline/camera_intrinsics.cc index 366129dd3d2..917f80e6926 100644 --- a/extern/libmv/libmv/simple_pipeline/camera_intrinsics.cc +++ b/extern/libmv/libmv/simple_pipeline/camera_intrinsics.cc @@ -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 diff --git a/extern/libmv/libmv/simple_pipeline/camera_intrinsics.h b/extern/libmv/libmv/simple_pipeline/camera_intrinsics.h index f4bf903c36c..e0556674ad5 100644 --- a/extern/libmv/libmv/simple_pipeline/camera_intrinsics.h +++ b/extern/libmv/libmv/simple_pipeline/camera_intrinsics.h @@ -21,11 +21,15 @@ #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_ diff --git a/extern/libmv/libmv/simple_pipeline/initialize_reconstruction.cc b/extern/libmv/libmv/simple_pipeline/initialize_reconstruction.cc index 0597f09f728..77fe2a530c4 100644 --- a/extern/libmv/libmv/simple_pipeline/initialize_reconstruction.cc +++ b/extern/libmv/libmv/simple_pipeline/initialize_reconstruction.cc @@ -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; } diff --git a/extern/libmv/libmv/simple_pipeline/pipeline.cc b/extern/libmv/libmv/simple_pipeline/pipeline.cc index 818c24cb5e7..9512a41c00f 100644 --- a/extern/libmv/libmv/simple_pipeline/pipeline.cc +++ b/extern/libmv/libmv/simple_pipeline/pipeline.cc @@ -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 index 00000000000..01edee3bbb5 --- /dev/null +++ b/extern/libmv/libmv/tracking/esm_region_tracker.cc @@ -0,0 +1,290 @@ +// 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. + +#define _USE_MATH_DEFINES + +#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 index 00000000000..c63417201ad --- /dev/null +++ b/extern/libmv/libmv/tracking/esm_region_tracker.h @@ -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_ diff --git a/extern/libmv/libmv/tracking/klt_region_tracker.cc b/extern/libmv/libmv/tracking/klt_region_tracker.cc index 299077be155..78ce0be603c 100644 --- a/extern/libmv/libmv/tracking/klt_region_tracker.cc +++ b/extern/libmv/libmv/tracking/klt_region_tracker.cc @@ -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 index 00000000000..5ac96e66175 --- /dev/null +++ b/extern/libmv/libmv/tracking/lmicklt_region_tracker.cc @@ -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 index 00000000000..744f2dfe277 --- /dev/null +++ b/extern/libmv/libmv/tracking/lmicklt_region_tracker.h @@ -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_ diff --git a/extern/libmv/libmv/tracking/pyramid_region_tracker.cc b/extern/libmv/libmv/tracking/pyramid_region_tracker.cc index 58f42b26d7c..c177f9c5a83 100644 --- a/extern/libmv/libmv/tracking/pyramid_region_tracker.cc +++ b/extern/libmv/libmv/tracking/pyramid_region_tracker.cc @@ -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; diff --git a/extern/libmv/libmv/tracking/trklt_region_tracker.cc b/extern/libmv/libmv/tracking/trklt_region_tracker.cc index 94319fcb7d3..7e51787ebac 100644 --- a/extern/libmv/libmv/tracking/trklt_region_tracker.cc +++ b/extern/libmv/libmv/tracking/trklt_region_tracker.cc @@ -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) { diff --git a/extern/libmv/third_party/ssba/Geometry/v3d_metricbundle.cpp b/extern/libmv/third_party/ssba/Geometry/v3d_metricbundle.cpp index 1c1f0cb2627..7eb2dfac5d9 100644 --- a/extern/libmv/third_party/ssba/Geometry/v3d_metricbundle.cpp +++ b/extern/libmv/third_party/ssba/Geometry/v3d_metricbundle.cpp @@ -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]; diff --git a/extern/libmv/third_party/ssba/Geometry/v3d_metricbundle.h b/extern/libmv/third_party/ssba/Geometry/v3d_metricbundle.h index 076a9e64346..339e174ed9e 100644 --- a/extern/libmv/third_party/ssba/Geometry/v3d_metricbundle.h +++ b/extern/libmv/third_party/ssba/Geometry/v3d_metricbundle.h @@ -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; } |