changed complex_urban config

.gitignore

@@ -99,3 +99,6 @@ my_settings.txt
 borrar/*
 
 */ExecMean.txt
+
+# Unignore selected files:
+!timeutil.*

Examples/Stereo/ComplexUrban.yaml

@@ -3,73 +3,54 @@
 #--------------------------------------------------------------------------------------------
 # Camera Parameters. Adjust them!
 #--------------------------------------------------------------------------------------------
-#File.version: "1.0"
+File.version: "1.0"
 
 Camera.type: "PinHole"
 
-# Camera calibration and distortion parameters (OpenCV) 
 Camera1.fx: 816.9037
 Camera1.fy: 811.5680
 Camera1.cx: 608.5072
 Camera1.cy: 263.4759
 
-Camera.fx: 816.9037
+Camera1.k1: -0.056143027782782
-Camera.fy: 811.5680
+Camera1.k2: 0.139525632030074
-Camera.cx: 608.5072
+Camera1.k3: -0.080878168794740
-Camera.cy: 263.4759
+Camera1.p1: -0.001215590686538
+Camera1.p2: -9.728139470848294e-04
 
-# Equidistant distortion 0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182
-#Camera.bFishEye: 1
-Camera.k1: -0.056143027782782
-Camera.k2: 0.139525632030074
-#Camera.k3: -0.080878168794740
-Camera.p1: -0.001215590686538
-Camera.p2: -9.728139470848294e-04
-
-# Second camera
-#Camera2.type: "PinHole"
 Camera2.fx: 8.137820553958778e+02
 Camera2.fy: 8.085216557426838e+02
 Camera2.cx: 6.138641953932292e+02
 Camera2.cy: 2.494104934865211e+02
 
-# Equidistant distortion 0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182
-#Camera.bFishEye: 1
 Camera2.k1: -0.054921981757479
 Camera2.k2: 0.142436574286058
-#Camera2.k3: -0.085665408330633
+Camera2.k3: -0.085665408330633
 Camera2.p1: 7.541230501084140e-05
-Camera2.p1: -6.756052742429136e-04
+Camera2.p2: -6.756052742429136e-04
 
-Tlr: !!opencv-matrix
-  rows: 3
-  cols: 4
-  dt: f
-  data: [ 0.999991433056271,   0.002082719758761,   0.003577162628705,   -4.751436000507748e-1,
-            -0.002066281899364,   0.999987318309473,   -0.004592787752592,   -1.144594387276745e-3,
-            -0.003586682754036,  0.004585356980047,   0.999983054960527,   2.012928530231533e-3]
-    
-# Camera resolution
 Camera.width: 1280
 Camera.height: 560
 
-# Lapping area between images
-Camera.lappingBegin: 0
-Camera.lappingEnd: 1279
-
-Camera2.lappingBegin: 0
-Camera2.lappingEnd: 1279
-
 # Camera frames per second 
-Camera.fps: 20
+Camera.fps: 10
 
 # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
 Camera.RGB: 1
 
 # Close/Far threshold. Baseline times.
-ThDepth: 40.0
+# Is taken from the stereoParam calibration file. 
+Stereo.ThDepth: 60.0
+Stereo.T_c1_c2: !!opencv-matrix
+  rows: 4
+  cols: 4
+  dt: f
+  data: [ 0.999991433056271,   0.002082719758761,   0.003577162628705,   -4.751436000507748e-1,
+            -0.002066281899364,   0.999987318309473,   -0.004592787752592,   -1.144594387276745e-3,
+            -0.003586682754036,  0.004585356980047,   0.999983054960527,   2.012928530231533e-3,
+            0, 0, 0, 1.000000000000000]
 
-Camera.bf: 388.151174248813
+#Camera.bf: 388.151174248813
 
 #--------------------------------------------------------------------------------------------
 # ORB Parameters

Thirdparty/g2o/g2o/stuff/timeutil.cpp

@@ -0,0 +1,124 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// 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.
+//
+// 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
+// HOLDER 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 "timeutil.h"
+#include <iostream>
+
+#ifdef _WINDOWS
+#include <time.h>
+#include <windows.h>
+#endif
+
+#ifdef UNIX
+#include <unistd.h>
+#endif
+
+namespace g2o {
+
+#ifdef _WINDOWS
+#if defined(_MSC_VER) || defined(_MSC_EXTENSIONS)
+  #define DELTA_EPOCH_IN_MICROSECS  11644473600000000Ui64
+#else
+  #define DELTA_EPOCH_IN_MICROSECS  11644473600000000ULL
+#endif
+
+struct timezone
+{
+  int  tz_minuteswest; /* minutes W of Greenwich */
+  int  tz_dsttime;     /* type of dst correction */
+};
+
+int gettimeofday(struct timeval *tv, struct timezone *tz)
+{
+// Define a structure to receive the current Windows filetime
+  FILETIME ft;
+ 
+// Initialize the present time to 0 and the timezone to UTC
+  unsigned __int64 tmpres = 0;
+  static int tzflag = 0;
+ 
+  if (NULL != tv)
+  {
+    GetSystemTimeAsFileTime(&ft);
+ 
+// The GetSystemTimeAsFileTime returns the number of 100 nanosecond 
+// intervals since Jan 1, 1601 in a structure. Copy the high bits to 
+// the 64 bit tmpres, shift it left by 32 then or in the low 32 bits.
+    tmpres |= ft.dwHighDateTime;
+    tmpres <<= 32;
+    tmpres |= ft.dwLowDateTime;
+ 
+// Convert to microseconds by dividing by 10
+    tmpres /= 10;
+ 
+// The Unix epoch starts on Jan 1 1970.  Need to subtract the difference 
+// in seconds from Jan 1 1601.
+    tmpres -= DELTA_EPOCH_IN_MICROSECS;
+ 
+// Finally change microseconds to seconds and place in the seconds value. 
+// The modulus picks up the microseconds.
+    tv->tv_sec = (long)(tmpres / 1000000UL);
+    tv->tv_usec = (long)(tmpres % 1000000UL);
+  }
+ 
+  if (NULL != tz) {
+    if (!tzflag) {
+      _tzset();
+      tzflag++;
+    }
+
+    long sec;
+    int hours;
+    _get_timezone(&sec);
+    _get_daylight(&hours);
+  
+// Adjust for the timezone west of Greenwich
+    tz->tz_minuteswest = sec / 60;
+    tz->tz_dsttime = hours;
+  }
+ 
+  return 0;
+}
+#endif
+
+ScopeTime::ScopeTime(const char* title) : _title(title), _startTime(get_monotonic_time()) {}
+
+ScopeTime::~ScopeTime() {
+  std::cerr << _title<<" took "<<1000*(get_monotonic_time()-_startTime)<<"ms.\n";
+}
+
+double get_monotonic_time()
+{
+#if (defined(_POSIX_TIMERS) && (_POSIX_TIMERS+0 >= 0) && defined(_POSIX_MONOTONIC_CLOCK))
+  struct timespec ts;
+  clock_gettime(CLOCK_MONOTONIC, &ts);
+  return ts.tv_sec + ts.tv_nsec*1e-9;
+#else
+  return get_time();
+#endif
+}
+
+} // end namespace

Thirdparty/g2o/g2o/stuff/timeutil.h

@@ -0,0 +1,132 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// 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.
+//
+// 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
+// HOLDER 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.
+
+#ifndef G2O_TIMEUTIL_H
+#define G2O_TIMEUTIL_H
+
+#ifdef _WINDOWS
+#include <time.h>
+#else
+#include <sys/time.h>
+#endif
+
+#include <string>
+
+
+/** @addtogroup utils **/
+// @{
+
+/** \file timeutil.h
+ * \brief utility functions for handling time related stuff
+ */
+
+/// Executes code, only if secs are gone since last exec.
+/// extended version, in which the current time is given, e.g., timestamp of IPC message
+#ifndef DO_EVERY_TS
+#define DO_EVERY_TS(secs, currentTime, code) \
+if (1) {\
+  static double s_lastDone_ = (currentTime); \
+  double s_now_ = (currentTime); \
+  if (s_lastDone_ > s_now_) \
+    s_lastDone_ = s_now_; \
+  if (s_now_ - s_lastDone_ > (secs)) { \
+    code; \
+    s_lastDone_ = s_now_; \
+  }\
+} else \
+  (void)0
+#endif
+
+/// Executes code, only if secs are gone since last exec.
+#ifndef DO_EVERY
+#define DO_EVERY(secs, code) DO_EVERY_TS(secs, g2o::get_time(), code)
+#endif
+
+#ifndef MEASURE_TIME
+#define MEASURE_TIME(text, code) \
+  if(1) { \
+    double _start_time_ = g2o::get_time(); \
+    code; \
+    fprintf(stderr, "%s took %f sec\n", text, g2o::get_time() - _start_time_); \
+  } else \
+    (void) 0
+#endif
+
+namespace g2o {
+
+#ifdef _WINDOWS
+typedef struct timeval {
+  long tv_sec;
+  long tv_usec;
+} timeval;
+ int gettimeofday(struct timeval *tv, struct timezone *tz);
+#endif
+
+/**
+ * return the current time in seconds since 1. Jan 1970
+ */
+inline double get_time() 
+{
+  struct timeval ts;
+  gettimeofday(&ts,0);
+  return ts.tv_sec + ts.tv_usec*1e-6;
+}
+
+/**
+ * return a monotonic increasing time which basically does not need to
+ * have a reference point. Consider this for measuring how long some
+ * code fragments required to execute.
+ *
+ * On Linux we call clock_gettime() on other systems we currently
+ * call get_time().
+ */
+ double get_monotonic_time();
+
+/**
+ * \brief Class to measure the time spent in a scope
+ *
+ * To use this class, e.g. to measure the time spent in a function,
+ * just create and instance at the beginning of the function.
+ */
+class  ScopeTime {
+  public: 
+    ScopeTime(const char* title);
+    ~ScopeTime();
+  private:
+    std::string _title;
+    double _startTime;
+};
+
+} // end namespace
+
+#ifndef MEASURE_FUNCTION_TIME
+#define MEASURE_FUNCTION_TIME \
+  g2o::ScopeTime scopeTime(__PRETTY_FUNCTION__)
+#endif
+
+
+// @}
+#endif