intersect.cc

Go to the documentation of this file.

// Copyright (c) 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/simple_pipeline/intersect.h"
 
#include "libmv/base/vector.h"
#include "libmv/logging/logging.h"
#include "libmv/multiview/nviewtriangulation.h"
#include "libmv/multiview/projection.h"
#include "libmv/multiview/triangulation.h"
#include "libmv/numeric/levenberg_marquardt.h"
#include "libmv/numeric/numeric.h"
#include "libmv/simple_pipeline/reconstruction.h"
#include "libmv/simple_pipeline/tracks.h"
 
#include "ceres/ceres.h"
 
namespace libmv {
 
namespace {
 
class EuclideanIntersectCostFunctor {
 public:
  EuclideanIntersectCostFunctor(const Marker& marker,
                                const EuclideanCamera& camera)
      : marker_(marker), camera_(camera) {}
 
  template <typename T>
  bool operator()(const T* X, T* residuals) const {
    typedef Eigen::Matrix<T, 3, 3> Mat3;
    typedef Eigen::Matrix<T, 3, 1> Vec3;
 
    Vec3 x(X);
    Mat3 R(camera_.R.cast<T>());
    Vec3 t(camera_.t.cast<T>());
 
    Vec3 projected = R * x + t;
    projected /= projected(2);
 
    residuals[0] = (projected(0) - T(marker_.x)) * marker_.weight;
    residuals[1] = (projected(1) - T(marker_.y)) * marker_.weight;
 
    return true;
  }
 
  const Marker& marker_;
  const EuclideanCamera& camera_;
};
 
}  // namespace
 
bool EuclideanIntersect(const vector<Marker>& markers,
                        EuclideanReconstruction* reconstruction) {
  if (markers.size() < 2) {
    return false;
  }
 
  // Compute projective camera matrices for the cameras the intersection is
  // going to use.
  Mat3 K = Mat3::Identity();
  vector<Mat34> cameras;
  Mat34 P;
  for (int i = 0; i < markers.size(); ++i) {
    EuclideanCamera* camera = reconstruction->CameraForImage(markers[i].image);
    P_From_KRt(K, camera->R, camera->t, &P);
    cameras.push_back(P);
  }
 
  // Stack the 2D coordinates together as required by NViewTriangulate.
  Mat2X points(2, markers.size());
  for (int i = 0; i < markers.size(); ++i) {
    points(0, i) = markers[i].x;
    points(1, i) = markers[i].y;
  }
 
  Vec4 Xp;
  LG << "Intersecting with " << markers.size() << " markers.";
  NViewTriangulateAlgebraic(points, cameras, &Xp);
 
  // Get euclidean version of the homogeneous point.
  Xp /= Xp(3);
  Vec3 X = Xp.head<3>();
 
  ceres::Problem problem;
 
  // Add residual blocks to the problem.
  int num_residuals = 0;
  for (int i = 0; i < markers.size(); ++i) {
    const Marker& marker = markers[i];
    if (marker.weight != 0.0) {
      const EuclideanCamera& camera =
          *reconstruction->CameraForImage(marker.image);
 
      problem.AddResidualBlock(
          new ceres::AutoDiffCostFunction<EuclideanIntersectCostFunctor,
                                          2, /* num_residuals */
                                          3>(
              new EuclideanIntersectCostFunctor(marker, camera)),
          NULL,
          &X(0));
      num_residuals++;
    }
  }
 
  // TODO(sergey): Once we'll update Ceres to the next version
  // we wouldn't need this check anymore -- Ceres will deal with
  // zero-sized problems nicely.
  LG << "Number of residuals: " << num_residuals;
  if (!num_residuals) {
    LG << "Skipping running minimizer with zero residuals";
 
    // We still add 3D point for the track regardless it was
    // optimized or not. If track is a constant zero it'll use
    // algebraic intersection result as a 3D coordinate.
 
    Vec3 point = X.head<3>();
    reconstruction->InsertPoint(markers[0].track, point);
 
    return true;
  }
 
  // 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.parameter_tolerance = 1e-16;
  solver_options.function_tolerance = 1e-16;
 
  // Run the solve.
  ceres::Solver::Summary summary;
  ceres::Solve(solver_options, &problem, &summary);
 
  VLOG(1) << "Summary:\n" << summary.FullReport();
 
  // Try projecting the point; make sure it's in front of everyone.
  for (int i = 0; i < cameras.size(); ++i) {
    const EuclideanCamera& camera =
        *reconstruction->CameraForImage(markers[i].image);
    Vec3 x = camera.R * X + camera.t;
    if (x(2) < 0) {
      LOG(ERROR) << "POINT BEHIND CAMERA " << markers[i].image << ": "
                 << x.transpose();
      return false;
    }
  }
 
  Vec3 point = X.head<3>();
  reconstruction->InsertPoint(markers[0].track, point);
 
  // TODO(keir): Add proper error checking.
  return true;
}
 
namespace {
 
struct ProjectiveIntersectCostFunction {
 public:
  typedef Vec FMatrixType;
  typedef Vec4 XMatrixType;
 
  ProjectiveIntersectCostFunction(
      const vector<Marker>& markers,
      const ProjectiveReconstruction& reconstruction)
      : markers(markers), reconstruction(reconstruction) {}
 
  Vec operator()(const Vec4& X) const {
    Vec residuals(2 * markers.size());
    residuals.setZero();
    for (int i = 0; i < markers.size(); ++i) {
      const ProjectiveCamera& camera =
          *reconstruction.CameraForImage(markers[i].image);
      Vec3 projected = camera.P * X;
      projected /= projected(2);
      residuals[2 * i + 0] = projected(0) - markers[i].x;
      residuals[2 * i + 1] = projected(1) - markers[i].y;
    }
    return residuals;
  }
  const vector<Marker>& markers;
  const ProjectiveReconstruction& reconstruction;
};
 
}  // namespace
 
bool ProjectiveIntersect(const vector<Marker>& markers,
                         ProjectiveReconstruction* reconstruction) {
  if (markers.size() < 2) {
    return false;
  }
 
  // Get the cameras to use for the intersection.
  vector<Mat34> cameras;
  for (int i = 0; i < markers.size(); ++i) {
    ProjectiveCamera* camera = reconstruction->CameraForImage(markers[i].image);
    cameras.push_back(camera->P);
  }
 
  // Stack the 2D coordinates together as required by NViewTriangulate.
  Mat2X points(2, markers.size());
  for (int i = 0; i < markers.size(); ++i) {
    points(0, i) = markers[i].x;
    points(1, i) = markers[i].y;
  }
 
  Vec4 X;
  LG << "Intersecting with " << markers.size() << " markers.";
  NViewTriangulateAlgebraic(points, cameras, &X);
  X /= X(3);
 
  typedef LevenbergMarquardt<ProjectiveIntersectCostFunction> Solver;
 
  ProjectiveIntersectCostFunction triangulate_cost(markers, *reconstruction);
  Solver::SolverParameters params;
  Solver solver(triangulate_cost);
 
  Solver::Results results = solver.minimize(params, &X);
  (void)results;  // TODO(keir): Ensure results are good.
 
  // Try projecting the point; make sure it's in front of everyone.
  for (int i = 0; i < cameras.size(); ++i) {
    const ProjectiveCamera& camera =
        *reconstruction->CameraForImage(markers[i].image);
    Vec3 x = camera.P * X;
    if (x(2) < 0) {
      LOG(ERROR) << "POINT BEHIND CAMERA " << markers[i].image << ": "
                 << x.transpose();
    }
  }
 
  reconstruction->InsertPoint(markers[0].track, X);
 
  // TODO(keir): Add proper error checking.
  return true;
}
 
}  // namespace libmv