path: root/intern/libmv/libmv/simple_pipeline/modal_solver.cc

// Copyright (c) 2015 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/simple_pipeline/modal_solver.h"

#include <cstdio>

#include "ceres/ceres.h"
#include "ceres/rotation.h"
#include "libmv/logging/logging.h"
#include "libmv/multiview/panography.h"

#ifdef _MSC_VER
#  define snprintf _snprintf
#endif

namespace libmv {

namespace {
void ProjectMarkerOnSphere(const Marker& marker, Vec3& X) {
  X(0) = marker.x;
  X(1) = marker.y;
  X(2) = 1.0;

  X *= 5.0 / X.norm();
}

void ModalSolverLogProgress(ProgressUpdateCallback* update_callback,
                            double progress) {
  if (update_callback) {
    char message[256];

    snprintf(message,
             sizeof(message),
             "Solving progress %d%%",
             (int)(progress * 100));

    update_callback->invoke(progress, message);
  }
}

struct ModalReprojectionError {
  ModalReprojectionError(double observed_x,
                         double observed_y,
                         const double weight,
                         const Vec3& bundle)
      : observed_x_(observed_x),
        observed_y_(observed_y),
        weight_(weight),
        bundle_(bundle) {}

  // TODO(keir): This should support bundling focal length as well.
  template <typename T>
  bool operator()(const T* quaternion, T* residuals) const {
    // Convert bundle position from double to T.
    T X[3] = {T(bundle_(0)), T(bundle_(1)), T(bundle_(2))};

    // Compute the point position in camera coordinates: x = RX.
    T x[3];

    // This flips the sense of the quaternion, to adhere to Blender conventions.
    T quaternion_inverse[4] = {
        quaternion[0],
        -quaternion[1],
        -quaternion[2],
        -quaternion[3],
    };
    ceres::QuaternionRotatePoint(quaternion_inverse, X, x);

    // Compute normalized coordinates by dividing out the depth.
    T xn = x[0] / x[2];
    T yn = x[1] / x[2];

    // The error is the difference between reprojected and observed marker
    // positions, weighted by the passed in weight.
    residuals[0] = T(weight_) * (xn - T(observed_x_));
    residuals[1] = T(weight_) * (yn - T(observed_y_));

    return true;
  }

  double observed_x_;
  double observed_y_;
  double weight_;
  Vec3 bundle_;
};
}  // namespace

void ModalSolver(const Tracks& tracks,
                 EuclideanReconstruction* reconstruction,
                 ProgressUpdateCallback* update_callback) {
  int max_image = tracks.MaxImage();
  int max_track = tracks.MaxTrack();

  LG << "Max image: " << max_image;
  LG << "Max track: " << max_track;

  // For minimization we're using quaternions.
  Vec3 zero_rotation = Vec3::Zero();
  Vec4 quaternion;
  ceres::AngleAxisToQuaternion(&zero_rotation(0), &quaternion(0));

  for (int image = 0; image <= max_image; ++image) {
    vector<Marker> all_markers = tracks.MarkersInImage(image);

    ModalSolverLogProgress(update_callback, (float)image / max_image);

    // Skip empty images without doing anything.
    if (all_markers.size() == 0) {
      LG << "Skipping image: " << image;
      continue;
    }

    // STEP 1: Estimate rotation analytically.
    Mat3 current_R;
    ceres::QuaternionToRotation(&quaternion(0), &current_R(0, 0));

    // Construct point cloud for current and previous images,
    // using markers appear at current image for which we know
    // 3D positions.
    Mat x1, x2;
    for (int i = 0; i < all_markers.size(); ++i) {
      Marker& marker = all_markers[i];
      EuclideanPoint* point = reconstruction->PointForTrack(marker.track);
      if (point) {
        Vec3 X;
        ProjectMarkerOnSphere(marker, X);

        int last_column = x1.cols();
        x1.conservativeResize(3, last_column + 1);
        x2.conservativeResize(3, last_column + 1);

        x1.col(last_column) = current_R * point->X;
        x2.col(last_column) = X;
      }
    }

    if (x1.cols() >= 2) {
      Mat3 delta_R;

      // Compute delta rotation matrix for two point clouds.
      // Could be a bit confusing at first glance, but order
      // of clouds is indeed so.
      GetR_FixedCameraCenter(x2, x1, 1.0, &delta_R);

      // Convert delta rotation form matrix to final image
      // rotation stored in a quaternion
      Vec3 delta_angle_axis;
      ceres::RotationMatrixToAngleAxis(&delta_R(0, 0), &delta_angle_axis(0));

      Vec3 current_angle_axis;
      ceres::QuaternionToAngleAxis(&quaternion(0), &current_angle_axis(0));

      Vec3 angle_axis = current_angle_axis + delta_angle_axis;

      ceres::AngleAxisToQuaternion(&angle_axis(0), &quaternion(0));

      LG << "Analytically computed quaternion " << quaternion.transpose();
    }

    // STEP 2: Refine rotation with Ceres.
    ceres::Problem problem;

    // NOTE: Parameterization is lazily initialized when it is really needed,
    // and is re-used by all parameters block.
    ceres::LocalParameterization* quaternion_parameterization = NULL;

    int num_residuals = 0;
    for (int i = 0; i < all_markers.size(); ++i) {
      Marker& marker = all_markers[i];
      EuclideanPoint* point = reconstruction->PointForTrack(marker.track);

      if (point && marker.weight != 0.0) {
        problem.AddResidualBlock(
            new ceres::AutoDiffCostFunction<ModalReprojectionError,
                                            2, /* num_residuals */
                                            4>(new ModalReprojectionError(
                marker.x, marker.y, marker.weight, point->X)),
            NULL,
            &quaternion(0));
        num_residuals++;

        if (quaternion_parameterization == NULL) {
          quaternion_parameterization = new ceres::QuaternionParameterization();
        }

        problem.SetParameterization(&quaternion(0),
                                    quaternion_parameterization);
      }
    }

    LG << "Number of residuals: " << num_residuals;

    if (num_residuals) {
      // Configure the solve.
      ceres::Solver::Options solver_options;
      solver_options.linear_solver_type = ceres::DENSE_QR;
      solver_options.max_num_iterations = 50;
      solver_options.update_state_every_iteration = true;
      solver_options.gradient_tolerance = 1e-36;
      solver_options.parameter_tolerance = 1e-36;
      solver_options.function_tolerance = 1e-36;

      // Run the solve.
      ceres::Solver::Summary summary;
      ceres::Solve(solver_options, &problem, &summary);

      LG << "Summary:\n" << summary.FullReport();
      LG << "Refined quaternion " << quaternion.transpose();
    }

    // Convert quaternion to rotation matrix.
    Mat3 R;
    ceres::QuaternionToRotation(&quaternion(0), &R(0, 0));
    reconstruction->InsertCamera(image, R, Vec3::Zero());

    // STEP 3: reproject all new markers appeared at image

    // Check if there're new markers appeared on current image
    // and reproject them on sphere to obtain 3D position/
    for (int track = 0; track <= max_track; ++track) {
      if (!reconstruction->PointForTrack(track)) {
        Marker marker = tracks.MarkerInImageForTrack(image, track);

        if (marker.image == image) {
          // New track appeared on this image,
          // project its position onto sphere.

          LG << "Projecting track " << track << " at image " << image;

          Vec3 X;
          ProjectMarkerOnSphere(marker, X);
          reconstruction->InsertPoint(track, R.inverse() * X);
        }
      }
    }
  }
}

}  // namespace libmv