intern/reconstruction.cc

Go to the documentation of this file.

/*
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software Foundation,
 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 *
 * The Original Code is Copyright (C) 2011 Blender Foundation.
 * All rights reserved.
 */
 
#include "intern/reconstruction.h"
#include "intern/camera_intrinsics.h"
#include "intern/tracks.h"
#include "intern/utildefines.h"
 
#include "libmv/logging/logging.h"
#include "libmv/simple_pipeline/bundle.h"
#include "libmv/simple_pipeline/initialize_reconstruction.h"
#include "libmv/simple_pipeline/keyframe_selection.h"
#include "libmv/simple_pipeline/modal_solver.h"
#include "libmv/simple_pipeline/pipeline.h"
#include "libmv/simple_pipeline/reconstruction_scale.h"
#include "libmv/simple_pipeline/tracks.h"
 
using libmv::CameraIntrinsics;
using libmv::EuclideanCamera;
using libmv::EuclideanPoint;
using libmv::EuclideanReconstruction;
using libmv::EuclideanScaleToUnity;
using libmv::Marker;
using libmv::ProgressUpdateCallback;
 
using libmv::EuclideanBundle;
using libmv::EuclideanCompleteReconstruction;
using libmv::EuclideanReconstructTwoFrames;
using libmv::EuclideanReprojectionError;
using libmv::PolynomialCameraIntrinsics;
using libmv::Tracks;
 
struct libmv_Reconstruction {
  EuclideanReconstruction reconstruction;
 
  /* Used for per-track average error calculation after reconstruction */
  Tracks tracks;
  CameraIntrinsics* intrinsics;
 
  double error;
  bool is_valid;
};
 
namespace {
 
class ReconstructUpdateCallback : public ProgressUpdateCallback {
 public:
  ReconstructUpdateCallback(
      reconstruct_progress_update_cb progress_update_callback,
      void* callback_customdata) {
    progress_update_callback_ = progress_update_callback;
    callback_customdata_ = callback_customdata;
  }
 
  void invoke(double progress, const char* message) {
    if (progress_update_callback_) {
      progress_update_callback_(callback_customdata_, progress, message);
    }
  }
 
 protected:
  reconstruct_progress_update_cb progress_update_callback_;
  void* callback_customdata_;
};
 
void libmv_solveRefineIntrinsics(
    const Tracks& tracks,
    const int refine_intrinsics,
    const int bundle_constraints,
    reconstruct_progress_update_cb progress_update_callback,
    void* callback_customdata,
    EuclideanReconstruction* reconstruction,
    CameraIntrinsics* intrinsics) {
  /* only a few combinations are supported but trust the caller/ */
  int bundle_intrinsics = 0;
 
  if (refine_intrinsics & LIBMV_REFINE_FOCAL_LENGTH) {
    bundle_intrinsics |= libmv::BUNDLE_FOCAL_LENGTH;
  }
  if (refine_intrinsics & LIBMV_REFINE_PRINCIPAL_POINT) {
    bundle_intrinsics |= libmv::BUNDLE_PRINCIPAL_POINT;
  }
 
#define SET_DISTORTION_FLAG_CHECKED(type, coefficient)                         \
  do {                                                                         \
    if (refine_intrinsics & LIBMV_REFINE_##type##_DISTORTION_##coefficient) {  \
      bundle_intrinsics |= libmv::BUNDLE_##type##_##coefficient;               \
    }                                                                          \
  } while (0)
 
  SET_DISTORTION_FLAG_CHECKED(RADIAL, K1);
  SET_DISTORTION_FLAG_CHECKED(RADIAL, K2);
  SET_DISTORTION_FLAG_CHECKED(RADIAL, K3);
  SET_DISTORTION_FLAG_CHECKED(RADIAL, K4);
 
  SET_DISTORTION_FLAG_CHECKED(TANGENTIAL, P1);
  SET_DISTORTION_FLAG_CHECKED(TANGENTIAL, P2);
 
#undef SET_DISTORTION_FLAG_CHECKED
 
  progress_update_callback(callback_customdata, 1.0, "Refining solution");
 
  EuclideanBundleCommonIntrinsics(tracks,
                                  bundle_intrinsics,
                                  bundle_constraints,
                                  reconstruction,
                                  intrinsics);
}
 
void finishReconstruction(
    const Tracks& tracks,
    const CameraIntrinsics& camera_intrinsics,
    libmv_Reconstruction* libmv_reconstruction,
    reconstruct_progress_update_cb progress_update_callback,
    void* callback_customdata) {
  EuclideanReconstruction& reconstruction =
      libmv_reconstruction->reconstruction;
 
  /* Reprojection error calculation. */
  progress_update_callback(callback_customdata, 1.0, "Finishing solution");
  libmv_reconstruction->tracks = tracks;
  libmv_reconstruction->error =
      EuclideanReprojectionError(tracks, reconstruction, camera_intrinsics);
}
 
bool selectTwoKeyframesBasedOnGRICAndVariance(
    Tracks& tracks,
    Tracks& normalized_tracks,
    CameraIntrinsics& camera_intrinsics,
    int& keyframe1,
    int& keyframe2) {
  libmv::vector<int> keyframes;
 
  /* Get list of all keyframe candidates first. */
  SelectKeyframesBasedOnGRICAndVariance(
      normalized_tracks, camera_intrinsics, keyframes);
 
  if (keyframes.size() < 2) {
    LG << "Not enough keyframes detected by GRIC";
    return false;
  } else if (keyframes.size() == 2) {
    keyframe1 = keyframes[0];
    keyframe2 = keyframes[1];
    return true;
  }
 
  /* Now choose two keyframes with minimal reprojection error after initial
   * reconstruction choose keyframes with the least reprojection error after
   * solving from two candidate keyframes.
   *
   * In fact, currently libmv returns single pair only, so this code will
   * not actually run. But in the future this could change, so let's stay
   * prepared.
   */
  int previous_keyframe = keyframes[0];
  double best_error = std::numeric_limits<double>::max();
  for (int i = 1; i < keyframes.size(); i++) {
    EuclideanReconstruction reconstruction;
    int current_keyframe = keyframes[i];
    libmv::vector<Marker> keyframe_markers =
        normalized_tracks.MarkersForTracksInBothImages(previous_keyframe,
                                                       current_keyframe);
 
    Tracks keyframe_tracks(keyframe_markers);
 
    /* get a solution from two keyframes only */
    EuclideanReconstructTwoFrames(keyframe_markers, &reconstruction);
    EuclideanBundle(keyframe_tracks, &reconstruction);
    EuclideanCompleteReconstruction(keyframe_tracks, &reconstruction, NULL);
 
    double current_error =
        EuclideanReprojectionError(tracks, reconstruction, camera_intrinsics);
 
    LG << "Error between " << previous_keyframe << " and " << current_keyframe
       << ": " << current_error;
 
    if (current_error < best_error) {
      best_error = current_error;
      keyframe1 = previous_keyframe;
      keyframe2 = current_keyframe;
    }
 
    previous_keyframe = current_keyframe;
  }
 
  return true;
}
 
Marker libmv_projectMarker(const EuclideanPoint& point,
                           const EuclideanCamera& camera,
                           const CameraIntrinsics& intrinsics) {
  libmv::Vec3 projected = camera.R * point.X + camera.t;
  projected /= projected(2);
 
  libmv::Marker reprojected_marker;
  intrinsics.ApplyIntrinsics(
      projected(0), projected(1), &reprojected_marker.x, &reprojected_marker.y);
 
  reprojected_marker.image = camera.image;
  reprojected_marker.track = point.track;
  return reprojected_marker;
}
 
void libmv_getNormalizedTracks(const Tracks& tracks,
                               const CameraIntrinsics& camera_intrinsics,
                               Tracks* normalized_tracks) {
  libmv::vector<Marker> markers = tracks.AllMarkers();
  for (int i = 0; i < markers.size(); ++i) {
    Marker& marker = markers[i];
    camera_intrinsics.InvertIntrinsics(
        marker.x, marker.y, &marker.x, &marker.y);
    normalized_tracks->Insert(
        marker.image, marker.track, marker.x, marker.y, marker.weight);
  }
}
 
}  // namespace
 
libmv_Reconstruction* libmv_solveReconstruction(
    const libmv_Tracks* libmv_tracks,
    const libmv_CameraIntrinsicsOptions* libmv_camera_intrinsics_options,
    libmv_ReconstructionOptions* libmv_reconstruction_options,
    reconstruct_progress_update_cb progress_update_callback,
    void* callback_customdata) {
  libmv_Reconstruction* libmv_reconstruction =
      LIBMV_OBJECT_NEW(libmv_Reconstruction);
 
  Tracks& tracks = *((Tracks*)libmv_tracks);
  EuclideanReconstruction& reconstruction =
      libmv_reconstruction->reconstruction;
 
  ReconstructUpdateCallback update_callback =
      ReconstructUpdateCallback(progress_update_callback, callback_customdata);
 
  /* Retrieve reconstruction options from C-API to libmv API. */
  CameraIntrinsics* camera_intrinsics;
  camera_intrinsics = libmv_reconstruction->intrinsics =
      libmv_cameraIntrinsicsCreateFromOptions(libmv_camera_intrinsics_options);
 
  /* Invert the camera intrinsics/ */
  Tracks normalized_tracks;
  libmv_getNormalizedTracks(tracks, *camera_intrinsics, &normalized_tracks);
 
  /* keyframe selection. */
  int keyframe1 = libmv_reconstruction_options->keyframe1,
      keyframe2 = libmv_reconstruction_options->keyframe2;
 
  if (libmv_reconstruction_options->select_keyframes) {
    LG << "Using automatic keyframe selection";
 
    update_callback.invoke(0, "Selecting keyframes");
 
    if (selectTwoKeyframesBasedOnGRICAndVariance(tracks,
                                                 normalized_tracks,
                                                 *camera_intrinsics,
                                                 keyframe1,
                                                 keyframe2)) {
      /* so keyframes in the interface would be updated */
      libmv_reconstruction_options->keyframe1 = keyframe1;
      libmv_reconstruction_options->keyframe2 = keyframe2;
    }
  }
 
  /* Actual reconstruction. */
  LG << "frames to init from: " << keyframe1 << " " << keyframe2;
 
  libmv::vector<Marker> keyframe_markers =
      normalized_tracks.MarkersForTracksInBothImages(keyframe1, keyframe2);
 
  LG << "number of markers for init: " << keyframe_markers.size();
 
  if (keyframe_markers.size() < 16) {
    LG << "No enough markers to initialize from";
    libmv_reconstruction->is_valid = false;
    return libmv_reconstruction;
  }
 
  update_callback.invoke(0, "Initial reconstruction");
 
  if (!EuclideanReconstructTwoFrames(keyframe_markers, &reconstruction)) {
    LG << "Failed to initialize reconstruction";
    libmv_reconstruction->is_valid = false;
    return libmv_reconstruction;
  }
 
  EuclideanBundle(normalized_tracks, &reconstruction);
  EuclideanCompleteReconstruction(
      normalized_tracks, &reconstruction, &update_callback);
 
  /* Refinement. */
  if (libmv_reconstruction_options->refine_intrinsics) {
    libmv_solveRefineIntrinsics(tracks,
                                libmv_reconstruction_options->refine_intrinsics,
                                libmv::BUNDLE_NO_CONSTRAINTS,
                                progress_update_callback,
                                callback_customdata,
                                &reconstruction,
                                camera_intrinsics);
  }
 
  /* Set reconstruction scale to unity. */
  EuclideanScaleToUnity(&reconstruction);
 
  /* Finish reconstruction. */
  finishReconstruction(tracks,
                       *camera_intrinsics,
                       libmv_reconstruction,
                       progress_update_callback,
                       callback_customdata);
 
  libmv_reconstruction->is_valid = true;
  return (libmv_Reconstruction*)libmv_reconstruction;
}
 
libmv_Reconstruction* libmv_solveModal(
    const libmv_Tracks* libmv_tracks,
    const libmv_CameraIntrinsicsOptions* libmv_camera_intrinsics_options,
    const libmv_ReconstructionOptions* libmv_reconstruction_options,
    reconstruct_progress_update_cb progress_update_callback,
    void* callback_customdata) {
  libmv_Reconstruction* libmv_reconstruction =
      LIBMV_OBJECT_NEW(libmv_Reconstruction);
 
  Tracks& tracks = *((Tracks*)libmv_tracks);
  EuclideanReconstruction& reconstruction =
      libmv_reconstruction->reconstruction;
 
  ReconstructUpdateCallback update_callback =
      ReconstructUpdateCallback(progress_update_callback, callback_customdata);
 
  /* Retrieve reconstruction options from C-API to libmv API. */
  CameraIntrinsics* camera_intrinsics;
  camera_intrinsics = libmv_reconstruction->intrinsics =
      libmv_cameraIntrinsicsCreateFromOptions(libmv_camera_intrinsics_options);
 
  /* Invert the camera intrinsics. */
  Tracks normalized_tracks;
  libmv_getNormalizedTracks(tracks, *camera_intrinsics, &normalized_tracks);
 
  /* Actual reconstruction. */
  ModalSolver(normalized_tracks, &reconstruction, &update_callback);
 
  PolynomialCameraIntrinsics empty_intrinsics;
  EuclideanBundleCommonIntrinsics(normalized_tracks,
                                  libmv::BUNDLE_NO_INTRINSICS,
                                  libmv::BUNDLE_NO_TRANSLATION,
                                  &reconstruction,
                                  &empty_intrinsics);
 
  /* Refinement. */
  if (libmv_reconstruction_options->refine_intrinsics) {
    libmv_solveRefineIntrinsics(tracks,
                                libmv_reconstruction_options->refine_intrinsics,
                                libmv::BUNDLE_NO_TRANSLATION,
                                progress_update_callback,
                                callback_customdata,
                                &reconstruction,
                                camera_intrinsics);
  }
 
  /* Finish reconstruction. */
  finishReconstruction(tracks,
                       *camera_intrinsics,
                       libmv_reconstruction,
                       progress_update_callback,
                       callback_customdata);
 
  libmv_reconstruction->is_valid = true;
  return (libmv_Reconstruction*)libmv_reconstruction;
}
 
int libmv_reconstructionIsValid(libmv_Reconstruction* libmv_reconstruction) {
  return libmv_reconstruction->is_valid;
}
 
void libmv_reconstructionDestroy(libmv_Reconstruction* libmv_reconstruction) {
  LIBMV_OBJECT_DELETE(libmv_reconstruction->intrinsics, CameraIntrinsics);
  LIBMV_OBJECT_DELETE(libmv_reconstruction, libmv_Reconstruction);
}
 
int libmv_reprojectionPointForTrack(
    const libmv_Reconstruction* libmv_reconstruction,
    int track,
    double pos[3]) {
  const EuclideanReconstruction* reconstruction =
      &libmv_reconstruction->reconstruction;
  const EuclideanPoint* point = reconstruction->PointForTrack(track);
  if (point) {
    pos[0] = point->X[0];
    pos[1] = point->X[2];
    pos[2] = point->X[1];
    return 1;
  }
  return 0;
}
 
double libmv_reprojectionErrorForTrack(
    const libmv_Reconstruction* libmv_reconstruction, int track) {
  const EuclideanReconstruction* reconstruction =
      &libmv_reconstruction->reconstruction;
  const CameraIntrinsics* intrinsics = libmv_reconstruction->intrinsics;
  libmv::vector<Marker> markers =
      libmv_reconstruction->tracks.MarkersForTrack(track);
 
  int num_reprojected = 0;
  double total_error = 0.0;
 
  for (int i = 0; i < markers.size(); ++i) {
    double weight = markers[i].weight;
    const EuclideanCamera* camera =
        reconstruction->CameraForImage(markers[i].image);
    const EuclideanPoint* point =
        reconstruction->PointForTrack(markers[i].track);
 
    if (!camera || !point || weight == 0.0) {
      continue;
    }
 
    num_reprojected++;
 
    Marker reprojected_marker =
        libmv_projectMarker(*point, *camera, *intrinsics);
    double ex = (reprojected_marker.x - markers[i].x) * weight;
    double ey = (reprojected_marker.y - markers[i].y) * weight;
 
    total_error += sqrt(ex * ex + ey * ey);
  }
 
  return total_error / num_reprojected;
}
 
double libmv_reprojectionErrorForImage(
    const libmv_Reconstruction* libmv_reconstruction, int image) {
  const EuclideanReconstruction* reconstruction =
      &libmv_reconstruction->reconstruction;
  const CameraIntrinsics* intrinsics = libmv_reconstruction->intrinsics;
  libmv::vector<Marker> markers =
      libmv_reconstruction->tracks.MarkersInImage(image);
  const EuclideanCamera* camera = reconstruction->CameraForImage(image);
  int num_reprojected = 0;
  double total_error = 0.0;
 
  if (!camera) {
    return 0.0;
  }
 
  for (int i = 0; i < markers.size(); ++i) {
    const EuclideanPoint* point =
        reconstruction->PointForTrack(markers[i].track);
 
    if (!point) {
      continue;
    }
 
    num_reprojected++;
 
    Marker reprojected_marker =
        libmv_projectMarker(*point, *camera, *intrinsics);
    double ex = (reprojected_marker.x - markers[i].x) * markers[i].weight;
    double ey = (reprojected_marker.y - markers[i].y) * markers[i].weight;
 
    total_error += sqrt(ex * ex + ey * ey);
  }
 
  return total_error / num_reprojected;
}
 
int libmv_reprojectionCameraForImage(
    const libmv_Reconstruction* libmv_reconstruction,
    int image,
    double mat[4][4]) {
  const EuclideanReconstruction* reconstruction =
      &libmv_reconstruction->reconstruction;
  const EuclideanCamera* camera = reconstruction->CameraForImage(image);
 
  if (camera) {
    for (int j = 0; j < 3; ++j) {
      for (int k = 0; k < 3; ++k) {
        int l = k;
 
        if (k == 1) {
          l = 2;
        } else if (k == 2) {
          l = 1;
        }
 
        if (j == 2) {
          mat[j][l] = -camera->R(j, k);
        } else {
          mat[j][l] = camera->R(j, k);
        }
      }
      mat[j][3] = 0.0;
    }
 
    libmv::Vec3 optical_center = -camera->R.transpose() * camera->t;
 
    mat[3][0] = optical_center(0);
    mat[3][1] = optical_center(2);
    mat[3][2] = optical_center(1);
 
    mat[3][3] = 1.0;
 
    return 1;
  }
 
  return 0;
}
 
double libmv_reprojectionError(
    const libmv_Reconstruction* libmv_reconstruction) {
  return libmv_reconstruction->error;
}
 
libmv_CameraIntrinsics* libmv_reconstructionExtractIntrinsics(
    libmv_Reconstruction* libmv_reconstruction) {
  return (libmv_CameraIntrinsics*)libmv_reconstruction->intrinsics;
}