Initial import for SWIG version of libviso2

.gitignore

@@ -0,0 +1,4 @@
+*~
+viso2_wrap.cpp
+*.so
+*.pyc

CMakeLists.txt

@@ -0,0 +1,24 @@
+# project
+cmake_minimum_required (VERSION 2.8)
+project (libviso2)
+
+# directories
+set (LIBVISO2_SRC_DIR src)
+
+# include directory
+include_directories(${LIBVISO2_SRC_DIR})
+include_directories(${PYTHON_INCLUDE_DIRS})
+
+# use sse3 instruction set
+SET(CMAKE_CXX_FLAGS "-msse3")
+
+# sources
+FILE(GLOB LIBVISO2_SRC_FILES "src/*.cpp")
+
+# make release version
+set(CMAKE_BUILD_TYPE Release)
+
+# demo program
+add_executable(viso2 ${LIBVISO2_SRC_FILES})
+target_link_libraries (viso2 png)
+

readme.txt

@@ -0,0 +1,109 @@
+####################################################################################
+# Copyright 2012. All rights reserved.                                             #
+# Institute of Measurement and Control Systems                                     #
+# Karlsruhe Institute of Technology, Germany                                       #
+#                                                                                  #
+# This file is part of libviso2.                                                   #
+# Authors:  Andreas Geiger                                                         #
+#           Please send any bugreports to [email protected]                           #
+#                                                                                  #
+# libviso2 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 any later version.               #
+#                                                                                  #
+# libviso2 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     #
+# libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin       #
+# Street, Fifth Floor, Boston, MA 02110-1301, USA                                  #
+####################################################################################
+
++++++++++++++++++++++++++++++++++++
++          INTRODUCTION           +
++++++++++++++++++++++++++++++++++++
+
+Libviso 2 (LIBrary for ViSual Odometry 2) is a cross-platfrom (Linux, Windows) C++
+library with MATLAB wrappers for computing the 6 DOF motion of a moving stereo
+camera. Input is a sequence of rectified stereo images. Output is a 4x4 matrix
+which projects a point from the previous to the current camera coordinates.
+
+Version 2 now also supports visual odometry from monocular sequences, and the
+feature matching functions implement sparse stereo, scene flow and optical flow.
+For more details, please have a look at the corresponding publication.
+
+If you distribute a software that uses libviso, you have to distribute it under GPL
+with the source code. Another option is to contact us to purchase a commercial license.
+
+If you find this software useful or if you use this software for your research,
+we would be happy if you cite the following related publication:
+
+@INPROCEEDINGS{Geiger11,
+ author = {Andreas Geiger and Julius Ziegler and Christoph Stiller},
+ title = {StereoScan: Dense 3d Reconstruction in Real-time},
+ booktitle = {IEEE Intelligent Vehicles Symposium},
+ year = {2011},
+ month = {June},
+ address = {Baden-Baden, Germany}
+}
+
++++++++++++++++++++++++++++++++++++
++    COMPILING MATLAB WRAPPERS    +
++++++++++++++++++++++++++++++++++++
+
+Prerequisites needed for compiling libviso2:
+- Matlab (if you want to compile the matlab wrappers)
+- gcc or Visual Studio (if you want to include the code in your C++ project)
+
+If you want to use libviso directly from MATLAB you can easily do this by using
+the MATLAB wrappers provided. They also include some demo files for testing.
+
+In the MATLAB directory of libviso, simply run 'make.m' to generate the mex wrappers.
+(Run mex -setup before to choose your desired compiler)
+
+Now try to run the demo*.m files!
+For some of them you will need the Karlsruhe dataset from www.cvlibs.net.
+
++++++++++++++++++++++++++++++++++++
++     BUILDING A C++ LIBRARY      +
++++++++++++++++++++++++++++++++++++
+
+Prerequisites needed for compiling and running the libviso2 demonstration
+tool via c++:
+
+- libpng (available at: http://www.libpng.org/pub/png/libpng.html)
+- libpng++ (available at: http://www.nongnu.org/pngpp/)
+- sequence 2010_03_09_drive_0019 (available at: http://www.cvlibs.net/)
+
+libpng and png++ are needed for reading the png input images. On a ubuntu
+box you can get them via apt:
+
+- sudo apt-get install libpng12-dev
+- sudo apt-get install libpng++-dev
+
+Linux:
+
+1) Move to libviso2 root directory
+2) Type 'cmake .'
+3) Type 'make'
+4) Run './viso2 path/to/sequence/2010_03_09_drive_0019'
+
+Windows:
+
+1) Modify CMakefile according to your needs (libpng(++) must be found)
+2) Start CMake GUI
+3) Set directories to libviso2 root directory
+4) Run configure, configure and generate
+5) Open the resulting Visual Studio solution with Visual Studio
+6) Switch to 'Release' mode and build all
+9) Run 'viso2.exe path/to/sequence/2010_03_09_drive_0019'
+
+For more information on CMake, have a look at the CMake documentation.
+
+For more information on the usage of the library, have a look into the
+MATLAB wrappers, demo.cpp, viso.h, viso_stereo.h, viso_mono.h and
+matcher.h.
+
+Please send any feedback and bugreports to [email protected]
+Andreas Geiger

src/demo.cpp

@@ -0,0 +1,136 @@
+/*
+Copyright 2012. All rights reserved.
+Institute of Measurement and Control Systems
+Karlsruhe Institute of Technology, Germany
+
+This file is part of libviso2.
+Authors: Andreas Geiger
+
+libviso2 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 any later version.
+
+libviso2 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
+libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin
+Street, Fifth Floor, Boston, MA 02110-1301, USA 
+*/
+
+/*
+  Documented C++ sample code of stereo visual odometry (modify to your needs)
+  To run this demonstration, download the Karlsruhe dataset sequence
+  '2010_03_09_drive_0019' from: www.cvlibs.net!
+  Usage: ./viso2 path/to/sequence/2010_03_09_drive_0019
+*/
+
+#include <iostream>
+#include <string>
+#include <vector>
+#include <stdint.h>
+
+#include <viso_stereo.h>
+#include <png++/png.hpp>
+
+using namespace std;
+
+int main (int argc, char** argv) {
+
+  // we need the path name to 2010_03_09_drive_0019 as input argument
+  if (argc<2) {
+    cerr << "Usage: ./viso2 path/to/sequence/2010_03_09_drive_0019" << endl;
+    return 1;
+  }
+
+  // sequence directory
+  string dir = argv[1];
+
+  // set most important visual odometry parameters
+  // for a full parameter list, look at: viso_stereo.h
+  VisualOdometryStereo::parameters param;
+
+  // calibration parameters for sequence 2010_03_09_drive_0019 
+  param.calib.f  = 645.24; // focal length in pixels
+  param.calib.cu = 635.96; // principal point (u-coordinate) in pixels
+  param.calib.cv = 194.13; // principal point (v-coordinate) in pixels
+  param.base     = 0.5707; // baseline in meters
+
+  // init visual odometry
+  VisualOdometryStereo viso(param);
+
+  // current pose (this matrix transforms a point from the current
+  // frame's camera coordinates to the first frame's camera coordinates)
+  Matrix pose = Matrix::eye(4);
+
+  // loop through all frames i=0:372
+  for (int32_t i=0; i<373; i++) {
+
+    // input file names
+    char base_name[256]; sprintf(base_name,"%06d.png",i);
+    string left_img_file_name  = dir + "/I1_" + base_name;
+    string right_img_file_name = dir + "/I2_" + base_name;
+
+    // catch image read/write errors here
+    try {
+
+      // load left and right input image
+      png::image< png::gray_pixel > left_img(left_img_file_name);
+      png::image< png::gray_pixel > right_img(right_img_file_name);
+
+      // image dimensions
+      int32_t width  = left_img.get_width();
+      int32_t height = left_img.get_height();
+
+      // convert input images to uint8_t buffer
+      uint8_t* left_img_data  = (uint8_t*)malloc(width*height*sizeof(uint8_t));
+      uint8_t* right_img_data = (uint8_t*)malloc(width*height*sizeof(uint8_t));
+      int32_t k=0;
+      for (int32_t v=0; v<height; v++) {
+        for (int32_t u=0; u<width; u++) {
+          left_img_data[k]  = left_img.get_pixel(u,v);
+          right_img_data[k] = right_img.get_pixel(u,v);
+          k++;
+        }
+      }
+
+      // status
+      cout << "Processing: Frame: " << i;
+
+      // compute visual odometry
+      int32_t dims[] = {width,height,width};
+      if (viso.process(left_img_data,right_img_data,dims)) {
+
+        // on success, update current pose
+        pose = pose * Matrix::inv(viso.getMotion());
+
+        // output some statistics
+        double num_matches = viso.getNumberOfMatches();
+        double num_inliers = viso.getNumberOfInliers();
+        cout << ", Matches: " << num_matches;
+        cout << ", Inliers: " << 100.0*num_inliers/num_matches << " %" << ", Current pose: " << endl;
+        cout << pose << endl << endl;
+
+      } else {
+        cout << " ... failed!" << endl;
+      }
+
+      // release uint8_t buffers
+      free(left_img_data);
+      free(right_img_data);
+
+    // catch image read errors here
+    } catch (...) {
+      cerr << "ERROR: Couldn't read input files!" << endl;
+      return 1;
+    }
+  }
+
+  // output
+  cout << "Demo complete! Exiting ..." << endl;
+
+  // exit
+  return 0;
+}
+

src/demo.py

@@ -0,0 +1,60 @@
+#!/usr/bin/env python
+
+import viso2
+from skimage.io import imread
+import pathlib as pl
+import sys
+
+def filter_by_name(name, imgs):
+    return sorted(filter(lambda x: str(x).find(name) >= 0, imgs))
+
+LEFT="image_00"
+RIGHT="image_01"
+
+if len(sys.argv) < 2:
+    print 'Usage: ./demo.py path/to/2011_09_26_drive_0106_sync'
+    sys.exit(0)
+
+base_dir = pl.Path(sys.argv[1])
+assert(base_dir.exists() and base_dir.is_dir())
+
+# set the most relevant parameters
+params = viso2.Stereo_parameters()
+params.calib.f  = 721.5377
+params.calib.cu = 609.5593
+params.calib.cv = 172.854
+params.base     = 0.537
+
+# initialize visual odometry 
+viso = viso2.VisualOdometryStereo(params)
+
+all_images = [f for f in base_dir.rglob("*.png")]
+left_gray = filter_by_name(LEFT, all_images)
+right_gray = filter_by_name(RIGHT, all_images)
+
+N = len(left_gray)
+assert(len(left_gray) == len(right_gray))
+
+pose = viso2.Matrix_eye(4)
+for i in xrange(N):
+    # load the images
+    left_img = imread(str(left_gray[i]))
+    right_img = imread(str(right_gray[i]))
+    assert(len(left_img.shape) == 2) # should be grayscale
+
+    print "Processing: Frame:", i
+
+    # compute visual odometry
+    if viso.process_frame(left_img, right_img):
+        est_motion = viso2.Matrix_inv(viso.getMotion())
+        pose = pose * est_motion
+
+        num_matches = viso.getNumberOfMatches()
+        num_inliers = viso.getNumberOfInliers()
+        print 'Matches:', num_matches, "Inliers:", 100*num_inliers/num_matches, '%, Current pose:'
+        print pose
+    else:
+        print '.... failed!'
+
+print 'Demo complete! Exiting...'
+