Add libelas to source tree and build in preparation for use as algorithm

CMakeLists.txt

@@ -100,9 +100,9 @@ if (WIN32) # TODO(nick) Should do based upon compiler (VS)
 	set(CMAKE_CXX_FLAGS_RELEASE "/O2")
 else()
 	add_definitions(-DUNIX)
-	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17")
+	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -msse3")
 	set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -D_DEBUG -pg -Wall")
-	set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 -msse3 -mfpmath=sse")
+	set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 -mfpmath=sse")
 endif()
 
 SET(CMAKE_USE_RELATIVE_PATHS ON)

cv-node/CMakeLists.txt

@@ -2,6 +2,8 @@
 include_directories(${PROJECT_SOURCE_DIR}/cv-node/include)
 #include_directories(${PROJECT_BINARY_DIR})
 
+add_subdirectory(lib)
+
 # Check for optional opencv components
 set(CMAKE_REQUIRED_INCLUDES ${OpenCV_INCLUDE_DIRS})
 check_include_file_cxx("opencv2/viz.hpp" HAVE_VIZ)

cv-node/include/elas.h

+/*
+Copyright 2011. All rights reserved.
+Institute of Measurement and Control Systems
+Karlsruhe Institute of Technology, Germany
+
+This file is part of libelas.
+Authors: Andreas Geiger
+
+libelas 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 3 of the License, or any later version.
+
+libelas 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
+libelas; if not, write to the Free Software Foundation, Inc., 51 Franklin
+Street, Fifth Floor, Boston, MA 02110-1301, USA 
+*/
+
+// Main header file. Include this to use libelas in your code.
+
+#ifndef __ELAS_H__
+#define __ELAS_H__
+
+#include <iostream>
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <vector>
+#include <emmintrin.h>
+
+// define fixed-width datatypes for Visual Studio projects
+#ifndef _MSC_VER
+  #include <stdint.h>
+#else
+  typedef __int8            int8_t;
+  typedef __int16           int16_t;
+  typedef __int32           int32_t;
+  typedef __int64           int64_t;
+  typedef unsigned __int8   uint8_t;
+  typedef unsigned __int16  uint16_t;
+  typedef unsigned __int32  uint32_t;
+  typedef unsigned __int64  uint64_t;
+#endif
+
+#ifdef PROFILE
+#include "timer.h"
+#endif
+
+class Elas {
+  
+public:
+  
+  enum setting {ROBOTICS,MIDDLEBURY};
+  
+  // parameter settings
+  struct parameters {
+    int32_t disp_min;               // min disparity
+    int32_t disp_max;               // max disparity
+    float   support_threshold;      // max. uniqueness ratio (best vs. second best support match)
+    int32_t support_texture;        // min texture for support points
+    int32_t candidate_stepsize;     // step size of regular grid on which support points are matched
+    int32_t incon_window_size;      // window size of inconsistent support point check
+    int32_t incon_threshold;        // disparity similarity threshold for support point to be considered consistent
+    int32_t incon_min_support;      // minimum number of consistent support points
+    bool    add_corners;            // add support points at image corners with nearest neighbor disparities
+    int32_t grid_size;              // size of neighborhood for additional support point extrapolation
+    float   beta;                   // image likelihood parameter
+    float   gamma;                  // prior constant
+    float   sigma;                  // prior sigma
+    float   sradius;                // prior sigma radius
+    int32_t match_texture;          // min texture for dense matching
+    int32_t lr_threshold;           // disparity threshold for left/right consistency check
+    float   speckle_sim_threshold;  // similarity threshold for speckle segmentation
+    int32_t speckle_size;           // maximal size of a speckle (small speckles get removed)
+    int32_t ipol_gap_width;         // interpolate small gaps (left<->right, top<->bottom)
+    bool    filter_median;          // optional median filter (approximated)
+    bool    filter_adaptive_mean;   // optional adaptive mean filter (approximated)
+    bool    postprocess_only_left;  // saves time by not postprocessing the right image
+    bool    subsampling;            // saves time by only computing disparities for each 2nd pixel
+                                    // note: for this option D1 and D2 must be passed with size
+                                    //       width/2 x height/2 (rounded towards zero)
+    
+    // constructor
+    parameters (setting s=ROBOTICS) {
+      
+      // default settings in a robotics environment
+      // (do not produce results in half-occluded areas
+      //  and are a bit more robust towards lighting etc.)
+      if (s==ROBOTICS) {
+        disp_min              = 0;
+        disp_max              = 255;
+        support_threshold     = 0.85;
+        support_texture       = 10;
+        candidate_stepsize    = 5;
+        incon_window_size     = 5;
+        incon_threshold       = 5;
+        incon_min_support     = 5;
+        add_corners           = 0;
+        grid_size             = 20;
+        beta                  = 0.02;
+        gamma                 = 3;
+        sigma                 = 1;
+        sradius               = 2;
+        match_texture         = 1;
+        lr_threshold          = 2;
+        speckle_sim_threshold = 1;
+        speckle_size          = 200;
+        ipol_gap_width        = 3;
+        filter_median         = 0;
+        filter_adaptive_mean  = 1;
+        postprocess_only_left = 1;
+        subsampling           = 0;
+        
+      // default settings for middlebury benchmark
+      // (interpolate all missing disparities)
+      } else {
+        disp_min              = 0;
+        disp_max              = 255;
+        support_threshold     = 0.95;
+        support_texture       = 10;
+        candidate_stepsize    = 5;
+        incon_window_size     = 5;
+        incon_threshold       = 5;
+        incon_min_support     = 5;
+        add_corners           = 1;
+        grid_size             = 20;
+        beta                  = 0.02;
+        gamma                 = 5;
+        sigma                 = 1;
+        sradius               = 3;
+        match_texture         = 0;
+        lr_threshold          = 2;
+        speckle_sim_threshold = 1;
+        speckle_size          = 200;
+        ipol_gap_width        = 5000;
+        filter_median         = 1;
+        filter_adaptive_mean  = 0;
+        postprocess_only_left = 0;
+        subsampling           = 0;
+      }
+    }
+  };
+
+  // constructor, input: parameters  
+  Elas (parameters param) : param(param) {}
+
+  // deconstructor
+  ~Elas () {}
+  
+  // matching function
+  // inputs: pointers to left (I1) and right (I2) intensity image (uint8, input)
+  //         pointers to left (D1) and right (D2) disparity image (float, output)
+  //         dims[0] = width of I1 and I2
+  //         dims[1] = height of I1 and I2
+  //         dims[2] = bytes per line (often equal to width, but allowed to differ)
+  //         note: D1 and D2 must be allocated before (bytes per line = width)
+  //               if subsampling is not active their size is width x height,
+  //               otherwise width/2 x height/2 (rounded towards zero)
+  void process (uint8_t* I1,uint8_t* I2,float* D1,float* D2,const int32_t* dims);
+  
+private:
+  
+  struct support_pt {
+    int32_t u;
+    int32_t v;
+    int32_t d;
+    support_pt(int32_t u,int32_t v,int32_t d):u(u),v(v),d(d){}
+  };
+
+  struct triangle {
+    int32_t c1,c2,c3;
+    float   t1a,t1b,t1c;
+    float   t2a,t2b,t2c;
+    triangle(int32_t c1,int32_t c2,int32_t c3):c1(c1),c2(c2),c3(c3){}
+  };
+
+  inline uint32_t getAddressOffsetImage (const int32_t& u,const int32_t& v,const int32_t& width) {
+    return v*width+u;
+  }
+
+  inline uint32_t getAddressOffsetGrid (const int32_t& x,const int32_t& y,const int32_t& d,const int32_t& width,const int32_t& disp_num) {
+    return (y*width+x)*disp_num+d;
+  }
+
+  // support point functions
+  void removeInconsistentSupportPoints (int16_t* D_can,int32_t D_can_width,int32_t D_can_height);
+  void removeRedundantSupportPoints (int16_t* D_can,int32_t D_can_width,int32_t D_can_height,
+                                     int32_t redun_max_dist, int32_t redun_threshold, bool vertical);
+  void addCornerSupportPoints (std::vector<support_pt> &p_support);
+  inline int16_t computeMatchingDisparity (const int32_t &u,const int32_t &v,uint8_t* I1_desc,uint8_t* I2_desc,const bool &right_image);
+  std::vector<support_pt> computeSupportMatches (uint8_t* I1_desc,uint8_t* I2_desc);
+
+  // triangulation & grid
+  std::vector<triangle> computeDelaunayTriangulation (std::vector<support_pt> p_support,int32_t right_image);
+  void computeDisparityPlanes (std::vector<support_pt> p_support,std::vector<triangle> &tri,int32_t right_image);
+  void createGrid (std::vector<support_pt> p_support,int32_t* disparity_grid,int32_t* grid_dims,bool right_image);
+
+  // matching
+  inline void updatePosteriorMinimum (__m128i* I2_block_addr,const int32_t &d,const int32_t &w,
+                                      const __m128i &xmm1,__m128i &xmm2,int32_t &val,int32_t &min_val,int32_t &min_d);
+  inline void updatePosteriorMinimum (__m128i* I2_block_addr,const int32_t &d,
+                                      const __m128i &xmm1,__m128i &xmm2,int32_t &val,int32_t &min_val,int32_t &min_d);
+  inline void findMatch (int32_t &u,int32_t &v,float &plane_a,float &plane_b,float &plane_c,
+                         int32_t* disparity_grid,int32_t *grid_dims,uint8_t* I1_desc,uint8_t* I2_desc,
+                         int32_t *P,int32_t &plane_radius,bool &valid,bool &right_image,float* D);
+  void computeDisparity (std::vector<support_pt> p_support,std::vector<triangle> tri,int32_t* disparity_grid,int32_t* grid_dims,
+                         uint8_t* I1_desc,uint8_t* I2_desc,bool right_image,float* D);
+
+  // L/R consistency check
+  void leftRightConsistencyCheck (float* D1,float* D2);
+  
+  // postprocessing
+  void removeSmallSegments (float* D);
+  void gapInterpolation (float* D);
+
+  // optional postprocessing
+  void adaptiveMean (float* D);
+  void median (float* D);
+  
+  // parameter set
+  parameters param;
+  
+  // memory aligned input images + dimensions
+  uint8_t *I1,*I2;
+  int32_t width,height,bpl;
+  
+  // profiling timer
+#ifdef PROFILE
+  Timer timer;
+#endif
+};
+
+#endif

cv-node/lib/CMakeLists.txt

+
+### Lib ELAS ###################################################################
+
+# directories
+set (LIBELAS_SRC_DIR elas)
+# sources
+FILE(GLOB LIBELAS_SRC_FILES "elas/*.cpp")
+
+add_library(libelas ${LIBELAS_SRC_FILES})
+target_include_directories(libelas
+	PRIVATE elas)
+
+

cv-node/lib/elas/descriptor.cpp

+/*
+Copyright 2011. All rights reserved.
+Institute of Measurement and Control Systems
+Karlsruhe Institute of Technology, Germany
+
+This file is part of libelas.
+Authors: Andreas Geiger
+
+libelas 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 3 of the License, or any later version.
+
+libelas 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
+libelas; if not, write to the Free Software Foundation, Inc., 51 Franklin
+Street, Fifth Floor, Boston, MA 02110-1301, USA 
+*/
+
+#include "descriptor.h"
+#include "filter.h"
+#include <emmintrin.h>
+
+using namespace std;
+
+Descriptor::Descriptor(uint8_t* I,int32_t width,int32_t height,int32_t bpl,bool half_resolution) {
+  I_desc        = (uint8_t*)_mm_malloc(16*width*height*sizeof(uint8_t),16);
+  uint8_t* I_du = (uint8_t*)_mm_malloc(bpl*height*sizeof(uint8_t),16);
+  uint8_t* I_dv = (uint8_t*)_mm_malloc(bpl*height*sizeof(uint8_t),16);
+  filter::sobel3x3(I,I_du,I_dv,bpl,height);
+  createDescriptor(I_du,I_dv,width,height,bpl,half_resolution);
+  _mm_free(I_du);
+  _mm_free(I_dv);
+}
+
+Descriptor::~Descriptor() {
+  _mm_free(I_desc);
+}
+
+void Descriptor::createDescriptor (uint8_t* I_du,uint8_t* I_dv,int32_t width,int32_t height,int32_t bpl,bool half_resolution) {
+
+  uint8_t *I_desc_curr;  
+  uint32_t addr_v0,addr_v1,addr_v2,addr_v3,addr_v4;
+  
+  // do not compute every second line
+  if (half_resolution) {
+  
+    // create filter strip
+    for (int32_t v=4; v<height-3; v+=2) {
+
+      addr_v2 = v*bpl;
+      addr_v0 = addr_v2-2*bpl;
+      addr_v1 = addr_v2-1*bpl;
+      addr_v3 = addr_v2+1*bpl;
+      addr_v4 = addr_v2+2*bpl;
+
+      for (int32_t u=3; u<width-3; u++) {
+        I_desc_curr = I_desc+(v*width+u)*16;
+        *(I_desc_curr++) = *(I_du+addr_v0+u+0);
+        *(I_desc_curr++) = *(I_du+addr_v1+u-2);
+        *(I_desc_curr++) = *(I_du+addr_v1+u+0);
+        *(I_desc_curr++) = *(I_du+addr_v1+u+2);
+        *(I_desc_curr++) = *(I_du+addr_v2+u-1);
+        *(I_desc_curr++) = *(I_du+addr_v2+u+0);
+        *(I_desc_curr++) = *(I_du+addr_v2+u+0);
+        *(I_desc_curr++) = *(I_du+addr_v2+u+1);
+        *(I_desc_curr++) = *(I_du+addr_v3+u-2);
+        *(I_desc_curr++) = *(I_du+addr_v3+u+0);
+        *(I_desc_curr++) = *(I_du+addr_v3+u+2);
+        *(I_desc_curr++) = *(I_du+addr_v4+u+0);
+        *(I_desc_curr++) = *(I_dv+addr_v1+u+0);
+        *(I_desc_curr++) = *(I_dv+addr_v2+u-1);
+        *(I_desc_curr++) = *(I_dv+addr_v2+u+1);
+        *(I_desc_curr++) = *(I_dv+addr_v3+u+0);
+      }
+    }
+    
+  // compute full descriptor images
+  } else {
+    
+    // create filter strip
+    for (int32_t v=3; v<height-3; v++) {
+
+      addr_v2 = v*bpl;
+      addr_v0 = addr_v2-2*bpl;
+      addr_v1 = addr_v2-1*bpl;
+      addr_v3 = addr_v2+1*bpl;
+      addr_v4 = addr_v2+2*bpl;
+
+      for (int32_t u=3; u<width-3; u++) {
+        I_desc_curr = I_desc+(v*width+u)*16;
+        *(I_desc_curr++) = *(I_du+addr_v0+u+0);
+        *(I_desc_curr++) = *(I_du+addr_v1+u-2);
+        *(I_desc_curr++) = *(I_du+addr_v1+u+0);
+        *(I_desc_curr++) = *(I_du+addr_v1+u+2);
+        *(I_desc_curr++) = *(I_du+addr_v2+u-1);
+        *(I_desc_curr++) = *(I_du+addr_v2+u+0);
+        *(I_desc_curr++) = *(I_du+addr_v2+u+0);
+        *(I_desc_curr++) = *(I_du+addr_v2+u+1);
+        *(I_desc_curr++) = *(I_du+addr_v3+u-2);
+        *(I_desc_curr++) = *(I_du+addr_v3+u+0);
+        *(I_desc_curr++) = *(I_du+addr_v3+u+2);
+        *(I_desc_curr++) = *(I_du+addr_v4+u+0);
+        *(I_desc_curr++) = *(I_dv+addr_v1+u+0);
+        *(I_desc_curr++) = *(I_dv+addr_v2+u-1);
+        *(I_desc_curr++) = *(I_dv+addr_v2+u+1);
+        *(I_desc_curr++) = *(I_dv+addr_v3+u+0);
+      }
+    }
+  }
+  
+}

cv-node/lib/elas/descriptor.h

+/*
+Copyright 2011. All rights reserved.
+Institute of Measurement and Control Systems
+Karlsruhe Institute of Technology, Germany
+
+This file is part of libelas.
+Authors: Andreas Geiger
+
+libelas 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 3 of the License, or any later version.
+
+libelas 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
+libelas; if not, write to the Free Software Foundation, Inc., 51 Franklin
+Street, Fifth Floor, Boston, MA 02110-1301, USA 
+*/
+
+// NOTE: This descripter is a sparse approximation to the 50-dimensional
+// descriptor described in the paper. It produces similar results, but
+// is faster to compute.
+
+#ifndef __DESCRIPTOR_H__
+#define __DESCRIPTOR_H__
+
+#include <iostream>
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <math.h>
+
+// Define fixed-width datatypes for Visual Studio projects
+#ifndef _MSC_VER
+  #include <stdint.h>
+#else
+  typedef __int8            int8_t;
+  typedef __int16           int16_t;
+  typedef __int32           int32_t;
+  typedef __int64           int64_t;
+  typedef unsigned __int8   uint8_t;
+  typedef unsigned __int16  uint16_t;
+  typedef unsigned __int32  uint32_t;
+  typedef unsigned __int64  uint64_t;
+#endif
+
+class Descriptor {
+  
+public:
+  
+  // constructor creates filters
+  Descriptor(uint8_t* I,int32_t width,int32_t height,int32_t bpl,bool half_resolution);
+  
+  // deconstructor releases memory
+  ~Descriptor();
+  
+  // descriptors accessible from outside
+  uint8_t* I_desc;
+  
+private:
+
+  // build descriptor I_desc from I_du and I_dv
+  void createDescriptor(uint8_t* I_du,uint8_t* I_dv,int32_t width,int32_t height,int32_t bpl,bool half_resolution);
+
+};
+
+#endif

cv-node/lib/elas/filter.h

+/*
+Copyright 2011. All rights reserved.
+Institute of Measurement and Control Systems
+Karlsruhe Institute of Technology, Germany
+
+This file is part of libelas.
+Authors: Julius Ziegler, Andreas Geiger
+
+libelas 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 3 of the License, or any later version.
+
+libelas 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
+libelas; if not, write to the Free Software Foundation, Inc., 51 Franklin
+Street, Fifth Floor, Boston, MA 02110-1301, USA 
+*/
+
+#ifndef __FILTER_H__
+#define __FILTER_H__
+
+#include <emmintrin.h>
+#include <pmmintrin.h>
+
+// define fixed-width datatypes for Visual Studio projects
+#ifndef _MSC_VER
+  #include <stdint.h>
+#else
+  typedef __int8            int8_t;
+  typedef __int16           int16_t;
+  typedef __int32           int32_t;
+  typedef __int64           int64_t;
+  typedef unsigned __int8   uint8_t;
+  typedef unsigned __int16  uint16_t;
+  typedef unsigned __int32  uint32_t;
+  typedef unsigned __int64  uint64_t;
+#endif
+
+// fast filters: implements 3x3 and 5x5 sobel filters and 
+//               5x5 blob and corner filters based on SSE2/3 instructions
+namespace filter {
+  
+  // private namespace, public user functions at the bottom of this file
+  namespace detail {
+    void integral_image( const uint8_t* in, int32_t* out, int w, int h );
+    void unpack_8bit_to_16bit( const __m128i a, __m128i& b0, __m128i& b1 );
+    void pack_16bit_to_8bit_saturate( const __m128i a0, const __m128i a1, __m128i& b );
+    
+    // convolve image with a (1,4,6,4,1) row vector. Result is accumulated into output.
+    // output is scaled by 1/128, then clamped to [-128,128], and finally shifted to [0,255].
+    void convolve_14641_row_5x5_16bit( const int16_t* in, uint8_t* out, int w, int h );
+    
+    // convolve image with a (1,2,0,-2,-1) row vector. Result is accumulated into output.
+    // This one works on 16bit input and 8bit output.
+    // output is scaled by 1/128, then clamped to [-128,128], and finally shifted to [0,255].
+    void convolve_12021_row_5x5_16bit( const int16_t* in, uint8_t* out, int w, int h );
+
+    // convolve image with a (1,2,1) row vector. Result is accumulated into output.
+    // This one works on 16bit input and 8bit output.
+    // output is scaled by 1/4, then clamped to [-128,128], and finally shifted to [0,255].
+    void convolve_121_row_3x3_16bit( const int16_t* in, uint8_t* out, int w, int h );
+    
+    // convolve image with a (1,0,-1) row vector. Result is accumulated into output.
+    // This one works on 16bit input and 8bit output.
+    // output is scaled by 1/4, then clamped to [-128,128], and finally shifted to [0,255].
+    void convolve_101_row_3x3_16bit( const int16_t* in, uint8_t* out, int w, int h );
+    
+    void convolve_cols_5x5( const unsigned char* in, int16_t* out_v, int16_t* out_h, int w, int h );
+    
+    void convolve_col_p1p1p0m1m1_5x5( const unsigned char* in, int16_t* out, int w, int h );
+    
+    void convolve_row_p1p1p0m1m1_5x5( const int16_t* in, int16_t* out, int w, int h );
+    
+    void convolve_cols_3x3( const unsigned char* in, int16_t* out_v, int16_t* out_h, int w, int h );
+  }
+  
+  void sobel3x3( const uint8_t* in, uint8_t* out_v, uint8_t* out_h, int w, int h );
+  
+  void sobel5x5( const uint8_t* in, uint8_t* out_v, uint8_t* out_h, int w, int h );
+  
+  // -1 -1  0  1  1
+  // -1 -1  0  1  1
+  //  0  0  0  0  0
+  //  1  1  0 -1 -1
+  //  1  1  0 -1 -1
+  void checkerboard5x5( const uint8_t* in, int16_t* out, int w, int h );
+  
+  // -1 -1 -1 -1 -1
+  // -1  1  1  1 -1
+  // -1  1  8  1 -1
+  // -1  1  1  1 -1
+  // -1 -1 -1 -1 -1
+  void blob5x5( const uint8_t* in, int16_t* out, int w, int h );
+};
+
+#endif

cv-node/lib/elas/matrix.h

+/*
+Copyright 2011. 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 
+*/
+
+#ifndef MATRIX_H
+#define MATRIX_H
+
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <iostream>
+#include <vector>
+
+#ifndef _MSC_VER
+  #include <stdint.h>
+#else
+  typedef __int8            int8_t;
+  typedef __int16           int16_t;
+  typedef __int32           int32_t;
+  typedef __int64           int64_t;
+  typedef unsigned __int8   uint8_t;
+  typedef unsigned __int16  uint16_t;
+  typedef unsigned __int32  uint32_t;
+  typedef unsigned __int64  uint64_t;
+#endif
+
+#define endll endl << endl // double end line definition
+
+typedef double FLOAT;      // double precision
+//typedef float  FLOAT;    // single precision
+
+class Matrix {
+
+public:
+
+  // constructor / deconstructor
+  Matrix ();                                                  // init empty 0x0 matrix
+  Matrix (const int32_t m,const int32_t n);                   // init empty mxn matrix
+  Matrix (const int32_t m,const int32_t n,const FLOAT* val_); // init mxn matrix with values from array 'val'
+  Matrix (const Matrix &M);                                   // creates deepcopy of M
+  ~Matrix ();
+
+  // assignment operator, copies contents of M
+  Matrix& operator= (const Matrix &M);
+
+  // copies submatrix of M into array 'val', default values copy whole row/column/matrix
+  void getData(FLOAT* val_,int32_t i1=0,int32_t j1=0,int32_t i2=-1,int32_t j2=-1);
+
+  // set or get submatrices of current matrix
+  Matrix getMat(int32_t i1,int32_t j1,int32_t i2=-1,int32_t j2=-1);
+  void   setMat(const Matrix &M,const int32_t i,const int32_t j);
+
+  // set sub-matrix to scalar (default 0), -1 as end replaces whole row/column/matrix
+  void setVal(FLOAT s,int32_t i1=0,int32_t j1=0,int32_t i2=-1,int32_t j2=-1);
+
+  // set (part of) diagonal to scalar, -1 as end replaces whole diagonal
+  void setDiag(FLOAT s,int32_t i1=0,int32_t i2=-1);
+
+  // clear matrix
+  void zero();
+  
+  // extract columns with given index
+  Matrix extractCols (std::vector<int> idx);
+
+  // create identity matrix
+  static Matrix eye (const int32_t m);
+  void          eye ();
+
+  // create diagonal matrix with nx1 or 1xn matrix M as elements
+  static Matrix diag(const Matrix &M);
+  
+  // returns the m-by-n matrix whose elements are taken column-wise from M
+  static Matrix reshape(const Matrix &M,int32_t m,int32_t n);
+
+  // create 3x3 rotation matrices (convention: http://en.wikipedia.org/wiki/Rotation_matrix)
+  static Matrix rotMatX(const FLOAT &angle);
+  static Matrix rotMatY(const FLOAT &angle);
+  static Matrix rotMatZ(const FLOAT &angle);
+
+  // simple arithmetic operations
+  Matrix  operator+ (const Matrix &M); // add matrix
+  Matrix  operator- (const Matrix &M); // subtract matrix
+  Matrix  operator* (const Matrix &M); // multiply with matrix
+  Matrix  operator* (const FLOAT &s);  // multiply with scalar
+  Matrix  operator/ (const Matrix &M); // divide elementwise by matrix (or vector)
+  Matrix  operator/ (const FLOAT &s);  // divide by scalar
+  Matrix  operator- ();                // negative matrix
+  Matrix  operator~ ();                // transpose
+  FLOAT   l2norm ();                   // euclidean norm (vectors) / frobenius norm (matrices)
+  FLOAT   mean ();                     // mean of all elements in matrix
+
+  // complex arithmetic operations
+  static Matrix cross (const Matrix &a, const Matrix &b);    // cross product of two vectors
+  static Matrix inv (const Matrix &M);                       // invert matrix M
+  bool   inv ();                                             // invert this matrix
+  FLOAT  det ();                                             // returns determinant of matrix
+  bool   solve (const Matrix &M,FLOAT eps=1e-20);            // solve linear system M*x=B, replaces *this and M
+  bool   lu(int32_t *idx, FLOAT &d, FLOAT eps=1e-20);        // replace *this by lower upper decomposition
+  void   svd(Matrix &U,Matrix &W,Matrix &V);                 // singular value decomposition *this = U*diag(W)*V^T
+
+  // print matrix to stream
+  friend std::ostream& operator<< (std::ostream& out,const Matrix& M);
+
+  // direct data access
+  FLOAT   **val;
+  int32_t   m,n;
+
+private:
+
+  void allocateMemory (const int32_t m_,const int32_t n_);
+  void releaseMemory ();
+  inline FLOAT pythag(FLOAT a,FLOAT b);
+
+};
+
+#endif // MATRIX_H