add io submodule for import scenes

modules/sfm/include/opencv2/sfm.hpp

@@ -38,6 +38,7 @@
 
 #include <opencv2/sfm/conditioning.hpp>
 #include <opencv2/sfm/fundamental.hpp>
+#include <opencv2/sfm/io.hpp>
 #include <opencv2/sfm/numeric.hpp>
 #include <opencv2/sfm/projection.hpp>
 #include <opencv2/sfm/triangulation.hpp>
@@ -77,6 +78,7 @@ This module has been originally developed as a project for Google Summer of Code
   @{
     @defgroup conditioning Conditioning
     @defgroup fundamental Fundamental
+    @defgroup io Input/Output
     @defgroup numeric Numeric
     @defgroup projection Projection
     @defgroup robust Robust Estimation

modules/sfm/include/opencv2/sfm/io.hpp

+/*M///////////////////////////////////////////////////////////////////////////////////////
+ //
+ //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+ //
+ //  By downloading, copying, installing or using the software you agree to this license.
+ //  If you do not agree to this license, do not download, install,
+ //  copy or use the software.
+ //
+ //
+ //                           License Agreement
+ //                For Open Source Computer Vision Library
+ //
+ // Copyright (C) 2015, OpenCV Foundation, all rights reserved.
+ // Third party copyrights are property of their respective owners.
+ //
+ // Redistribution and use in source and binary forms, with or without modification,
+ // are permitted provided that the following conditions are met:
+ //
+ //   * Redistribution's of source code must retain the above copyright notice,
+ //     this list of conditions and the following disclaimer.
+ //
+ //   * Redistribution's 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.
+ //
+ //   * The name of the copyright holders may not 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 Intel Corporation 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.
+ //
+ //M*/
+
+#ifndef __OPENCV_SFM_IO_HPP__
+#define __OPENCV_SFM_IO_HPP__
+
+#include <opencv2/core.hpp>
+
+namespace cv
+{
+namespace sfm
+{
+
+//! @addtogroup io
+//! @{
+
+/** @brief Different supported file formats.
+ */
+enum {
+  SFM_IO_BUNDLER = 0,
+  SFM_IO_VISUALSFM = 1,
+  SFM_IO_OPENSFM = 2,
+  SFM_IO_OPENMVG = 3,
+  SFM_IO_THEIASFM = 4
+};
+
+/** @brief Import a reconstruction file.
+  @param file The path to the file.
+  @param Rs Output vector of 3x3 rotations of the camera
+  @param Ts Output vector of 3x1 translations of the camera.
+  @param Ks Output vector of 3x3 instrinsics of the camera.
+  @param points3d Output array with 3d points. Is 3 x N.
+  @param file_format The format of the file to import.
+
+  The function supports reconstructions from Bundler.
+*/
+CV_EXPORTS_W
+void
+importReconstruction(const cv::String &file, OutputArrayOfArrays Rs,
+                     OutputArrayOfArrays Ts, OutputArrayOfArrays Ks,
+                     OutputArray points3d, int file_format = SFM_IO_BUNDLER);
+
+//! @} sfm
+
+} /* namespace sfm */
+} /* namespace cv */
+
+#endif
+
+/* End of file. */

modules/sfm/samples/import_reconstruction.cpp

+#include <opencv2/sfm.hpp>
+#include <opencv2/viz.hpp>
+
+#include <iostream>
+
+using namespace std;
+using namespace cv;
+using namespace cv::sfm;
+
+static void help() {
+  cout
+      << "\n---------------------------------------------------------------------------\n"
+      << " This program shows how to import a reconstructed scene in the \n"
+      << " OpenCV Structure From Motion (SFM) module.\n"
+      << " Usage:\n"
+      << "        example_sfm_import_reconstruction <path_to_file>\n"
+      << " where: file_path is the absolute path file into your system which contains\n"
+      << "        the reconstructed scene. \n"
+      << "---------------------------------------------------------------------------\n\n"
+      << endl;
+}
+
+
+int main(int argc, char* argv[])
+{
+  /// Read input parameters
+
+  if ( argc != 2 ) {
+    help();
+    exit(0);
+  }
+
+  /// Immport a reconstructed scene
+
+  vector<Mat> Rs, Ts, Ks, points3d;
+  importReconstruction(argv[1], Rs, Ts, Ks, points3d, SFM_IO_BUNDLER);
+
+
+  /// Create 3D windows
+
+  viz::Viz3d window("Coordinate Frame");
+             window.setWindowSize(Size(500,500));
+             window.setWindowPosition(Point(150,150));
+             window.setBackgroundColor(); // black by default
+
+
+  /// Create the pointcloud
+
+  vector<Vec3d> point_cloud;
+  for (int i = 0; i < points3d.size(); ++i){
+    point_cloud.push_back(Vec3f(points3d[i]));
+  }
+
+
+  /// Recovering cameras
+
+  vector<Affine3d> path;
+  for (size_t i = 0; i < Rs.size(); ++i)
+    path.push_back(Affine3d(Rs[i], Ts[i]));
+
+
+  /// Create and show widgets
+
+  viz::WCloud cloud_widget(point_cloud, viz::Color::green());
+  viz::WTrajectory trajectory(path, viz::WTrajectory::FRAMES, 0.5);
+  viz::WTrajectoryFrustums frustums(path, Vec2f(0.889484, 0.523599), 0.5,
+                                    viz::Color::yellow());
+
+  window.showWidget("point_cloud", cloud_widget);
+  window.showWidget("cameras", trajectory);
+  window.showWidget("frustums", frustums);
+
+
+  /// Wait for key 'q' to close the window
+  cout << endl << "Press 'q' to close each windows ... " << endl;
+
+  window.spin();
+
+  return 0;
+}
\ No newline at end of file

modules/sfm/src/io.cpp

+/*M///////////////////////////////////////////////////////////////////////////////////////
+ //
+ //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+ //
+ //  By downloading, copying, installing or using the software you agree to this license.
+ //  If you do not agree to this license, do not download, install,
+ //  copy or use the software.
+ //
+ //
+ //                           License Agreement
+ //                For Open Source Computer Vision Library
+ //
+ // Copyright (C) 2015, OpenCV Foundation, all rights reserved.
+ // Third party copyrights are property of their respective owners.
+ //
+ // Redistribution and use in source and binary forms, with or without modification,
+ // are permitted provided that the following conditions are met:
+ //
+ //   * Redistribution's of source code must retain the above copyright notice,
+ //     this list of conditions and the following disclaimer.
+ //
+ //   * Redistribution's 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.
+ //
+ //   * The name of the copyright holders may not 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 Intel Corporation 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.
+ //
+ //M*/
+
+#include <opencv2/sfm/io.hpp>
+#include "io/io_bundler.h"
+
+namespace cv
+{
+namespace sfm
+{
+
+void
+importReconstruction(const cv::String &file, OutputArrayOfArrays _Rs,
+                     OutputArrayOfArrays _Ts, OutputArrayOfArrays _Ks,
+                     OutputArray _points3d, int file_format) {
+
+    std::vector<Matx33d> Rs, Ks;
+    std::vector<Vec3d> Ts, points3d;
+
+    if (file_format == SFM_IO_BUNDLER) {
+        readBundlerFile(file, Rs, Ts, Ks, points3d);
+    } else if (file_format == SFM_IO_VISUALSFM) {
+        CV_Error(Error::StsNotImplemented, "The requested function/feature is not implemented");
+    } else if (file_format == SFM_IO_OPENSFM) {
+        CV_Error(Error::StsNotImplemented, "The requested function/feature is not implemented");
+    } else if (file_format == SFM_IO_OPENMVG) {
+        CV_Error(Error::StsNotImplemented, "The requested function/feature is not implemented");
+    } else if (file_format == SFM_IO_THEIASFM) {
+        CV_Error(Error::StsNotImplemented, "The requested function/feature is not implemented");
+    } else {
+        CV_Error(Error::StsBadArg, "The file format one of SFM_IO_BUNDLER, SFM_IO_VISUALSFM, SFM_IO_OPENSFM, SFM_IO_OPENMVG or SFM_IO_THEIASFM");
+    }
+
+    const size_t num_cameras = Rs.size();
+    const size_t num_points = points3d.size();
+
+    _Rs.create(num_cameras, 1, CV_64F);
+    _Ts.create(num_cameras, 1, CV_64F);
+    _Ks.create(num_cameras, 1, CV_64F);
+    _points3d.create(num_points, 1, CV_64F);
+
+    for (size_t i = 0; i < num_cameras; ++i) {
+        Mat(Rs[i]).copyTo(_Rs.getMatRef(i));
+        Mat(Ts[i]).copyTo(_Ts.getMatRef(i));
+        Mat(Ks[i]).copyTo(_Ks.getMatRef(i));
+    }
+
+    for (size_t i = 0; i < num_points; ++i)
+        Mat(points3d[i]).copyTo(_points3d.getMatRef(i));
+}
+
+
+} /* namespace sfm */
+} /* namespace cv */
\ No newline at end of file

modules/sfm/src/io/io_bundler.h

+/*
+ Based on TheiaSfM library.
+ https://github.com/sweeneychris/TheiaSfM/blob/master/src/theia/io/read_bundler_files.cc
+
+ Adapted by Edgar Riba <edgar.riba@gmail.com>
+
+*/
+
+#include <iostream>
+#include <fstream>
+#include <opencv2/core.hpp>
+
+// The bundle files contain the estimated scene and camera geometry have the
+// following format:
+//     # Bundle file v0.3
+//     <num_cameras> <num_points>   [two integers]
+//     <camera1>
+//     <camera2>
+//        ...
+//     <cameraN>
+//     <point1>
+//     <point2>
+//        ...
+//     <pointM>
+// Each camera entry <cameraI> contains the estimated camera intrinsics and
+// extrinsics, and has the form:
+//     <f> <k1> <k2>   [the focal length, followed by two radial distortion
+//                      coeffs]
+//     <R>             [a 3x3 matrix representing the camera rotation]
+//     <t>             [a 3-vector describing the camera translation]
+// The cameras are specified in the order they appear in the list of images.
+//
+// Each point entry has the form:
+//     <position>      [a 3-vector describing the 3D position of the point]
+//     <color>         [a 3-vector describing the RGB color of the point]
+//     <view list>     [a list of views the point is visible in]
+//
+// The view list begins with the length of the list (i.e., the number of cameras
+// the point is visible in). The list is then given as a list of quadruplets
+// <camera> <key> <x> <y>, where <camera> is a camera index, <key> the index of
+// the SIFT keypoint where the point was detected in that camera, and <x> and
+// <y> are the detected positions of that keypoint. Both indices are 0-based
+// (e.g., if camera 0 appears in the list, this corresponds to the first camera
+// in the scene file and the first image in "list.txt"). The pixel positions are
+// floating point numbers in a coordinate system where the origin is the center
+// of the image, the x-axis increases to the right, and the y-axis increases
+// towards the top of the image. Thus, (-w/2, -h/2) is the lower-left corner of
+// the image, and (w/2, h/2) is the top-right corner (where w and h are the
+// width and height of the image).
+bool readBundlerFile(const std::string &file,
+                     std::vector<cv::Matx33d> &Rs,
+                     std::vector<cv::Vec3d> &Ts,
+                     std::vector<cv::Matx33d> &Ks,
+                     std::vector<cv::Vec3d> &points3d) {
+
+  // Read in num cameras, num points.
+  std::ifstream ifs(file.c_str(), std::ios::in);
+  if (!ifs.is_open()) {
+    std::cout << "Cannot read the file from " << file << std::endl;
+    return false;
+  }
+
+  const cv::Matx33d bundler_to_opencv(1, 0, 0, 0, -1, 0, 0, 0, -1);
+
+  std::string header_string;
+  std::getline(ifs, header_string);
+
+  // If the first line starts with '#' then it is a comment, so skip it!
+  if (header_string[0] == '#') {
+    std::getline(ifs, header_string);
+  }
+  const char* p = header_string.c_str();
+  char* p2;
+  const int num_cameras = strtol(p, &p2, 10);
+
+  p = p2;
+  const int num_points = strtol(p, &p2, 10);
+
+  // Read in the camera params.
+  for (int i = 0; i < num_cameras; i++) {
+    // Read in focal length, radial distortion.
+    std::string internal_params;
+    std::getline(ifs, internal_params);
+    p = internal_params.c_str();
+    const double focal_length = strtod(p, &p2);
+    p = p2;
+    //const double k1 = strtod(p, &p2);
+    p = p2;
+    //const double k2 = strtod(p, &p2);
+    p = p2;
+
+    cv::Matx33d intrinsics;
+    intrinsics(0,0) = intrinsics(1,1) = focal_length;
+    Ks.push_back(intrinsics);
+
+    // Read in rotation (row-major).
+    cv::Matx33d rotation;
+    for (int r = 0; r < 3; r++) {
+      std::string rotation_row;
+      std::getline(ifs, rotation_row);
+      p = rotation_row.c_str();
+
+      for (int c = 0; c < 3; c++) {
+        rotation(r, c) = strtod(p, &p2);
+        p = p2;
+      }
+    }
+
+    std::string translation_string;
+    std::getline(ifs, translation_string);
+    p = translation_string.c_str();
+    cv::Vec3d translation;
+    for (int j = 0; j < 3; j++) {
+      translation(j) = strtod(p, &p2);
+      p = p2;
+    }
+
+    rotation = bundler_to_opencv * rotation;
+    translation =  bundler_to_opencv * translation;
+
+    cv::Matx33d rotation_t = rotation.t();
+    translation = -1.0 * rotation_t * translation;
+
+    Rs.push_back(rotation);
+    Ts.push_back(translation);
+
+    if ((i + 1) % 100 == 0 || i == num_cameras - 1) {
+      std::cout << "\r Loading parameters for camera " << i + 1 << " / "
+                << num_cameras << std::flush;
+    }
+  }
+  std::cout << std::endl;
+
+  // Read in each 3D point and correspondences.
+  for (int i = 0; i < num_points; i++) {
+    // Read position.
+    std::string position_str;
+    std::getline(ifs, position_str);
+    p = position_str.c_str();
+    cv::Vec3d position;
+    for (int j = 0; j < 3; j++) {
+      position(j) = strtod(p, &p2);
+      p = p2;
+    }
+    points3d.push_back(position);
+
+    // Read color.
+    std::string color_str;
+    std::getline(ifs, color_str);
+    p = color_str.c_str();
+    cv::Vec3d color;
+    for (int j = 0; j < 3; j++) {
+      color(j) = static_cast<double>(strtol(p, &p2, 10)) / 255.0;
+      p = p2;
+    }
+
+    // Read viewlist.
+    std::string view_list_string;
+    std::getline(ifs, view_list_string);
+    p = view_list_string.c_str();
+    const int num_views = strtol(p, &p2, 10);
+    p = p2;
+
+    // Reserve the view list for this 3D point.
+    for (int j = 0; j < num_views; j++) {
+      // Camera key x y
+      //const int camera_index = strtol(p, &p2, 10);
+      p = p2;
+      // Returns the index of the sift descriptor in the camera for this track.
+      strtol(p, &p2, 10);
+      p = p2;
+      //const float x_pos = strtof(p, &p2);
+      p = p2;
+      //const float y_pos = strtof(p, &p2);
+      p = p2;
+
+    }
+
+    if ((i + 1) % 100 == 0 || i == num_points - 1) {
+      std::cout << "\r Loading 3D points " << i + 1 << " / " << num_points
+                << std::flush;
+    }
+  }
+
+  std::cout << std::endl;
+  ifs.close();
+
+  return true;
+}
\ No newline at end of file

modules/sfm/tutorials/sfm_import_reconstruction/sfm_import_reconstruction.markdown

+Import Reconstruction {#tutorial_sfm_import_reconstruction}
+=====================
+
+Goal
+----
+
+In this tutorial you will learn how to import a reconstruction from a given file obtained with Bundler [1]:
+
+-   Load a file containing a set of cameras and 3D points.
+-   Show obtained results using Viz.
+
+
+Code
+----
+
+@include sfm/samples/import_reconstruction.cpp
+
+Results
+-------
+
+The following picture shows a reconstruction from la *Sagrada Familia* (BCN) using dataset [2].
+
+![](pics/import_sagrada_familia.png)
+
+[1] [http://www.cs.cornell.edu/~snavely/bundler](http://www.cs.cornell.edu/~snavely/bundler)
+
+[2] Penate Sanchez, A. and Moreno-Noguer, F. and Andrade Cetto, J. and Fleuret, F. (2014). LETHA: Learning from High Quality Inputs for 3D Pose Estimation in Low Quality Images. Proceedings of the International Conference on 3D vision (3DV).
+[URL](http://www.iri.upc.edu/research/webprojects/pau/datasets/sagfam)

modules/sfm/tutorials/sfm_trajectory_estimation/sfm_trajectory_estimation.markdown

@@ -6,7 +6,7 @@ Goal
 
 In this tutorial you will learn how to use the reconstruction api for camera motion estimation:
 
--   Load and file with the tracked 2d points and build the container over all the frames.
+-   Load a file with the tracked 2d points and build the container over all the frames.
 -   Run libmv reconstruction pipeline.
 -   Show obtained results using Viz.
 

modules/sfm/tutorials/table_of_content_sfm.markdown

@@ -23,4 +23,12 @@ Structure From Motion {#tutorial_table_of_content_sfm}
 
     *Author:* Edgar Riba
 
-    Sparse scene reconstruction from a given set of images.
\ No newline at end of file
+    Sparse scene reconstruction from a given set of images.
+
+-   @subpage tutorial_sfm_import_reconstruction
+
+    *Compatibility:* \> OpenCV 3.0
+
+    *Author:* Edgar Riba
+
+    Import a scene reconstruction.
\ No newline at end of file