opencv_contrib/modules/sfm/src/simple_pipeline.cpp

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2009, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include "precomp.hpp"

#if CERES_FOUND

// Eigen
#include <Eigen/Core>

#include <opencv2/core/eigen.hpp>
#include <opencv2/xfeatures2d.hpp>

#include "libmv_capi.h"

using namespace std;

namespace cv
{
namespace sfm
{

/* Parses a given array of 2d points into the libmv tracks structure
 */

static void
parser_2D_tracks( const std::vector<Mat> &points2d, libmv::Tracks &tracks )
{
  const int nframes = static_cast<int>(points2d.size());
  for (int frame = 0; frame < nframes; ++frame) {
    const int ntracks = points2d[frame].cols;
    for (int track = 0; track < ntracks; ++track) {
      const Vec2d track_pt = points2d[frame].col(track);
      if ( track_pt[0] > 0 && track_pt[1] > 0 )
        tracks.Insert(frame, track, track_pt[0], track_pt[1]);
    }
  }
}

///////////////////////////////////////////////////////////////////////////////////////////////

/* Parses a given set of matches into the libmv tracks structure
 */

static void
parser_2D_tracks( const libmv::Matches &matches, libmv::Tracks &tracks )
{
  std::set<Matches::ImageID>::const_iterator iter_image =
    matches.get_images().begin();

  bool is_first_time = true;

  for (; iter_image != matches.get_images().end(); ++iter_image) {
    // Exports points
    Matches::Features<PointFeature> pfeatures =
      matches.InImage<PointFeature>(*iter_image);

    while(pfeatures) {

      double x = pfeatures.feature()->x(),
             y = pfeatures.feature()->y();

      // valid marker
      if ( x > 0 && y > 0 )
      {
        tracks.Insert(*iter_image, pfeatures.track(), x, y);

        if ( is_first_time )
          is_first_time = false;
      }

      // lost track
      else if ( x < 0 && y < 0 )
      {
        is_first_time = true;
      }

      pfeatures.operator++();
    }
  }
}

///////////////////////////////////////////////////////////////////////////////////////////////

/* Computes the 2d features matches between a given set of images and call the
 * reconstruction pipeline.
 */

static libmv_Reconstruction *libmv_solveReconstructionImpl(
  const std::vector<String> &images,
  const libmv_CameraIntrinsicsOptions* libmv_camera_intrinsics_options,
  libmv_ReconstructionOptions* libmv_reconstruction_options)
{
  Ptr<Feature2D> edetector = ORB::create(10000);
  Ptr<Feature2D> edescriber = xfeatures2d::DAISY::create();
  //Ptr<Feature2D> edescriber = xfeatures2d::LATCH::create(64, true, 4);
  std::vector<std::string> sImages;
  for (int i=0;i<images.size();i++)
      sImages.push_back(images[i].c_str());
  cout << "Initialize nViewMatcher ... ";
  libmv::correspondence::nRobustViewMatching nViewMatcher(edetector, edescriber);

  cout << "OK" << endl << "Performing Cross Matching ... ";
  nViewMatcher.computeCrossMatch(sImages); cout << "OK" << endl;

  // Building tracks
  libmv::Tracks tracks;
  libmv::Matches matches = nViewMatcher.getMatches();
  parser_2D_tracks( matches, tracks );

  // Perform reconstruction
  return libmv_solveReconstruction(tracks,
                                   libmv_camera_intrinsics_options,
                                   libmv_reconstruction_options);
}

///////////////////////////////////////////////////////////////////////////////////////////////

template <class T>
class SFMLibmvReconstructionImpl : public T
{
public:
  SFMLibmvReconstructionImpl(const libmv_CameraIntrinsicsOptions &camera_instrinsic_options,
                             const libmv_ReconstructionOptions &reconstruction_options) :
    libmv_reconstruction_options_(reconstruction_options),
    libmv_camera_intrinsics_options_(camera_instrinsic_options) {}

  /* Run the pipeline given 2d points
   */

  virtual void run(InputArrayOfArrays _points2d)
  {
    std::vector<Mat> points2d;
    _points2d.getMatVector(points2d);
    CV_Assert( _points2d.total() >= 2 );

    // Parse 2d points to Tracks
    Tracks tracks;
    parser_2D_tracks(points2d, tracks);

    // Set libmv logs level
    libmv_initLogging("");

    if (libmv_reconstruction_options_.verbosity_level >= 0)
    {
      libmv_startDebugLogging();
      libmv_setLoggingVerbosity(
        libmv_reconstruction_options_.verbosity_level);
    }

    // Perform reconstruction
    libmv_reconstruction_ =
      *libmv_solveReconstruction(tracks,
                                 &libmv_camera_intrinsics_options_,
                                 &libmv_reconstruction_options_);
  }

  virtual void run(InputArrayOfArrays points2d, InputOutputArray K, OutputArray Rs,
                   OutputArray Ts, OutputArray points3d)
  {
    // Run the pipeline
    run(points2d);

    // Extract Data
    extractLibmvReconstructionData(K, Rs, Ts, points3d);
  }


  /* Run the pipeline given a set of images
   */

  virtual void run(const std::vector <String> &images)
  {
    // Set libmv logs level
    libmv_initLogging("");

    if (libmv_reconstruction_options_.verbosity_level >= 0)
    {
      libmv_startDebugLogging();
      libmv_setLoggingVerbosity(
        libmv_reconstruction_options_.verbosity_level);
    }

    // Perform reconstruction

    libmv_reconstruction_ =
      *libmv_solveReconstructionImpl(images,
                                     &libmv_camera_intrinsics_options_,
                                     &libmv_reconstruction_options_);
  }


  virtual void run(const std::vector <String> &images, InputOutputArray K, OutputArray Rs,
                   OutputArray Ts, OutputArray points3d)
  {
    // Run the pipeline
    run(images);

    // Extract Data
    extractLibmvReconstructionData(K, Rs, Ts, points3d);
  }

  virtual double getError() const { return libmv_reconstruction_.error; }

  virtual void
  getPoints(OutputArray points3d) {
    const size_t n_points =
      libmv_reconstruction_.reconstruction.AllPoints().size();

    points3d.create(n_points, 1, CV_64F);

    Vec3d point3d;
    for ( size_t i = 0; i < n_points; ++i )
    {
      for ( int j = 0; j < 3; ++j )
        point3d[j] =
          libmv_reconstruction_.reconstruction.AllPoints()[i].X[j];
      Mat(point3d).copyTo(points3d.getMatRef(i));
    }

  }

  virtual cv::Mat getIntrinsics() const {
    Mat K;
    eigen2cv(libmv_reconstruction_.intrinsics->K(), K);
    return K;
  }

  virtual void
  getCameras(OutputArray Rs, OutputArray Ts) {
    const size_t n_views =
      libmv_reconstruction_.reconstruction.AllCameras().size();

    Rs.create(n_views, 1, CV_64F);
    Ts.create(n_views, 1, CV_64F);

    Matx33d R;
    Vec3d t;
    for(size_t i = 0; i < n_views; ++i)
    {
      eigen2cv(libmv_reconstruction_.reconstruction.AllCameras()[i].R, R);
      eigen2cv(libmv_reconstruction_.reconstruction.AllCameras()[i].t, t);
      Mat(R).copyTo(Rs.getMatRef(i));
      Mat(t).copyTo(Ts.getMatRef(i));
    }
  }

  virtual void setReconstructionOptions(
    const libmv_ReconstructionOptions &libmv_reconstruction_options) {
      libmv_reconstruction_options_ = libmv_reconstruction_options;
  }

  virtual void setCameraIntrinsicOptions(
    const libmv_CameraIntrinsicsOptions &libmv_camera_intrinsics_options) {
      libmv_camera_intrinsics_options_ = libmv_camera_intrinsics_options;
  }

private:

  void
  extractLibmvReconstructionData(InputOutputArray K,
                                 OutputArray Rs,
                                 OutputArray Ts,
                                 OutputArray points3d)
  {
    getCameras(Rs, Ts);
    getPoints(points3d);
    getIntrinsics().copyTo(K.getMat());
  }

  libmv_Reconstruction libmv_reconstruction_;
  libmv_ReconstructionOptions libmv_reconstruction_options_;
  libmv_CameraIntrinsicsOptions libmv_camera_intrinsics_options_;
};

///////////////////////////////////////////////////////////////////////////////////////////////

Ptr<SFMLibmvEuclideanReconstruction>
SFMLibmvEuclideanReconstruction::create(const libmv_CameraIntrinsicsOptions &camera_instrinsic_options,
                                        const libmv_ReconstructionOptions &reconstruction_options)
{
  return makePtr<SFMLibmvReconstructionImpl<SFMLibmvEuclideanReconstruction> >(camera_instrinsic_options,reconstruction_options);
}

///////////////////////////////////////////////////////////////////////////////////////////////

} /* namespace cv */
} /* namespace sfm */

#endif

/* End of file. */