init GPU

CPU_version/svo/.idea/.gitignore

@@ -0,0 +1,8 @@
+# Default ignored files
+/shelf/
+/workspace.xml
+# Datasource local storage ignored files
+/dataSources/
+/dataSources.local.xml
+# Editor-based HTTP Client requests
+/httpRequests/

CPU_version/svo/.idea/misc.xml

@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="CMakeWorkspace" PROJECT_DIR="$PROJECT_DIR$" />
+</project>

CPU_version/svo/.idea/modules.xml

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectModuleManager">
+    <modules>
+      <module fileurl="file://$PROJECT_DIR$/.idea/svo.iml" filepath="$PROJECT_DIR$/.idea/svo.iml" />
+    </modules>
+  </component>
+</project>

CPU_version/svo/.idea/svo.iml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<module classpath="CMake" type="CPP_MODULE" version="4" />

CPU_version/svo/.idea/vcs.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="VcsDirectoryMappings">
+    <mapping directory="$PROJECT_DIR$/.." vcs="Git" />
+  </component>
+</project>

CPU_version/svo/CMakeLists.txt

@@ -0,0 +1,156 @@
+################################################################################
+# user build settings
+
+SET(TRACE TRUE)
+SET(HAVE_G2O FALSE) 
+SET(USE_ROS TRUE) # Set FALSE if you want to build this package without Catkin
+SET(DEBUG_OUTPUT FALSE) # Only relevant if build without ROS
+
+################################################################################
+
+SET(PROJECT_NAME svo)
+PROJECT(${PROJECT_NAME})
+CMAKE_MINIMUM_REQUIRED (VERSION 2.8.3)
+SET(CMAKE_BUILD_TYPE Release) # Release, RelWithDebInfo
+SET(CMAKE_VERBOSE_MAKEFILE OFF)
+SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/CMakeModules/")
+set(CMAKE_C_STANDARD 14)
+set(CMAKE_CXX_STANDARD 14)
+
+# Set definitions
+IF(TRACE)
+  ADD_DEFINITIONS(-DSVO_TRACE)
+ENDIF()
+IF(HAVE_G2O)
+  ADD_DEFINITIONS(-DUSE_BUNDLE_ADJUSTMENT)
+ENDIF()
+IF(USE_ROS)
+  ADD_DEFINITIONS(-DSVO_USE_ROS)
+ENDIF()
+IF(DEBUG_OUTPUT)
+  ADD_DEFINITIONS(-DSVO_DEBUG_OUTPUT)
+ENDIF()
+
+# Set build flags, set ARM_ARCHITECTURE environment variable on Odroid
+# Set build flags. Set IS_ARM on odroid board as environment variable
+SET(CMAKE_CXX_FLAGS "-Wall -D_LINUX -D_REENTRANT -march=native -Wno-unused-variable -Wno-unused-but-set-variable -Wno-unknown-pragmas")
+IF(DEFINED ENV{ARM_ARCHITECTURE})
+  SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ")
+ELSE()
+  SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mmmx -msse -msse -msse2 -msse3 -mssse3")
+ENDIF()
+IF(CMAKE_COMPILER_IS_GNUCC)
+  SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+ELSE()
+  SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+ENDIF()
+SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops")
+
+# Add plain cmake packages 
+FIND_PACKAGE(OpenCV REQUIRED)
+FIND_PACKAGE(Eigen REQUIRED)
+FIND_PACKAGE(Sophus REQUIRED)
+FIND_PACKAGE(fast REQUIRED)
+FIND_PACKAGE(Boost REQUIRED COMPONENTS thread system)
+FIND_PACKAGE(GTSAM REQUIRED)
+
+set(GTSAM_INCLUDE_DIRS "${GTSAM_INCLUDE_DIR}")
+IF(USE_ROS)
+  FIND_PACKAGE(catkin REQUIRED COMPONENTS roscpp roslib cmake_modules vikit_common vikit_ros)
+  catkin_package(
+      DEPENDS Eigen OpenCV Sophus Boost fast GTSAM
+      CATKIN_DEPENDS roscpp roslib vikit_common vikit_ros
+      INCLUDE_DIRS include
+      LIBRARIES svo
+  )
+ELSE()
+  FIND_PACKAGE(vikit_common REQUIRED)
+  SET(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+  SET(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+ENDIF()
+
+# Include dirs
+INCLUDE_DIRECTORIES(
+        include
+  ${Eigen_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
+  ${Sophus_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIRS}
+  ${fast_INCLUDE_DIRS} ${GTSAM_INCLUDE_DIR}
+)
+
+# Set link libraries
+LIST(APPEND LINK_LIBS 
+  ${OpenCV_LIBS}
+  ${Sophus_LIBRARIES}
+  ${Boost_LIBRARIES}
+  ${fast_LIBRARIES}
+)
+IF(USE_ROS)
+  INCLUDE_DIRECTORIES(${catkin_INCLUDE_DIRS})
+  LIST(APPEND LINK_LIBS ${catkin_LIBRARIES})
+ELSE()
+  INCLUDE_DIRECTORIES(${vikit_common_INCLUDE_DIRS})
+  LIST(APPEND LINK_LIBS ${vikit_common_LIBRARIES})
+ENDIF()
+
+# Set sourcefiles
+LIST(APPEND SOURCEFILES
+        src/frame_handler_mono.cpp
+        src/frame_handler_base.cpp
+        src/frame.cpp
+        src/point.cpp
+        src/map.cpp
+        src/pose_optimizer.cpp
+        src/initialization.cpp
+        src/matcher.cpp
+        src/reprojector.cpp
+        src/feature_alignment.cpp
+        src/feature_detection.cpp
+        src/depth_filter.cpp
+        src/config.cpp
+        src/sparse_img_align.cpp src/imu_integration.cpp)
+
+# Add g2o if available
+IF(HAVE_G2O)
+  FIND_PACKAGE(G2O REQUIRED)
+  INCLUDE_DIRECTORIES(
+    ${G2O_INCLUDE_DIR}
+    /usr/include/suitesparse # for cholmod
+  )
+  LIST(APPEND LINK_LIBS
+    ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY} ${G2O_SOLVER_CHOLMOD} ${G2O_SOLVER_CSPARSE} ${G2O_SOLVER_DENSE}
+    ${G2O_SOLVER_PCG} ${G2O_TYPES_SBA} cholmod cxsparse
+  )
+  LIST(APPEND SOURCEFILES src/bundle_adjustment.cpp)
+ENDIF(HAVE_G2O)
+
+# Create svo library
+ADD_LIBRARY(svo SHARED ${SOURCEFILES})
+TARGET_LINK_LIBRARIES(svo ${LINK_LIBS} gtsam)
+set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 14)
+set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD_REQUIRED ON)
+
+################################################################################
+# TESTS
+
+#ADD_EXECUTABLE(test_feature_align test/test_feature_alignment.cpp)
+#TARGET_LINK_LIBRARIES(test_feature_align svo)
+
+#ADD_EXECUTABLE(test_pipeline test/test_pipeline.cpp)
+#TARGET_LINK_LIBRARIES(test_pipeline svo)
+
+#ADD_EXECUTABLE(test_matcher test/test_matcher.cpp)
+#TARGET_LINK_LIBRARIES(test_matcher svo)
+
+#ADD_EXECUTABLE(test_feature_detection test/test_feature_detection.cpp)
+#TARGET_LINK_LIBRARIES(test_feature_detection svo)
+
+#ADD_EXECUTABLE(test_depth_filter test/test_depth_filter.cpp)
+#TARGET_LINK_LIBRARIES(test_depth_filter svo)
+
+#ADD_EXECUTABLE(test_sparse_img_align test/test_sparse_img_align.cpp)
+#TARGET_LINK_LIBRARIES(test_sparse_img_align svo)
+
+#ADD_EXECUTABLE(test_pose_optimizer test/test_pose_optimizer.cpp)
+#TARGET_LINK_LIBRARIES(test_pose_optimizer svo)

CPU_version/svo/CMakeModules/FindEigen.cmake

@@ -0,0 +1,81 @@
+###############################################################################
+# 
+# CMake script for finding the Eigen library.
+# 
+# http://eigen.tuxfamily.org/index.php?title=Main_Page
+# 
+# Copyright (c) 2006, 2007 Montel Laurent, <[email protected]>
+# Copyright (c) 2008, 2009 Gael Guennebaud, <[email protected]>
+# Copyright (c) 2009 Benoit Jacob <[email protected]>
+# Redistribution and use is allowed according to the terms of the 2-clause BSD
+# license.
+# 
+# 
+# Input variables:
+# 
+# - Eigen_ROOT_DIR (optional): When specified, header files and libraries
+#   will be searched for in `${Eigen_ROOT_DIR}/include` and
+#   `${Eigen_ROOT_DIR}/libs` respectively, and the default CMake search order
+#   will be ignored. When unspecified, the default CMake search order is used.
+#   This variable can be specified either as a CMake or environment variable.
+#   If both are set, preference is given to the CMake variable.
+#   Use this variable for finding packages installed in a nonstandard location,
+#   or for enforcing that one of multiple package installations is picked up.
+# 
+# Cache variables (not intended to be used in CMakeLists.txt files)
+# 
+# - Eigen_INCLUDE_DIR: Absolute path to package headers.
+# 
+# 
+# Output variables:
+# 
+# - Eigen_FOUND: Boolean that indicates if the package was found
+# - Eigen_INCLUDE_DIRS: Paths to the necessary header files
+# - Eigen_VERSION: Version of Eigen library found
+# - Eigen_DEFINITIONS: Definitions to be passed on behalf of eigen
+# 
+# 
+# Example usage:
+# 
+#   # Passing the version means Eigen_FOUND will only be TRUE if a
+#   # version >= the provided version is found.
+#   find_package(Eigen 3.1.2)
+#   if(NOT Eigen_FOUND)
+#     # Error handling
+#   endif()
+#   ...
+#   add_definitions(${Eigen_DEFINITIONS})
+#   ...
+#   include_directories(${Eigen_INCLUDE_DIRS} ...)
+# 
+###############################################################################
+
+find_package(PkgConfig)
+pkg_check_modules(PC_EIGEN eigen3)
+set(EIGEN_DEFINITIONS ${PC_EIGEN_CFLAGS_OTHER})
+
+
+find_path(EIGEN_INCLUDE_DIR Eigen/Core
+    HINTS ${PC_EIGEN_INCLUDEDIR} ${PC_EIGEN_INCLUDE_DIRS}
+          "${Eigen_ROOT_DIR}" "$ENV{EIGEN_ROOT_DIR}"
+          "${EIGEN_ROOT}" "$ENV{EIGEN_ROOT}"  # Backwards Compatibility
+    PATHS "$ENV{PROGRAMFILES}/Eigen" "$ENV{PROGRAMW6432}/Eigen"
+          "$ENV{PROGRAMFILES}/Eigen 3.0.0" "$ENV{PROGRAMW6432}/Eigen 3.0.0"
+    PATH_SUFFIXES eigen3 include/eigen3 include)
+
+set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR})
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR)
+
+mark_as_advanced(EIGEN_INCLUDE_DIR)
+
+if(EIGEN_FOUND)
+  message(STATUS "Eigen found (include: ${EIGEN_INCLUDE_DIRS})")
+endif(EIGEN_FOUND)
+
+
+set(Eigen_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS})
+set(Eigen_FOUND ${EIGEN_FOUND})
+set(Eigen_VERSION ${EIGEN_VERSION})
+set(Eigen_DEFINITIONS ${EIGEN_DEFINITIONS})

CPU_version/svo/CMakeModules/FindG2O.cmake

@@ -0,0 +1,113 @@
+# Find the header files
+
+FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h
+  ${G2O_ROOT}/include
+  $ENV{G2O_ROOT}/include
+  $ENV{G2O_ROOT}
+  /usr/local/include
+  /usr/include
+  /opt/local/include
+  /sw/local/include
+  /sw/include
+  NO_DEFAULT_PATH
+  )
+
+# Macro to unify finding both the debug and release versions of the
+# libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY
+# macro.
+
+MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME)
+
+  FIND_LIBRARY("${MYLIBRARY}_DEBUG"
+    NAMES "g2o_${MYLIBRARYNAME}_d"
+    PATHS
+    ${G2O_ROOT}/lib/Debug
+    ${G2O_ROOT}/lib
+    $ENV{G2O_ROOT}/lib/Debug
+    $ENV{G2O_ROOT}/lib
+    NO_DEFAULT_PATH
+    )
+
+  FIND_LIBRARY("${MYLIBRARY}_DEBUG"
+    NAMES "g2o_${MYLIBRARYNAME}_d"
+    PATHS
+    ~/Library/Frameworks
+    /Library/Frameworks
+    /usr/local/lib
+    /usr/local/lib64
+    /usr/lib
+    /usr/lib64
+    /opt/local/lib
+    /sw/local/lib
+    /sw/lib
+    )
+  
+  FIND_LIBRARY(${MYLIBRARY}
+    NAMES "g2o_${MYLIBRARYNAME}"
+    PATHS
+    ${G2O_ROOT}/lib/Release
+    ${G2O_ROOT}/lib
+    $ENV{G2O_ROOT}/lib/Release
+    $ENV{G2O_ROOT}/lib
+    NO_DEFAULT_PATH
+    )
+
+  FIND_LIBRARY(${MYLIBRARY}
+    NAMES "g2o_${MYLIBRARYNAME}"
+    PATHS
+    ~/Library/Frameworks
+    /Library/Frameworks
+    /usr/local/lib
+    /usr/local/lib64
+    /usr/lib
+    /usr/lib64
+    /opt/local/lib
+    /sw/local/lib
+    /sw/lib
+    )
+  
+  IF(NOT ${MYLIBRARY}_DEBUG)
+    IF(MYLIBRARY)
+      SET(${MYLIBRARY}_DEBUG ${MYLIBRARY})
+    ENDIF(MYLIBRARY)
+  ENDIF( NOT ${MYLIBRARY}_DEBUG)
+  
+ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME)
+
+# Find the core elements
+FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff)
+FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core)
+
+# Find the CLI library
+FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli)
+
+# Find the pluggable solvers
+FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod)
+FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse)
+FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension)
+FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense)
+FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg)
+FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear)
+FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only)
+FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen)
+
+# Find the predefined types
+FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data)
+FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp)
+FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba)
+FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d)
+FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3)
+FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d)
+FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d)
+
+# G2O solvers declared found if we found at least one solver
+SET(G2O_SOLVERS_FOUND "NO")
+IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN)
+  SET(G2O_SOLVERS_FOUND "YES")
+ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN)
+
+# G2O itself declared found if we found the core libraries and at least one solver
+SET(G2O_FOUND "NO")
+IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)
+  SET(G2O_FOUND "YES")
+ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)

CPU_version/svo/doc/.gitignore

@@ -0,0 +1,2 @@
+.gitignore
+!.gitignore

CPU_version/svo/include/svo/bundle_adjustment.h

@@ -0,0 +1,109 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_BUNDLE_ADJUSTMENT_H_
+#define SVO_BUNDLE_ADJUSTMENT_H_
+
+#include <svo/global.h>
+
+namespace g2o {
+class EdgeProjectXYZ2UV;
+class SparseOptimizer;
+class VertexSE3Expmap;
+class VertexSBAPointXYZ;
+}
+
+namespace svo {
+
+typedef g2o::EdgeProjectXYZ2UV g2oEdgeSE3;
+typedef g2o::VertexSE3Expmap g2oFrameSE3;
+typedef g2o::VertexSBAPointXYZ g2oPoint;
+
+class Frame;
+class Point;
+class Feature;
+class Map;
+
+/// Local, global and 2-view bundle adjustment with g2o
+namespace ba {
+
+/// Temporary container to hold the g2o edge with reference to frame and point.
+struct EdgeContainerSE3
+{
+  g2oEdgeSE3*     edge;
+  Frame*          frame;
+  Feature*        feature;
+  bool            is_deleted;
+  EdgeContainerSE3(g2oEdgeSE3* e, Frame* frame, Feature* feature) :
+    edge(e), frame(frame), feature(feature), is_deleted(false)
+  {}
+};
+
+/// Optimize two camera frames and their observed 3D points.
+/// Is used after initialization.
+void twoViewBA(Frame* frame1, Frame* frame2, double reproj_thresh, Map* map);
+
+/// Local bundle adjustment.
+/// Optimizes core_kfs and their observed map points while keeping the
+/// neighbourhood fixed.
+void localBA(
+    Frame* center_kf,
+    set<FramePtr>* core_kfs,
+    Map* map,
+    size_t& n_incorrect_edges_1,
+    size_t& n_incorrect_edges_2,
+    double& init_error,
+    double& final_error);
+
+/// Global bundle adjustment.
+/// Optimizes the whole map. Is currently not used in SVO.
+void globalBA(Map* map);
+
+/// Initialize g2o with solver type, optimization strategy and camera model.
+void setupG2o(g2o::SparseOptimizer * optimizer);
+
+/// Run the optimization on the provided graph.
+void runSparseBAOptimizer(
+    g2o::SparseOptimizer* optimizer,
+    unsigned int num_iter,
+    double& init_error,
+    double& final_error);
+
+/// Create a g2o vertice from a keyframe object.
+g2oFrameSE3* createG2oFrameSE3(
+    Frame* kf,
+    size_t id,
+    bool fixed);
+
+/// Creates a g2o vertice from a mappoint object.
+g2oPoint* createG2oPoint(
+    Vector3d pos,
+    size_t id,
+    bool fixed);
+
+/// Creates a g2o edge between a g2o keyframe and mappoint vertice with the provided measurement.
+g2oEdgeSE3* createG2oEdgeSE3(
+    g2oFrameSE3* v_kf,
+    g2oPoint* v_mp,
+    const Vector2d& f_up,
+    bool robust_kernel,
+    double huber_width,
+    double weight = 1);
+
+} // namespace ba
+} // namespace svo
+
+#endif // SVO_BUNDLE_ADJUSTMENT_H_

CPU_version/svo/include/svo/config.h

@@ -0,0 +1,177 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_CONFIG_H_
+#define SVO_CONFIG_H_
+
+#include <string>
+#include <stdint.h>
+#include <stdio.h>
+
+namespace svo {
+
+using std::string;
+
+/// Global configuration file of SVO.
+/// Implements the Singleton design pattern to allow global access and to ensure
+/// that only one instance exists.
+class Config
+{
+public:
+  static Config& getInstance();
+
+  /// Base-name of the tracefiles.
+  static string& traceName() { return getInstance().trace_name; }
+
+  /// Directory where the tracefiles are saved.
+  static string& traceDir() { return getInstance().trace_dir; }
+
+  /// Number of pyramid levels used for features.
+  static size_t& nPyrLevels() { return getInstance().n_pyr_levels; }
+
+  /// Use the IMU to get relative rotations.
+  static bool& useImu() { return getInstance().use_imu; }
+
+  /// Number of keyframes in the core. The core-kfs are optimized through bundle adjustment.
+  static size_t& coreNKfs() { return getInstance().core_n_kfs; }
+
+  /// Initial scale of the map. Depends on the distance the camera is moved for the initialization.
+  static double& mapScale() { return getInstance().map_scale; }
+
+  /// Feature grid size of a cell in [px].
+  static size_t& gridSize() { return getInstance().grid_size; }
+
+  /// Initialization: Minimum required disparity between the first two frames.
+  static double& initMinDisparity() { return getInstance().init_min_disparity; }
+
+  /// Initialization: Minimum number of tracked features.
+  static size_t& initMinTracked() { return getInstance().init_min_tracked; }
+
+  /// Initialization: Minimum number of inliers after RANSAC.
+  static size_t& initMinInliers() { return getInstance().init_min_inliers; }
+
+  /// Maximum level of the Lucas Kanade tracker.
+  static size_t& kltMaxLevel() { return getInstance().klt_max_level; }
+
+  /// Minimum level of the Lucas Kanade tracker.
+  static size_t& kltMinLevel() { return getInstance().klt_min_level; }
+
+  /// Reprojection threshold [px].
+  static double& reprojThresh() { return getInstance().reproj_thresh; }
+
+  /// Reprojection threshold after pose optimization.
+  static double& poseOptimThresh() { return getInstance().poseoptim_thresh; }
+
+  /// Number of iterations in local bundle adjustment.
+  static size_t& poseOptimNumIter() { return getInstance().poseoptim_num_iter; }
+
+  /// Maximum number of points to optimize at every iteration.
+  static size_t& structureOptimMaxPts() { return getInstance().structureoptim_max_pts; }
+
+  /// Number of iterations in structure optimization.
+  static size_t& structureOptimNumIter() { return getInstance().structureoptim_num_iter; }
+
+  /// Reprojection threshold after bundle adjustment.
+  static double& lobaThresh() { return getInstance().loba_thresh; }
+
+  /// Threshold for the robust Huber kernel of the local bundle adjustment.
+  static double& lobaRobustHuberWidth() { return getInstance().loba_robust_huber_width; }
+
+  /// Number of iterations in the local bundle adjustment.
+  static size_t& lobaNumIter() { return getInstance().loba_num_iter; }
+
+  /// Minimum distance between two keyframes. Relative to the average height in the map.
+  static double& kfSelectMinDist() { return getInstance().kfselect_mindist; }
+
+  /// Select only features with a minimum Harris corner score for triangulation.
+  static double& triangMinCornerScore() { return getInstance().triang_min_corner_score; }
+
+  /// Subpixel refinement of reprojection and triangulation. Set to 0 if no subpix refinement required!
+  static size_t& subpixNIter() { return getInstance().subpix_n_iter; }
+
+  /// Limit the number of keyframes in the map. This makes nslam essentially.
+  /// a Visual Odometry. Set to 0 if unlimited number of keyframes are allowed.
+  /// Minimum number of keyframes is 3.
+  static size_t& maxNKfs() { return getInstance().max_n_kfs; }
+
+  /// How much (in milliseconds) is the camera delayed with respect to the imu.
+  static double& imgImuDelay() { return getInstance().img_imu_delay; }
+
+  /// Maximum number of features that should be tracked.
+  static size_t& maxFts() { return getInstance().max_fts; }
+
+  /// If the number of tracked features drops below this threshold. Tracking quality is bad.
+  static size_t& qualityMinFts() { return getInstance().quality_min_fts; }
+
+  /// If within one frame, this amount of features are dropped. Tracking quality is bad.
+  static int& qualityMaxFtsDrop() { return getInstance().quality_max_drop_fts; }
+
+  /// acc white noise in continuous.
+  static double& ACC_Noise() { return getInstance().ACC_noise; }
+
+  /// acc white noise in continuous.
+  static double& GYO_Noise() { return getInstance().GYO_noise; }
+  /// integration uncertainty continuous
+  static double& IUC() { return getInstance().iuc; }
+    // acc bias in continuous
+  static double& ABC() { return getInstance().abc; }
+    // gyro bias in continuous
+  static double& GBC() { return getInstance().gbc; }
+    // error in the bias used for preintegration
+  static double& EBP() { return getInstance().ebp; }
+    // the noise used for projection
+  static double& NRP() { return getInstance().nrp; }
+
+
+private:
+  Config();
+  Config(Config const&);
+  void operator=(Config const&);
+  string trace_name;
+  string trace_dir;
+  size_t n_pyr_levels;
+  bool use_imu;
+  size_t core_n_kfs;
+  double map_scale;
+  size_t grid_size;
+  double init_min_disparity;
+  size_t init_min_tracked;
+  size_t init_min_inliers;
+  size_t klt_max_level;
+  size_t klt_min_level;
+  double reproj_thresh;
+  double poseoptim_thresh;
+  size_t poseoptim_num_iter;
+  size_t structureoptim_max_pts;
+  size_t structureoptim_num_iter;
+  double loba_thresh;
+  double loba_robust_huber_width;
+  size_t loba_num_iter;
+  double kfselect_mindist;
+  double triang_min_corner_score;
+  size_t triang_half_patch_size;
+  size_t subpix_n_iter;
+  size_t max_n_kfs;
+  double img_imu_delay;
+  size_t max_fts;
+  size_t quality_min_fts;
+  int quality_max_drop_fts;
+  double nrp,ebp,gbc,abc,iuc,ACC_noise,GYO_noise;
+};
+
+} // namespace svo
+
+#endif // SVO_CONFIG_H_

CPU_version/svo/include/svo/depth_filter.h

@@ -0,0 +1,171 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_DEPTH_FILTER_H_
+#define SVO_DEPTH_FILTER_H_
+
+#include <queue>
+#include <boost/thread.hpp>
+#include <boost/function.hpp>
+#include <vikit/performance_monitor.h>
+#include <svo/global.h>
+#include <svo/feature_detection.h>
+#include <svo/matcher.h>
+
+namespace svo {
+
+class Frame;
+class Feature;
+class Point;
+
+/// A seed is a probabilistic depth estimate for a single pixel.
+struct Seed
+{
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  static int batch_counter;
+  static int seed_counter;
+  int batch_id;                //!< Batch id is the id of the keyframe for which the seed was created.
+  int id;                      //!< Seed ID, only used for visualization.
+  Feature* ftr;                //!< Feature in the keyframe for which the depth should be computed.
+  float a;                     //!< a of Beta distribution: When high, probability of inlier is large.
+  float b;                     //!< b of Beta distribution: When high, probability of outlier is large.
+  float mu;                    //!< Mean of normal distribution.
+  float z_range;               //!< Max range of the possible depth.
+  float sigma2;                //!< Variance of normal distribution.
+  Matrix2d patch_cov;          //!< Patch covariance in reference image.
+  Seed(Feature* ftr, float depth_mean, float depth_min);
+};
+
+/// Depth filter implements the Bayesian Update proposed in:
+/// "Video-based, Real-Time Multi View Stereo" by G. Vogiatzis and C. Hernández.
+/// In Image and Vision Computing, 29(7):434-441, 2011.
+///
+/// The class uses a callback mechanism such that it can be used also by other
+/// algorithms than nslam and for simplified testing.
+class DepthFilter
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  typedef boost::unique_lock<boost::mutex> lock_t;
+  typedef boost::function<void ( Point*, double )> callback_t;
+
+  /// Depth-filter config parameters
+  struct Options
+  {
+    bool check_ftr_angle;                       //!< gradient features are only updated if the epipolar line is orthogonal to the gradient.
+    bool epi_search_1d;                         //!< restrict Gauss Newton in the epipolar search to the epipolar line.
+    bool verbose;                               //!< display output.
+    bool use_photometric_disparity_error;       //!< use photometric disparity error instead of 1px error in tau computation.
+    int max_n_kfs;                              //!< maximum number of keyframes for which we maintain seeds.
+    double sigma_i_sq;                          //!< image noise.
+    double seed_convergence_sigma2_thresh;      //!< threshold on depth uncertainty for convergence.
+    Options()
+    : check_ftr_angle(false),
+      epi_search_1d(false),
+      verbose(false),
+      use_photometric_disparity_error(false),
+      max_n_kfs(3),
+      sigma_i_sq(5e-4),
+      seed_convergence_sigma2_thresh(200.0)
+    {}
+  } options_;
+
+  DepthFilter(
+      feature_detection::DetectorPtr feature_detector,
+      callback_t seed_converged_cb);
+
+  virtual ~DepthFilter();
+
+  /// Start this thread when seed updating should be in a parallel thread.
+  void startThread();
+
+  /// Stop the parallel thread that is running.
+  void stopThread();
+
+  /// Add frame to the queue to be processed.
+  void addFrame(FramePtr frame);
+
+  /// Add new keyframe to the queue
+  void addKeyframe(FramePtr frame, double depth_mean, double depth_min);
+
+  /// Remove all seeds which are initialized from the specified keyframe. This
+  /// function is used to make sure that no seeds points to a non-existent frame
+  /// when a frame is removed from the map.
+  void removeKeyframe(FramePtr frame);
+
+  /// If the map is reset, call this function such that we don't have pointers
+  /// to old frames.
+  void reset();
+
+  /// Returns a copy of the seeds belonging to frame. Thread-safe.
+  /// Can be used to compute the Next-Best-View in parallel.
+  /// IMPORTANT! Make sure you hold a valid reference counting pointer to frame
+  /// so it is not being deleted while you use it.
+  void getSeedsCopy(const FramePtr& frame, std::list<Seed>& seeds);
+
+  /// Return a reference to the seeds. This is NOT THREAD SAFE!
+  //std::list<Seed>& getSeeds() { return seeds_; } Bug 238
+  std::list<Seed, Eigen::aligned_allocator<Seed>>& getSeeds() { return seeds_; }
+
+  /// Bayes update of the seed, x is the measurement, tau2 the measurement uncertainty
+  static void updateSeed(
+      const float x,
+      const float tau2,
+      Seed* seed);
+
+  /// Compute the uncertainty of the measurement.
+  static double computeTau(
+      const SE3& T_ref_cur,
+      const Vector3d& f,
+      const double z,
+      const double px_error_angle);
+
+protected:
+  feature_detection::DetectorPtr feature_detector_;
+  callback_t seed_converged_cb_;
+  //std::list<Seed> seeds_;
+  std::list<Seed, Eigen::aligned_allocator<Seed>> seeds_;
+  boost::mutex seeds_mut_;
+  bool seeds_updating_halt_;            //!< Set this value to true when seeds updating should be interrupted.
+  boost::thread* thread_;
+  std::queue<FramePtr> frame_queue_;
+  boost::mutex frame_queue_mut_;
+  boost::condition_variable frame_queue_cond_;
+  FramePtr new_keyframe_;               //!< Next keyframe to extract new seeds.
+  bool new_keyframe_set_;               //!< Do we have a new keyframe to process?.
+  double new_keyframe_min_depth_;       //!< Minimum depth in the new keyframe. Used for range in new seeds.
+  double new_keyframe_mean_depth_;      //!< Maximum depth in the new keyframe. Used for range in new seeds.
+  vk::PerformanceMonitor permon_;       //!< Separate performance monitor since the DepthFilter runs in a parallel thread.
+  Matcher matcher_;
+
+  /// Initialize new seeds from a frame.
+  void initializeSeeds(FramePtr frame);
+
+  /// Update all seeds with a new measurement frame.
+  virtual void updateSeeds(FramePtr frame);
+
+  /// When a new keyframe arrives, the frame queue should be cleared.
+  void clearFrameQueue();
+
+  /// A thread that is continuously updating the seeds.
+  void updateSeedsLoop();
+};
+
+} // namespace svo
+
+#endif // SVO_DEPTH_FILTER_H_

CPU_version/svo/include/svo/feature.h

@@ -0,0 +1,75 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_FEATURE_H_
+#define SVO_FEATURE_H_
+
+#include <svo/frame.h>
+
+namespace svo {
+
+/// A salient image region that is tracked across frames.
+struct Feature
+{
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  enum FeatureType {
+    CORNER,
+    EDGELET
+  };
+
+  FeatureType type;     //!< Type can be corner or edgelet.
+  Frame* frame;         //!< Pointer to frame in which the feature was detected.
+  Vector2d px;          //!< Coordinates in pixels on pyramid level 0.
+  Vector3d f;           //!< Unit-bearing vector of the feature.
+  int level;            //!< Image pyramid level where feature was extracted.
+  Point* point;         //!< Pointer to 3D point which corresponds to the feature.
+  Vector2d grad;        //!< Dominant gradient direction for edglets, normalized.
+
+  Feature(Frame* _frame, const Vector2d& _px, int _level) :
+    type(CORNER),
+    frame(_frame),
+    px(_px),
+    f(frame->cam_->cam2world(px)),
+    level(_level),
+    point(NULL),
+    grad(1.0,0.0)
+  {}
+
+  Feature(Frame* _frame, const Vector2d& _px, const Vector3d& _f, int _level) :
+    type(CORNER),
+    frame(_frame),
+    px(_px),
+    f(_f),
+    level(_level),
+    point(NULL),
+    grad(1.0,0.0)
+  {}
+
+  Feature(Frame* _frame, Point* _point, const Vector2d& _px, const Vector3d& _f, int _level) :
+    type(CORNER),
+    frame(_frame),
+    px(_px),
+    f(_f),
+    level(_level),
+    point(_point),
+    grad(1.0,0.0)
+  {}
+};
+
+} // namespace svo
+
+#endif // SVO_FEATURE_H_

CPU_version/svo/include/svo/feature_alignment.h

@@ -0,0 +1,63 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_FEATURE_ALIGNMENT_H_
+#define SVO_FEATURE_ALIGNMENT_H_
+
+#include <svo/global.h>
+
+namespace svo {
+
+/// Subpixel refinement of a reference feature patch with the current image.
+/// Implements the inverse-compositional approach (see "Lucas-Kanade 20 Years on"
+/// paper by Baker.
+namespace feature_alignment {
+
+bool align1D(
+    const cv::Mat& cur_img,
+    const Vector2f& dir,                  // direction in which the patch is allowed to move
+    uint8_t* ref_patch_with_border,
+    uint8_t* ref_patch,
+    const int n_iter,
+    Vector2d& cur_px_estimate,
+    double& h_inv);
+
+bool align2D(
+    const cv::Mat& cur_img,
+    uint8_t* ref_patch_with_border,
+    uint8_t* ref_patch,
+    const int n_iter,
+    Vector2d& cur_px_estimate,
+    bool no_simd = false);
+
+bool align2D_SSE2(
+    const cv::Mat& cur_img,
+    uint8_t* ref_patch_with_border,
+    uint8_t* ref_patch,
+    const int n_iter,
+    Vector2d& cur_px_estimate);
+
+bool align2D_NEON(
+    const cv::Mat& cur_img,
+    uint8_t* ref_patch_with_border,
+    uint8_t* ref_patch,
+    const int n_iter,
+    Vector2d& cur_px_estimate);
+
+} // namespace feature_alignment
+} // namespace svo
+
+#endif // SVO_FEATURE_ALIGNMENT_H_

CPU_version/svo/include/svo/feature_detection.h

@@ -0,0 +1,107 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_FEATURE_DETECTION_H_
+#define SVO_FEATURE_DETECTION_H_
+
+#include <svo/global.h>
+#include <svo/frame.h>
+
+namespace svo {
+
+/// Implementation of various feature detectors.
+namespace feature_detection {
+
+/// Temporary container used for corner detection. Features are initialized from these.
+struct Corner
+{
+  int x;        //!< x-coordinate of corner in the image.
+  int y;        //!< y-coordinate of corner in the image.
+  int level;    //!< pyramid level of the corner.
+  float score;  //!< shi-tomasi score of the corner.
+  float angle;  //!< for gradient-features: dominant gradient angle.
+  Corner(int x, int y, float score, int level, float angle) :
+    x(x), y(y), level(level), score(score), angle(angle)
+  {}
+};
+typedef vector<Corner> Corners;
+
+/// All detectors should derive from this abstract class.
+class AbstractDetector
+{
+public:
+  AbstractDetector(
+      const int img_width,
+      const int img_height,
+      const int cell_size,
+      const int n_pyr_levels);
+
+  virtual ~AbstractDetector() {};
+
+  virtual void detect(
+      Frame* frame,
+      const ImgPyr& img_pyr,
+      const double detection_threshold,
+      Features& fts) = 0;
+
+  /// Flag the grid cell as occupied
+  void setGridOccpuancy(const Vector2d& px);
+
+  /// Set grid cells of existing features as occupied
+  void setExistingFeatures(const Features& fts);
+
+protected:
+
+  static const int border_ = 8; //!< no feature should be within 8px of border.
+  const int cell_size_;
+  const int n_pyr_levels_;
+  const int grid_n_cols_;
+  const int grid_n_rows_;
+  vector<bool> grid_occupancy_;
+
+  void resetGrid();
+
+  inline int getCellIndex(int x, int y, int level)
+  {
+    const int scale = (1<<level);
+    return (scale*y)/cell_size_*grid_n_cols_ + (scale*x)/cell_size_;
+  }
+};
+typedef boost::shared_ptr<AbstractDetector> DetectorPtr;
+
+/// FAST detector by Edward Rosten.
+class FastDetector : public AbstractDetector
+{
+public:
+  FastDetector(
+      const int img_width,
+      const int img_height,
+      const int cell_size,
+      const int n_pyr_levels);
+
+  virtual ~FastDetector() {}
+
+  virtual void detect(
+      Frame* frame,
+      const ImgPyr& img_pyr,
+      const double detection_threshold,
+      Features& fts);
+};
+
+} // namespace feature_detection
+} // namespace svo
+
+#endif // SVO_FEATURE_DETECTION_H_

CPU_version/svo/include/svo/frame.h

@@ -0,0 +1,154 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_FRAME_H_
+#define SVO_FRAME_H_
+
+#include <sophus/se3.h>
+#include <vikit/math_utils.h>
+#include <vikit/abstract_camera.h>
+#include <boost/noncopyable.hpp>
+#include <svo/global.h>
+
+namespace g2o {
+class VertexSE3Expmap;
+}
+typedef g2o::VertexSE3Expmap g2oFrameSE3;
+
+namespace svo {
+
+class Point;
+struct Feature;
+
+typedef list<Feature*> Features;
+typedef vector<cv::Mat> ImgPyr;
+
+/// A frame saves the image, the associated features and the estimated pose.
+class Frame : boost::noncopyable
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    
+  static int                    frame_counter_;         //!< Counts the number of created frames. Used to set the unique id.
+  int                           id_;                    //!< Unique id of the frame.
+  double                        timestamp_;             //!< Timestamp of when the image was recorded.
+  vk::AbstractCamera*           cam_;                   //!< Camera model.
+  Sophus::SE3                   T_f_w_;                 //!< Transform (f)rame from (w)orld.
+  Matrix<double, 6, 6>          Cov_;                   //!< Covariance.
+  ImgPyr                        img_pyr_;               //!< Image Pyramid.
+  Features                      fts_;                   //!< List of features in the image.
+  vector<Feature*>              key_pts_;               //!< Five features and associated 3D points which are used to detect if two frames have overlapping field of view.
+  bool                          is_keyframe_;           //!< Was this frames selected as keyframe?
+  g2oFrameSE3*                  v_kf_;                  //!< Temporary pointer to the g2o node object of the keyframe.
+  int                           last_published_ts_;     //!< Timestamp of last publishing.
+
+  Frame(vk::AbstractCamera* cam, const cv::Mat& img, double timestamp);
+  ~Frame();
+
+  /// Initialize new frame and create image pyramid.
+  void initFrame(const cv::Mat& img);
+
+  /// Select this frame as keyframe.
+  void setKeyframe();
+
+  /// Add a feature to the image
+  void addFeature(Feature* ftr);
+
+  /// The KeyPoints are those five features which are closest to the 4 image corners
+  /// and to the center and which have a 3D point assigned. These points are used
+  /// to quickly check whether two frames have overlapping field of view.
+  void setKeyPoints();
+
+  /// Check if we can select five better key-points.
+  void checkKeyPoints(Feature* ftr);
+
+  /// If a point is deleted, we must remove the corresponding key-point.
+  void removeKeyPoint(Feature* ftr);
+
+  /// Return number of point observations.
+  inline size_t nObs() const { return fts_.size(); }
+
+  /// Check if a point in (w)orld coordinate frame is visible in the image.
+  bool isVisible(const Vector3d& xyz_w) const;
+
+  /// Full resolution image stored in the frame.
+  inline const cv::Mat& img() const { return img_pyr_[0]; }
+
+  /// Was this frame selected as keyframe?
+  inline bool isKeyframe() const { return is_keyframe_; }
+
+  /// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c).
+  inline Vector2d w2c(const Vector3d& xyz_w) const { return cam_->world2cam( T_f_w_ * xyz_w ); }
+
+  /// Transforms pixel coordinates (c) to frame unit sphere coordinates (f).
+  inline Vector3d c2f(const Vector2d& px) const { return cam_->cam2world(px[0], px[1]); }
+
+  /// Transforms pixel coordinates (c) to frame unit sphere coordinates (f).
+  inline Vector3d c2f(const double x, const double y) const { return cam_->cam2world(x, y); }
+
+  /// Transforms point coordinates in world-frame (w) to camera-frams (f).
+  inline Vector3d w2f(const Vector3d& xyz_w) const { return T_f_w_ * xyz_w; }
+
+  /// Transforms point from frame unit sphere (f) frame to world coordinate frame (w).
+  inline Vector3d f2w(const Vector3d& f) const { return T_f_w_.inverse() * f; }
+
+  /// Projects Point from unit sphere (f) in camera pixels (c).
+  inline Vector2d f2c(const Vector3d& f) const { return cam_->world2cam( f ); }
+
+  /// Return the pose of the frame in the (w)orld coordinate frame.
+  inline Vector3d pos() const { return T_f_w_.inverse().translation(); }
+
+  /// Frame jacobian for projection of 3D point in (f)rame coordinate to
+  /// unit plane coordinates uv (focal length = 1).
+  inline static void jacobian_xyz2uv(
+      const Vector3d& xyz_in_f,
+      Matrix<double,2,6>& J)
+  {
+    const double x = xyz_in_f[0];
+    const double y = xyz_in_f[1];
+    const double z_inv = 1./xyz_in_f[2];
+    const double z_inv_2 = z_inv*z_inv;
+
+    J(0,0) = -z_inv;              // -1/z
+    J(0,1) = 0.0;                 // 0
+    J(0,2) = x*z_inv_2;           // x/z^2
+    J(0,3) = y*J(0,2);            // x*y/z^2
+    J(0,4) = -(1.0 + x*J(0,2));   // -(1.0 + x^2/z^2)
+    J(0,5) = y*z_inv;             // y/z
+
+    J(1,0) = 0.0;                 // 0
+    J(1,1) = -z_inv;              // -1/z
+    J(1,2) = y*z_inv_2;           // y/z^2
+    J(1,3) = 1.0 + y*J(1,2);      // 1.0 + y^2/z^2
+    J(1,4) = -J(0,3);             // -x*y/z^2
+    J(1,5) = -x*z_inv;            // x/z
+  }
+};
+
+
+/// Some helper functions for the frame object.
+namespace frame_utils {
+
+/// Creates an image pyramid of half-sampled images.
+void createImgPyramid(const cv::Mat& img_level_0, int n_levels, ImgPyr& pyr);
+
+/// Get the average depth of the features in the image.
+bool getSceneDepth(const Frame& frame, double& depth_mean, double& depth_min);
+
+} // namespace frame_utils
+} // namespace svo
+
+#endif // SVO_FRAME_H_

CPU_version/svo/include/svo/frame_handler_base.h

@@ -0,0 +1,123 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_FRAME_HANDLER_BASE_H_
+#define SVO_FRAME_HANDLER_BASE_H_
+
+#include <queue>
+#include <vikit/timer.h>
+#include <vikit/ringbuffer.h>
+#include <boost/noncopyable.hpp>
+#include <boost/function.hpp>
+#include <boost/thread.hpp>
+#include <svo/global.h>
+#include <svo/map.h>
+
+namespace vk
+{
+class AbstractCamera;
+class PerformanceMonitor;
+}
+
+namespace svo
+{
+class Point;
+class Matcher;
+class DepthFilter;
+
+/// Base class for various VO pipelines. Manages the map and the state machine.
+class FrameHandlerBase : boost::noncopyable
+{
+public:
+  enum Stage {
+    STAGE_PAUSED,
+    STAGE_FIRST_FRAME,
+    STAGE_SECOND_FRAME,
+    STAGE_DEFAULT_FRAME,
+    STAGE_RELOCALIZING
+  };
+  enum TrackingQuality {
+    TRACKING_INSUFFICIENT,
+    TRACKING_BAD,
+    TRACKING_GOOD
+  };
+  enum UpdateResult {
+    RESULT_NO_KEYFRAME,
+    RESULT_IS_KEYFRAME,
+    RESULT_FAILURE
+  };
+
+  FrameHandlerBase();
+
+  virtual ~FrameHandlerBase();
+
+  /// Get the current map.
+  const Map& map() const { return map_; }
+
+  /// Will reset the map as soon as the current frame is finished processing.
+  void reset() { set_reset_ = true; }
+
+  /// Start processing.
+  void start() { set_start_ = true; }
+
+  /// Get the current stage of the algorithm.
+  Stage stage() const { return stage_; }
+
+  /// Get tracking quality.
+  TrackingQuality trackingQuality() const { return tracking_quality_; }
+
+  /// Get the processing time of the previous iteration.
+  double lastProcessingTime() const { return timer_.getTime(); }
+
+  /// Get the number of feature observations of the last frame.
+  size_t lastNumObservations() const { return num_obs_last_; }
+
+protected:
+  Stage stage_;                 //!< Current stage of the algorithm.
+  bool set_reset_;              //!< Flag that the user can set. Will reset the system before the next iteration.
+  bool set_start_;              //!< Flag the user can set to start the system when the next image is received.
+  Map map_;                     //!< Map of keyframes created by the slam system.
+  vk::Timer timer_;             //!< Stopwatch to measure time to process frame.
+  vk::RingBuffer<double> acc_frame_timings_;    //!< Total processing time of the last 10 frames, used to give some user feedback on the performance.
+  vk::RingBuffer<size_t> acc_num_obs_;          //!< Number of observed features of the last 10 frames, used to give some user feedback on the tracking performance.
+  size_t num_obs_last_;                         //!< Number of observations in the previous frame.
+  TrackingQuality tracking_quality_;            //!< An estimate of the tracking quality based on the number of tracked features.
+
+  /// Before a frame is processed, this function is called.
+  bool startFrameProcessingCommon(const double timestamp);
+
+  /// When a frame is finished processing, this function is called.
+  int finishFrameProcessingCommon(
+      const size_t update_id,
+      const UpdateResult dropout,
+      const size_t num_observations);
+
+  /// Reset the map and frame handler to start from scratch.
+  void resetCommon();
+
+  /// Reset the frame handler. Implement in derived class.
+  virtual void resetAll() { resetCommon(); }
+
+  /// Set the tracking quality based on the number of tracked features.
+  virtual void setTrackingQuality(const size_t num_observations);
+
+  /// Optimize some of the observed 3D points.
+  virtual void optimizeStructure(FramePtr frame, size_t max_n_pts, int max_iter);
+};
+
+} // namespace nslam
+
+#endif // SVO_FRAME_HANDLER_BASE_H_

CPU_version/svo/include/svo/frame_handler_mono.h

@@ -0,0 +1,104 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_FRAME_HANDLER_H_
+#define SVO_FRAME_HANDLER_H_
+
+#include <set>
+#include <vikit/abstract_camera.h>
+#include <svo/frame_handler_base.h>
+#include <svo/reprojector.h>
+#include <svo/initialization.h>
+#include <svo/imu_integration.h>
+
+namespace svo {
+
+/// Monocular Visual Odometry Pipeline as described in the SVO paper.
+class FrameHandlerMono : public FrameHandlerBase
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  
+  FrameHandlerMono(vk::AbstractCamera* cam,Sophus::SE3& SE_init);
+  virtual ~FrameHandlerMono();
+
+  /// Provide an image.
+  void addImage(const cv::Mat& img, double timestamp);
+
+  /// Set the first frame (used for synthetic datasets in benchmark node)
+  void setFirstFrame(const FramePtr& first_frame);
+
+  /// Get the last frame that has been processed.
+  FramePtr lastFrame() { return last_frame_; }
+
+  /// Get the set of spatially closest keyframes of the last frame.
+  const set<FramePtr>& coreKeyframes() { return core_kfs_; }
+
+  /// Return the feature track to visualize the KLT tracking during initialization.
+  const vector<cv::Point2f>& initFeatureTrackRefPx() const { return klt_homography_init_.px_ref_; }
+  const vector<cv::Point2f>& initFeatureTrackCurPx() const { return klt_homography_init_.px_cur_; }
+
+  /// Access the depth filter.
+  DepthFilter* depthFilter() const { return depth_filter_; }
+
+  /// An external place recognition module may know where to relocalize.
+  bool relocalizeFrameAtPose(
+      const int keyframe_id,
+      const SE3& T_kf_f,
+      const cv::Mat& img,
+      const double timestamp);
+  std::unique_ptr<Imu_Integration> imu_integPtr_;  //Integration of IMU
+
+protected:
+  vk::AbstractCamera* cam_;                     //!< Camera model, can be ATAN, Pinhole or Ocam (see vikit).
+  Reprojector reprojector_;                     //!< Projects points from other keyframes into the current frame
+  FramePtr new_frame_;                          //!< Current frame.
+  FramePtr last_frame_;                         //!< Last frame, not necessarily a keyframe.
+  set<FramePtr> core_kfs_;                      //!< Keyframes in the closer neighbourhood.
+  vector< pair<FramePtr,size_t> > overlap_kfs_; //!< All keyframes with overlapping field of view. the paired number specifies how many common mappoints are observed TODO: why vector!?
+  initialization::KltHomographyInit klt_homography_init_; //!< Used to estimate pose of the first two keyframes by estimating a homography.
+  DepthFilter* depth_filter_;                   //!< Depth estimation algorithm runs in a parallel thread and is used to initialize new 3D points.
+
+
+  /// Initialize the visual odometry algorithm.
+  virtual void initialize();
+
+  /// Processes the first frame and sets it as a keyframe.
+  virtual UpdateResult processFirstFrame();
+
+  /// Processes all frames after the first frame until a keyframe is selected.
+  virtual UpdateResult processSecondFrame();
+
+  /// Processes all frames after the first two keyframes.
+  virtual UpdateResult processFrame();
+
+  /// Try relocalizing the frame at relative position to provided keyframe.
+  virtual UpdateResult relocalizeFrame(
+      const SE3& T_cur_ref,
+      FramePtr ref_keyframe);
+
+  /// Reset the frame handler. Implement in derived class.
+  virtual void resetAll();
+
+  /// Keyframe selection criterion.
+  virtual bool needNewKf(double scene_depth_mean);
+
+  void setCoreKfs(size_t n_closest);
+};
+
+} // namespace svo
+
+#endif // SVO_FRAME_HANDLER_H_

CPU_version/svo/include/svo/global.h

@@ -0,0 +1,101 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_GLOBAL_H_
+#define SVO_GLOBAL_H_
+
+#include <list>
+#include <vector>
+#include <string>
+#include <stdint.h>
+#include <stdio.h>
+#include <math.h>
+
+#include <Eigen/Core>
+#include <opencv2/opencv.hpp>
+#include <sophus/se3.h>
+#include <vikit/performance_monitor.h>
+#include <boost/shared_ptr.hpp>
+#include<Eigen/StdVector>
+#ifndef RPG_SVO_VIKIT_IS_VECTOR_SPECIALIZED //Guard for rpg_vikit
+#define RPG_SVO_VIKIT_IS_VECTOR_SPECIALIZED
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector3d)
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2d)
+#endif
+
+#ifdef SVO_USE_ROS
+  #include <ros/console.h>
+  #define SVO_DEBUG_STREAM(x) ROS_DEBUG_STREAM(x)
+  #define SVO_INFO_STREAM(x) ROS_INFO_STREAM(x)
+  #define SVO_WARN_STREAM(x) ROS_WARN_STREAM(x)
+  #define SVO_WARN_STREAM_THROTTLE(rate, x) ROS_WARN_STREAM_THROTTLE(rate, x)
+  #define SVO_ERROR_STREAM(x) ROS_ERROR_STREAM(x)
+#else
+  #define SVO_INFO_STREAM(x) std::cerr<<"\033[0;0m[INFO] "<<x<<"\033[0;0m"<<std::endl;
+  #ifdef SVO_DEBUG_OUTPUT
+    #define SVO_DEBUG_STREAM(x) SVO_INFO_STREAM(x)
+  #else
+    #define SVO_DEBUG_STREAM(x)
+  #endif
+  #define SVO_WARN_STREAM(x) std::cerr<<"\033[0;33m[WARN] "<<x<<"\033[0;0m"<<std::endl;
+  #define SVO_ERROR_STREAM(x) std::cerr<<"\033[1;31m[ERROR] "<<x<<"\033[0;0m"<<std::endl;
+  #include <chrono> // Adapted from rosconsole. Copyright (c) 2008, Willow Garage, Inc.
+  #define SVO_WARN_STREAM_THROTTLE(rate, x) \
+    do { \
+      static double __log_stream_throttle__last_hit__ = 0.0; \
+      std::chrono::time_point<std::chrono::system_clock> __log_stream_throttle__now__ = \
+      std::chrono::system_clock::now(); \
+      if (__log_stream_throttle__last_hit__ + rate <= \
+          std::chrono::duration_cast<std::chrono::seconds>( \
+          __log_stream_throttle__now__.time_since_epoch()).count()) { \
+        __log_stream_throttle__last_hit__ = \
+        std::chrono::duration_cast<std::chrono::seconds>( \
+        __log_stream_throttle__now__.time_since_epoch()).count(); \
+        SVO_WARN_STREAM(x); \
+      } \
+    } while(0)
+#endif
+
+namespace svo
+{
+  using namespace Eigen;
+  using namespace Sophus;
+
+  const double EPS = 0.0000000001;
+  const double PI = 3.14159265;
+
+#ifdef SVO_TRACE
+  extern vk::PerformanceMonitor* g_permon;
+  #define SVO_LOG(value) g_permon->log(std::string((#value)),(value))
+  #define SVO_LOG2(value1, value2) SVO_LOG(value1); SVO_LOG(value2)
+  #define SVO_LOG3(value1, value2, value3) SVO_LOG2(value1, value2); SVO_LOG(value3)
+  #define SVO_LOG4(value1, value2, value3, value4) SVO_LOG2(value1, value2); SVO_LOG2(value3, value4)
+  #define SVO_START_TIMER(name) g_permon->startTimer((name))
+  #define SVO_STOP_TIMER(name) g_permon->stopTimer((name))
+#else
+  #define SVO_LOG(v)
+  #define SVO_LOG2(v1, v2)
+  #define SVO_LOG3(v1, v2, v3)
+  #define SVO_LOG4(v1, v2, v3, v4)
+  #define SVO_START_TIMER(name)
+  #define SVO_STOP_TIMER(name)
+#endif
+
+  class Frame;
+  typedef boost::shared_ptr<Frame> FramePtr;
+} // namespace svo
+
+#endif // SVO_GLOBAL_H_

CPU_version/svo/include/svo/imu_integration.h

@@ -0,0 +1,121 @@
+//
+// Created by gufe on 4/9/21.
+//
+
+#ifndef SVO_IMU_INTEGRATION_H
+#define SVO_IMU_INTEGRATION_H
+#include <gtsam/navigation/CombinedImuFactor.h>
+#include <gtsam/navigation/ImuFactor.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/navigation/ImuBias.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/slam/ProjectionFactor.h>
+#include <gtsam/nonlinear/ISAM2.h>
+#include <sophus/se3.h>
+#include <svo/frame.h>
+
+using gtsam::symbol_shorthand::B;  // Bias  (ax,ay,az,gx,gy,gz)
+using gtsam::symbol_shorthand::V;  // Vel   (xdot,ydot,zdot)
+using gtsam::symbol_shorthand::P;  // Pose3 (x,y,z,r,p,y)
+using gtsam::symbol_shorthand::C;  // Camera pose
+
+//https://stackoverflow.com/questions/24881799/get-index-in-c11-foreach-loop
+// Wrapper class
+template <typename T>
+class enumerate_impl
+{
+public:
+    // The return value of the operator* of the iterator, this
+    // is what you will get inside of the for loop
+    struct item
+    {
+        size_t index;
+        typename T::value_type & item;
+    };
+    typedef item value_type;
+
+    // Custom iterator with minimal interface
+    struct iterator
+    {
+        iterator(typename T::iterator _it, size_t counter=0) :
+                it(_it), counter(counter)
+        {}
+
+        iterator operator++()
+        {
+            return iterator(++it, ++counter);
+        }
+
+        bool operator!=(iterator other)
+        {
+            return it != other.it;
+        }
+
+        typename T::iterator::value_type item()
+        {
+            return *it;
+        }
+
+        value_type operator*()
+        {
+            return value_type{counter, *it};
+        }
+
+        size_t index()
+        {
+            return counter;
+        }
+
+    private:
+        typename T::iterator it;
+        size_t counter;
+    };
+
+    enumerate_impl(T & t) : container(t) {}
+
+    iterator begin()
+    {
+        return iterator(container.begin());
+    }
+
+    iterator end()
+    {
+        return iterator(container.end());
+    }
+
+private:
+    T & container;
+};
+
+// A templated free function allows you to create the wrapper class
+// conveniently
+template <typename T>
+enumerate_impl<T> enumerate(T & t)
+{
+    return enumerate_impl<T>(t);
+}
+
+class Imu_Integration{
+public:
+    Imu_Integration(Sophus::SE3& SE_init);
+    ~Imu_Integration();
+    bool reset(gtsam::ISAM2& optimizer,boost::shared_ptr<svo::Frame>& new_frame);
+    bool reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost::shared_ptr<svo::Frame>& new_frame);
+    bool update(double* imu= nullptr);
+    bool predict(boost::shared_ptr<svo::Frame>&,std::size_t&,const double reproj_thresh);
+private:
+    std::shared_ptr<gtsam::NavState> statePtr;
+    std::shared_ptr<gtsam::imuBias::ConstantBias> imu_biasPtr;
+    std::shared_ptr<gtsam::Values> valuesPtr;
+    std::shared_ptr<gtsam::NonlinearFactorGraph> graphPtr;
+    boost::shared_ptr<gtsam::PreintegratedCombinedMeasurements::Params> parameterPtr;
+    std::shared_ptr<gtsam::PreintegratedCombinedMeasurements> preintegratedPtr;
+    boost::shared_ptr<gtsam::noiseModel::Isotropic> ProjectNoisePtr;
+    std::chrono::steady_clock::time_point t_1;
+    std::uint32_t imu_factor_id=0;
+    uint imu_n=0;
+    std::shared_ptr<gtsam::ISAM2Params> optimizerParamPtr;
+    bool syn=false;
+};
+#endif //SVO_IMU_INTEGRATION_H

CPU_version/svo/include/svo/initialization.h

@@ -0,0 +1,83 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_INITIALIZATION_H
+#define SVO_INITIALIZATION_H
+
+#include <svo/global.h>
+
+namespace svo {
+
+class FrameHandlerMono;
+
+/// Bootstrapping the map from the first two views.
+namespace initialization {
+
+enum InitResult { FAILURE, NO_KEYFRAME, SUCCESS };
+
+/// Tracks features using Lucas-Kanade tracker and then estimates a homography.
+class KltHomographyInit {
+  friend class svo::FrameHandlerMono;
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  FramePtr frame_ref_;
+  KltHomographyInit() {};
+  ~KltHomographyInit() {};
+  InitResult addFirstFrame(FramePtr frame_ref);
+  InitResult addSecondFrame(FramePtr frame_ref);
+  void reset();
+
+protected:
+  vector<cv::Point2f> px_ref_;      //!< keypoints to be tracked in reference frame.
+  vector<cv::Point2f> px_cur_;      //!< tracked keypoints in current frame.
+  vector<Vector3d> f_ref_;          //!< bearing vectors corresponding to the keypoints in the reference image.
+  vector<Vector3d> f_cur_;          //!< bearing vectors corresponding to the keypoints in the current image.
+  vector<double> disparities_;      //!< disparity between first and second frame.
+  vector<int> inliers_;             //!< inliers after the geometric check (e.g., Homography).
+  vector<Vector3d> xyz_in_cur_;     //!< 3D points computed during the geometric check.
+  SE3 T_cur_from_ref_;              //!< computed transformation between the first two frames.
+};
+
+/// Detect Fast corners in the image.
+void detectFeatures(
+    FramePtr frame,
+    vector<cv::Point2f>& px_vec,
+    vector<Vector3d>& f_vec);
+
+/// Compute optical flow (Lucas Kanade) for selected keypoints.
+void trackKlt(
+    FramePtr frame_ref,
+    FramePtr frame_cur,
+    vector<cv::Point2f>& px_ref,
+    vector<cv::Point2f>& px_cur,
+    vector<Vector3d>& f_ref,
+    vector<Vector3d>& f_cur,
+    vector<double>& disparities);
+
+void computeHomography(
+    const vector<Vector3d>& f_ref,
+    const vector<Vector3d>& f_cur,
+    double focal_length,
+    double reprojection_threshold,
+    vector<int>& inliers,
+    vector<Vector3d>& xyz_in_cur,
+    SE3& T_cur_from_ref);
+
+} // namespace initialization
+} // namespace svo
+
+#endif // SVO_INITIALIZATION_H

CPU_version/svo/include/svo/map.h

@@ -0,0 +1,136 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_MAP_H_
+#define SVO_MAP_H_
+
+#include <queue>
+#include <boost/noncopyable.hpp>
+#include <boost/thread.hpp>
+#include <svo/global.h>
+
+namespace svo {
+
+class Point;
+class Feature;
+class Seed;
+
+/// Container for converged 3D points that are not already assigned to two keyframes.
+class MapPointCandidates
+{
+public:
+  typedef pair<Point*, Feature*> PointCandidate;
+  typedef list<PointCandidate> PointCandidateList;
+
+  /// The depth-filter is running in a parallel thread and fills the canidate list.
+  /// This mutex controls concurrent access to point_candidates.
+  boost::mutex mut_;
+
+  /// Candidate points are created from converged seeds.
+  /// Until the next keyframe, these points can be used for reprojection and pose optimization.
+  PointCandidateList candidates_;
+  list< Point* > trash_points_;
+
+  MapPointCandidates();
+  ~MapPointCandidates();
+
+  /// Add a candidate point.
+  void newCandidatePoint(Point* point, double depth_sigma2);
+
+  /// Adds the feature to the frame and deletes candidate from list.
+  void addCandidatePointToFrame(FramePtr frame);
+
+  /// Remove a candidate point from the list of candidates.
+  bool deleteCandidatePoint(Point* point);
+
+  /// Remove all candidates that belong to a frame.
+  void removeFrameCandidates(FramePtr frame);
+
+  /// Reset the candidate list, remove and delete all points.
+  void reset();
+
+  void deleteCandidate(PointCandidate& c);
+
+  void emptyTrash();
+};
+
+/// Map object which saves all keyframes which are in a map.
+class Map : boost::noncopyable
+{
+public:
+  list< FramePtr > keyframes_;          //!< List of keyframes in the map.
+  list< Point* > trash_points_;         //!< A deleted point is moved to the trash bin. Now and then this is cleaned. One reason is that the visualizer must remove the points also.
+  MapPointCandidates point_candidates_;
+
+  Map();
+  ~Map();
+
+  /// Reset the map. Delete all keyframes and reset the frame and point counters.
+  void reset();
+
+  /// Delete a point in the map and remove all references in keyframes to it.
+  void safeDeletePoint(Point* pt);
+
+  /// Moves the point to the trash queue which is cleaned now and then.
+  void deletePoint(Point* pt);
+
+  /// Moves the frame to the trash queue which is cleaned now and then.
+  bool safeDeleteFrame(FramePtr frame);
+
+  /// Remove the references between a point and a frame.
+  void removePtFrameRef(Frame* frame, Feature* ftr);
+
+  /// Add a new keyframe to the map.
+  void addKeyframe(FramePtr new_keyframe);
+
+  /// Given a frame, return all keyframes which have an overlapping field of view.
+  void getCloseKeyframes(const FramePtr& frame, list< pair<FramePtr,double> >& close_kfs) const;
+
+  /// Return the keyframe which is spatially closest and has overlapping field of view.
+  FramePtr getClosestKeyframe(const FramePtr& frame) const;
+
+  /// Return the keyframe which is furthest apart from pos.
+  FramePtr getFurthestKeyframe(const Vector3d& pos) const;
+
+  bool getKeyframeById(const int id, FramePtr& frame) const;
+
+  /// Transform the whole map with rotation R, translation t and scale s.
+  void transform(const Matrix3d& R, const Vector3d& t, const double& s);
+
+  /// Empty trash bin of deleted keyframes and map points. We don't delete the
+  /// points immediately to ensure proper cleanup and to provide the visualizer
+  /// a list of objects which must be removed.
+  void emptyTrash();
+
+  /// Return the keyframe which was last inserted in the map.
+  inline FramePtr lastKeyframe() { return keyframes_.back(); }
+
+  /// Return the number of keyframes in the map
+  inline size_t size() const { return keyframes_.size(); }
+};
+
+/// A collection of debug functions to check the data consistency.
+namespace map_debug {
+
+void mapStatistics(Map* map);
+void mapValidation(Map* map, int id);
+void frameValidation(Frame* frame, int id);
+void pointValidation(Point* point, int id);
+
+} // namespace map_debug
+} // namespace svo
+
+#endif // SVO_MAP_H_

CPU_version/svo/include/svo/matcher.h

@@ -0,0 +1,130 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_MATCHER_H_
+#define SVO_MATCHER_H_
+
+#include <svo/global.h>
+
+namespace vk {
+  class AbstractCamera;
+  namespace patch_score {
+    template<int HALF_PATCH_SIZE> class ZMSSD;
+  }
+}
+
+namespace svo {
+
+class Point;
+class Frame;
+class Feature;
+
+/// Warp a patch from the reference view to the current view.
+namespace warp {
+
+void getWarpMatrixAffine(
+    const vk::AbstractCamera& cam_ref,
+    const vk::AbstractCamera& cam_cur,
+    const Vector2d& px_ref,
+    const Vector3d& f_ref,
+    const double depth_ref,
+    const SE3& T_cur_ref,
+    const int level_ref,
+    Matrix2d& A_cur_ref);
+
+int getBestSearchLevel(
+    const Matrix2d& A_cur_ref,
+    const int max_level);
+
+void warpAffine(
+    const Matrix2d& A_cur_ref,
+    const cv::Mat& img_ref,
+    const Vector2d& px_ref,
+    const int level_ref,
+    const int level_cur,
+    const int halfpatch_size,
+    uint8_t* patch);
+
+} // namespace warp
+
+/// Patch-matcher for reprojection-matching and epipolar search in triangulation.
+class Matcher
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  static const int halfpatch_size_ = 4;
+  static const int patch_size_ = 8;
+
+  typedef vk::patch_score::ZMSSD<halfpatch_size_> PatchScore;
+
+  struct Options
+  {
+    bool align_1d;              //!< in epipolar search: align patch 1D along epipolar line
+    int align_max_iter;         //!< number of iterations for aligning the feature patches in gauss newton
+    double max_epi_length_optim;//!< max length of epipolar line to skip epipolar search and directly go to img align
+    size_t max_epi_search_steps;//!< max number of evaluations along epipolar line
+    bool subpix_refinement;     //!< do gauss newton feature patch alignment after epipolar search
+    bool epi_search_edgelet_filtering;
+    double epi_search_edgelet_max_angle;
+    Options() :
+      align_1d(false),
+      align_max_iter(10),
+      max_epi_length_optim(2.0),
+      max_epi_search_steps(1000),
+      subpix_refinement(true),
+      epi_search_edgelet_filtering(true),
+      epi_search_edgelet_max_angle(0.7)
+    {}
+  } options_;
+
+  uint8_t patch_[patch_size_*patch_size_] __attribute__ ((aligned (16)));
+  uint8_t patch_with_border_[(patch_size_+2)*(patch_size_+2)] __attribute__ ((aligned (16)));
+  Matrix2d A_cur_ref_;          //!< affine warp matrix
+  Vector2d epi_dir_;
+  double epi_length_;           //!< length of epipolar line segment in pixels (only used for epipolar search)
+  double h_inv_;                //!< hessian of 1d image alignment along epipolar line
+  int search_level_;
+  bool reject_;
+  Feature* ref_ftr_;
+  Vector2d px_cur_;
+
+  Matcher() = default;
+  ~Matcher() = default;
+
+  /// Find a match by directly applying subpix refinement.
+  /// IMPORTANT! This function assumes that px_cur is already set to an estimate that is within ~2-3 pixel of the final result!
+  bool findMatchDirect(
+      const Point& pt,
+      const Frame& frame,
+      Vector2d& px_cur);
+
+  /// Find a match by searching along the epipolar line without using any features.
+  bool findEpipolarMatchDirect(
+      const Frame& ref_frame,
+      const Frame& cur_frame,
+      const Feature& ref_ftr,
+      const double d_estimate,
+      const double d_min,
+      const double d_max,
+      double& depth);
+
+  void createPatchFromPatchWithBorder();
+};
+
+} // namespace svo
+
+#endif // SVO_MATCHER_H_

CPU_version/svo/include/svo/point.h

@@ -0,0 +1,108 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_POINT_H_
+#define SVO_POINT_H_
+
+#include <boost/noncopyable.hpp>
+#include <svo/global.h>
+
+namespace g2o {
+  class VertexSBAPointXYZ;
+}
+typedef g2o::VertexSBAPointXYZ g2oPoint;
+
+namespace svo {
+
+class Feature;
+
+typedef Matrix<double, 2, 3> Matrix23d;
+
+/// A 3D point on the surface of the scene.
+class Point : boost::noncopyable
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  
+  enum PointType {
+    TYPE_DELETED,
+    TYPE_CANDIDATE,
+    TYPE_UNKNOWN,
+    TYPE_GOOD
+  };
+
+  static int                  point_counter_;           //!< Counts the number of created points. Used to set the unique id.
+  int                         id_;                      //!< Unique ID of the point.
+  Vector3d                    pos_;                     //!< 3d pos of the point in the world coordinate frame.
+  Vector3d                    normal_;                  //!< Surface normal at point.
+  Matrix3d                    normal_information_;      //!< Inverse covariance matrix of normal estimation.
+  bool                        normal_set_;              //!< Flag whether the surface normal was estimated or not.
+  list<Feature*>              obs_;                     //!< References to keyframes which observe the point.
+  size_t                      n_obs_;                   //!< Number of obervations: Keyframes AND successful reprojections in intermediate frames.
+  g2oPoint*                   v_pt_;                    //!< Temporary pointer to the point-vertex in g2o during bundle adjustment.
+  int                         last_published_ts_;       //!< Timestamp of last publishing.
+  int                         last_projected_kf_id_;    //!< Flag for the reprojection: don't reproject a pt twice.
+  PointType                   type_;                    //!< Quality of the point.
+  int                         n_failed_reproj_;         //!< Number of failed reprojections. Used to assess the quality of the point.
+  int                         n_succeeded_reproj_;      //!< Number of succeeded reprojections. Used to assess the quality of the point.
+  int                         last_structure_optim_;    //!< Timestamp of last point optimization
+
+  Point(const Vector3d& pos);
+  Point(const Vector3d& pos, Feature* ftr);
+  ~Point();
+
+  /// Add a reference to a frame.
+  void addFrameRef(Feature* ftr);
+
+  /// Remove reference to a frame.
+  bool deleteFrameRef(Frame* frame);
+
+  /// Initialize point normal. The inital estimate will point towards the frame.
+  void initNormal();
+
+  /// Check whether mappoint has reference to a frame.
+  Feature* findFrameRef(Frame* frame);
+
+  /// Get Frame with similar viewpoint.
+  bool getCloseViewObs(const Vector3d& pos, Feature*& obs) const;
+
+  /// Get number of observations.
+  inline size_t nRefs() const { return obs_.size(); }
+
+  /// Optimize point position through minimizing the reprojection error.
+  void optimize(const size_t n_iter);
+
+  /// Jacobian of point projection on unit plane (focal length = 1) in frame (f).
+  inline static void jacobian_xyz2uv(
+      const Vector3d& p_in_f,
+      const Matrix3d& R_f_w,
+      Matrix23d& point_jac)
+  {
+    const double z_inv = 1.0/p_in_f[2];
+    const double z_inv_sq = z_inv*z_inv;
+    point_jac(0, 0) = z_inv;
+    point_jac(0, 1) = 0.0;
+    point_jac(0, 2) = -p_in_f[0] * z_inv_sq;
+    point_jac(1, 0) = 0.0;
+    point_jac(1, 1) = z_inv;
+    point_jac(1, 2) = -p_in_f[1] * z_inv_sq;
+    point_jac = - point_jac * R_f_w;
+  }
+};
+
+} // namespace svo
+
+#endif // SVO_POINT_H_

CPU_version/svo/include/svo/pose_optimizer.h

@@ -0,0 +1,50 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_POSE_OPTIMIZER_H_
+#define SVO_POSE_OPTIMIZER_H_
+
+#include <svo/global.h>
+
+namespace svo {
+
+using namespace Eigen;
+using namespace Sophus;
+using namespace std;
+
+typedef Matrix<double,6,6> Matrix6d;
+typedef Matrix<double,2,6> Matrix26d;
+typedef Matrix<double,6,1> Vector6d;
+
+class Point;
+
+/// Motion-only bundle adjustment. Minimize the reprojection error of a single frame.
+namespace pose_optimizer {
+
+void optimizeGaussNewton(
+    const double reproj_thresh,
+    const size_t n_iter,
+    const bool verbose,
+    FramePtr& frame,
+    double& estimated_scale,
+    double& error_init,
+    double& error_final,
+    size_t& num_obs);
+
+} // namespace pose_optimizer
+} // namespace svo
+
+#endif // SVO_POSE_OPTIMIZER_H_

CPU_version/svo/include/svo/reprojector.h

@@ -0,0 +1,101 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_REPROJECTION_H_
+#define SVO_REPROJECTION_H_
+
+#include <svo/global.h>
+#include <svo/matcher.h>
+
+namespace vk {
+class AbstractCamera;
+}
+
+namespace svo {
+
+class Map;
+class Point;
+
+/// Project points from the map into the image and find the corresponding
+/// feature (corner). We don't search a match for every point but only for one
+/// point per cell. Thereby, we achieve a homogeneously distributed set of
+/// matched features and at the same time we can save processing time by not
+/// projecting all points.
+class Reprojector
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /// Reprojector config parameters
+  struct Options {
+    size_t max_n_kfs;   //!< max number of keyframes to reproject from
+    bool find_match_direct;
+    Options()
+    : max_n_kfs(10),
+      find_match_direct(true)
+    {}
+  } options_;
+
+  size_t n_matches_;
+  size_t n_trials_;
+
+  Reprojector(vk::AbstractCamera* cam, Map& map);
+
+  ~Reprojector();
+
+  /// Project points from the map into the image. First finds keyframes with
+  /// overlapping field of view and projects only those map-points.
+  void reprojectMap(
+      FramePtr frame,
+      std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs);
+
+private:
+
+  /// A candidate is a point that projects into the image plane and for which we
+  /// will search a maching feature in the image.
+  struct Candidate {
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    Point* pt;       //!< 3D point.
+    Vector2d px;     //!< projected 2D pixel location.
+    Candidate(Point* pt, Vector2d& px) : pt(pt), px(px) {}
+  };
+  typedef std::list<Candidate > Cell;
+  typedef std::vector<Cell*> CandidateGrid;
+
+  /// The grid stores a set of candidate matches. For every grid cell we try to find one match.
+  struct Grid
+  {
+    CandidateGrid cells;
+    vector<int> cell_order;
+    int cell_size;
+    int grid_n_cols;
+    int grid_n_rows;
+  };
+
+  Grid grid_;
+  Matcher matcher_;
+  Map& map_;
+
+  static bool pointQualityComparator(Candidate& lhs, Candidate& rhs);
+  void initializeGrid(vk::AbstractCamera* cam);
+  void resetGrid();
+  bool reprojectCell(Cell& cell, FramePtr frame);
+  bool reprojectPoint(FramePtr frame, Point* point);
+};
+
+} // namespace svo
+
+#endif // SVO_REPROJECTION_H_

CPU_version/svo/include/svo/sparse_img_align.h

@@ -0,0 +1,83 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_SPARSE_IMG_ALIGN_H_
+#define SVO_SPARSE_IMG_ALIGN_H_
+
+#include <vikit/nlls_solver.h>
+#include <vikit/performance_monitor.h>
+#include <svo/global.h>
+
+namespace vk {
+class AbstractCamera;
+}
+
+namespace svo {
+
+class Feature;
+
+/// Optimize the pose of the frame by minimizing the photometric error of feature patches.
+class SparseImgAlign : public vk::NLLSSolver<6, SE3>
+{
+  static const int patch_halfsize_ = 2;
+  static const int patch_size_ = 2*patch_halfsize_;
+  static const int patch_area_ = patch_size_*patch_size_;
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  cv::Mat resimg_;
+
+  SparseImgAlign(
+      int n_levels,
+      int min_level,
+      int n_iter,
+      Method method,
+      bool display,
+      bool verbose);
+
+  size_t run(
+      FramePtr ref_frame,
+      FramePtr cur_frame);
+
+  /// Return fisher information matrix, i.e. the Hessian of the log-likelihood
+  /// at the converged state.
+  Matrix<double, 6, 6> getFisherInformation();
+
+protected:
+  FramePtr ref_frame_;            //!< reference frame, has depth for gradient pixels.
+  FramePtr cur_frame_;            //!< only the image is known!
+  int level_;                     //!< current pyramid level on which the optimization runs.
+  bool display_;                  //!< display residual image.
+  int max_level_;                 //!< coarsest pyramid level for the alignment.
+  int min_level_;                 //!< finest pyramid level for the alignment.
+
+  // cache:
+  Matrix<double, 6, Dynamic, ColMajor> jacobian_cache_;
+  bool have_ref_patch_cache_;
+  cv::Mat ref_patch_cache_;
+  std::vector<bool> visible_fts_;
+
+  void precomputeReferencePatches();
+  virtual double computeResiduals(const SE3& model, bool linearize_system, bool compute_weight_scale = false);
+  virtual int solve();
+  virtual void update (const ModelType& old_model, ModelType& new_model);
+  virtual void startIteration();
+  virtual void finishIteration();
+};
+
+} // namespace svo
+
+#endif // SVO_SPARSE_IMG_ALIGN_H_

CPU_version/svo/include/svo/svoVisionFactor.h

@@ -0,0 +1,70 @@
+//
+// Created by root on 4/13/21.
+//
+
+#ifndef SVO_SVOVISIONFACTOR_H
+#define SVO_SVOVISIONFACTOR_H
+#pragma once
+
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/geometry/Pose3.h>
+#include <vikit/robust_cost.h>
+#include <vikit/math_utils.h>
+#include <svo/frame.h>
+
+namespace svo {
+
+    class VisionFactor: public gtsam::NoiseModelFactor1<gtsam::Pose3> {
+
+    private:
+        // measurement information
+        gtsam::Point3 feature;   ///<Unit-bearing vector of the feature.
+        gtsam::Point3 landmark;   ///<Unit-bearing vector of the feature.
+        boost::optional<gtsam::Point3> body_P_sensor_; ///< TODO The pose of the sensor in the body frame
+        int level;            ///< Image pyramid level where feature was extracted.
+
+    public:
+        /// shorthand for a smart pointer to a factor
+        typedef boost::shared_ptr<VisionFactor> shared_ptr;
+
+        /**
+        * Constructor
+        * @param feature Unit-bearing vector of the feature.
+        * @param model is the standard deviation
+        * @param pointKey is the index of the landmark
+        * @param poseKey is the index of the camera pose
+        * @param landmark 3D position of the landmark
+        * @param body_P_sensor is the transform from body to sensor frame (default identity)
+        * @param level Image pyramid level where feature was extracted.
+        */
+        VisionFactor(const gtsam::Point3& feature, const gtsam::Point3& landmark,const int level, const gtsam::SharedNoiseModel& Hp,
+                      gtsam::Key poseKey,boost::optional<gtsam::Point3> body_P_sensor = boost::none) :
+                gtsam::NoiseModelFactor1<gtsam::Pose3>(Hp, poseKey), feature(feature), landmark(landmark),body_P_sensor_(body_P_sensor),level(level){}
+        virtual ~VisionFactor(){}
+
+        /// error function
+        /// @param p    the Estimated pose in Pose3
+        /// @param H    the optional Jacobian matrix, which use boost optional and has default null pointer
+        gtsam::Vector evaluateError(const gtsam::Pose3& p, boost::optional<gtsam::Matrix&> Hp = boost::none) const {
+            // note that use boost optional like a pointer
+            // only calculate jacobian matrix when non-null pointer exists
+            gtsam::Matrix26 J;
+            gtsam::Point3 point(p*landmark);
+            svo::Frame::jacobian_xyz2uv(point, J);
+            Vector2d e = (vk::project2d(feature) - vk::project2d(point))*1.0 / (1<<level);
+            if(Hp)(*Hp) = (gtsam::Matrix(2,6)<< J * 1.0 / (1<<level)).finished();
+            return e;
+        }
+        // The second is a 'clone' function that allows the factor to be copied. Under most
+        // circumstances, the following code that employs the default copy constructor should
+        // work fine.
+        virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+            return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+                    gtsam::NonlinearFactor::shared_ptr(new VisionFactor(*this))); }
+
+    };
+
+} // namespace gtsamexamples
+#endif //SVO_SVOVISIONFACTOR_H

CPU_version/svo/package.xml

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<package>
+  <name>svo</name>
+  <version>0.1.0</version>
+  <description>
+    Monocular Visual Odometry
+  </description>
+  <author>Christian Forster</author>
+  <maintainer email="[email protected]">Christian Forster</maintainer>
+  <license>GPLv3</license>
+
+  <!-- Dependencies which this package needs to build itself. -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <!-- Dependencies needed to compile this package. -->
+  <build_depend>cmake_modules</build_depend> <!-- for FindEigen.cmake -->
+  <build_depend>roscpp</build_depend>
+  <build_depend>roslib</build_depend>
+  <build_depend>vikit_common</build_depend>
+  <build_depend>vikit_ros</build_depend>
+
+  <!-- Dependencies needed after this package is compiled. -->
+  <run_depend>roscpp</run_depend>
+  <run_depend>roslib</run_depend>
+  <run_depend>vikit_common</run_depend>
+  <run_depend>vikit_ros</run_depend>
+</package>

CPU_version/svo/src/bundle_adjustment.cpp

@@ -0,0 +1,435 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <vikit/math_utils.h>
+#include <boost/thread.hpp>
+#include <g2o/core/sparse_optimizer.h>
+#include <g2o/core/block_solver.h>
+#include <g2o/core/solver.h>
+#include <g2o/core/robust_kernel_impl.h>
+#include <g2o/core/optimization_algorithm_levenberg.h>
+#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
+#include <g2o/solvers/csparse/linear_solver_csparse.h>
+#include <g2o/solvers/dense/linear_solver_dense.h>
+#include <g2o/types/sba/types_six_dof_expmap.h>
+#include <g2o/solvers/structure_only/structure_only_solver.h>
+#include <svo/bundle_adjustment.h>
+#include <svo/frame.h>
+#include <svo/feature.h>
+#include <svo/point.h>
+#include <svo/config.h>
+#include <svo/map.h>
+
+#define SCHUR_TRICK 1
+
+namespace svo {
+namespace ba {
+
+void twoViewBA(
+    Frame* frame1,
+    Frame* frame2,
+    double reproj_thresh,
+    Map* map)
+{
+  // scale reprojection threshold in pixels to unit plane
+  reproj_thresh /= frame1->cam_->errorMultiplier2();
+
+  // init g2o
+  g2o::SparseOptimizer optimizer;
+  setupG2o(&optimizer);
+
+  list<EdgeContainerSE3> edges;
+  size_t v_id = 0;
+
+  // New Keyframe Vertex 1: This Keyframe is set to fixed!
+  g2oFrameSE3* v_frame1 = createG2oFrameSE3(frame1, v_id++, true);
+  optimizer.addVertex(v_frame1);
+
+  // New Keyframe Vertex 2
+  g2oFrameSE3* v_frame2 = createG2oFrameSE3(frame2, v_id++, false);
+  optimizer.addVertex(v_frame2);
+
+  // Create Point Vertices
+  for(Features::iterator it_ftr=frame1->fts_.begin(); it_ftr!=frame1->fts_.end(); ++it_ftr)
+  {
+    Point* pt = (*it_ftr)->point;
+    if(pt == NULL)
+      continue;
+    g2oPoint* v_pt = createG2oPoint(pt->pos_, v_id++, false);
+    optimizer.addVertex(v_pt);
+    pt->v_pt_ = v_pt;
+    g2oEdgeSE3* e = createG2oEdgeSE3(v_frame1, v_pt, vk::project2d((*it_ftr)->f), true, reproj_thresh*Config::lobaRobustHuberWidth());
+    optimizer.addEdge(e);
+    edges.push_back(EdgeContainerSE3(e, frame1, *it_ftr)); // TODO feature now links to frame, so we can simplify edge container!
+
+    // find at which index the second frame observes the point
+    Feature* ftr_frame2 = pt->findFrameRef(frame2);
+    e = createG2oEdgeSE3(v_frame2, v_pt, vk::project2d(ftr_frame2->f), true, reproj_thresh*Config::lobaRobustHuberWidth());
+    optimizer.addEdge(e);
+    edges.push_back(EdgeContainerSE3(e, frame2, ftr_frame2));
+  }
+
+  // Optimization
+  double init_error, final_error;
+  runSparseBAOptimizer(&optimizer, Config::lobaNumIter(), init_error, final_error);
+  printf("2-View BA: Error before/after = %f / %f\n", init_error, final_error);
+
+  // Update Keyframe Positions
+  frame1->T_f_w_.rotation_matrix() = v_frame1->estimate().rotation().toRotationMatrix();
+  frame1->T_f_w_.translation() = v_frame1->estimate().translation();
+  frame2->T_f_w_.rotation_matrix() = v_frame2->estimate().rotation().toRotationMatrix();
+  frame2->T_f_w_.translation() = v_frame2->estimate().translation();
+
+  // Update Mappoint Positions
+  for(Features::iterator it=frame1->fts_.begin(); it!=frame1->fts_.end(); ++it)
+  {
+    if((*it)->point == NULL)
+     continue;
+    (*it)->point->pos_ = (*it)->point->v_pt_->estimate();
+    (*it)->point->v_pt_ = NULL;
+  }
+
+  // Find Mappoints with too large reprojection error
+  const double reproj_thresh_squared = reproj_thresh*reproj_thresh;
+  size_t n_incorrect_edges = 0;
+  for(list<EdgeContainerSE3>::iterator it_e = edges.begin(); it_e != edges.end(); ++it_e)
+    if(it_e->edge->chi2() > reproj_thresh_squared)
+    {
+      if(it_e->feature->point != NULL)
+      {
+        map->safeDeletePoint(it_e->feature->point);
+        it_e->feature->point = NULL;
+      }
+      ++n_incorrect_edges;
+    }
+
+  printf("2-View BA: Wrong edges =  %zu\n", n_incorrect_edges);
+}
+
+void localBA(
+    Frame* center_kf,
+    set<FramePtr>* core_kfs,
+    Map* map,
+    size_t& n_incorrect_edges_1,
+    size_t& n_incorrect_edges_2,
+    double& init_error,
+    double& final_error)
+{
+
+  // init g2o
+  g2o::SparseOptimizer optimizer;
+  setupG2o(&optimizer);
+
+  list<EdgeContainerSE3> edges;
+  set<Point*> mps;
+  list<Frame*> neib_kfs;
+  size_t v_id = 0;
+  size_t n_mps = 0;
+  size_t n_fix_kfs = 0;
+  size_t n_var_kfs = 1;
+  size_t n_edges = 0;
+  n_incorrect_edges_1 = 0;
+  n_incorrect_edges_2 = 0;
+
+  // Add all core keyframes
+  for(set<FramePtr>::iterator it_kf = core_kfs->begin(); it_kf != core_kfs->end(); ++it_kf)
+  {
+    g2oFrameSE3* v_kf = createG2oFrameSE3(it_kf->get(), v_id++, false);
+    (*it_kf)->v_kf_ = v_kf;
+    ++n_var_kfs;
+    assert(optimizer.addVertex(v_kf));
+
+    // all points that the core keyframes observe are also optimized:
+    for(Features::iterator it_pt=(*it_kf)->fts_.begin(); it_pt!=(*it_kf)->fts_.end(); ++it_pt)
+      if((*it_pt)->point != NULL)
+        mps.insert((*it_pt)->point);
+  }
+
+  // Now go throug all the points and add a measurement. Add a fixed neighbour
+  // Keyframe if it is not in the set of core kfs
+  double reproj_thresh_2 = Config::lobaThresh() / center_kf->cam_->errorMultiplier2();
+  double reproj_thresh_1 = Config::poseOptimThresh() / center_kf->cam_->errorMultiplier2();
+  double reproj_thresh_1_squared = reproj_thresh_1*reproj_thresh_1;
+  for(set<Point*>::iterator it_pt = mps.begin(); it_pt!=mps.end(); ++it_pt)
+  {
+    // Create point vertex
+    g2oPoint* v_pt = createG2oPoint((*it_pt)->pos_, v_id++, false);
+    (*it_pt)->v_pt_ = v_pt;
+    assert(optimizer.addVertex(v_pt));
+    ++n_mps;
+
+    // Add edges
+    list<Feature*>::iterator it_obs=(*it_pt)->obs_.begin();
+    while(it_obs!=(*it_pt)->obs_.end())
+    {
+      Vector2d error = vk::project2d((*it_obs)->f) - vk::project2d((*it_obs)->frame->w2f((*it_pt)->pos_));
+
+      if((*it_obs)->frame->v_kf_ == NULL)
+      {
+        // frame does not have a vertex yet -> it belongs to the neib kfs and
+        // is fixed. create one:
+        g2oFrameSE3* v_kf = createG2oFrameSE3((*it_obs)->frame, v_id++, true);
+        (*it_obs)->frame->v_kf_ = v_kf;
+        ++n_fix_kfs;
+        assert(optimizer.addVertex(v_kf));
+        neib_kfs.push_back((*it_obs)->frame);
+      }
+
+      // create edge
+      g2oEdgeSE3* e = createG2oEdgeSE3((*it_obs)->frame->v_kf_, v_pt,
+                                       vk::project2d((*it_obs)->f),
+                                       true,
+                                       reproj_thresh_2*Config::lobaRobustHuberWidth(),
+                                       1.0 / (1<<(*it_obs)->level));
+      assert(optimizer.addEdge(e));
+      edges.push_back(EdgeContainerSE3(e, (*it_obs)->frame, *it_obs));
+      ++n_edges;
+      ++it_obs;
+    }
+  }
+
+  // structure only
+  g2o::StructureOnlySolver<3> structure_only_ba;
+  g2o::OptimizableGraph::VertexContainer points;
+  for (g2o::OptimizableGraph::VertexIDMap::const_iterator it = optimizer.vertices().begin(); it != optimizer.vertices().end(); ++it)
+  {
+    g2o::OptimizableGraph::Vertex* v = static_cast<g2o::OptimizableGraph::Vertex*>(it->second);
+      if (v->dimension() == 3 && v->edges().size() >= 2)
+        points.push_back(v);
+  }
+  structure_only_ba.calc(points, 10);
+
+  // Optimization
+  if(Config::lobaNumIter() > 0)
+    runSparseBAOptimizer(&optimizer, Config::lobaNumIter(), init_error, final_error);
+
+  // Update Keyframes
+  for(set<FramePtr>::iterator it = core_kfs->begin(); it != core_kfs->end(); ++it)
+  {
+    (*it)->T_f_w_ = SE3( (*it)->v_kf_->estimate().rotation(),
+                         (*it)->v_kf_->estimate().translation());
+    (*it)->v_kf_ = NULL;
+  }
+
+  for(list<Frame*>::iterator it = neib_kfs.begin(); it != neib_kfs.end(); ++it)
+    (*it)->v_kf_ = NULL;
+
+  // Update Mappoints
+  for(set<Point*>::iterator it = mps.begin(); it != mps.end(); ++it)
+  {
+    (*it)->pos_ = (*it)->v_pt_->estimate();
+    (*it)->v_pt_ = NULL;
+  }
+
+  // Remove Measurements with too large reprojection error
+  double reproj_thresh_2_squared = reproj_thresh_2*reproj_thresh_2;
+  for(list<EdgeContainerSE3>::iterator it = edges.begin(); it != edges.end(); ++it)
+  {
+    if(it->edge->chi2() > reproj_thresh_2_squared) //*(1<<it->feature_->level))
+    {
+      map->removePtFrameRef(it->frame, it->feature);
+      ++n_incorrect_edges_2;
+    }
+  }
+
+  // TODO: delete points and edges!
+  init_error = sqrt(init_error)*center_kf->cam_->errorMultiplier2();
+  final_error = sqrt(final_error)*center_kf->cam_->errorMultiplier2();
+}
+
+void globalBA(Map* map)
+{
+  // init g2o
+  g2o::SparseOptimizer optimizer;
+  setupG2o(&optimizer);
+
+  list<EdgeContainerSE3> edges;
+  list< pair<FramePtr,Feature*> > incorrect_edges;
+
+  // Go through all Keyframes
+  size_t v_id = 0;
+  double reproj_thresh_2 = Config::lobaThresh() / map->lastKeyframe()->cam_->errorMultiplier2();
+  double reproj_thresh_1_squared = Config::poseOptimThresh() / map->lastKeyframe()->cam_->errorMultiplier2();
+  reproj_thresh_1_squared *= reproj_thresh_1_squared;
+  for(list<FramePtr>::iterator it_kf = map->keyframes_.begin();
+      it_kf != map->keyframes_.end(); ++it_kf)
+  {
+    // New Keyframe Vertex
+    g2oFrameSE3* v_kf = createG2oFrameSE3(it_kf->get(), v_id++, false);
+    (*it_kf)->v_kf_ = v_kf;
+    optimizer.addVertex(v_kf);
+    for(Features::iterator it_ftr=(*it_kf)->fts_.begin(); it_ftr!=(*it_kf)->fts_.end(); ++it_ftr)
+    {
+      // for each keyframe add edges to all observed mapoints
+      Point* mp = (*it_ftr)->point;
+      if(mp == NULL)
+        continue;
+      g2oPoint* v_mp = mp->v_pt_;
+      if(v_mp == NULL)
+      {
+        // mappoint-vertex doesn't exist yet. create a new one:
+        v_mp = createG2oPoint(mp->pos_, v_id++, false);
+        mp->v_pt_ = v_mp;
+        optimizer.addVertex(v_mp);
+      }
+
+      // Due to merging of mappoints it is possible that references in kfs suddenly
+      // have a very large reprojection error which may result in distorted results.
+      Vector2d error = vk::project2d((*it_ftr)->f) - vk::project2d((*it_kf)->w2f(mp->pos_));
+      if(error.squaredNorm() > reproj_thresh_1_squared)
+        incorrect_edges.push_back(pair<FramePtr,Feature*>(*it_kf, *it_ftr));
+      else
+      {
+        g2oEdgeSE3* e = createG2oEdgeSE3(v_kf, v_mp, vk::project2d((*it_ftr)->f),
+                                         true,
+                                         reproj_thresh_2*Config::lobaRobustHuberWidth());
+
+        edges.push_back(EdgeContainerSE3(e, it_kf->get(), *it_ftr));
+        optimizer.addEdge(e);
+      }
+    }
+  }
+
+  // Optimization
+  double init_error=0.0, final_error=0.0;
+  if(Config::lobaNumIter() > 0)
+    runSparseBAOptimizer(&optimizer, Config::lobaNumIter(), init_error, final_error);
+
+  // Update Keyframe and MapPoint Positions
+  for(list<FramePtr>::iterator it_kf = map->keyframes_.begin();
+        it_kf != map->keyframes_.end(); ++it_kf)
+  {
+    (*it_kf)->T_f_w_ = SE3( (*it_kf)->v_kf_->estimate().rotation(),
+                            (*it_kf)->v_kf_->estimate().translation());
+    (*it_kf)->v_kf_ = NULL;
+    for(Features::iterator it_ftr=(*it_kf)->fts_.begin(); it_ftr!=(*it_kf)->fts_.end(); ++it_ftr)
+    {
+      Point* mp = (*it_ftr)->point;
+      if(mp == NULL)
+        continue;
+      if(mp->v_pt_ == NULL)
+        continue;       // mp was updated before
+      mp->pos_ = mp->v_pt_->estimate();
+      mp->v_pt_ = NULL;
+    }
+  }
+
+  // Remove Measurements with too large reprojection error
+  for(list< pair<FramePtr,Feature*> >::iterator it=incorrect_edges.begin();
+      it!=incorrect_edges.end(); ++it)
+    map->removePtFrameRef(it->first.get(), it->second);
+
+  double reproj_thresh_2_squared = reproj_thresh_2*reproj_thresh_2;
+  for(list<EdgeContainerSE3>::iterator it = edges.begin(); it != edges.end(); ++it)
+  {
+    if(it->edge->chi2() > reproj_thresh_2_squared)
+    {
+      map->removePtFrameRef(it->frame, it->feature);
+    }
+  }
+}
+
+void setupG2o(g2o::SparseOptimizer * optimizer)
+{
+  // TODO: What's happening with all this HEAP stuff? Memory Leak?
+  optimizer->setVerbose(false);
+
+#if SCHUR_TRICK
+  // solver
+  g2o::BlockSolver_6_3::LinearSolverType* linearSolver;
+  linearSolver = new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>();
+  //linearSolver = new g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType>();
+
+  g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
+  g2o::OptimizationAlgorithmLevenberg * solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
+#else
+  g2o::BlockSolverX::LinearSolverType * linearSolver;
+  linearSolver = new g2o::LinearSolverCholmod<g2o::BlockSolverX::PoseMatrixType>();
+  //linearSolver = new g2o::LinearSolverCSparse<g2o::BlockSolverX::PoseMatrixType>();
+  g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver);
+  g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
+#endif
+
+  solver->setMaxTrialsAfterFailure(5);
+  optimizer->setAlgorithm(solver);
+
+  // setup camera
+  g2o::CameraParameters * cam_params = new g2o::CameraParameters(1.0, Vector2d(0.,0.), 0.);
+  cam_params->setId(0);
+  if (!optimizer->addParameter(cam_params)) {
+    assert(false);
+  }
+}
+
+void
+runSparseBAOptimizer(g2o::SparseOptimizer* optimizer,
+                     unsigned int num_iter,
+                     double& init_error, double& final_error)
+{
+  optimizer->initializeOptimization();
+  optimizer->computeActiveErrors();
+  init_error = optimizer->activeChi2();
+  optimizer->optimize(num_iter);
+  final_error = optimizer->activeChi2();
+}
+
+g2oFrameSE3*
+createG2oFrameSE3(Frame* frame, size_t id, bool fixed)
+{
+  g2oFrameSE3* v = new g2oFrameSE3();
+  v->setId(id);
+  v->setFixed(fixed);
+  v->setEstimate(g2o::SE3Quat(frame->T_f_w_.unit_quaternion(), frame->T_f_w_.translation()));
+  return v;
+}
+
+g2oPoint*
+createG2oPoint(Vector3d pos,
+               size_t id,
+               bool fixed)
+{
+  g2oPoint* v = new g2oPoint();
+  v->setId(id);
+#if SCHUR_TRICK
+  v->setMarginalized(true);
+#endif
+  v->setFixed(fixed);
+  v->setEstimate(pos);
+  return v;
+}
+
+g2oEdgeSE3*
+createG2oEdgeSE3( g2oFrameSE3* v_frame,
+                  g2oPoint* v_point,
+                  const Vector2d& f_up,
+                  bool robust_kernel,
+                  double huber_width,
+                  double weight)
+{
+  g2oEdgeSE3* e = new g2oEdgeSE3();
+  e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_point));
+  e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_frame));
+  e->setMeasurement(f_up);
+  e->information() = weight * Eigen::Matrix2d::Identity(2,2);
+  g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber();      // TODO: memory leak
+  rk->setDelta(huber_width);
+  e->setRobustKernel(rk);
+  e->setParameterId(0, 0); //old: e->setId(v_point->id());
+  return e;
+}
+
+} // namespace ba
+} // namespace svo

CPU_version/svo/src/config.cpp

@@ -0,0 +1,102 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifdef SVO_USE_ROS
+#include <vikit/params_helper.h>
+#endif
+#include <svo/config.h>
+
+namespace svo {
+
+Config::Config() :
+#ifdef SVO_USE_ROS
+    trace_name(vk::getParam<string>("svo/trace_name", "svo")),
+    trace_dir(vk::getParam<string>("svo/trace_dir", "/tmp")),
+    n_pyr_levels(vk::getParam<int>("svo/n_pyr_levels", 3)),
+    use_imu(vk::getParam<bool>("svo/use_imu", false)),
+    core_n_kfs(vk::getParam<int>("svo/core_n_kfs", 3)),
+    map_scale(vk::getParam<double>("svo/map_scale", 1.0)),
+    grid_size(vk::getParam<int>("svo/grid_size", 30)),
+    init_min_disparity(vk::getParam<double>("svo/init_min_disparity", 50.0)),
+    init_min_tracked(vk::getParam<int>("svo/init_min_tracked", 50)),
+    init_min_inliers(vk::getParam<int>("svo/init_min_inliers", 40)),
+    klt_max_level(vk::getParam<int>("svo/klt_max_level", 4)),
+    klt_min_level(vk::getParam<int>("svo/klt_min_level", 2)),
+    reproj_thresh(vk::getParam<double>("svo/reproj_thresh", 2.0)),
+    poseoptim_thresh(vk::getParam<double>("svo/poseoptim_thresh", 2.0)),
+    poseoptim_num_iter(vk::getParam<int>("svo/poseoptim_num_iter", 10)),
+    structureoptim_max_pts(vk::getParam<int>("svo/structureoptim_max_pts", 20)),
+    structureoptim_num_iter(vk::getParam<int>("svo/structureoptim_num_iter", 5)),
+    loba_thresh(vk::getParam<double>("svo/loba_thresh", 2.0)),
+    loba_robust_huber_width(vk::getParam<double>("svo/loba_robust_huber_width", 1.0)),
+    loba_num_iter(vk::getParam<int>("svo/loba_num_iter", 0)),
+    kfselect_mindist(vk::getParam<double>("svo/kfselect_mindist", 0.12)),
+    triang_min_corner_score(vk::getParam<double>("svo/triang_min_corner_score", 20.0)),
+    triang_half_patch_size(vk::getParam<int>("svo/triang_half_patch_size", 4)),
+    subpix_n_iter(vk::getParam<int>("svo/subpix_n_iter", 10)),
+    max_n_kfs(vk::getParam<int>("svo/max_n_kfs", 10)),
+    img_imu_delay(vk::getParam<double>("svo/img_imu_delay", 0.0)),
+    max_fts(vk::getParam<int>("svo/max_fts", 120)),
+    quality_min_fts(vk::getParam<int>("svo/quality_min_fts", 50)),
+    quality_max_drop_fts(vk::getParam<int>("svo/quality_max_drop_fts", 40)),
+    ACC_noise(vk::getParam<double>("svo/ACC_noise", 1e-4)),
+    GYO_noise(vk::getParam<double>("svo/GYO_noise", 1e-4)),
+    iuc(vk::getParam<double>("svo/IUC", 1e-1)),
+    abc(vk::getParam<double>("svo/ABC", 1e-1)),
+    gbc(vk::getParam<double>("svo/GBC", 1e-1)),
+    ebp(vk::getParam<double>("svo/EBP", 1e-2)),
+    nrp(vk::getParam<double>("svo/NRP", 1e-5))
+#else
+    trace_name("svo"),
+    trace_dir("/tmp"),
+    n_pyr_levels(3),
+    use_imu(false),
+    core_n_kfs(3),
+    map_scale(1.0),
+    grid_size(25),
+    init_min_disparity(50.0),
+    init_min_tracked(50),
+    init_min_inliers(40),
+    klt_max_level(4),
+    klt_min_level(2),
+    reproj_thresh(2.0),
+    poseoptim_thresh(2.0),
+    poseoptim_num_iter(10),
+    structureoptim_max_pts(20),
+    structureoptim_num_iter(5),
+    loba_thresh(2.0),
+    loba_robust_huber_width(1.0),
+    loba_num_iter(0),
+    kfselect_mindist(0.12),
+    triang_min_corner_score(20.0),
+    triang_half_patch_size(4),
+    subpix_n_iter(10),
+    max_n_kfs(0),
+    img_imu_delay(0.0),
+    max_fts(120),
+    quality_min_fts(50),
+    quality_max_drop_fts(40)
+#endif
+{}
+
+Config& Config::getInstance()
+{
+  static Config instance; // Instantiated on first use and guaranteed to be destroyed
+  return instance;
+}
+
+} // namespace svo
+

CPU_version/svo/src/depth_filter.cpp

@@ -0,0 +1,352 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <algorithm>
+#include <vikit/math_utils.h>
+#include <vikit/abstract_camera.h>
+#include <vikit/vision.h>
+#include <boost/bind.hpp>
+#include <boost/math/distributions/normal.hpp>
+#include <svo/global.h>
+#include <svo/depth_filter.h>
+#include <svo/frame.h>
+#include <svo/point.h>
+#include <svo/feature.h>
+#include <svo/matcher.h>
+#include <svo/config.h>
+#include <svo/feature_detection.h>
+
+namespace svo {
+
+int Seed::batch_counter = 0;
+int Seed::seed_counter = 0;
+
+Seed::Seed(Feature* ftr, float depth_mean, float depth_min) :
+    batch_id(batch_counter),
+    id(seed_counter++),
+    ftr(ftr),
+    a(10),
+    b(10),
+    mu(1.0/depth_mean),
+    z_range(1.0/depth_min),
+    sigma2(z_range*z_range/36)
+{}
+
+DepthFilter::DepthFilter(feature_detection::DetectorPtr feature_detector, callback_t seed_converged_cb) :
+    feature_detector_(feature_detector),
+    seed_converged_cb_(seed_converged_cb),
+    seeds_updating_halt_(false),
+    thread_(NULL),
+    new_keyframe_set_(false),
+    new_keyframe_min_depth_(0.0),
+    new_keyframe_mean_depth_(0.0)
+{}
+
+DepthFilter::~DepthFilter()
+{
+  stopThread();
+  SVO_INFO_STREAM("DepthFilter destructed.");
+}
+
+void DepthFilter::startThread()
+{
+  thread_ = new boost::thread(&DepthFilter::updateSeedsLoop, this);
+}
+
+void DepthFilter::stopThread()
+{
+  SVO_INFO_STREAM("DepthFilter stop thread invoked.");
+  if(thread_ != NULL)
+  {
+    SVO_INFO_STREAM("DepthFilter interrupt and join thread... ");
+    seeds_updating_halt_ = true;
+    thread_->interrupt();
+    thread_->join();
+    thread_ = NULL;
+  }
+}
+
+void DepthFilter::addFrame(FramePtr frame)
+{
+  if(thread_ != NULL)
+  {
+    {
+      lock_t lock(frame_queue_mut_);
+      if(frame_queue_.size() > 2)
+        frame_queue_.pop();
+      frame_queue_.push(frame);
+    }
+    seeds_updating_halt_ = false;
+    frame_queue_cond_.notify_one();
+  }
+  else
+    updateSeeds(frame);
+}
+
+void DepthFilter::addKeyframe(FramePtr frame, double depth_mean, double depth_min)
+{
+  new_keyframe_min_depth_ = depth_min;
+  new_keyframe_mean_depth_ = depth_mean;
+  if(thread_ != NULL)
+  {
+    new_keyframe_ = frame;
+    new_keyframe_set_ = true;
+    seeds_updating_halt_ = true;
+    frame_queue_cond_.notify_one();
+  }
+  else
+    initializeSeeds(frame);
+}
+
+void DepthFilter::initializeSeeds(FramePtr frame)
+{
+  Features new_features;
+  feature_detector_->setExistingFeatures(frame->fts_);
+  feature_detector_->detect(frame.get(), frame->img_pyr_,
+                            Config::triangMinCornerScore(), new_features);
+
+  // initialize a seed for every new feature
+  seeds_updating_halt_ = true;
+  lock_t lock(seeds_mut_); // by locking the updateSeeds function stops
+  ++Seed::batch_counter;
+  std::for_each(new_features.begin(), new_features.end(), [&](Feature* ftr){
+    seeds_.push_back(Seed(ftr, new_keyframe_mean_depth_, new_keyframe_min_depth_));
+  });
+
+  if(options_.verbose)
+    SVO_INFO_STREAM("DepthFilter: Initialized "<<new_features.size()<<" new seeds");
+  seeds_updating_halt_ = false;
+}
+
+void DepthFilter::removeKeyframe(FramePtr frame)
+{
+  seeds_updating_halt_ = true;
+  lock_t lock(seeds_mut_);
+  std::list<Seed>::iterator it=seeds_.begin();
+  size_t n_removed = 0;
+  while(it!=seeds_.end())
+  {
+    if(it->ftr->frame == frame.get())
+    {
+      it = seeds_.erase(it);
+      ++n_removed;
+    }
+    else
+      ++it;
+  }
+  seeds_updating_halt_ = false;
+}
+
+void DepthFilter::reset()
+{
+  seeds_updating_halt_ = true;
+  {
+    lock_t lock(seeds_mut_);
+    seeds_.clear();
+  }
+  lock_t lock();
+  while(!frame_queue_.empty())
+    frame_queue_.pop();
+  seeds_updating_halt_ = false;
+
+  if(options_.verbose)
+    SVO_INFO_STREAM("DepthFilter: RESET.");
+}
+
+void DepthFilter::updateSeedsLoop()
+{
+  while(!boost::this_thread::interruption_requested())
+  {
+    FramePtr frame;
+    {
+      lock_t lock(frame_queue_mut_);
+      while(frame_queue_.empty() && new_keyframe_set_ == false)
+        frame_queue_cond_.wait(lock);
+      if(new_keyframe_set_)
+      {
+        new_keyframe_set_ = false;
+        seeds_updating_halt_ = false;
+        clearFrameQueue();
+        frame = new_keyframe_;
+      }
+      else
+      {
+        frame = frame_queue_.front();
+        frame_queue_.pop();
+      }
+    }
+    updateSeeds(frame);
+    if(frame->isKeyframe())
+      initializeSeeds(frame);
+  }
+}
+
+void DepthFilter::updateSeeds(FramePtr frame)
+{
+  // update only a limited number of seeds, because we don't have time to do it
+  // for all the seeds in every frame!
+  size_t n_updates=0, n_failed_matches=0, n_seeds = seeds_.size();
+  lock_t lock(seeds_mut_);
+  std::list<Seed>::iterator it=seeds_.begin();
+
+  const double focal_length = frame->cam_->errorMultiplier2();
+  double px_noise = 1.0;
+  double px_error_angle = atan(px_noise/(2.0*focal_length))*2.0; // law of chord (sehnensatz)
+
+  while( it!=seeds_.end())
+  {
+    // set this value true when seeds updating should be interrupted
+    if(seeds_updating_halt_)
+      return;
+
+    // check if seed is not already too old
+    if((Seed::batch_counter - it->batch_id) > options_.max_n_kfs) {
+      it = seeds_.erase(it);
+      continue;
+    }
+
+    // check if point is visible in the current image
+    SE3 T_ref_cur = it->ftr->frame->T_f_w_ * frame->T_f_w_.inverse();
+    const Vector3d xyz_f(T_ref_cur.inverse()*(1.0/it->mu * it->ftr->f) );
+    if(xyz_f.z() < 0.0)  {
+      ++it; // behind the camera
+      continue;
+    }
+    if(!frame->cam_->isInFrame(frame->f2c(xyz_f).cast<int>())) {
+      ++it; // point does not project in image
+      continue;
+    }
+
+    // we are using inverse depth coordinates
+    float z_inv_min = it->mu + sqrt(it->sigma2);
+    float z_inv_max = max(it->mu - sqrt(it->sigma2), 0.00000001f);
+    double z;
+    if(!matcher_.findEpipolarMatchDirect(
+        *it->ftr->frame, *frame, *it->ftr, 1.0/it->mu, 1.0/z_inv_min, 1.0/z_inv_max, z))
+    {
+      it->b++; // increase outlier probability when no match was found
+      ++it;
+      ++n_failed_matches;
+      continue;
+    }
+
+    // compute tau
+    double tau = computeTau(T_ref_cur, it->ftr->f, z, px_error_angle);
+    double tau_inverse = 0.5 * (1.0/max(0.0000001, z-tau) - 1.0/(z+tau));
+
+    // update the estimate
+    updateSeed(1./z, tau_inverse*tau_inverse, &*it);
+    ++n_updates;
+
+    if(frame->isKeyframe())
+    {
+      // The feature detector should not initialize new seeds close to this location
+      feature_detector_->setGridOccpuancy(matcher_.px_cur_);
+    }
+
+    // if the seed has converged, we initialize a new candidate point and remove the seed
+    if(sqrt(it->sigma2) < it->z_range/options_.seed_convergence_sigma2_thresh)
+    {
+      assert(it->ftr->point == NULL); // TODO this should not happen anymore
+      Vector3d xyz_world(it->ftr->frame->T_f_w_.inverse() * (it->ftr->f * (1.0/it->mu)));
+      Point* point = new Point(xyz_world, it->ftr);
+      it->ftr->point = point;
+      /* FIXME it is not threadsafe to add a feature to the frame here.
+      if(frame->isKeyframe())
+      {
+        Feature* ftr = new Feature(frame.get(), matcher_.px_cur_, matcher_.search_level_);
+        ftr->point = point;
+        point->addFrameRef(ftr);
+        frame->addFeature(ftr);
+        it->ftr->frame->addFeature(it->ftr);
+      }
+      else
+      */
+      {
+        seed_converged_cb_(point, it->sigma2); // put in candidate list
+      }
+      it = seeds_.erase(it);
+    }
+    else if(isnan(z_inv_min))
+    {
+      SVO_WARN_STREAM("z_min is NaN");
+      it = seeds_.erase(it);
+    }
+    else
+      ++it;
+  }
+}
+
+void DepthFilter::clearFrameQueue()
+{
+  while(!frame_queue_.empty())
+    frame_queue_.pop();
+}
+
+void DepthFilter::getSeedsCopy(const FramePtr& frame, std::list<Seed>& seeds)
+{
+  lock_t lock(seeds_mut_);
+  for(std::list<Seed>::iterator it=seeds_.begin(); it!=seeds_.end(); ++it)
+  {
+    if (it->ftr->frame == frame.get())
+      seeds.push_back(*it);
+  }
+}
+
+void DepthFilter::updateSeed(const float x, const float tau2, Seed* seed)
+{
+  float norm_scale = sqrt(seed->sigma2 + tau2);
+  if(std::isnan(norm_scale))
+    return;
+  boost::math::normal_distribution<float> nd(seed->mu, norm_scale);
+  float s2 = 1./(1./seed->sigma2 + 1./tau2);
+  float m = s2*(seed->mu/seed->sigma2 + x/tau2);
+  float C1 = seed->a/(seed->a+seed->b) * boost::math::pdf(nd, x);
+  float C2 = seed->b/(seed->a+seed->b) * 1./seed->z_range;
+  float normalization_constant = C1 + C2;
+  C1 /= normalization_constant;
+  C2 /= normalization_constant;
+  float f = C1*(seed->a+1.)/(seed->a+seed->b+1.) + C2*seed->a/(seed->a+seed->b+1.);
+  float e = C1*(seed->a+1.)*(seed->a+2.)/((seed->a+seed->b+1.)*(seed->a+seed->b+2.))
+          + C2*seed->a*(seed->a+1.0f)/((seed->a+seed->b+1.0f)*(seed->a+seed->b+2.0f));
+
+  // update parameters
+  float mu_new = C1*m+C2*seed->mu;
+  seed->sigma2 = C1*(s2 + m*m) + C2*(seed->sigma2 + seed->mu*seed->mu) - mu_new*mu_new;
+  seed->mu = mu_new;
+  seed->a = (e-f)/(f-e/f);
+  seed->b = seed->a*(1.0f-f)/f;
+}
+
+double DepthFilter::computeTau(
+      const SE3& T_ref_cur,
+      const Vector3d& f,
+      const double z,
+      const double px_error_angle)
+{
+  Vector3d t(T_ref_cur.translation());
+  Vector3d a = f*z-t;
+  double t_norm = t.norm();
+  double a_norm = a.norm();
+  double alpha = acos(f.dot(t)/t_norm); // dot product
+  double beta = acos(a.dot(-t)/(t_norm*a_norm)); // dot product
+  double beta_plus = beta + px_error_angle;
+  double gamma_plus = PI-alpha-beta_plus; // triangle angles sum to PI
+  double z_plus = t_norm*sin(beta_plus)/sin(gamma_plus); // law of sines
+  return (z_plus - z); // tau
+}
+
+} // namespace svo

CPU_version/svo/src/feature_alignment.cpp

@@ -0,0 +1,601 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifdef __SSE2__
+#include <emmintrin.h>
+#endif
+#ifdef __ARM_NEON__
+#include <arm_neon.h>
+#endif
+#include <svo/feature_alignment.h>
+
+namespace svo {
+namespace feature_alignment {
+
+#define SUBPIX_VERBOSE 0
+
+bool align1D(
+    const cv::Mat& cur_img,
+    const Vector2f& dir,                  // direction in which the patch is allowed to move
+    uint8_t* ref_patch_with_border,
+    uint8_t* ref_patch,
+    const int n_iter,
+    Vector2d& cur_px_estimate,
+    double& h_inv)
+{
+  const int halfpatch_size_ = 4;
+  const int patch_size = 8;
+  const int patch_area = 64;
+  bool converged=false;
+
+  // compute derivative of template and prepare inverse compositional
+  float __attribute__((__aligned__(16))) ref_patch_dv[patch_area];
+  Matrix2f H; H.setZero();
+
+  // compute gradient and hessian
+  const int ref_step = patch_size+2;
+  float* it_dv = ref_patch_dv;
+  for(int y=0; y<patch_size; ++y)
+  {
+    uint8_t* it = ref_patch_with_border + (y+1)*ref_step + 1;
+    for(int x=0; x<patch_size; ++x, ++it, ++it_dv)
+    {
+      Vector2f J;
+      J[0] = 0.5*(dir[0]*(it[1] - it[-1]) + dir[1]*(it[ref_step] - it[-ref_step]));
+      J[1] = 1;
+      *it_dv = J[0];
+      H += J*J.transpose();
+    }
+  }
+  h_inv = 1.0/H(0,0)*patch_size*patch_size;
+  Matrix2f Hinv = H.inverse();
+  float mean_diff = 0;
+
+  // Compute pixel location in new image:
+  float u = cur_px_estimate.x();
+  float v = cur_px_estimate.y();
+
+  // termination condition
+  const float min_update_squared = 0.03*0.03;
+  const int cur_step = cur_img.step.p[0];
+  float chi2 = 0;
+  Vector2f update; update.setZero();
+  for(int iter = 0; iter<n_iter; ++iter)
+  {
+    int u_r = floor(u);
+    int v_r = floor(v);
+    if(u_r < halfpatch_size_ || v_r < halfpatch_size_ || u_r >= cur_img.cols-halfpatch_size_ || v_r >= cur_img.rows-halfpatch_size_)
+      break;
+
+    if(isnan(u) || isnan(v)) // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check
+      return false;
+
+    // compute interpolation weights
+    float subpix_x = u-u_r;
+    float subpix_y = v-v_r;
+    float wTL = (1.0-subpix_x)*(1.0-subpix_y);
+    float wTR = subpix_x * (1.0-subpix_y);
+    float wBL = (1.0-subpix_x)*subpix_y;
+    float wBR = subpix_x * subpix_y;
+
+    // loop through search_patch, interpolate
+    uint8_t* it_ref = ref_patch;
+    float* it_ref_dv = ref_patch_dv;
+    float new_chi2 = 0.0;
+    Vector2f Jres; Jres.setZero();
+    for(int y=0; y<patch_size; ++y)
+    {
+      uint8_t* it = (uint8_t*) cur_img.data + (v_r+y-halfpatch_size_)*cur_step + u_r-halfpatch_size_;
+      for(int x=0; x<patch_size; ++x, ++it, ++it_ref, ++it_ref_dv)
+      {
+        float search_pixel = wTL*it[0] + wTR*it[1] + wBL*it[cur_step] + wBR*it[cur_step+1];
+        float res = search_pixel - *it_ref + mean_diff;
+        Jres[0] -= res*(*it_ref_dv);
+        Jres[1] -= res;
+        new_chi2 += res*res;
+      }
+    }
+
+    if(iter > 0 && new_chi2 > chi2)
+    {
+#if SUBPIX_VERBOSE
+      cout << "error increased." << endl;
+#endif
+      u -= update[0];
+      v -= update[1];
+      break;
+    }
+
+    chi2 = new_chi2;
+    update = Hinv * Jres;
+    u += update[0]*dir[0];
+    v += update[0]*dir[1];
+    mean_diff += update[1];
+
+#if SUBPIX_VERBOSE
+    cout << "Iter " << iter << ":"
+         << "\t u=" << u << ", v=" << v
+         << "\t update = " << update[0] << ", " << update[1]
+         << "\t new chi2 = " << new_chi2 << endl;
+#endif
+
+    if(update[0]*update[0]+update[1]*update[1] < min_update_squared)
+    {
+#if SUBPIX_VERBOSE
+      cout << "converged." << endl;
+#endif
+      converged=true;
+      break;
+    }
+  }
+
+  cur_px_estimate << u, v;
+  return converged;
+}
+
+bool align2D(
+    const cv::Mat& cur_img,
+    uint8_t* ref_patch_with_border,
+    uint8_t* ref_patch,
+    const int n_iter,
+    Vector2d& cur_px_estimate,
+    bool no_simd)
+{
+#ifdef __ARM_NEON__
+  if(!no_simd)
+    return align2D_NEON(cur_img, ref_patch_with_border, ref_patch, n_iter, cur_px_estimate);
+#endif
+
+  const int halfpatch_size_ = 4;
+  const int patch_size_ = 8;
+  const int patch_area_ = 64;
+  bool converged=false;
+
+  // compute derivative of template and prepare inverse compositional
+  float __attribute__((__aligned__(16))) ref_patch_dx[patch_area_];
+  float __attribute__((__aligned__(16))) ref_patch_dy[patch_area_];
+  Matrix3f H; H.setZero();
+
+  // compute gradient and hessian
+  const int ref_step = patch_size_+2;
+  float* it_dx = ref_patch_dx;
+  float* it_dy = ref_patch_dy;
+  for(int y=0; y<patch_size_; ++y)
+  {
+    uint8_t* it = ref_patch_with_border + (y+1)*ref_step + 1;
+    for(int x=0; x<patch_size_; ++x, ++it, ++it_dx, ++it_dy)
+    {
+      Vector3f J;
+      J[0] = 0.5 * (it[1] - it[-1]);
+      J[1] = 0.5 * (it[ref_step] - it[-ref_step]);
+      J[2] = 1;
+      *it_dx = J[0];
+      *it_dy = J[1];
+      H += J*J.transpose();
+    }
+  }
+  Matrix3f Hinv = H.inverse();
+  float mean_diff = 0;
+
+  // Compute pixel location in new image:
+  float u = cur_px_estimate.x();
+  float v = cur_px_estimate.y();
+
+  // termination condition
+  const float min_update_squared = 0.03*0.03;
+  const int cur_step = cur_img.step.p[0];
+//  float chi2 = 0;
+  Vector3f update; update.setZero();
+  for(int iter = 0; iter<n_iter; ++iter)
+  {
+    int u_r = floor(u);
+    int v_r = floor(v);
+    if(u_r < halfpatch_size_ || v_r < halfpatch_size_ || u_r >= cur_img.cols-halfpatch_size_ || v_r >= cur_img.rows-halfpatch_size_)
+      break;
+
+    if(isnan(u) || isnan(v)) // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check
+      return false;
+
+    // compute interpolation weights
+    float subpix_x = u-u_r;
+    float subpix_y = v-v_r;
+    float wTL = (1.0-subpix_x)*(1.0-subpix_y);
+    float wTR = subpix_x * (1.0-subpix_y);
+    float wBL = (1.0-subpix_x)*subpix_y;
+    float wBR = subpix_x * subpix_y;
+
+    // loop through search_patch, interpolate
+    uint8_t* it_ref = ref_patch;
+    float* it_ref_dx = ref_patch_dx;
+    float* it_ref_dy = ref_patch_dy;
+//    float new_chi2 = 0.0;
+    Vector3f Jres; Jres.setZero();
+    for(int y=0; y<patch_size_; ++y)
+    {
+      uint8_t* it = (uint8_t*) cur_img.data + (v_r+y-halfpatch_size_)*cur_step + u_r-halfpatch_size_;
+      for(int x=0; x<patch_size_; ++x, ++it, ++it_ref, ++it_ref_dx, ++it_ref_dy)
+      {
+        float search_pixel = wTL*it[0] + wTR*it[1] + wBL*it[cur_step] + wBR*it[cur_step+1];
+        float res = search_pixel - *it_ref + mean_diff;
+        Jres[0] -= res*(*it_ref_dx);
+        Jres[1] -= res*(*it_ref_dy);
+        Jres[2] -= res;
+//        new_chi2 += res*res;
+      }
+    }
+
+
+/*
+    if(iter > 0 && new_chi2 > chi2)
+    {
+#if SUBPIX_VERBOSE
+      cout << "error increased." << endl;
+#endif
+      u -= update[0];
+      v -= update[1];
+      break;
+    }
+    chi2 = new_chi2;
+*/
+    update = Hinv * Jres;
+    u += update[0];
+    v += update[1];
+    mean_diff += update[2];
+
+#if SUBPIX_VERBOSE
+    cout << "Iter " << iter << ":"
+         << "\t u=" << u << ", v=" << v
+         << "\t update = " << update[0] << ", " << update[1]
+//         << "\t new chi2 = " << new_chi2 << endl;
+#endif
+
+    if(update[0]*update[0]+update[1]*update[1] < min_update_squared)
+    {
+#if SUBPIX_VERBOSE
+      cout << "converged." << endl;
+#endif
+      converged=true;
+      break;
+    }
+  }
+
+  cur_px_estimate << u, v;
+  return converged;
+}
+
+#define  DESCALE(x,n)     (((x) + (1 << ((n)-1))) >> (n)) // rounds to closest integer and descales
+
+bool align2D_SSE2(
+    const cv::Mat& cur_img,
+    uint8_t* ref_patch_with_border,
+    uint8_t* ref_patch,
+    const int n_iter,
+    Vector2d& cur_px_estimate)
+{
+  // TODO: This function should not be used as the alignment is not robust to illumination changes!
+  const int halfpatch_size = 4;
+  const int patch_size = 8;
+  const int patch_area = 64;
+  bool converged=false;
+  const int W_BITS = 14;
+
+  // compute derivative of template and prepare inverse compositional
+  int16_t __attribute__((__aligned__(16))) ref_patch_dx[patch_area];
+  int16_t __attribute__((__aligned__(16))) ref_patch_dy[patch_area];
+
+  // compute gradient and hessian
+  const int ref_step = patch_size+2;
+  int16_t* it_dx = ref_patch_dx;
+  int16_t* it_dy = ref_patch_dy;
+  float A11=0, A12=0, A22=0;
+  for(int y=0; y<patch_size; ++y)
+  {
+    uint8_t* it = ref_patch_with_border + (y+1)*ref_step + 1;
+    for(int x=0; x<patch_size; ++x, ++it, ++it_dx, ++it_dy)
+    {
+      int16_t dx = static_cast<int16_t>(it[1]) - it[-1];
+      int16_t dy = static_cast<int16_t>(it[ref_step]) - it[-ref_step];
+      *it_dx = dx;
+      *it_dy = dy;  // we are missing a factor 1/2
+      A11 += static_cast<float>(dx*dx); // we are missing a factor 1/4
+      A12 += static_cast<float>(dx*dy);
+      A22 += static_cast<float>(dy*dy);
+    }
+  }
+
+  // Compute pixel location in new image:
+  float u = cur_px_estimate.x();
+  float v = cur_px_estimate.y();
+
+  // termination condition
+  const float min_update_squared = 0.03*0.03;
+  const int cur_step = cur_img.step.p[0];
+  const float Dinv = 1.0f/(A11*A22 - A12*A12); // we are missing an extra factor 16
+  float chi2 = 0;
+  float update_u = 0, update_v = 0;
+
+  for(int iter = 0; iter<n_iter; ++iter)
+  {
+    int u_r = floor(u);
+    int v_r = floor(v);
+    if(u_r < halfpatch_size || v_r < halfpatch_size || u_r >= cur_img.cols-halfpatch_size || v_r >= cur_img.rows-halfpatch_size)
+      break;
+
+    if(isnan(u) || isnan(v)) // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check
+      return false;
+
+    float subpix_x = u-u_r;
+    float subpix_y = v-v_r;
+    float b1=0, b2=0;
+    float new_chi2 = 0.0;
+
+#ifdef __SSE2__
+    // compute bilinear interpolation weights
+    int wTL = static_cast<int>((1.0f-subpix_x)*(1.0f-subpix_y)*(1 << W_BITS));
+    int wTR = static_cast<int>(subpix_x * (1.0f-subpix_y)*(1 << W_BITS));
+    int wBL = static_cast<int>((1.0f-subpix_x)*subpix_y*(1 << W_BITS));
+    int wBR = (1 << W_BITS) - wTL - wTR - wBL;
+
+    __m128i qw0 = _mm_set1_epi32(wTL + (wTR << 16)); // Sets the 4 signed 32-bit integer values to [wTL, wTR].
+    __m128i qw1 = _mm_set1_epi32(wBL + (wBR << 16));
+    __m128i z = _mm_setzero_si128();
+    __m128 qb0 = _mm_setzero_ps(); // 4 floats
+    __m128 qb1 = _mm_setzero_ps(); // 4 floats
+    __m128i qdelta = _mm_set1_epi32(1 << (W_BITS-1));
+    for(int y=0; y<patch_size; ++y)
+    {
+      const uint8_t* it  = (const uint8_t*) cur_img.data + (v_r+y-halfpatch_size)*cur_step + u_r-halfpatch_size;
+
+      // Iptr is aligned!
+      //__m128i diff0 = _mm_load_si128((const __m128i*)(ref_patch + y*8));
+      __m128i diff = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(ref_patch + y*8)), z);
+
+      // load the lower 64 bits and unpack [8u 0 8u 0..]
+      __m128i v00 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(it)), z);
+      __m128i v01 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(it + 1)), z);
+      __m128i v10 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(it + cur_step)), z);
+      __m128i v11 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(it + cur_step + 1)), z);
+
+      // interpolate top row and bottom row
+      // _mm_unpacklo_epi16: Interleaves the lower 4 signed or unsigned 16-bit integers in a with the lower 4 signed or unsigned 16-bit integers in b.
+      // _mm_madd_epi16: Multiplies the 8 signed 16-bit integers from a by the 8 signed 16-bit integers from b.
+      __m128i t0 = _mm_add_epi32(_mm_madd_epi16(_mm_unpacklo_epi16(v00, v01), qw0),
+                                 _mm_madd_epi16(_mm_unpacklo_epi16(v10, v11), qw1));
+      __m128i t1 = _mm_add_epi32(_mm_madd_epi16(_mm_unpackhi_epi16(v00, v01), qw0),
+                                 _mm_madd_epi16(_mm_unpackhi_epi16(v10, v11), qw1));
+
+      // _mm_srai_epi32: Shifts the 4 signed 32-bit integers in A right by count bits while shifting in the sign bit.
+      t0 = _mm_srai_epi32(_mm_add_epi32(t0, qdelta), W_BITS); // adding qdelta is for rounding closest int
+      t1 = _mm_srai_epi32(_mm_add_epi32(t1, qdelta), W_BITS);
+
+      // compute 8xres:
+      // _mm_packs_epi32: Packs the 8 signed 32-bit integers from a and b into signed 16-bit integers and saturates.
+      diff = _mm_subs_epi16(_mm_packs_epi32(t0, t1), diff);
+
+      // load gradient dX and dY, both are aligned!
+      v00 = _mm_load_si128((const __m128i*)(ref_patch_dx + y*patch_size)); // [dx1, dx2, dx3, dx4 ...]
+      v01 = _mm_load_si128((const __m128i*)(ref_patch_dy + y*patch_size)); // [dy1, dy2, dy3, dy4 ...]
+
+      // _mm_mulhi_epi16: Multiplies the 8 signed 16-bit integers from a by the 8 signed 16-bit integers from b.
+      v10 = _mm_mullo_epi16(v00, diff); // Packs the lower 16 bits of the 8 signed 32-bit results. [15:0]
+      v11 = _mm_mulhi_epi16(v00, diff); // Packs the upper 16 bits of the 8 signed 32-bit results. [31:16]
+
+      // _mm_unpacklo_epi16: Interleaves the lower 4 signed or unsigned 16-bit integers with the lower 4 signed or unsigned 16-bit integers in b.
+      v00 = _mm_unpacklo_epi16(v10, v11);
+      v10 = _mm_unpackhi_epi16(v10, v11);
+
+      // convert to float and add to dx
+      qb0 = _mm_add_ps(qb0, _mm_cvtepi32_ps(v00));
+      qb0 = _mm_add_ps(qb0, _mm_cvtepi32_ps(v10));
+
+      // same with dY
+      v10 = _mm_mullo_epi16(v01, diff);
+      v11 = _mm_mulhi_epi16(v01, diff);
+      v00 = _mm_unpacklo_epi16(v10, v11);
+      v10 = _mm_unpackhi_epi16(v10, v11);
+      qb1 = _mm_add_ps(qb1, _mm_cvtepi32_ps(v00));
+      qb1 = _mm_add_ps(qb1, _mm_cvtepi32_ps(v10));
+    }
+
+    float __attribute__((__aligned__(16))) buf[4];
+    _mm_store_ps(buf, qb0);
+    b1 += buf[0]+buf[1]+buf[2]+buf[3];
+    _mm_store_ps(buf, qb1);
+    b2 += buf[0]+buf[1]+buf[2]+buf[3];
+#endif
+
+    // compute -A^-1*b
+    update_u = ((A12*b2 - A22*b1) * Dinv)*2; // * 2 to compensate because above, we did not compute the derivative correctly
+    update_v = ((A12*b1 - A11*b2) * Dinv)*2;
+    u += update_u;
+    v += update_v;
+
+#if SUBPIX_VERBOSE
+    cout << "Iter " << iter << ":"
+         << "\t u=" << u << ", v=" << v
+         << "\t update = " << update_u << ", " << update_v
+         << "\t new chi2 = " << new_chi2 << endl;
+#endif
+
+    if(update_u*update_u+update_v*update_v < min_update_squared)
+    {
+#if SUBPIX_VERBOSE
+      cout << "converged." << endl;
+#endif
+      converged=true;
+      break;
+    }
+    chi2 = new_chi2;
+  }
+
+  cur_px_estimate << u, v;
+  return converged;
+}
+
+bool align2D_NEON (
+    const cv::Mat& cur_img,
+    uint8_t* ref_patch_with_border,
+    uint8_t* ref_patch,
+    const int n_iter,
+    Vector2d& cur_px_estimate)
+{
+  const int halfpatch_size = 4;
+  const int patch_size = 8;
+  const int patch_area = 64;
+  bool converged=false;
+  const int W_BITS = 14;
+
+  // compute derivative of template and prepare inverse compositional
+  int16_t __attribute__((__aligned__(16))) ref_patch_dx[patch_area];
+  int16_t __attribute__((__aligned__(16))) ref_patch_dy[patch_area];
+  
+  // compute gradient and hessian
+  const int ref_step = patch_size+2;
+  int16_t* it_dx = ref_patch_dx;
+  int16_t* it_dy = ref_patch_dy;
+  Matrix3f H; H.setZero();
+  for(int y=0; y<patch_size; ++y)
+  {
+    uint8_t* it = ref_patch_with_border + (y+1)*ref_step + 1;
+    for(int x=0; x<patch_size; ++x, ++it, ++it_dx, ++it_dy)
+    {
+      *it_dx = static_cast<int16_t>(it[1] - it[-1]);
+      *it_dy = static_cast<int16_t>(it[ref_step] - it[-ref_step]); // divide by 2 missing
+      Vector3f J(*it_dx, *it_dy, 1.0f);
+      H += J*J.transpose();
+    }
+  }
+  Matrix3f Hinv = H.inverse();
+  float mean_diff = 0.0;
+
+  // Compute pixel location in new image:
+  float u = cur_px_estimate.x();
+  float v = cur_px_estimate.y();
+
+  // termination condition
+  const float min_update_squared = 0.03*0.03;
+  const int cur_step = cur_img.step.p[0];
+  Vector3f update;
+  Vector3f Jres;
+  for(int iter = 0; iter<n_iter; ++iter)
+  {
+    int u_r = floor(u);
+    int v_r = floor(v);
+    if(u_r < halfpatch_size || v_r < halfpatch_size || u_r >= cur_img.cols-halfpatch_size || v_r >= cur_img.rows-halfpatch_size)
+      break;
+
+    if(isnan(u) || isnan(v)) // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check
+      return false;
+
+    float subpix_x = u-u_r;
+    float subpix_y = v-v_r;
+    float b1=0, b2=0;
+
+#ifdef __ARM_NEON__
+    const int SHIFT_BITS = 7;
+    const uint16_t wTL = static_cast<uint16_t>((1.0f-subpix_x)*(1.0f-subpix_y)*(1<<SHIFT_BITS));
+    const uint16_t wTR = static_cast<uint16_t>(subpix_x*(1.0f-subpix_y)*(1<<SHIFT_BITS));
+    const uint16_t wBL = static_cast<uint16_t>((1.0f-subpix_x)*subpix_y*(1<<SHIFT_BITS));
+    const uint16_t wBR = static_cast<uint16_t>((1 << SHIFT_BITS) - wTL - wTR - wBL);
+
+    // initialize result to zero
+    int32x4_t vb1 = vdupq_n_s32(0);
+    int32x4_t vb2 = vdupq_n_s32(0);
+    int16x8_t vmean_diff = vdupq_n_s16( (int16_t) (mean_diff+0.5) );
+    int16x8_t vres_sum = vdupq_n_s16(0);
+    for(int y=0; y<patch_size; ++y)
+    {
+      const uint8_t* it  = (const uint8_t*) cur_img.data + (v_r+y-halfpatch_size)*cur_step + u_r-halfpatch_size;
+
+      // load and convert from uint8 to uint16
+      uint16x8_t v00 = vmovl_u8( vld1_u8( it ) );
+      uint16x8_t v01 = vmovl_u8( vld1_u8( it + 1 ) );
+      uint16x8_t v10 = vmovl_u8( vld1_u8( it + cur_step ) );
+      uint16x8_t v11 = vmovl_u8( vld1_u8( it + cur_step + 1 ) );
+
+      // vector multiply by scalar
+      v00 = vmulq_n_u16( v00, wTL );
+      v01 = vmulq_n_u16( v01, wTR );
+      v10 = vmulq_n_u16( v10, wBL );
+      v11 = vmulq_n_u16( v11, wBR );
+
+      // add all results together
+      v00 = vaddq_u16( v00, vaddq_u16( v01, vaddq_u16( v10, v11 ) ) );
+
+      // descale: shift right by constant
+      v00 = vshrq_n_u16(v00, SHIFT_BITS);
+
+      // compute difference between reference and interpolated patch,
+      // use reinterpet-cast to make signed [-255,255]
+      int16x8_t res = vsubq_s16(vreinterpretq_s16_u16(v00), vreinterpretq_s16_u16(vmovl_u8(vld1_u8( ref_patch + y*8 ))));
+
+      // correct res with mean difference
+      res = vaddq_s16(res, vmean_diff);
+
+      // compute sum of the residual
+      vres_sum = vaddq_s16(vres_sum, res); 
+
+      // Vector multiply accumulate long: vmla -> Vr[i] := Va[i] + Vb[i] * Vc[i]
+      // int32x4_t  vmlal_s16(int32x4_t a, int16x4_t b, int16x4_t c);    // VMLAL.S16 q0,d0,d0
+      int16x8_t grad = vld1q_s16(ref_patch_dx + y*patch_size);
+      vb1 = vmlal_s16(vb1, vget_low_s16(grad), vget_low_s16(res));
+      vb1 = vmlal_s16(vb1, vget_high_s16(grad), vget_high_s16(res));
+
+      grad = vld1q_s16(ref_patch_dy + y*patch_size);
+      vb2 = vmlal_s16(vb2, vget_low_s16(grad), vget_low_s16(res));
+      vb2 = vmlal_s16(vb2, vget_high_s16(grad), vget_high_s16(res));
+    }
+
+    // finally, sum results of vb1, vb2 and vres_sum
+    int32x2_t tmp;
+    tmp = vpadd_s32(vget_low_s32(vb1), vget_high_s32(vb1));
+    Jres[0] = -vget_lane_s32(tmp, 0) - vget_lane_s32(tmp, 1);
+
+    tmp = vpadd_s32(vget_low_s32(vb2), vget_high_s32(vb2));
+    Jres[1] = -vget_lane_s32(tmp, 0) - vget_lane_s32(tmp, 1);
+
+    int32x4_t vres_sum1 = vpaddlq_s16(vres_sum);
+    tmp = vpadd_s32(vget_low_s32(vres_sum1), vget_high_s32(vres_sum1));
+    Jres[2] = -vget_lane_s32(tmp, 0) - vget_lane_s32(tmp, 1);
+#endif
+
+    update = Hinv * Jres * 2; // * 2 to compensate because above, we did not compute the derivative correctly
+    u += update[0];
+    v += update[1];
+    mean_diff += update[2];
+
+#if SUBPIX_VERBOSE
+    cout << "Iter " << iter << ":"
+         << "\t u=" << u << ", v=" << v
+         << "\t update = " << update[0] << ", " << update[1] << endl;
+#endif
+
+    if(update[0]*update[0]+update[1]*update[1] < min_update_squared)
+    {
+#if SUBPIX_VERBOSE
+      cout << "converged." << endl;
+#endif
+      converged=true;
+      break;
+    }
+  }
+
+  cur_px_estimate << u, v;
+  return converged;
+}
+
+} // namespace feature_alignment
+} // namespace svo

CPU_version/svo/src/feature_detection.cpp

@@ -0,0 +1,118 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <svo/feature_detection.h>
+#include <svo/feature.h>
+#include <fast/fast.h>
+#include <vikit/vision.h>
+
+namespace svo {
+namespace feature_detection {
+
+AbstractDetector::AbstractDetector(
+    const int img_width,
+    const int img_height,
+    const int cell_size,
+    const int n_pyr_levels) :
+        cell_size_(cell_size),
+        n_pyr_levels_(n_pyr_levels),
+        grid_n_cols_(ceil(static_cast<double>(img_width)/cell_size_)),
+        grid_n_rows_(ceil(static_cast<double>(img_height)/cell_size_)),
+        grid_occupancy_(grid_n_cols_*grid_n_rows_, false)
+{}
+
+void AbstractDetector::resetGrid()
+{
+  std::fill(grid_occupancy_.begin(), grid_occupancy_.end(), false);
+}
+
+void AbstractDetector::setExistingFeatures(const Features& fts)
+{
+  std::for_each(fts.begin(), fts.end(), [&](Feature* i){
+    grid_occupancy_.at(
+        static_cast<int>(i->px[1]/cell_size_)*grid_n_cols_
+        + static_cast<int>(i->px[0]/cell_size_)) = true;
+  });
+}
+
+void AbstractDetector::setGridOccpuancy(const Vector2d& px)
+{
+  grid_occupancy_.at(
+      static_cast<int>(px[1]/cell_size_)*grid_n_cols_
+    + static_cast<int>(px[0]/cell_size_)) = true;
+}
+
+FastDetector::FastDetector(
+    const int img_width,
+    const int img_height,
+    const int cell_size,
+    const int n_pyr_levels) :
+        AbstractDetector(img_width, img_height, cell_size, n_pyr_levels)
+{}
+
+void FastDetector::detect(
+    Frame* frame,
+    const ImgPyr& img_pyr,
+    const double detection_threshold,
+    Features& fts)
+{
+  Corners corners(grid_n_cols_*grid_n_rows_, Corner(0,0,detection_threshold,0,0.0f));
+  for(int L=0; L<n_pyr_levels_; ++L)
+  {
+    const int scale = (1<<L);
+    vector<fast::fast_xy> fast_corners;
+#if __SSE2__
+      fast::fast_corner_detect_10_sse2(
+          (fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols,
+          img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners);
+#elif HAVE_FAST_NEON
+      fast::fast_corner_detect_9_neon(
+          (fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols,
+          img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners);
+#else
+      fast::fast_corner_detect_10(
+          (fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols,
+          img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners);
+#endif
+    vector<int> scores, nm_corners;
+    fast::fast_corner_score_10((fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols, fast_corners, 20, scores);
+    fast::fast_nonmax_3x3(fast_corners, scores, nm_corners);
+
+    for(auto it=nm_corners.begin(), ite=nm_corners.end(); it!=ite; ++it)
+    {
+      fast::fast_xy& xy = fast_corners.at(*it);
+      const int k = static_cast<int>((xy.y*scale)/cell_size_)*grid_n_cols_
+                  + static_cast<int>((xy.x*scale)/cell_size_);
+      if(grid_occupancy_[k])
+        continue;
+      const float score = vk::shiTomasiScore(img_pyr[L], xy.x, xy.y);
+      if(score > corners.at(k).score)
+        corners.at(k) = Corner(xy.x*scale, xy.y*scale, score, L, 0.0f);
+    }
+  }
+
+  // Create feature for every corner that has high enough corner score
+  std::for_each(corners.begin(), corners.end(), [&](Corner& c) {
+    if(c.score > detection_threshold)
+      fts.push_back(new Feature(frame, Vector2d(c.x, c.y), c.level));
+  });
+
+  resetGrid();
+}
+
+} // namespace feature_detection
+} // namespace svo
+

CPU_version/svo/src/frame.cpp

@@ -0,0 +1,190 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <stdexcept>
+#include <svo/frame.h>
+#include <svo/feature.h>
+#include <svo/point.h>
+#include <svo/config.h>
+#include <vikit/math_utils.h>
+#include <vikit/vision.h>
+#include <vikit/performance_monitor.h>
+#include <fast/fast.h>
+
+namespace svo {
+
+int Frame::frame_counter_ = 0;
+
+Frame::Frame(vk::AbstractCamera* cam, const cv::Mat& img, double timestamp) :
+    id_(frame_counter_++),
+    timestamp_(timestamp),
+    cam_(cam),
+    key_pts_(5),
+    is_keyframe_(false),
+    v_kf_(NULL)
+{
+  initFrame(img);
+}
+
+Frame::~Frame()
+{
+  std::for_each(fts_.begin(), fts_.end(), [&](Feature* i){delete i;});
+}
+
+void Frame::initFrame(const cv::Mat& img)
+{
+  // check image
+  if(img.empty() || img.type() != CV_8UC1 || img.cols != cam_->width() || img.rows != cam_->height())
+    throw std::runtime_error("Frame: provided image has not the same size as the camera model or image is not grayscale");
+
+  // Set keypoints to NULL
+  std::for_each(key_pts_.begin(), key_pts_.end(), [&](Feature* ftr){ ftr=NULL; });
+
+  // Build Image Pyramid
+  frame_utils::createImgPyramid(img, max(Config::nPyrLevels(), Config::kltMaxLevel()+1), img_pyr_);
+}
+
+void Frame::setKeyframe()
+{
+  is_keyframe_ = true;
+  setKeyPoints();
+}
+
+void Frame::addFeature(Feature* ftr)
+{
+  fts_.push_back(ftr);
+}
+
+void Frame::setKeyPoints()
+{
+  for(size_t i = 0; i < 5; ++i)
+    if(key_pts_[i] != NULL)
+      if(key_pts_[i]->point == NULL)
+        key_pts_[i] = NULL;
+
+  std::for_each(fts_.begin(), fts_.end(), [&](Feature* ftr){ if(ftr->point != NULL) checkKeyPoints(ftr); });
+}
+
+void Frame::checkKeyPoints(Feature* ftr)
+{
+  const int cu = cam_->width()/2;
+  const int cv = cam_->height()/2;
+
+  // center pixel
+  if(key_pts_[0] == NULL)
+    key_pts_[0] = ftr;
+  else if(std::max(std::fabs(ftr->px[0]-cu), std::fabs(ftr->px[1]-cv))
+        < std::max(std::fabs(key_pts_[0]->px[0]-cu), std::fabs(key_pts_[0]->px[1]-cv)))
+    key_pts_[0] = ftr;
+
+  if(ftr->px[0] >= cu && ftr->px[1] >= cv)
+  {
+    if(key_pts_[1] == NULL)
+      key_pts_[1] = ftr;
+    else if((ftr->px[0]-cu) * (ftr->px[1]-cv)
+          > (key_pts_[1]->px[0]-cu) * (key_pts_[1]->px[1]-cv))
+      key_pts_[1] = ftr;
+  }
+  if(ftr->px[0] >= cu && ftr->px[1] < cv)
+  {
+    if(key_pts_[2] == NULL)
+      key_pts_[2] = ftr;
+    else if((ftr->px[0]-cu) * (ftr->px[1]-cv)
+          > (key_pts_[2]->px[0]-cu) * (key_pts_[2]->px[1]-cv))
+      key_pts_[2] = ftr;
+  }
+  if(ftr->px[0] < cv && ftr->px[1] < cv)
+  {
+    if(key_pts_[3] == NULL)
+      key_pts_[3] = ftr;
+    else if((ftr->px[0]-cu) * (ftr->px[1]-cv)
+          > (key_pts_[3]->px[0]-cu) * (key_pts_[3]->px[1]-cv))
+      key_pts_[3] = ftr;
+  }
+  if(ftr->px[0] < cv && ftr->px[1] >= cv)
+  {
+    if(key_pts_[4] == NULL)
+      key_pts_[4] = ftr;
+    else if((ftr->px[0]-cu) * (ftr->px[1]-cv)
+          > (key_pts_[4]->px[0]-cu) * (key_pts_[4]->px[1]-cv))
+      key_pts_[4] = ftr;
+  }
+}
+
+void Frame::removeKeyPoint(Feature* ftr)
+{
+  bool found = false;
+  std::for_each(key_pts_.begin(), key_pts_.end(), [&](Feature*& i){
+    if(i == ftr) {
+      i = NULL;
+      found = true;
+    }
+  });
+  if(found)
+    setKeyPoints();
+}
+
+bool Frame::isVisible(const Vector3d& xyz_w) const
+{
+  Vector3d xyz_f = T_f_w_*xyz_w;
+  if(xyz_f.z() < 0.0)
+    return false; // point is behind the camera
+  Vector2d px = f2c(xyz_f);
+  if(px[0] >= 0.0 && px[1] >= 0.0 && px[0] < cam_->width() && px[1] < cam_->height())
+    return true;
+  return false;
+}
+
+
+/// Utility functions for the Frame class
+namespace frame_utils {
+
+void createImgPyramid(const cv::Mat& img_level_0, int n_levels, ImgPyr& pyr)
+{
+  pyr.resize(n_levels);
+  pyr[0] = img_level_0;
+  for(int i=1; i<n_levels; ++i)
+  {
+    pyr[i] = cv::Mat(pyr[i-1].rows/2, pyr[i-1].cols/2, CV_8U);
+    vk::halfSample(pyr[i-1], pyr[i]);
+  }
+}
+
+bool getSceneDepth(const Frame& frame, double& depth_mean, double& depth_min)
+{
+  vector<double> depth_vec;
+  depth_vec.reserve(frame.fts_.size());
+  depth_min = std::numeric_limits<double>::max();
+  for(auto it=frame.fts_.begin(), ite=frame.fts_.end(); it!=ite; ++it)
+  {
+    if((*it)->point != NULL)
+    {
+      const double z = frame.w2f((*it)->point->pos_).z();
+      depth_vec.push_back(z);
+      depth_min = fmin(z, depth_min);
+    }
+  }
+  if(depth_vec.empty())
+  {
+    SVO_WARN_STREAM("Cannot set scene depth. Frame has no point-observations!");
+    return false;
+  }
+  depth_mean = vk::getMedian(depth_vec);
+  return true;
+}
+
+} // namespace frame_utils
+} // namespace svo

CPU_version/svo/src/frame_handler_base.cpp

@@ -0,0 +1,199 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <vikit/abstract_camera.h>
+#include <stdlib.h>
+#include <Eigen/StdVector>
+#include <boost/bind.hpp>
+#include <fstream>
+#include <svo/frame_handler_base.h>
+#include <svo/config.h>
+#include <svo/feature.h>
+#include <svo/matcher.h>
+#include <svo/map.h>
+#include <svo/point.h>
+
+namespace svo
+{
+
+// definition of global and static variables which were declared in the header
+#ifdef SVO_TRACE
+vk::PerformanceMonitor* g_permon = NULL;
+#endif
+
+FrameHandlerBase::FrameHandlerBase() :
+  stage_(STAGE_PAUSED),
+  set_reset_(false),
+  set_start_(false),
+  acc_frame_timings_(10),
+  acc_num_obs_(10),
+  num_obs_last_(0),
+  tracking_quality_(TRACKING_INSUFFICIENT)
+{
+#ifdef SVO_TRACE
+  // Initialize Performance Monitor
+  g_permon = new vk::PerformanceMonitor();
+  g_permon->addTimer("pyramid_creation");
+  g_permon->addTimer("sparse_img_align");
+  g_permon->addTimer("reproject");
+  g_permon->addTimer("reproject_kfs");
+  g_permon->addTimer("reproject_candidates");
+  g_permon->addTimer("feature_align");
+  g_permon->addTimer("pose_optimizer");
+  g_permon->addTimer("point_optimizer");
+  g_permon->addTimer("local_ba");
+  g_permon->addTimer("tot_time");
+  g_permon->addLog("timestamp");
+  g_permon->addLog("img_align_n_tracked");
+  g_permon->addLog("repr_n_mps");
+  g_permon->addLog("repr_n_new_references");
+  g_permon->addLog("sfba_thresh");
+  g_permon->addLog("sfba_error_init");
+  g_permon->addLog("sfba_error_final");
+  g_permon->addLog("sfba_n_edges_final");
+  g_permon->addLog("loba_n_erredges_init");
+  g_permon->addLog("loba_n_erredges_fin");
+  g_permon->addLog("loba_err_init");
+  g_permon->addLog("loba_err_fin");
+  g_permon->addLog("n_candidates");
+  g_permon->addLog("dropout");
+  g_permon->init(Config::traceName(), Config::traceDir());
+#endif
+
+  SVO_INFO_STREAM("SVO initialized");
+}
+
+FrameHandlerBase::~FrameHandlerBase()
+{
+  SVO_INFO_STREAM("SVO destructor invoked");
+#ifdef SVO_TRACE
+  delete g_permon;
+#endif
+}
+
+bool FrameHandlerBase::startFrameProcessingCommon(const double timestamp)
+{
+  if(set_start_)
+  {
+    resetAll();
+    stage_ = STAGE_FIRST_FRAME;
+  }
+
+  if(stage_ == STAGE_PAUSED)
+    return false;
+
+  SVO_LOG(timestamp);
+  SVO_DEBUG_STREAM("New Frame");
+  SVO_START_TIMER("tot_time");
+  timer_.start();
+
+  // some cleanup from last iteration, can't do before because of visualization
+  map_.emptyTrash();
+  return true;
+}
+
+int FrameHandlerBase::finishFrameProcessingCommon(
+    const size_t update_id,
+    const UpdateResult dropout,
+    const size_t num_observations)
+{
+  SVO_DEBUG_STREAM("Frame: "<<update_id<<"\t fps-avg = "<< 1.0/acc_frame_timings_.getMean()<<"\t nObs = "<<acc_num_obs_.getMean());
+  SVO_LOG(dropout);
+
+  // save processing time to calculate fps
+  acc_frame_timings_.push_back(timer_.stop());
+  if(stage_ == STAGE_DEFAULT_FRAME)
+    acc_num_obs_.push_back(num_observations);
+  num_obs_last_ = num_observations;
+  SVO_STOP_TIMER("tot_time");
+
+#ifdef SVO_TRACE
+  g_permon->writeToFile();
+  {
+    boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_);
+    size_t n_candidates = map_.point_candidates_.candidates_.size();
+    SVO_LOG(n_candidates);
+  }
+#endif
+
+  if(dropout == RESULT_FAILURE &&
+      (stage_ == STAGE_DEFAULT_FRAME || stage_ == STAGE_RELOCALIZING ))
+  {
+    stage_ = STAGE_RELOCALIZING;
+    tracking_quality_ = TRACKING_INSUFFICIENT;
+  }
+  else if (dropout == RESULT_FAILURE)
+    resetAll();
+  if(set_reset_)
+    resetAll();
+
+  return 0;
+}
+
+void FrameHandlerBase::resetCommon()
+{
+  map_.reset();
+  stage_ = STAGE_PAUSED;
+  set_reset_ = false;
+  set_start_ = false;
+  tracking_quality_ = TRACKING_INSUFFICIENT;
+  num_obs_last_ = 0;
+  SVO_INFO_STREAM("RESET");
+}
+
+void FrameHandlerBase::setTrackingQuality(const size_t num_observations)
+{
+  tracking_quality_ = TRACKING_GOOD;
+  if(num_observations < Config::qualityMinFts())
+  {
+    SVO_WARN_STREAM_THROTTLE(0.5, "Tracking less than "<< Config::qualityMinFts() <<" features!");
+    tracking_quality_ = TRACKING_INSUFFICIENT;
+  }
+  const int feature_drop = static_cast<int>(std::min(num_obs_last_, Config::maxFts())) - num_observations;
+  if(feature_drop > Config::qualityMaxFtsDrop())
+  {
+    SVO_WARN_STREAM("Lost "<< feature_drop <<" features!");
+    tracking_quality_ = TRACKING_INSUFFICIENT;
+  }
+}
+
+bool ptLastOptimComparator(Point* lhs, Point* rhs)
+{
+  return (lhs->last_structure_optim_ < rhs->last_structure_optim_);
+}
+
+void FrameHandlerBase::optimizeStructure(
+    FramePtr frame,
+    size_t max_n_pts,
+    int max_iter)
+{
+  deque<Point*> pts;
+  for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
+  {
+    if((*it)->point != NULL)
+      pts.push_back((*it)->point);
+  }
+  max_n_pts = min(max_n_pts, pts.size());
+  nth_element(pts.begin(), pts.begin() + max_n_pts, pts.end(), ptLastOptimComparator);
+  for(deque<Point*>::iterator it=pts.begin(); it!=pts.begin()+max_n_pts; ++it)
+  {
+    (*it)->optimize(max_iter);
+    (*it)->last_structure_optim_ = frame->id_;
+  }
+}
+
+
+} // namespace svo

CPU_version/svo/src/frame_handler_mono.cpp

@@ -0,0 +1,307 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <svo/config.h>
+#include <svo/frame_handler_mono.h>
+#include <svo/map.h>
+#include <svo/frame.h>
+#include <svo/feature.h>
+#include <svo/point.h>
+#include <svo/pose_optimizer.h>
+#include <svo/sparse_img_align.h>
+#include <vikit/performance_monitor.h>
+#include <svo/depth_filter.h>
+#ifdef USE_BUNDLE_ADJUSTMENT
+#include <svo/bundle_adjustment.h>
+#endif
+
+namespace svo {
+
+FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Sophus::SE3& SE_init) :
+  FrameHandlerBase(),
+  cam_(cam),
+  reprojector_(cam_, map_),
+  depth_filter_(NULL)
+{
+  initialize();
+  imu_integPtr_=std::make_unique<Imu_Integration>(SE_init);
+}
+
+void FrameHandlerMono::initialize()
+{
+  feature_detection::DetectorPtr feature_detector(
+      new feature_detection::FastDetector(
+          cam_->width(), cam_->height(), Config::gridSize(), Config::nPyrLevels()));
+  DepthFilter::callback_t depth_filter_cb = boost::bind(
+      &MapPointCandidates::newCandidatePoint, &map_.point_candidates_, _1, _2);
+  depth_filter_ = new DepthFilter(feature_detector, depth_filter_cb);
+  depth_filter_->startThread();
+}
+
+FrameHandlerMono::~FrameHandlerMono()
+{
+  delete depth_filter_;
+}
+
+void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp)
+{
+  if(!startFrameProcessingCommon(timestamp))
+    return;
+
+  // some cleanup from last iteration, can't do before because of visualization
+  core_kfs_.clear();
+  overlap_kfs_.clear();
+
+  // create new frame
+  new_frame_.reset(new Frame(cam_, img.clone(), timestamp));
+
+  // process frame
+  UpdateResult res = RESULT_FAILURE;
+  if(stage_ == STAGE_DEFAULT_FRAME)
+    res = processFrame();
+  else if(stage_ == STAGE_SECOND_FRAME)
+    res = processSecondFrame();
+  else if(stage_ == STAGE_FIRST_FRAME)
+    res = processFirstFrame();
+  else if(stage_ == STAGE_RELOCALIZING)
+    res = relocalizeFrame(SE3(Matrix3d::Identity(), Vector3d::Zero()),
+                          map_.getClosestKeyframe(last_frame_));
+
+  // set last frame
+  last_frame_ = new_frame_;
+  new_frame_.reset();
+  // finish processing
+  finishFrameProcessingCommon(last_frame_->id_, res, last_frame_->nObs());
+}
+
+FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame()
+{
+  new_frame_->T_f_w_ = SE3(Matrix3d::Identity(), Vector3d::Zero());
+  if(klt_homography_init_.addFirstFrame(new_frame_) == initialization::FAILURE)
+    return RESULT_NO_KEYFRAME;
+  new_frame_->setKeyframe();
+  map_.addKeyframe(new_frame_);
+  stage_ = STAGE_SECOND_FRAME;
+  SVO_INFO_STREAM("Init: Selected first frame.");
+  return RESULT_IS_KEYFRAME;
+}
+
+FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
+{
+  initialization::InitResult res = klt_homography_init_.addSecondFrame(new_frame_);
+  if(res == initialization::FAILURE)
+    return RESULT_FAILURE;
+  else if(res == initialization::NO_KEYFRAME)
+    return RESULT_NO_KEYFRAME;
+
+  // two-frame bundle adjustment
+#ifdef USE_BUNDLE_ADJUSTMENT
+  ba::twoViewBA(new_frame_.get(), map_.lastKeyframe().get(), Config::lobaThresh(), &map_);
+#endif
+
+  new_frame_->setKeyframe();
+  double depth_mean, depth_min;
+  frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min);
+  depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min);
+
+  // add frame to map
+  map_.addKeyframe(new_frame_);
+  stage_ = STAGE_DEFAULT_FRAME;
+  klt_homography_init_.reset();
+  SVO_INFO_STREAM("Init: Selected second frame, triangulated initial map.");
+  return RESULT_IS_KEYFRAME;
+}
+
+FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
+{
+  // Set initial pose TODO use prior
+  new_frame_->T_f_w_ = last_frame_->T_f_w_;
+
+  // sparse image align
+  SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),
+                           30, SparseImgAlign::GaussNewton, false, false);
+  img_align.run(last_frame_, new_frame_);
+  // map reprojection & feature alignment
+  reprojector_.reprojectMap(new_frame_, overlap_kfs_);
+  std::cerr<<"Reprojection Map:\t nPoints = "<<reprojector_.n_trials_<<"\t \t nMatches = "<<reprojector_.n_matches_<<" \t Quality min: "<<Config::qualityMinFts()<<'\n';
+  if(reprojector_.n_matches_< Config::qualityMinFts())
+  {
+    new_frame_->T_f_w_ = last_frame_->T_f_w_; // reset to avoid crazy pose jumps
+    tracking_quality_ = TRACKING_INSUFFICIENT;
+    return RESULT_FAILURE;
+  }
+
+  // pose optimization subject to change for adding IMU
+  size_t sfba_n_edges_final;
+  if(!imu_integPtr_->predict(new_frame_,sfba_n_edges_final,Config::poseOptimThresh()))
+      return RESULT_FAILURE;
+
+
+  std::cerr<<"Reprojected points after opmization: "<<sfba_n_edges_final<<'\n';
+  if(sfba_n_edges_final < Config::qualityMinFts())
+    return RESULT_FAILURE;
+
+  // structure optimization
+  //SVO_START_TIMER("point_optimizer");
+  optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
+  //SVO_STOP_TIMER("point_optimizer");
+
+  // select keyframe
+  core_kfs_.insert(new_frame_);
+  setTrackingQuality(sfba_n_edges_final);
+  if(tracking_quality_ == TRACKING_INSUFFICIENT)
+  {
+    new_frame_->T_f_w_ = last_frame_->T_f_w_; // reset to avoid crazy pose jumps
+    return RESULT_FAILURE;
+  }
+  double depth_mean, depth_min;
+  frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min);
+  if(!needNewKf(depth_mean) || tracking_quality_ == TRACKING_BAD)
+  {
+    depth_filter_->addFrame(new_frame_);
+    return RESULT_NO_KEYFRAME;
+  }
+  new_frame_->setKeyframe();
+
+  // new keyframe selected
+  for(Features::iterator it=new_frame_->fts_.begin(); it!=new_frame_->fts_.end(); ++it)
+    if((*it)->point != NULL)
+      (*it)->point->addFrameRef(*it);
+  map_.point_candidates_.addCandidatePointToFrame(new_frame_);
+
+  // optional bundle adjustment
+#ifdef USE_BUNDLE_ADJUSTMENT
+  if(Config::lobaNumIter() > 0)
+  {
+    SVO_START_TIMER("local_ba");
+    setCoreKfs(Config::coreNKfs());
+    size_t loba_n_erredges_init, loba_n_erredges_fin;
+    double loba_err_init, loba_err_fin;
+    ba::localBA(new_frame_.get(), &core_kfs_, &map_,
+                loba_n_erredges_init, loba_n_erredges_fin,
+                loba_err_init, loba_err_fin);
+    SVO_STOP_TIMER("local_ba");
+    SVO_LOG4(loba_n_erredges_init, loba_n_erredges_fin, loba_err_init, loba_err_fin);
+    SVO_DEBUG_STREAM("Local BA:\t RemovedEdges {"<<loba_n_erredges_init<<", "<<loba_n_erredges_fin<<"} \t "
+                     "Error {"<<loba_err_init<<", "<<loba_err_fin<<"}");
+  }
+#endif
+
+  // init new depth-filters
+  depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min);
+
+  // if limited number of keyframes, remove the one furthest apart
+  if(Config::maxNKfs() > 2 && map_.size() >= Config::maxNKfs())
+  {
+    FramePtr furthest_frame = map_.getFurthestKeyframe(new_frame_->pos());
+    depth_filter_->removeKeyframe(furthest_frame); // TODO this interrupts the mapper thread, maybe we can solve this better
+    map_.safeDeleteFrame(furthest_frame);
+  }
+
+  // add keyframe to map
+  map_.addKeyframe(new_frame_);
+  std::cerr<<"Svo pipline was finished\n";
+  return RESULT_IS_KEYFRAME;
+}
+
+FrameHandlerMono::UpdateResult FrameHandlerMono::relocalizeFrame(
+    const SE3& T_cur_ref,
+    FramePtr ref_keyframe)
+{
+  if(ref_keyframe == nullptr)
+  {
+    return RESULT_FAILURE;
+  }
+  SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),
+                           30, SparseImgAlign::GaussNewton, false, false);
+  size_t img_align_n_tracked = img_align.run(ref_keyframe, new_frame_);
+  usleep(2000);
+  if(img_align_n_tracked > Config::qualityMinFts())
+  {
+    SE3 T_f_w_last = last_frame_->T_f_w_;
+    last_frame_ = ref_keyframe;
+    FrameHandlerMono::UpdateResult res = processFrame();
+    if(res != RESULT_FAILURE)
+    {
+      stage_ = STAGE_DEFAULT_FRAME;
+    }
+    else
+      new_frame_->T_f_w_ = T_f_w_last; // reset to last well localized pose
+    return res;
+  }
+  return RESULT_FAILURE;
+}
+
+bool FrameHandlerMono::relocalizeFrameAtPose(
+    const int keyframe_id,
+    const SE3& T_f_kf,
+    const cv::Mat& img,
+    const double timestamp)
+{
+  FramePtr ref_keyframe;
+  if(!map_.getKeyframeById(keyframe_id, ref_keyframe))
+    return false;
+  new_frame_.reset(new Frame(cam_, img.clone(), timestamp));
+  UpdateResult res = relocalizeFrame(T_f_kf, ref_keyframe);
+  if(res != RESULT_FAILURE) {
+    last_frame_ = new_frame_;
+    return true;
+  }
+  return false;
+}
+
+void FrameHandlerMono::resetAll()
+{
+  resetCommon();
+  last_frame_.reset();
+  new_frame_.reset();
+  core_kfs_.clear();
+  overlap_kfs_.clear();
+  depth_filter_->reset();
+}
+
+void FrameHandlerMono::setFirstFrame(const FramePtr& first_frame)
+{
+  resetAll();
+  last_frame_ = first_frame;
+  last_frame_->setKeyframe();
+  map_.addKeyframe(last_frame_);
+  stage_ = STAGE_DEFAULT_FRAME;
+}
+
+bool FrameHandlerMono::needNewKf(double scene_depth_mean)
+{
+  for(auto it=overlap_kfs_.begin(), ite=overlap_kfs_.end(); it!=ite; ++it)
+  {
+    Vector3d relpos = new_frame_->w2f(it->first->pos());
+    if(fabs(relpos.x())/scene_depth_mean < Config::kfSelectMinDist() &&
+       fabs(relpos.y())/scene_depth_mean < Config::kfSelectMinDist()*0.8 &&
+       fabs(relpos.z())/scene_depth_mean < Config::kfSelectMinDist()*1.3)
+      return false;
+  }
+  return true;
+}
+
+void FrameHandlerMono::setCoreKfs(size_t n_closest)
+{
+  size_t n = min(n_closest, overlap_kfs_.size()-1);
+  std::partial_sort(overlap_kfs_.begin(), overlap_kfs_.begin()+n, overlap_kfs_.end(),
+                    boost::bind(&pair<FramePtr, size_t>::second, _1) >
+                    boost::bind(&pair<FramePtr, size_t>::second, _2));
+  std::for_each(overlap_kfs_.begin(), overlap_kfs_.end(), [&](pair<FramePtr,size_t>& i){ core_kfs_.insert(i.first); });
+}
+
+} // namespace svo

CPU_version/svo/src/imu_integration.cpp

@@ -0,0 +1,151 @@
+//
+// Created by gfuse on 4/9/21.
+//
+//#define NDEBUG
+#include "svo/imu_integration.h"
+#include <memory>
+#include <utility>
+#include "svo/feature.h"
+#include "svo/point.h"
+#include "svo/svoVisionFactor.h"
+#include <gtsam/nonlinear/Marginals.h>
+#include <gtsam/geometry/Pose3.h>
+#include <vikit/params_helper.h>
+#include <svo/config.h>
+
+Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
+    graphPtr=std::make_shared<gtsam::NonlinearFactorGraph>();
+    valuesPtr=std::make_shared<gtsam::Values>();
+    parameterPtr = gtsam::PreintegratedCombinedMeasurements::Params::MakeSharedD();// TODO check the IMU frame
+    parameterPtr->accelerometerCovariance=gtsam::I_3x3 * svo::Config::ACC_Noise(); // acc white noise in continuous
+    parameterPtr->gyroscopeCovariance=gtsam::I_3x3 * svo::Config::GYO_Noise();// gyro white noise in continuous
+    parameterPtr->integrationCovariance=gtsam::I_3x3 * svo::Config::IUC(); // integration uncertainty continuous
+    parameterPtr->biasAccCovariance = gtsam::I_3x3 * svo::Config::ABC();   // acc bias in continuous
+    parameterPtr->biasOmegaCovariance = gtsam::I_3x3 * svo::Config::GBC();  // gyro bias in continuous
+    parameterPtr->biasAccOmegaInt = gtsam::I_6x6 * svo::Config::EBP();  // error in the bias used for preintegration
+    imu_biasPtr=std::make_shared<gtsam::imuBias::ConstantBias>(gtsam::Vector3(1e-8,1e-8,1e-8),gtsam::Vector3(1e-8,1e-8,1e-8));
+    preintegratedPtr = std::make_shared<gtsam::PreintegratedCombinedMeasurements>(parameterPtr,*imu_biasPtr);
+    ProjectNoisePtr = gtsam::noiseModel::Isotropic::Sigma(2, svo::Config::NRP()); // the noise used for projection
+    optimizerParamPtr=std::make_shared<gtsam::ISAM2Params>();
+    optimizerParamPtr->relinearizeThreshold = 0.1;
+    optimizerParamPtr->relinearizeSkip = 1;
+    //optimizerParamPtr->evaluateNonlinearError=true;
+    assert(preintegratedPtr);
+}
+Imu_Integration::~Imu_Integration(){
+
+}
+bool Imu_Integration::update(double* imu){
+    if(syn)return false;
+    if(imu_factor_id<1)return false;
+    auto in=std::chrono::steady_clock::now();
+    if(std::chrono::duration<double>(in-t_1).count()>20.0){
+        t_1=in;
+        preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0],imu[1],imu[2]),Eigen::Vector3d(imu[3],imu[4],imu[5]),0.005);
+    }else{
+        preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0],imu[1],imu[2]),Eigen::Vector3d(imu[3],imu[4],imu[5]),std::chrono::duration<double>(in-t_1).count());
+        t_1=in;
+        ++imu_n;
+    }
+    return true;
+
+}
+bool Imu_Integration::reset(gtsam::ISAM2& optimizer,boost::shared_ptr<svo::Frame>& new_frame){
+    gtsam::Values result = optimizer.calculateEstimate();
+    // Overwrite the beginning of the preintegration for the next step.
+    statePtr=std::make_shared<gtsam::NavState>(result.at<gtsam::Pose3>(P(imu_factor_id)), result.at<gtsam::Vector3>(V(imu_factor_id)));
+    imu_biasPtr=std::make_shared<gtsam::imuBias::ConstantBias>(result.at<gtsam::imuBias::ConstantBias>(B(imu_factor_id)));
+    preintegratedPtr->resetIntegrationAndSetBias(*imu_biasPtr);
+    ++imu_factor_id;
+    syn=false;
+    new_frame->T_f_w_=Sophus::SE3(statePtr->R(),statePtr->t());
+    new_frame->Cov_=optimizer.marginalCovariance(P(imu_factor_id-1));
+    graphPtr->resize(0);
+    valuesPtr->clear();
+    valuesPtr->insert(P(imu_factor_id-1),statePtr->pose());
+    valuesPtr->insert(V(imu_factor_id-1),statePtr->v());
+    valuesPtr->insert(B(imu_factor_id-1),*imu_biasPtr);
+    graphPtr->addPrior(P(imu_factor_id-1),statePtr->pose(),optimizer.marginalCovariance(P(imu_factor_id-1)));
+    graphPtr->addPrior(V(imu_factor_id-1),statePtr->v(),optimizer.marginalCovariance(V(imu_factor_id-1)));
+    graphPtr->addPrior(B(imu_factor_id-1),*imu_biasPtr,optimizer.marginalCovariance(B(imu_factor_id-1)));
+    return true;
+}
+bool Imu_Integration::reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost::shared_ptr<svo::Frame>& new_frame){
+    gtsam::Values result = optimizer.optimize();
+    // Overwrite the beginning of the preintegration for the next step.
+    statePtr=std::make_shared<gtsam::NavState>(result.at<gtsam::Pose3>(P(imu_factor_id)), result.at<gtsam::Vector3>(V(imu_factor_id)));
+    imu_biasPtr=std::make_shared<gtsam::imuBias::ConstantBias>(result.at<gtsam::imuBias::ConstantBias>(B(imu_factor_id)));
+    preintegratedPtr->resetIntegrationAndSetBias(*imu_biasPtr);
+    ++imu_factor_id;
+    syn=false;
+    gtsam::Marginals marginals(*graphPtr,result);
+    new_frame->T_f_w_=Sophus::SE3(statePtr->R(),statePtr->t());
+    new_frame->Cov_=marginals.marginalCovariance(P(imu_factor_id-1));
+    graphPtr->resize(0);
+    valuesPtr->clear();
+    valuesPtr->insert(P(imu_factor_id-1),statePtr->pose());
+    valuesPtr->insert(V(imu_factor_id-1),statePtr->v());
+    valuesPtr->insert(B(imu_factor_id-1),*imu_biasPtr);
+    graphPtr->addPrior(P(imu_factor_id-1),statePtr->pose(),marginals.marginalCovariance(P(imu_factor_id-1)));
+    graphPtr->addPrior(V(imu_factor_id-1),statePtr->v(),marginals.marginalCovariance(V(imu_factor_id-1)));
+    graphPtr->addPrior(B(imu_factor_id-1),*imu_biasPtr,marginals.marginalCovariance(B(imu_factor_id-1)));
+    return true;
+}
+bool Imu_Integration::predict(boost::shared_ptr<svo::Frame>& new_frame,std::size_t& num_obs,const double reproj_thresh){
+    usleep(5000);
+    syn=true;
+    if(imu_factor_id<1){
+        statePtr=std::make_shared<gtsam::NavState>(gtsam::Pose3(new_frame->T_f_w_.matrix()),
+                                                   gtsam::Vector3(1e-12,1e-12,1e-12));
+        valuesPtr->insert(P(0),statePtr->pose());
+        valuesPtr->insert(V(0),statePtr->v());
+        valuesPtr->insert(B(0),*imu_biasPtr);
+        graphPtr->addPrior(P(0),statePtr->pose(),gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9).finished()));
+        graphPtr->addPrior(V(0),statePtr->v(),gtsam::noiseModel::Isotropic::Sigma(3, 1e-4));
+        graphPtr->addPrior(B(0),*imu_biasPtr,gtsam::noiseModel::Isotropic::Sigma(6, 1e-2));
+    }else{
+        if(imu_n<1)return false;
+        auto preint_imu_combined =dynamic_cast<const gtsam::PreintegratedCombinedMeasurements&>(*preintegratedPtr);
+        valuesPtr->insert(P(imu_factor_id), gtsam::Pose3(new_frame->T_f_w_.matrix()));
+        valuesPtr->insert(V(imu_factor_id), statePtr->v());
+        valuesPtr->insert(B(imu_factor_id), *imu_biasPtr);
+        gtsam::CombinedImuFactor imu_factor(P(imu_factor_id - 1), V(imu_factor_id - 1), P(imu_factor_id), V(imu_factor_id), B(imu_factor_id - 1), B(imu_factor_id),
+                                            preint_imu_combined);
+        graphPtr->emplace_shared<gtsam::CombinedImuFactor>(imu_factor);
+        imu_n=0;
+    }
+
+    num_obs=0;
+    for(auto&& f:new_frame->fts_){
+       if(f->point==NULL)continue;
+        graphPtr->emplace_shared<svo::VisionFactor>(f->f, f->point->pos_,f->level, ProjectNoisePtr,P(imu_factor_id));
+        ++num_obs;
+    }
+    // Now optimize.
+    //gtsam::ISAM2 optimizer(*optimizerParamPtr);
+    //optimizer.update(*graphPtr, *valuesPtr);
+    // Each call to iSAM2 update(*) performs one iteration of the iterative
+    // nonlinear solver. If accuracy is desired at the expense of time,
+    // update(*) can be called additional times to perform multiple optimizer
+    // iterations every step.
+    //optimizer.update();
+   // optimizer.print();
+    gtsam::LevenbergMarquardtOptimizer optimizer(*graphPtr, *valuesPtr);
+
+    reset(optimizer,new_frame);
+    // Remove Measurements with too large reprojection error
+    double reproj_thresh_scaled = reproj_thresh / new_frame->cam_->errorMultiplier2();
+    for(auto&& f:new_frame->fts_)
+    {
+        if(f->point == NULL)
+            continue;
+        Eigen::Vector2d e = (vk::project2d(f->f) - vk::project2d(new_frame->T_f_w_ * f->point->pos_))*1.0 / (1<<f->level);
+        if(e.norm() > reproj_thresh_scaled)
+        {
+            // we don't need to delete a reference in the point since it was not created yet
+            f->point = NULL;
+            --num_obs;
+        }
+    }
+    return true;
+}

CPU_version/svo/src/initialization.cpp

@@ -0,0 +1,199 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <svo/config.h>
+#include <svo/frame.h>
+#include <svo/point.h>
+#include <svo/feature.h>
+#include <svo/initialization.h>
+#include <svo/feature_detection.h>
+#include <vikit/math_utils.h>
+#include <vikit/homography.h>
+
+namespace svo {
+namespace initialization {
+
+InitResult KltHomographyInit::addFirstFrame(FramePtr frame_ref)
+{
+  reset();
+  detectFeatures(frame_ref, px_ref_, f_ref_);
+  if(px_ref_.size() < 100)
+  {
+    SVO_WARN_STREAM_THROTTLE(2.0, "First image has less than 100 features. Retry in more textured environment.");
+    return FAILURE;
+  }
+  frame_ref_ = frame_ref;
+  px_cur_.insert(px_cur_.begin(), px_ref_.begin(), px_ref_.end());
+  return SUCCESS;
+}
+
+InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
+{
+  trackKlt(frame_ref_, frame_cur, px_ref_, px_cur_, f_ref_, f_cur_, disparities_);
+  SVO_INFO_STREAM("Init: KLT tracked "<< disparities_.size() <<" features");
+
+  if(disparities_.size() < Config::initMinTracked())
+    return FAILURE;
+
+  double disparity = vk::getMedian(disparities_);
+  SVO_INFO_STREAM("Init: KLT "<<disparity<<"px average disparity.");
+  if(disparity < Config::initMinDisparity())
+    return NO_KEYFRAME;
+
+  computeHomography(
+      f_ref_, f_cur_,
+      frame_ref_->cam_->errorMultiplier2(), Config::poseOptimThresh(),
+      inliers_, xyz_in_cur_, T_cur_from_ref_);
+  SVO_INFO_STREAM("Init: Homography RANSAC "<<inliers_.size()<<" inliers.");
+
+  if(inliers_.size() < Config::initMinInliers())
+  {
+    SVO_WARN_STREAM("Init WARNING: "<<Config::initMinInliers()<<" inliers minimum required.");
+    return FAILURE;
+  }
+
+  // Rescale the map such that the mean scene depth is equal to the specified scale
+  vector<double> depth_vec;
+  for(size_t i=0; i<xyz_in_cur_.size(); ++i)
+    depth_vec.push_back((xyz_in_cur_[i]).z());
+  double scene_depth_median = vk::getMedian(depth_vec);
+  double scale = Config::mapScale()/scene_depth_median;
+  frame_cur->T_f_w_ = T_cur_from_ref_ * frame_ref_->T_f_w_;
+  frame_cur->T_f_w_.translation() =
+      -frame_cur->T_f_w_.rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos()));
+
+  // For each inlier create 3D point and add feature in both frames
+  SE3 T_world_cur = frame_cur->T_f_w_.inverse();
+  for(vector<int>::iterator it=inliers_.begin(); it!=inliers_.end(); ++it)
+  {
+    Vector2d px_cur(px_cur_[*it].x, px_cur_[*it].y);
+    Vector2d px_ref(px_ref_[*it].x, px_ref_[*it].y);
+    if(frame_ref_->cam_->isInFrame(px_cur.cast<int>(), 10) && frame_ref_->cam_->isInFrame(px_ref.cast<int>(), 10) && xyz_in_cur_[*it].z() > 0)
+    {
+      Vector3d pos = T_world_cur * (xyz_in_cur_[*it]*scale);
+      Point* new_point = new Point(pos);
+
+      Feature* ftr_cur(new Feature(frame_cur.get(), new_point, px_cur, f_cur_[*it], 0));
+      frame_cur->addFeature(ftr_cur);
+      new_point->addFrameRef(ftr_cur);
+
+      Feature* ftr_ref(new Feature(frame_ref_.get(), new_point, px_ref, f_ref_[*it], 0));
+      frame_ref_->addFeature(ftr_ref);
+      new_point->addFrameRef(ftr_ref);
+    }
+  }
+  return SUCCESS;
+}
+
+void KltHomographyInit::reset()
+{
+  px_cur_.clear();
+  frame_ref_.reset();
+}
+
+void detectFeatures(
+    FramePtr frame,
+    vector<cv::Point2f>& px_vec,
+    vector<Vector3d>& f_vec)
+{
+  Features new_features;
+  feature_detection::FastDetector detector(
+      frame->img().cols, frame->img().rows, Config::gridSize(), Config::nPyrLevels());
+  detector.detect(frame.get(), frame->img_pyr_, Config::triangMinCornerScore(), new_features);
+
+  // now for all maximum corners, initialize a new seed
+  px_vec.clear(); px_vec.reserve(new_features.size());
+  f_vec.clear(); f_vec.reserve(new_features.size());
+  std::for_each(new_features.begin(), new_features.end(), [&](Feature* ftr){
+    px_vec.push_back(cv::Point2f(ftr->px[0], ftr->px[1]));
+    f_vec.push_back(ftr->f);
+    delete ftr;
+  });
+}
+
+void trackKlt(
+    FramePtr frame_ref,
+    FramePtr frame_cur,
+    vector<cv::Point2f>& px_ref,
+    vector<cv::Point2f>& px_cur,
+    vector<Vector3d>& f_ref,
+    vector<Vector3d>& f_cur,
+    vector<double>& disparities)
+{
+  const double klt_win_size = 30.0;
+  const int klt_max_iter = 30;
+  const double klt_eps = 0.001;
+  vector<uchar> status;
+  vector<float> error;
+  vector<float> min_eig_vec;
+  cv::TermCriteria termcrit(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, klt_max_iter, klt_eps);
+  cv::calcOpticalFlowPyrLK(frame_ref->img_pyr_[0], frame_cur->img_pyr_[0],
+                           px_ref, px_cur,
+                           status, error,
+                           cv::Size2i(klt_win_size, klt_win_size),
+                           4, termcrit, cv::OPTFLOW_USE_INITIAL_FLOW);
+
+  vector<cv::Point2f>::iterator px_ref_it = px_ref.begin();
+  vector<cv::Point2f>::iterator px_cur_it = px_cur.begin();
+  vector<Vector3d>::iterator f_ref_it = f_ref.begin();
+  f_cur.clear(); f_cur.reserve(px_cur.size());
+  disparities.clear(); disparities.reserve(px_cur.size());
+  for(size_t i=0; px_ref_it != px_ref.end(); ++i)
+  {
+    if(!status[i])
+    {
+      px_ref_it = px_ref.erase(px_ref_it);
+      px_cur_it = px_cur.erase(px_cur_it);
+      f_ref_it = f_ref.erase(f_ref_it);
+      continue;
+    }
+    f_cur.push_back(frame_cur->c2f(px_cur_it->x, px_cur_it->y));
+    disparities.push_back(Vector2d(px_ref_it->x - px_cur_it->x, px_ref_it->y - px_cur_it->y).norm());
+    ++px_ref_it;
+    ++px_cur_it;
+    ++f_ref_it;
+  }
+}
+
+void computeHomography(
+    const vector<Vector3d>& f_ref,
+    const vector<Vector3d>& f_cur,
+    double focal_length,
+    double reprojection_threshold,
+    vector<int>& inliers,
+    vector<Vector3d>& xyz_in_cur,
+    SE3& T_cur_from_ref)
+{
+  vector<Vector2d > uv_ref(f_ref.size());
+  vector<Vector2d > uv_cur(f_cur.size());
+  for(size_t i=0, i_max=f_ref.size(); i<i_max; ++i)
+  {
+    uv_ref[i] = vk::project2d(f_ref[i]);
+    uv_cur[i] = vk::project2d(f_cur[i]);
+  }
+  vk::Homography Homography(uv_ref, uv_cur, focal_length, reprojection_threshold);
+  Homography.computeSE3fromMatches();
+  vector<int> outliers;
+  vk::computeInliers(f_cur, f_ref,
+                     Homography.T_c2_from_c1.rotation_matrix(), Homography.T_c2_from_c1.translation(),
+                     reprojection_threshold, focal_length,
+                     xyz_in_cur, inliers, outliers);
+  T_cur_from_ref = Homography.T_c2_from_c1;
+}
+
+
+} // namespace initialization
+} // namespace svo

CPU_version/svo/src/map.cpp

@@ -0,0 +1,368 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <set>
+#include <svo/map.h>
+#include <svo/point.h>
+#include <svo/frame.h>
+#include <svo/feature.h>
+#include <boost/bind.hpp>
+
+namespace svo {
+
+Map::Map() {}
+
+Map::~Map()
+{
+  reset();
+  SVO_INFO_STREAM("Map destructed");
+}
+
+void Map::reset()
+{
+  keyframes_.clear();
+  point_candidates_.reset();
+  emptyTrash();
+}
+
+bool Map::safeDeleteFrame(FramePtr frame)
+{
+  bool found = false;
+  for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it)
+  {
+    if(*it == frame)
+    {
+      std::for_each((*it)->fts_.begin(), (*it)->fts_.end(), [&](Feature* ftr){
+        removePtFrameRef(it->get(), ftr);
+      });
+      keyframes_.erase(it);
+      found = true;
+      break;
+    }
+  }
+
+  point_candidates_.removeFrameCandidates(frame);
+
+  if(found)
+    return true;
+
+  SVO_ERROR_STREAM("Tried to delete Keyframe in map which was not there.");
+  return false;
+}
+
+void Map::removePtFrameRef(Frame* frame, Feature* ftr)
+{
+  if(ftr->point == NULL)
+    return; // mappoint may have been deleted in a previous ref. removal
+  Point* pt = ftr->point;
+  ftr->point = NULL;
+  if(pt->obs_.size() <= 2)
+  {
+    // If the references list of mappoint has only size=2, delete mappoint
+    safeDeletePoint(pt);
+    return;
+  }
+  pt->deleteFrameRef(frame);  // Remove reference from map_point
+  frame->removeKeyPoint(ftr); // Check if mp was keyMp in keyframe
+}
+
+void Map::safeDeletePoint(Point* pt)
+{
+  // Delete references to mappoints in all keyframes
+  std::for_each(pt->obs_.begin(), pt->obs_.end(), [&](Feature* ftr){
+    ftr->point=NULL;
+    ftr->frame->removeKeyPoint(ftr);
+  });
+  pt->obs_.clear();
+
+  // Delete mappoint
+  deletePoint(pt);
+}
+
+void Map::deletePoint(Point* pt)
+{
+  pt->type_ = Point::TYPE_DELETED;
+  trash_points_.push_back(pt);
+}
+
+void Map::addKeyframe(FramePtr new_keyframe)
+{
+  keyframes_.push_back(new_keyframe);
+}
+
+void Map::getCloseKeyframes(
+    const FramePtr& frame,
+    std::list< std::pair<FramePtr,double> >& close_kfs) const
+{
+  for(auto kf : keyframes_)
+  {
+    // check if kf has overlaping field of view with frame, use therefore KeyPoints
+    for(auto keypoint : kf->key_pts_)
+    {
+      if(keypoint == nullptr)
+        continue;
+
+      if(frame->isVisible(keypoint->point->pos_))
+      {
+        close_kfs.push_back(
+            std::make_pair(
+                kf, (frame->T_f_w_.translation()-kf->T_f_w_.translation()).norm()));
+        break; // this keyframe has an overlapping field of view -> add to close_kfs
+      }
+    }
+  }
+}
+
+FramePtr Map::getClosestKeyframe(const FramePtr& frame) const
+{
+  list< pair<FramePtr,double> > close_kfs;
+  getCloseKeyframes(frame, close_kfs);
+  if(close_kfs.empty())
+  {
+    return nullptr;
+  }
+
+
+  // Sort KFs with overlap according to their closeness
+  close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <
+                 boost::bind(&std::pair<FramePtr, double>::second, _2));
+
+  if(close_kfs.front().first != frame)
+    return close_kfs.front().first;
+  close_kfs.pop_front();
+  return close_kfs.front().first;
+}
+
+FramePtr Map::getFurthestKeyframe(const Vector3d& pos) const
+{
+  FramePtr furthest_kf;
+  double maxdist = 0.0;
+  for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it)
+  {
+    double dist = ((*it)->pos()-pos).norm();
+    if(dist > maxdist) {
+      maxdist = dist;
+      furthest_kf = *it;
+    }
+  }
+  return furthest_kf;
+}
+
+bool Map::getKeyframeById(const int id, FramePtr& frame) const
+{
+  bool found = false;
+  for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it)
+    if((*it)->id_ == id) {
+      found = true;
+      frame = *it;
+      break;
+    }
+  return found;
+}
+
+void Map::transform(const Matrix3d& R, const Vector3d& t, const double& s)
+{
+  for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it)
+  {
+    Vector3d pos = s*R*(*it)->pos() + t;
+    Matrix3d rot = R*(*it)->T_f_w_.rotation_matrix().inverse();
+    (*it)->T_f_w_ = SE3(rot, pos).inverse();
+    for(auto ftr=(*it)->fts_.begin(); ftr!=(*it)->fts_.end(); ++ftr)
+    {
+      if((*ftr)->point == NULL)
+        continue;
+      if((*ftr)->point->last_published_ts_ == -1000)
+        continue;
+      (*ftr)->point->last_published_ts_ = -1000;
+      (*ftr)->point->pos_ = s*R*(*ftr)->point->pos_ + t;
+    }
+  }
+}
+
+void Map::emptyTrash()
+{
+  std::for_each(trash_points_.begin(), trash_points_.end(), [&](Point*& pt){
+    delete pt;
+    pt=NULL;
+  });
+  trash_points_.clear();
+  point_candidates_.emptyTrash();
+}
+
+MapPointCandidates::MapPointCandidates()
+{}
+
+MapPointCandidates::~MapPointCandidates()
+{
+  reset();
+}
+
+void MapPointCandidates::newCandidatePoint(Point* point, double depth_sigma2)
+{
+  point->type_ = Point::TYPE_CANDIDATE;
+  boost::unique_lock<boost::mutex> lock(mut_);
+  candidates_.push_back(PointCandidate(point, point->obs_.front()));
+}
+
+void MapPointCandidates::addCandidatePointToFrame(FramePtr frame)
+{
+  boost::unique_lock<boost::mutex> lock(mut_);
+  PointCandidateList::iterator it=candidates_.begin();
+  while(it != candidates_.end())
+  {
+    if(it->first->obs_.front()->frame == frame.get())
+    {
+      // insert feature in the frame
+      it->first->type_ = Point::TYPE_UNKNOWN;
+      it->first->n_failed_reproj_ = 0;
+      it->second->frame->addFeature(it->second);
+      it = candidates_.erase(it);
+    }
+    else
+      ++it;
+  }
+}
+
+bool MapPointCandidates::deleteCandidatePoint(Point* point)
+{
+  boost::unique_lock<boost::mutex> lock(mut_);
+  for(auto it=candidates_.begin(), ite=candidates_.end(); it!=ite; ++it)
+  {
+    if(it->first == point)
+    {
+      deleteCandidate(*it);
+      candidates_.erase(it);
+      return true;
+    }
+  }
+  return false;
+}
+
+void MapPointCandidates::removeFrameCandidates(FramePtr frame)
+{
+  boost::unique_lock<boost::mutex> lock(mut_);
+  auto it=candidates_.begin();
+  while(it!=candidates_.end())
+  {
+    if(it->second->frame == frame.get())
+    {
+      deleteCandidate(*it);
+      it = candidates_.erase(it);
+    }
+    else
+      ++it;
+  }
+}
+
+void MapPointCandidates::reset()
+{
+  boost::unique_lock<boost::mutex> lock(mut_);
+  std::for_each(candidates_.begin(), candidates_.end(), [&](PointCandidate& c){
+    delete c.first;
+    delete c.second;
+  });
+  candidates_.clear();
+}
+
+void MapPointCandidates::deleteCandidate(PointCandidate& c)
+{
+  // camera-rig: another frame might still be pointing to the candidate point
+  // therefore, we can't delete it right now.
+  delete c.second; c.second=NULL;
+  c.first->type_ = Point::TYPE_DELETED;
+  trash_points_.push_back(c.first);
+}
+
+void MapPointCandidates::emptyTrash()
+{
+  std::for_each(trash_points_.begin(), trash_points_.end(), [&](Point*& p){
+    delete p; p=NULL;
+  });
+  trash_points_.clear();
+}
+
+namespace map_debug {
+
+void mapValidation(Map* map, int id)
+{
+  for(auto it=map->keyframes_.begin(); it!=map->keyframes_.end(); ++it)
+    frameValidation(it->get(), id);
+}
+
+void frameValidation(Frame* frame, int id)
+{
+  for(auto it = frame->fts_.begin(); it!=frame->fts_.end(); ++it)
+  {
+    if((*it)->point==NULL)
+      continue;
+
+    if((*it)->point->type_ == Point::TYPE_DELETED)
+      printf("ERROR DataValidation %i: Referenced point was deleted.\n", id);
+
+    if(!(*it)->point->findFrameRef(frame))
+      printf("ERROR DataValidation %i: Frame has reference but point does not have a reference back.\n", id);
+
+    pointValidation((*it)->point, id);
+  }
+  for(auto it=frame->key_pts_.begin(); it!=frame->key_pts_.end(); ++it)
+    if(*it != NULL)
+      if((*it)->point == NULL)
+        printf("ERROR DataValidation %i: KeyPoints not correct!\n", id);
+}
+
+void pointValidation(Point* point, int id)
+{
+  for(auto it=point->obs_.begin(); it!=point->obs_.end(); ++it)
+  {
+    bool found=false;
+    for(auto it_ftr=(*it)->frame->fts_.begin(); it_ftr!=(*it)->frame->fts_.end(); ++it_ftr)
+     if((*it_ftr)->point == point) {
+       found=true; break;
+     }
+    if(!found)
+      printf("ERROR DataValidation %i: Point %i has inconsistent reference in frame %i, is candidate = %i\n", id, point->id_, (*it)->frame->id_, (int) point->type_);
+  }
+}
+
+void mapStatistics(Map* map)
+{
+  // compute average number of features which each frame observes
+  size_t n_pt_obs(0);
+  for(auto it=map->keyframes_.begin(); it!=map->keyframes_.end(); ++it)
+    n_pt_obs += (*it)->nObs();
+  printf("\n\nMap Statistics: Frame avg. point obs = %f\n", (float) n_pt_obs/map->size());
+
+  // compute average number of observations that each point has
+  size_t n_frame_obs(0);
+  size_t n_pts(0);
+  std::set<Point*> points;
+  for(auto it=map->keyframes_.begin(); it!=map->keyframes_.end(); ++it)
+  {
+    for(auto ftr=(*it)->fts_.begin(); ftr!=(*it)->fts_.end(); ++ftr)
+    {
+      if((*ftr)->point == NULL)
+        continue;
+      if(points.insert((*ftr)->point).second) {
+        ++n_pts;
+        n_frame_obs += (*ftr)->point->nRefs();
+      }
+    }
+  }
+  printf("Map Statistics: Point avg. frame obs = %f\n\n", (float) n_frame_obs/n_pts);
+}
+
+} // namespace map_debug
+} // namespace svo

CPU_version/svo/src/matcher.cpp

@@ -0,0 +1,323 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <cstdlib>
+#include <vikit/abstract_camera.h>
+#include <vikit/vision.h>
+#include <vikit/math_utils.h>
+#include <vikit/patch_score.h>
+#include <svo/matcher.h>
+#include <svo/frame.h>
+#include <svo/feature.h>
+#include <svo/point.h>
+#include <svo/config.h>
+#include <svo/feature_alignment.h>
+
+namespace svo {
+
+namespace warp {
+
+void getWarpMatrixAffine(
+    const vk::AbstractCamera& cam_ref,
+    const vk::AbstractCamera& cam_cur,
+    const Vector2d& px_ref,
+    const Vector3d& f_ref,
+    const double depth_ref,
+    const SE3& T_cur_ref,
+    const int level_ref,
+    Matrix2d& A_cur_ref)
+{
+  // Compute affine warp matrix A_ref_cur
+  const int halfpatch_size = 5;
+  const Vector3d xyz_ref(f_ref*depth_ref);
+  Vector3d xyz_du_ref(cam_ref.cam2world(px_ref + Vector2d(halfpatch_size,0)*(1<<level_ref)));
+  Vector3d xyz_dv_ref(cam_ref.cam2world(px_ref + Vector2d(0,halfpatch_size)*(1<<level_ref)));
+  xyz_du_ref *= xyz_ref[2]/xyz_du_ref[2];
+  xyz_dv_ref *= xyz_ref[2]/xyz_dv_ref[2];
+  const Vector2d px_cur(cam_cur.world2cam(T_cur_ref*(xyz_ref)));
+  const Vector2d px_du(cam_cur.world2cam(T_cur_ref*(xyz_du_ref)));
+  const Vector2d px_dv(cam_cur.world2cam(T_cur_ref*(xyz_dv_ref)));
+  A_cur_ref.col(0) = (px_du - px_cur)/halfpatch_size;
+  A_cur_ref.col(1) = (px_dv - px_cur)/halfpatch_size;
+}
+
+int getBestSearchLevel(
+    const Matrix2d& A_cur_ref,
+    const int max_level)
+{
+  // Compute patch level in other image
+  int search_level = 0;
+  double D = A_cur_ref.determinant();
+  while(D > 3.0 && search_level < max_level)
+  {
+    search_level += 1;
+    D *= 0.25;
+  }
+  return search_level;
+}
+
+void warpAffine(
+    const Matrix2d& A_cur_ref,
+    const cv::Mat& img_ref,
+    const Vector2d& px_ref,
+    const int level_ref,
+    const int search_level,
+    const int halfpatch_size,
+    uint8_t* patch)
+{
+  const int patch_size = halfpatch_size*2 ;
+  const Matrix2f A_ref_cur = A_cur_ref.inverse().cast<float>();
+  if(isnan(A_ref_cur(0,0)))
+  {
+    printf("Affine warp is NaN, probably camera has no translation\n"); // TODO
+    return;
+  }
+
+  // Perform the warp on a larger patch.
+  uint8_t* patch_ptr = patch;
+  const Vector2f px_ref_pyr = px_ref.cast<float>() / (1<<level_ref);
+  for (int y=0; y<patch_size; ++y)
+  {
+    for (int x=0; x<patch_size; ++x, ++patch_ptr)
+    {
+      Vector2f px_patch(x-halfpatch_size, y-halfpatch_size);
+      px_patch *= (1<<search_level);
+      const Vector2f px(A_ref_cur*px_patch + px_ref_pyr);
+      if (px[0]<0 || px[1]<0 || px[0]>=img_ref.cols-1 || px[1]>=img_ref.rows-1)
+        *patch_ptr = 0;
+      else
+        *patch_ptr = (uint8_t) vk::interpolateMat_8u(img_ref, px[0], px[1]);
+    }
+  }
+}
+
+} // namespace warp
+
+bool depthFromTriangulation(
+    const SE3& T_search_ref,
+    const Vector3d& f_ref,
+    const Vector3d& f_cur,
+    double& depth)
+{
+  Matrix<double,3,2> A; A << T_search_ref.rotation_matrix() * f_ref, f_cur;
+  const Matrix2d AtA = A.transpose()*A;
+  if(AtA.determinant() < 0.000001)
+    return false;
+  const Vector2d depth2 = - AtA.inverse()*A.transpose()*T_search_ref.translation();
+  depth = fabs(depth2[0]);
+  return true;
+}
+
+void Matcher::createPatchFromPatchWithBorder()
+{
+  uint8_t* ref_patch_ptr = patch_;
+  for(int y=1; y<patch_size_+1; ++y, ref_patch_ptr += patch_size_)
+  {
+    uint8_t* ref_patch_border_ptr = patch_with_border_ + y*(patch_size_+2) + 1;
+    for(int x=0; x<patch_size_; ++x)
+      ref_patch_ptr[x] = ref_patch_border_ptr[x];
+  }
+}
+
+bool Matcher::findMatchDirect(
+    const Point& pt,
+    const Frame& cur_frame,
+    Vector2d& px_cur)
+{
+  if(!pt.getCloseViewObs(cur_frame.pos(), ref_ftr_))
+    return false;
+
+  if(!ref_ftr_->frame->cam_->isInFrame(
+      ref_ftr_->px.cast<int>()/(1<<ref_ftr_->level), halfpatch_size_+2, ref_ftr_->level))
+    return false;
+
+  // warp affine
+  warp::getWarpMatrixAffine(
+      *ref_ftr_->frame->cam_, *cur_frame.cam_, ref_ftr_->px, ref_ftr_->f,
+      (ref_ftr_->frame->pos() - pt.pos_).norm(),
+      cur_frame.T_f_w_ * ref_ftr_->frame->T_f_w_.inverse(), ref_ftr_->level, A_cur_ref_);
+  search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1);
+  warp::warpAffine(A_cur_ref_, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px,
+                   ref_ftr_->level, search_level_, halfpatch_size_+1, patch_with_border_);
+  createPatchFromPatchWithBorder();
+
+  // px_cur should be set
+  Vector2d px_scaled(px_cur/(1<<search_level_));
+
+  bool success = false;
+  if(ref_ftr_->type == Feature::EDGELET)
+  {
+    Vector2d dir_cur(A_cur_ref_*ref_ftr_->grad);
+    dir_cur.normalize();
+    success = feature_alignment::align1D(
+          cur_frame.img_pyr_[search_level_], dir_cur.cast<float>(),
+          patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_);
+  }
+  else
+  {
+    success = feature_alignment::align2D(
+      cur_frame.img_pyr_[search_level_], patch_with_border_, patch_,
+      options_.align_max_iter, px_scaled);
+  }
+  px_cur = px_scaled * (1<<search_level_);
+  return success;
+}
+
+bool Matcher::findEpipolarMatchDirect(
+    const Frame& ref_frame,
+    const Frame& cur_frame,
+    const Feature& ref_ftr,
+    const double d_estimate,
+    const double d_min,
+    const double d_max,
+    double& depth)
+{
+  SE3 T_cur_ref = cur_frame.T_f_w_ * ref_frame.T_f_w_.inverse();
+  int zmssd_best = PatchScore::threshold();
+  Vector2d uv_best;
+
+  // Compute start and end of epipolar line in old_kf for match search, on unit plane!
+  Vector2d A = vk::project2d(T_cur_ref * (ref_ftr.f*d_min));
+  Vector2d B = vk::project2d(T_cur_ref * (ref_ftr.f*d_max));
+  epi_dir_ = A - B;
+
+  // Compute affine warp matrix
+  warp::getWarpMatrixAffine(
+      *ref_frame.cam_, *cur_frame.cam_, ref_ftr.px, ref_ftr.f,
+      d_estimate, T_cur_ref, ref_ftr.level, A_cur_ref_);
+
+  // feature pre-selection
+  reject_ = false;
+  if(ref_ftr.type == Feature::EDGELET && options_.epi_search_edgelet_filtering)
+  {
+    const Vector2d grad_cur = (A_cur_ref_ * ref_ftr.grad).normalized();
+    const double cosangle = fabs(grad_cur.dot(epi_dir_.normalized()));
+    if(cosangle < options_.epi_search_edgelet_max_angle) {
+      reject_ = true;
+      return false;
+    }
+  }
+
+  search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1);
+
+  // Find length of search range on epipolar line
+  Vector2d px_A(cur_frame.cam_->world2cam(A));
+  Vector2d px_B(cur_frame.cam_->world2cam(B));
+  epi_length_ = (px_A-px_B).norm() / (1<<search_level_);
+
+  // Warp reference patch at ref_level
+  warp::warpAffine(A_cur_ref_, ref_frame.img_pyr_[ref_ftr.level], ref_ftr.px,
+                   ref_ftr.level, search_level_, halfpatch_size_+1, patch_with_border_);
+  createPatchFromPatchWithBorder();
+
+  if(epi_length_ < 2.0)
+  {
+    px_cur_ = (px_A+px_B)/2.0;
+    Vector2d px_scaled(px_cur_/(1<<search_level_));
+    bool res;
+    if(options_.align_1d)
+      res = feature_alignment::align1D(
+          cur_frame.img_pyr_[search_level_], (px_A-px_B).cast<float>().normalized(),
+          patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_);
+    else
+      res = feature_alignment::align2D(
+          cur_frame.img_pyr_[search_level_], patch_with_border_, patch_,
+          options_.align_max_iter, px_scaled);
+    if(res)
+    {
+      px_cur_ = px_scaled*(1<<search_level_);
+      if(depthFromTriangulation(T_cur_ref, ref_ftr.f, cur_frame.cam_->cam2world(px_cur_), depth))
+        return true;
+    }
+    return false;
+  }
+
+  size_t n_steps = epi_length_/0.7; // one step per pixel
+  Vector2d step = epi_dir_/n_steps;
+
+  if(n_steps > options_.max_epi_search_steps)
+  {
+    printf("WARNING: skip epipolar search: %zu evaluations, px_lenght=%f, d_min=%f, d_max=%f.\n",
+           n_steps, epi_length_, d_min, d_max);
+    return false;
+  }
+
+  // for matching, precompute sum and sum2 of warped reference patch
+  int pixel_sum = 0;
+  int pixel_sum_square = 0;
+  PatchScore patch_score(patch_);
+
+  // now we sample along the epipolar line
+  Vector2d uv = B-step;
+  Vector2i last_checked_pxi(0,0);
+  ++n_steps;
+  for(size_t i=0; i<n_steps; ++i, uv+=step)
+  {
+    Vector2d px(cur_frame.cam_->world2cam(uv));
+    Vector2i pxi(px[0]/(1<<search_level_)+0.5,
+                 px[1]/(1<<search_level_)+0.5); // +0.5 to round to closest int
+
+    if(pxi == last_checked_pxi)
+      continue;
+    last_checked_pxi = pxi;
+
+    // check if the patch is full within the new frame
+    if(!cur_frame.cam_->isInFrame(pxi, patch_size_, search_level_))
+      continue;
+
+    // TODO interpolation would probably be a good idea
+    uint8_t* cur_patch_ptr = cur_frame.img_pyr_[search_level_].data
+                             + (pxi[1]-halfpatch_size_)*cur_frame.img_pyr_[search_level_].cols
+                             + (pxi[0]-halfpatch_size_);
+    int zmssd = patch_score.computeScore(cur_patch_ptr, cur_frame.img_pyr_[search_level_].cols);
+
+    if(zmssd < zmssd_best) {
+      zmssd_best = zmssd;
+      uv_best = uv;
+    }
+  }
+
+  if(zmssd_best < PatchScore::threshold())
+  {
+    if(options_.subpix_refinement)
+    {
+      px_cur_ = cur_frame.cam_->world2cam(uv_best);
+      Vector2d px_scaled(px_cur_/(1<<search_level_));
+      bool res;
+      if(options_.align_1d)
+        res = feature_alignment::align1D(
+            cur_frame.img_pyr_[search_level_], (px_A-px_B).cast<float>().normalized(),
+            patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_);
+      else
+        res = feature_alignment::align2D(
+            cur_frame.img_pyr_[search_level_], patch_with_border_, patch_,
+            options_.align_max_iter, px_scaled);
+      if(res)
+      {
+        px_cur_ = px_scaled*(1<<search_level_);
+        if(depthFromTriangulation(T_cur_ref, ref_ftr.f, cur_frame.cam_->cam2world(px_cur_), depth))
+          return true;
+      }
+      return false;
+    }
+    px_cur_ = cur_frame.cam_->world2cam(uv_best);
+    if(depthFromTriangulation(T_cur_ref, ref_ftr.f, vk::unproject2d(uv_best).normalized(), depth))
+      return true;
+  }
+  return false;
+}
+
+} // namespace svo

CPU_version/svo/src/point.cpp

@@ -0,0 +1,179 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <stdexcept>
+#include <vikit/math_utils.h>
+#include <svo/point.h>
+#include <svo/frame.h>
+#include <svo/feature.h>
+ 
+namespace svo {
+
+int Point::point_counter_ = 0;
+
+Point::Point(const Vector3d& pos) :
+  id_(point_counter_++),
+  pos_(pos),
+  normal_set_(false),
+  n_obs_(0),
+  v_pt_(NULL),
+  last_published_ts_(0),
+  last_projected_kf_id_(-1),
+  type_(TYPE_UNKNOWN),
+  n_failed_reproj_(0),
+  n_succeeded_reproj_(0),
+  last_structure_optim_(0)
+{}
+
+Point::Point(const Vector3d& pos, Feature* ftr) :
+  id_(point_counter_++),
+  pos_(pos),
+  normal_set_(false),
+  n_obs_(1),
+  v_pt_(NULL),
+  last_published_ts_(0),
+  last_projected_kf_id_(-1),
+  type_(TYPE_UNKNOWN),
+  n_failed_reproj_(0),
+  n_succeeded_reproj_(0),
+  last_structure_optim_(0)
+{
+  obs_.push_front(ftr);
+}
+
+Point::~Point()
+{}
+
+void Point::addFrameRef(Feature* ftr)
+{
+  obs_.push_front(ftr);
+  ++n_obs_;
+}
+
+Feature* Point::findFrameRef(Frame* frame)
+{
+  for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it)
+    if((*it)->frame == frame)
+      return *it;
+  return NULL;    // no keyframe found
+}
+
+bool Point::deleteFrameRef(Frame* frame)
+{
+  for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it)
+  {
+    if((*it)->frame == frame)
+    {
+      obs_.erase(it);
+      return true;
+    }
+  }
+  return false;
+}
+
+void Point::initNormal()
+{
+  assert(!obs_.empty());
+  const Feature* ftr = obs_.back();
+  assert(ftr->frame != NULL);
+  normal_ = ftr->frame->T_f_w_.rotation_matrix().transpose()*(-ftr->f);
+  normal_information_ = DiagonalMatrix<double,3,3>(pow(20/(pos_-ftr->frame->pos()).norm(),2), 1.0, 1.0);
+  normal_set_ = true;
+}
+
+bool Point::getCloseViewObs(const Vector3d& framepos, Feature*& ftr) const
+{
+  // TODO: get frame with same point of view AND same pyramid level!
+  Vector3d obs_dir(framepos - pos_); obs_dir.normalize();
+  auto min_it=obs_.begin();
+  double min_cos_angle = 0;
+  for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it)
+  {
+    Vector3d dir((*it)->frame->pos() - pos_); dir.normalize();
+    double cos_angle = obs_dir.dot(dir);
+    if(cos_angle > min_cos_angle)
+    {
+      min_cos_angle = cos_angle;
+      min_it = it;
+    }
+  }
+  ftr = *min_it;
+  if(min_cos_angle < 0.5) // assume that observations larger than 60° are useless
+    return false;
+  return true;
+}
+
+void Point::optimize(const size_t n_iter)
+{
+  Vector3d old_point = pos_;
+  double chi2 = 0.0;
+  Matrix3d A;
+  Vector3d b;
+
+  for(size_t i=0; i<n_iter; i++)
+  {
+    A.setZero();
+    b.setZero();
+    double new_chi2 = 0.0;
+
+    // compute residuals
+    for(auto it=obs_.begin(); it!=obs_.end(); ++it)
+    {
+      Matrix23d J;
+      const Vector3d p_in_f((*it)->frame->T_f_w_ * pos_);
+      Point::jacobian_xyz2uv(p_in_f, (*it)->frame->T_f_w_.rotation_matrix(), J);
+      const Vector2d e(vk::project2d((*it)->f) - vk::project2d(p_in_f));
+      new_chi2 += e.squaredNorm();
+      A.noalias() += J.transpose() * J;
+      b.noalias() -= J.transpose() * e;
+    }
+
+    // solve linear system
+    const Vector3d dp(A.ldlt().solve(b));
+
+    // check if error increased
+    if((i > 0 && new_chi2 > chi2) || (bool) std::isnan((double)dp[0]))
+    {
+#ifdef POINT_OPTIMIZER_DEBUG
+      cout << "it " << i
+           << "\t FAILURE \t new_chi2 = " << new_chi2 << endl;
+#endif
+      pos_ = old_point; // roll-back
+      break;
+    }
+
+    // update the model
+    Vector3d new_point = pos_ + dp;
+    old_point = pos_;
+    pos_ = new_point;
+    chi2 = new_chi2;
+#ifdef POINT_OPTIMIZER_DEBUG
+    cout << "it " << i
+         << "\t Success \t new_chi2 = " << new_chi2
+         << "\t norm(b) = " << vk::norm_max(b)
+         << endl;
+#endif
+
+    // stop when converged
+    if(vk::norm_max(dp) <= EPS)
+      break;
+  }
+#ifdef POINT_OPTIMIZER_DEBUG
+  cout << endl;
+#endif
+}
+
+} // namespace svo

CPU_version/svo/src/pose_optimizer.cpp

@@ -0,0 +1,165 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <stdexcept>
+#include <svo/pose_optimizer.h>
+#include <svo/frame.h>
+#include <svo/feature.h>
+#include <svo/point.h>
+#include <vikit/robust_cost.h>
+#include <vikit/math_utils.h>
+
+namespace svo {
+namespace pose_optimizer {
+
+void optimizeGaussNewton(
+    const double reproj_thresh,
+    const size_t n_iter,
+    const bool verbose,
+    FramePtr& frame,
+    double& estimated_scale,
+    double& error_init,
+    double& error_final,
+    size_t& num_obs)
+{
+  // init
+  double chi2(0.0);
+  vector<double> chi2_vec_init, chi2_vec_final;
+  vk::robust_cost::TukeyWeightFunction weight_function;
+  SE3 T_old(frame->T_f_w_);
+  Matrix6d A;
+  Vector6d b;
+
+  // compute the scale of the error for robust estimation
+  std::vector<float> errors; errors.reserve(frame->fts_.size());
+  for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
+  {
+    if((*it)->point == NULL)
+      continue;
+    //Reprojection error
+    Vector2d e = vk::project2d((*it)->f)
+               - vk::project2d(frame->T_f_w_ * (*it)->point->pos_);
+    e *= 1.0 / (1<<(*it)->level);
+    errors.push_back(e.norm());
+  }
+  if(errors.empty())
+    return;
+  vk::robust_cost::MADScaleEstimator scale_estimator;
+  estimated_scale = scale_estimator.compute(errors);
+
+  num_obs = errors.size();
+  chi2_vec_init.reserve(num_obs);
+  chi2_vec_final.reserve(num_obs);
+  double scale = estimated_scale;
+  for(size_t iter=0; iter<n_iter; iter++)
+  {
+    // overwrite scale
+    if(iter == 5)
+      scale = 0.85/frame->cam_->errorMultiplier2();
+
+    b.setZero();
+    A.setZero();
+    double new_chi2(0.0);
+
+    // compute residual
+    for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
+    {
+      if((*it)->point == NULL)
+        continue;
+      Matrix26d J;
+      Vector3d xyz_f(frame->T_f_w_ * (*it)->point->pos_);
+      Frame::jacobian_xyz2uv(xyz_f, J);
+      Vector2d e = vk::project2d((*it)->f) - vk::project2d(xyz_f);
+      double sqrt_inv_cov = 1.0 / (1<<(*it)->level);
+      e *= sqrt_inv_cov;
+      if(iter == 0)
+        chi2_vec_init.push_back(e.squaredNorm()); // just for debug
+      J *= sqrt_inv_cov;
+      double weight = weight_function.value(e.norm()/scale);
+      A.noalias() += J.transpose()*J*weight;
+      b.noalias() -= J.transpose()*e*weight;
+      new_chi2 += e.squaredNorm()*weight;
+    }
+
+    // solve linear system
+    const Vector6d dT(A.ldlt().solve(b));
+
+    // check if error increased
+    if((iter > 0 && new_chi2 > chi2) || (bool) std::isnan((double)dT[0]))
+    {
+      if(verbose)
+        std::cout << "it " << iter
+                  << "\t FAILURE \t new_chi2 = " << new_chi2 << std::endl;
+      frame->T_f_w_ = T_old; // roll-back
+      break;
+    }
+
+    // update the model
+    SE3 T_new = SE3::exp(dT)*frame->T_f_w_;
+    T_old = frame->T_f_w_;
+    frame->T_f_w_ = T_new;
+    chi2 = new_chi2;
+    if(verbose)
+      std::cout << "it " << iter
+                << "\t Success \t new_chi2 = " << new_chi2
+                << "\t norm(dT) = " << vk::norm_max(dT) << std::endl;
+
+    // stop when converged
+    if(vk::norm_max(dT) <= EPS)
+      break;
+  }
+
+  // Set covariance as inverse information matrix. Optimistic estimator!
+  const double pixel_variance=1.0;
+  frame->Cov_ = pixel_variance*(A*std::pow(frame->cam_->errorMultiplier2(),2)).inverse();
+
+  // Remove Measurements with too large reprojection error
+  double reproj_thresh_scaled = reproj_thresh / frame->cam_->errorMultiplier2();
+  size_t n_deleted_refs = 0;
+  for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
+  {
+    if((*it)->point == NULL)
+      continue;
+    Vector2d e = vk::project2d((*it)->f) - vk::project2d(frame->T_f_w_ * (*it)->point->pos_);
+    double sqrt_inv_cov = 1.0 / (1<<(*it)->level);
+    e *= sqrt_inv_cov;
+    chi2_vec_final.push_back(e.squaredNorm());
+    if(e.norm() > reproj_thresh_scaled)
+    {
+      // we don't need to delete a reference in the point since it was not created yet
+      (*it)->point = NULL;
+      ++n_deleted_refs;
+    }
+  }
+
+  error_init=0.0;
+  error_final=0.0;
+  if(!chi2_vec_init.empty())
+    error_init = sqrt(vk::getMedian(chi2_vec_init))*frame->cam_->errorMultiplier2();
+  if(!chi2_vec_final.empty())
+    error_final = sqrt(vk::getMedian(chi2_vec_final))*frame->cam_->errorMultiplier2();
+
+  estimated_scale *= frame->cam_->errorMultiplier2();
+  if(verbose)
+    std::cout << "n deleted obs = " << n_deleted_refs
+              << "\t scale = " << estimated_scale
+              << "\t error init = " << error_init
+              << "\t error end = " << error_final << std::endl;
+  num_obs -= n_deleted_refs;
+}
+
+} // namespace pose_optimizer
+} // namespace svo

CPU_version/svo/src/reprojector.cpp

@@ -0,0 +1,219 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <algorithm>
+#include <stdexcept>
+#include <svo/reprojector.h>
+#include <svo/frame.h>
+#include <svo/point.h>
+#include <svo/feature.h>
+#include <svo/map.h>
+#include <svo/config.h>
+#include <boost/bind.hpp>
+#include <boost/thread.hpp>
+#include <vikit/abstract_camera.h>
+#include <vikit/math_utils.h>
+#include <vikit/timer.h>
+
+namespace svo {
+
+Reprojector::Reprojector(vk::AbstractCamera* cam, Map& map) :
+    map_(map)
+{
+  initializeGrid(cam);
+}
+
+Reprojector::~Reprojector()
+{
+  std::for_each(grid_.cells.begin(), grid_.cells.end(), [&](Cell* c){ delete c; });
+}
+
+void Reprojector::initializeGrid(vk::AbstractCamera* cam)
+{
+  grid_.cell_size = Config::gridSize();
+  grid_.grid_n_cols = ceil(static_cast<double>(cam->width())/grid_.cell_size);
+  grid_.grid_n_rows = ceil(static_cast<double>(cam->height())/grid_.cell_size);
+  grid_.cells.resize(grid_.grid_n_cols*grid_.grid_n_rows);
+  std::for_each(grid_.cells.begin(), grid_.cells.end(), [&](Cell*& c){ c = new Cell; });
+  grid_.cell_order.resize(grid_.cells.size());
+  for(size_t i=0; i<grid_.cells.size(); ++i)
+    grid_.cell_order[i] = i;
+  random_shuffle(grid_.cell_order.begin(), grid_.cell_order.end()); // maybe we should do it at every iteration!
+}
+
+void Reprojector::resetGrid()
+{
+  n_matches_ = 0;
+  n_trials_ = 0;
+  std::for_each(grid_.cells.begin(), grid_.cells.end(), [&](Cell* c){ c->clear(); });
+}
+
+void Reprojector::reprojectMap(
+    FramePtr frame,
+    std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs)
+{
+  resetGrid();
+
+  // Identify those Keyframes which share a common field of view.
+  SVO_START_TIMER("reproject_kfs");
+  list< pair<FramePtr,double> > close_kfs;
+  map_.getCloseKeyframes(frame, close_kfs);
+
+  // Sort KFs with overlap according to their closeness
+  close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <
+                 boost::bind(&std::pair<FramePtr, double>::second, _2));
+
+  // Reproject all mappoints of the closest N kfs with overlap. We only store
+  // in which grid cell the points fall.
+  size_t n = 0;
+  overlap_kfs.reserve(options_.max_n_kfs);
+  for(auto it_frame=close_kfs.begin(), ite_frame=close_kfs.end();
+      it_frame!=ite_frame && n<options_.max_n_kfs; ++it_frame, ++n)
+  {
+    FramePtr ref_frame = it_frame->first;
+    overlap_kfs.push_back(pair<FramePtr,size_t>(ref_frame,0));
+
+    // Try to reproject each mappoint that the other KF observes
+    for(auto it_ftr=ref_frame->fts_.begin(), ite_ftr=ref_frame->fts_.end();
+        it_ftr!=ite_ftr; ++it_ftr)
+    {
+      // check if the feature has a mappoint assigned
+      if((*it_ftr)->point == NULL)
+        continue;
+
+      // make sure we project a point only once
+      if((*it_ftr)->point->last_projected_kf_id_ == frame->id_)
+        continue;
+      (*it_ftr)->point->last_projected_kf_id_ = frame->id_;
+      if(reprojectPoint(frame, (*it_ftr)->point))
+        overlap_kfs.back().second++;
+    }
+  }
+  SVO_STOP_TIMER("reproject_kfs");
+
+  // Now project all point candidates
+  SVO_START_TIMER("reproject_candidates");
+  {
+    boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_);
+    auto it=map_.point_candidates_.candidates_.begin();
+    while(it!=map_.point_candidates_.candidates_.end())
+    {
+      if(!reprojectPoint(frame, it->first))
+      {
+        it->first->n_failed_reproj_ += 3;
+        if(it->first->n_failed_reproj_ > 30)
+        {
+          map_.point_candidates_.deleteCandidate(*it);
+          it = map_.point_candidates_.candidates_.erase(it);
+          continue;
+        }
+      }
+      ++it;
+    }
+  } // unlock the mutex when out of scope
+  SVO_STOP_TIMER("reproject_candidates");
+
+  // Now we go through each grid cell and select one point to match.
+  // At the end, we should have at maximum one reprojected point per cell.
+  SVO_START_TIMER("feature_align");
+  for(size_t i=0; i<grid_.cells.size(); ++i)
+  {
+    // we prefer good quality points over unkown quality (more likely to match)
+    // and unknown quality over candidates (position not optimized)
+    if(reprojectCell(*grid_.cells.at(grid_.cell_order[i]), frame))
+      ++n_matches_;
+    if(n_matches_ > (size_t) Config::maxFts())
+      break;
+  }
+  SVO_STOP_TIMER("feature_align");
+}
+
+bool Reprojector::pointQualityComparator(Candidate& lhs, Candidate& rhs)
+{
+  if(lhs.pt->type_ > rhs.pt->type_)
+    return true;
+  return false;
+}
+
+bool Reprojector::reprojectCell(Cell& cell, FramePtr frame)
+{
+  cell.sort(boost::bind(&Reprojector::pointQualityComparator, _1, _2));
+  Cell::iterator it=cell.begin();
+  while(it!=cell.end())
+  {
+    ++n_trials_;
+
+    if(it->pt->type_ == Point::TYPE_DELETED)
+    {
+      it = cell.erase(it);
+      continue;
+    }
+
+    bool found_match = true;
+    if(options_.find_match_direct)
+      found_match = matcher_.findMatchDirect(*it->pt, *frame, it->px);
+    if(!found_match)
+    {
+      it->pt->n_failed_reproj_++;
+      if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_failed_reproj_ > 15)
+        map_.safeDeletePoint(it->pt);
+      if(it->pt->type_ == Point::TYPE_CANDIDATE  && it->pt->n_failed_reproj_ > 30)
+        map_.point_candidates_.deleteCandidatePoint(it->pt);
+      it = cell.erase(it);
+      continue;
+    }
+    it->pt->n_succeeded_reproj_++;
+    if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_succeeded_reproj_ > 10)
+      it->pt->type_ = Point::TYPE_GOOD;
+
+    Feature* new_feature = new Feature(frame.get(), it->px, matcher_.search_level_);
+    frame->addFeature(new_feature);
+
+    // Here we add a reference in the feature to the 3D point, the other way
+    // round is only done if this frame is selected as keyframe.
+    new_feature->point = it->pt;
+
+    if(matcher_.ref_ftr_->type == Feature::EDGELET)
+    {
+      new_feature->type = Feature::EDGELET;
+      new_feature->grad = matcher_.A_cur_ref_*matcher_.ref_ftr_->grad;
+      new_feature->grad.normalize();
+    }
+
+    // If the keyframe is selected and we reproject the rest, we don't have to
+    // check this point anymore.
+    it = cell.erase(it);
+
+    // Maximum one point per cell.
+    return true;
+  }
+  return false;
+}
+
+bool Reprojector::reprojectPoint(FramePtr frame, Point* point)
+{
+  Vector2d px(frame->w2c(point->pos_));
+  if(frame->cam_->isInFrame(px.cast<int>(), 8)) // 8px is the patch size in the matcher
+  {
+    const int k = static_cast<int>(px[1]/grid_.cell_size)*grid_.grid_n_cols
+                + static_cast<int>(px[0]/grid_.cell_size);
+    grid_.cells.at(k)->push_back(Candidate(point, px));
+    return true;
+  }
+  return false;
+}
+
+} // namespace svo

CPU_version/svo/src/sparse_img_align.cpp

@@ -0,0 +1,274 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <algorithm>
+#include <svo/sparse_img_align.h>
+#include <svo/frame.h>
+#include <svo/feature.h>
+#include <svo/config.h>
+#include <svo/point.h>
+#include <vikit/abstract_camera.h>
+#include <vikit/vision.h>
+#include <vikit/math_utils.h>
+
+namespace svo {
+
+SparseImgAlign::SparseImgAlign(
+    int max_level, int min_level, int n_iter,
+    Method method, bool display, bool verbose) :
+        display_(display),
+        max_level_(max_level),
+        min_level_(min_level)
+{
+  n_iter_ = n_iter;
+  n_iter_init_ = n_iter_;
+  method_ = method;
+  verbose_ = verbose;
+  eps_ = 0.000001;
+}
+
+size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame)
+{
+  reset();
+
+  if(ref_frame->fts_.empty())
+  {
+    SVO_WARN_STREAM("SparseImgAlign: no features to track!");
+    return 0;
+  }
+
+  ref_frame_ = ref_frame;
+  cur_frame_ = cur_frame;
+  ref_patch_cache_ = cv::Mat(ref_frame_->fts_.size(), patch_area_, CV_32F);
+  jacobian_cache_.resize(Eigen::NoChange, ref_patch_cache_.rows*patch_area_);
+  visible_fts_.resize(ref_patch_cache_.rows, false); // TODO: should it be reset at each level?
+
+  SE3 T_cur_from_ref(cur_frame_->T_f_w_ * ref_frame_->T_f_w_.inverse());
+
+  for(level_=max_level_; level_>=min_level_; --level_)
+  {
+    mu_ = 0.1;
+    jacobian_cache_.setZero();
+    have_ref_patch_cache_ = false;
+    if(verbose_)
+      printf("\nPYRAMID LEVEL %i\n---------------\n", level_);
+    optimize(T_cur_from_ref);
+  }
+  cur_frame_->T_f_w_ = T_cur_from_ref * ref_frame_->T_f_w_;
+
+
+
+  return n_meas_/patch_area_;
+}
+
+Matrix<double, 6, 6> SparseImgAlign::getFisherInformation()
+{
+  double sigma_i_sq = 5e-4*255*255; // image noise
+  Matrix<double,6,6> I = H_/sigma_i_sq;
+  return I;
+}
+
+void SparseImgAlign::precomputeReferencePatches()
+{
+  const int border = patch_halfsize_+1;
+  const cv::Mat& ref_img = ref_frame_->img_pyr_.at(level_);
+  const int stride = ref_img.cols;
+  const float scale = 1.0f/(1<<level_);
+  const Vector3d ref_pos = ref_frame_->pos();
+  const double focal_length = ref_frame_->cam_->errorMultiplier2();
+  size_t feature_counter = 0;
+  std::vector<bool>::iterator visiblity_it = visible_fts_.begin();
+  for(auto it=ref_frame_->fts_.begin(), ite=ref_frame_->fts_.end();
+      it!=ite; ++it, ++feature_counter, ++visiblity_it)
+  {
+    // check if reference with patch size is within image
+    const float u_ref = (*it)->px[0]*scale;
+    const float v_ref = (*it)->px[1]*scale;
+    const int u_ref_i = floorf(u_ref);
+    const int v_ref_i = floorf(v_ref);
+    if((*it)->point == NULL || u_ref_i-border < 0 || v_ref_i-border < 0 || u_ref_i+border >= ref_img.cols || v_ref_i+border >= ref_img.rows)
+      continue;
+    *visiblity_it = true;
+
+    // cannot just take the 3d points coordinate because of the reprojection errors in the reference image!!!
+    const double depth(((*it)->point->pos_ - ref_pos).norm());
+    const Vector3d xyz_ref((*it)->f*depth);
+
+    // evaluate projection jacobian
+    Matrix<double,2,6> frame_jac;
+    Frame::jacobian_xyz2uv(xyz_ref, frame_jac);
+
+    // compute bilateral interpolation weights for reference image
+    const float subpix_u_ref = u_ref-u_ref_i;
+    const float subpix_v_ref = v_ref-v_ref_i;
+    const float w_ref_tl = (1.0-subpix_u_ref) * (1.0-subpix_v_ref);
+    const float w_ref_tr = subpix_u_ref * (1.0-subpix_v_ref);
+    const float w_ref_bl = (1.0-subpix_u_ref) * subpix_v_ref;
+    const float w_ref_br = subpix_u_ref * subpix_v_ref;
+    size_t pixel_counter = 0;
+    float* cache_ptr = reinterpret_cast<float*>(ref_patch_cache_.data) + patch_area_*feature_counter;
+    for(int y=0; y<patch_size_; ++y)
+    {
+      uint8_t* ref_img_ptr = (uint8_t*) ref_img.data + (v_ref_i+y-patch_halfsize_)*stride + (u_ref_i-patch_halfsize_);
+      for(int x=0; x<patch_size_; ++x, ++ref_img_ptr, ++cache_ptr, ++pixel_counter)
+      {
+        // precompute interpolated reference patch color
+        *cache_ptr = w_ref_tl*ref_img_ptr[0] + w_ref_tr*ref_img_ptr[1] + w_ref_bl*ref_img_ptr[stride] + w_ref_br*ref_img_ptr[stride+1];
+
+        // we use the inverse compositional: thereby we can take the gradient always at the same position
+        // get gradient of warped image (~gradient at warped position)
+        float dx = 0.5f * ((w_ref_tl*ref_img_ptr[1] + w_ref_tr*ref_img_ptr[2] + w_ref_bl*ref_img_ptr[stride+1] + w_ref_br*ref_img_ptr[stride+2])
+                          -(w_ref_tl*ref_img_ptr[-1] + w_ref_tr*ref_img_ptr[0] + w_ref_bl*ref_img_ptr[stride-1] + w_ref_br*ref_img_ptr[stride]));
+        float dy = 0.5f * ((w_ref_tl*ref_img_ptr[stride] + w_ref_tr*ref_img_ptr[1+stride] + w_ref_bl*ref_img_ptr[stride*2] + w_ref_br*ref_img_ptr[stride*2+1])
+                          -(w_ref_tl*ref_img_ptr[-stride] + w_ref_tr*ref_img_ptr[1-stride] + w_ref_bl*ref_img_ptr[0] + w_ref_br*ref_img_ptr[1]));
+
+        // cache the jacobian
+        jacobian_cache_.col(feature_counter*patch_area_ + pixel_counter) =
+            (dx*frame_jac.row(0) + dy*frame_jac.row(1))*(focal_length / (1<<level_));
+      }
+    }
+  }
+  have_ref_patch_cache_ = true;
+}
+
+double SparseImgAlign::computeResiduals(
+    const SE3& T_cur_from_ref,
+    bool linearize_system,
+    bool compute_weight_scale)
+{
+  // Warp the (cur)rent image such that it aligns with the (ref)erence image
+  const cv::Mat& cur_img = cur_frame_->img_pyr_.at(level_);
+
+  if(linearize_system && display_)
+    resimg_ = cv::Mat(cur_img.size(), CV_32F, cv::Scalar(0));
+
+  if(have_ref_patch_cache_ == false)
+    precomputeReferencePatches();
+
+  // compute the weights on the first iteration
+  std::vector<float> errors;
+  if(compute_weight_scale)
+    errors.reserve(visible_fts_.size());
+  const int stride = cur_img.cols;
+  const int border = patch_halfsize_+1;
+  const float scale = 1.0f/(1<<level_);
+  const Vector3d ref_pos(ref_frame_->pos());
+  float chi2 = 0.0;
+  size_t feature_counter = 0; // is used to compute the index of the cached jacobian
+  std::vector<bool>::iterator visiblity_it = visible_fts_.begin();
+  for(auto it=ref_frame_->fts_.begin(); it!=ref_frame_->fts_.end();
+      ++it, ++feature_counter, ++visiblity_it)
+  {
+    // check if feature is within image
+    if(!*visiblity_it)
+      continue;
+
+    // compute pixel location in cur img
+    const double depth = ((*it)->point->pos_ - ref_pos).norm();
+    const Vector3d xyz_ref((*it)->f*depth);
+    const Vector3d xyz_cur(T_cur_from_ref * xyz_ref);
+    const Vector2f uv_cur_pyr(cur_frame_->cam_->world2cam(xyz_cur).cast<float>() * scale);
+    const float u_cur = uv_cur_pyr[0];
+    const float v_cur = uv_cur_pyr[1];
+    const int u_cur_i = floorf(u_cur);
+    const int v_cur_i = floorf(v_cur);
+
+    // check if projection is within the image
+    if(u_cur_i < 0 || v_cur_i < 0 || u_cur_i-border < 0 || v_cur_i-border < 0 || u_cur_i+border >= cur_img.cols || v_cur_i+border >= cur_img.rows)
+      continue;
+
+    // compute bilateral interpolation weights for the current image
+    const float subpix_u_cur = u_cur-u_cur_i;
+    const float subpix_v_cur = v_cur-v_cur_i;
+    const float w_cur_tl = (1.0-subpix_u_cur) * (1.0-subpix_v_cur);
+    const float w_cur_tr = subpix_u_cur * (1.0-subpix_v_cur);
+    const float w_cur_bl = (1.0-subpix_u_cur) * subpix_v_cur;
+    const float w_cur_br = subpix_u_cur * subpix_v_cur;
+    float* ref_patch_cache_ptr = reinterpret_cast<float*>(ref_patch_cache_.data) + patch_area_*feature_counter;
+    size_t pixel_counter = 0; // is used to compute the index of the cached jacobian
+    for(int y=0; y<patch_size_; ++y)
+    {
+      uint8_t* cur_img_ptr = (uint8_t*) cur_img.data + (v_cur_i+y-patch_halfsize_)*stride + (u_cur_i-patch_halfsize_);
+
+      for(int x=0; x<patch_size_; ++x, ++pixel_counter, ++cur_img_ptr, ++ref_patch_cache_ptr)
+      {
+        // compute residual
+        const float intensity_cur = w_cur_tl*cur_img_ptr[0] + w_cur_tr*cur_img_ptr[1] + w_cur_bl*cur_img_ptr[stride] + w_cur_br*cur_img_ptr[stride+1];
+        const float res = intensity_cur - (*ref_patch_cache_ptr);
+
+        // used to compute scale for robust cost
+        if(compute_weight_scale)
+          errors.push_back(fabsf(res));
+
+        // robustification
+        float weight = 1.0;
+        if(use_weights_) {
+          weight = weight_function_->value(res/scale_);
+        }
+
+        chi2 += res*res*weight;
+        n_meas_++;
+
+        if(linearize_system)
+        {
+          // compute Jacobian, weighted Hessian and weighted "steepest descend images" (times error)
+          const Vector6d J(jacobian_cache_.col(feature_counter*patch_area_ + pixel_counter));
+          H_.noalias() += J*J.transpose()*weight;
+          Jres_.noalias() -= J*res*weight;
+          if(display_)
+            resimg_.at<float>((int) v_cur+y-patch_halfsize_, (int) u_cur+x-patch_halfsize_) = res/255.0;
+        }
+      }
+    }
+  }
+
+  // compute the weights on the first iteration
+  if(compute_weight_scale && iter_ == 0)
+    scale_ = scale_estimator_->compute(errors);
+
+  return chi2/n_meas_;
+}
+
+int SparseImgAlign::solve()
+{
+  x_ = H_.ldlt().solve(Jres_);
+  if((bool) std::isnan((double) x_[0]))
+    return 0;
+  return 1;
+}
+
+void SparseImgAlign::update(
+    const ModelType& T_curold_from_ref,
+    ModelType& T_curnew_from_ref)
+{
+  T_curnew_from_ref =  T_curold_from_ref * SE3::exp(-x_);
+}
+
+void SparseImgAlign::startIteration()
+{}
+
+void SparseImgAlign::finishIteration()
+{
+  if(display_)
+  {
+    cv::namedWindow("residuals", CV_WINDOW_AUTOSIZE);
+    cv::imshow("residuals", resimg_*10);
+    cv::waitKey(0);
+  }
+}
+
+} // namespace svo
+

CPU_version/svo/test/README.md

@@ -0,0 +1,24 @@
+#### Download Test Data
+
+Create a folder where you will store all datasets, e.g.:
+
+    mkdir Datasets
+
+In your `.bashrc` script, create a new environment variable pointing to the dataset folder:
+
+    export SVO_DATASET_DIR=${HOME}/Datasets
+
+Source your new `.bashrc` script, go to the new dataset folder and download the test data:
+
+    source ~/.bashrc
+    cd ${SVO_DATASET_DIR}
+    wget http://rpg.ifi.uzh.ch/datasets/sin2_tex2_h1_v8_d.tar.gz -O - | tar -xz
+
+You can then run the tests, e.g.:
+
+    rosrun svo test_pipeline
+
+or if you are not using ROS:
+
+    cd svo/bin
+    ./test_pipeline

CPU_version/svo/test/benchmark.csv

@@ -0,0 +1,307 @@
+local_ba,point_optimizer,pose_optimizer,pyramid_creation,reproject,sparse_img_align,tot_time,dropout,img_align_n_tracked,loba_err_fin,loba_err_init,loba_n_erredges_fin,loba_n_erredges_init,n_candidates,repr_n_mps,repr_n_new_references,sfba_error_final,sfba_error_init,sfba_n_edges_final,sfba_thresh
+0.000000000000000,0.000000000000000,0.000000000000000,0.000231000000000,0.000000000000000,0.000000000000000,0.005536000000000,1.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000
+0.000000000000000,0.000000000000000,0.000000000000000,0.000326000000000,0.000000000000000,0.000000000000000,0.016564000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000
+0.000000000000000,0.000000000000000,0.000000000000000,0.000189000000000,0.000000000000000,0.000000000000000,0.016754000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000
+0.000000000000000,0.000000000000000,0.000000000000000,0.000133000000000,0.000000000000000,0.000000000000000,0.017082000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000
+0.000000000000000,0.000000000000000,0.000000000000000,0.000136000000000,0.000000000000000,0.000000000000000,0.023259000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000
+0.000000000000000,0.000000000000000,0.000000000000000,0.000120000000000,0.000000000000000,0.000000000000000,0.029114000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000
+0.000000000000000,0.000000000000000,0.000000000000000,0.000231000000000,0.000000000000000,0.000000000000000,0.032538000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000
+0.000000000000000,0.000000000000000,0.000000000000000,0.000123000000000,0.000000000000000,0.000000000000000,0.031375000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000
+0.000000000000000,0.000000000000000,0.000000000000000,0.000137000000000,0.000000000000000,0.000000000000000,0.029995000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000
+0.000000000000000,0.000000000000000,0.000000000000000,0.000119000000000,0.000000000000000,0.000000000000000,0.030888000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000
+0.000000000000000,0.000000000000000,0.000000000000000,0.000461000000000,0.000000000000000,0.000000000000000,0.036835000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000
+0.000000000000000,0.000000000000000,0.000000000000000,0.000159000000000,0.000000000000000,0.000000000000000,0.090994000000000,1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000
+0.000000000000000,0.000036000000000,0.000143000000000,0.000117000000000,0.000836000000000,0.001616000000000,0.002862000000000,0.000000000000000,279.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,148.000000000000000,121.000000000000000,0.170175036288445,0.209622100274678,121.000000000000000,0.310240723182540
+0.000000000000000,0.000025000000000,0.000180000000000,0.000123000000000,0.000817000000000,0.000802000000000,0.001981000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,151.000000000000000,121.000000000000000,0.200086249066279,0.226929968835210,119.000000000000000,0.335856340084193
+0.000000000000000,0.000043000000000,0.000247000000000,0.000202000000000,0.000836000000000,0.000882000000000,0.002289000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,150.000000000000000,121.000000000000000,0.220021653137765,0.250579787207819,120.000000000000000,0.370858080741496
+0.000000000000000,0.000027000000000,0.000173000000000,0.000125000000000,0.000796000000000,0.000863000000000,0.002018000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,153.000000000000000,121.000000000000000,0.241933923583207,0.315090761433966,119.000000000000000,0.466334367626374
+0.000000000000000,0.000025000000000,0.000171000000000,0.000123000000000,0.000752000000000,0.000893000000000,0.002012000000000,1.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,145.000000000000000,121.000000000000000,0.272951476803891,0.278442112643762,118.000000000000000,0.412094340922640
+0.000000000000000,0.000058000000000,0.000180000000000,0.000213000000000,0.001057000000000,0.000834000000000,0.002415000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,176.000000000000000,121.000000000000000,0.295685268713144,0.345204254249458,120.000000000000000,0.510902319377397
+0.000000000000000,0.000029000000000,0.000104000000000,0.000125000000000,0.000868000000000,0.000845000000000,0.002006000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,154.000000000000000,121.000000000000000,0.280726297104857,0.351937246921774,119.000000000000000,0.520867118388044
+0.000000000000000,0.000030000000000,0.000173000000000,0.000117000000000,0.000872000000000,0.000829000000000,0.002054000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,160.000000000000000,121.000000000000000,0.299712702632539,0.325868379793256,119.000000000000000,0.482285213145440
+0.000000000000000,0.000030000000000,0.000135000000000,0.000312000000000,0.001611000000000,0.000955000000000,0.003094000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,0.000000000000000,157.000000000000000,121.000000000000000,0.266414418920375,0.325723952270968,121.000000000000000,0.482071463677943
+0.000000000000000,0.000019000000000,0.000134000000000,0.000120000000000,0.000807000000000,0.000746000000000,0.001858000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,52.000000000000000,146.000000000000000,121.000000000000000,0.268513470166789,0.275002624777888,120.000000000000000,0.407003878905728
+0.000000000000000,0.000023000000000,0.000116000000000,0.000137000000000,0.000918000000000,0.000845000000000,0.002086000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,76.000000000000000,141.000000000000000,121.000000000000000,0.291779184224341,0.283057831418736,119.000000000000000,0.418925579815900
+0.000000000000000,0.000020000000000,0.000174000000000,0.000139000000000,0.000841000000000,0.000842000000000,0.002050000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,97.000000000000000,146.000000000000000,121.000000000000000,0.284752630550729,0.313033048850307,118.000000000000000,0.463288905244113
+0.000000000000000,0.000018000000000,0.000125000000000,0.000116000000000,0.000918000000000,0.000737000000000,0.001957000000000,1.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,108.000000000000000,164.000000000000000,121.000000000000000,0.266984542461140,0.310809088449218,117.000000000000000,0.459997446489587
+0.000000000000000,0.000025000000000,0.000187000000000,0.000215000000000,0.001123000000000,0.000838000000000,0.002412000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,82.000000000000000,172.000000000000000,121.000000000000000,0.281472742741066,0.318106456171594,118.000000000000000,0.470797581856187
+0.000000000000000,0.000032000000000,0.000104000000000,0.000133000000000,0.000982000000000,0.001074000000000,0.002360000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,82.000000000000000,169.000000000000000,121.000000000000000,0.259578544711984,0.296051216412790,118.000000000000000,0.438155829408007
+0.000000000000000,0.000026000000000,0.000173000000000,0.000114000000000,0.000975000000000,0.000678000000000,0.001991000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,87.000000000000000,169.000000000000000,121.000000000000000,0.260854021690335,0.463271429083291,119.000000000000000,0.685641725628487
+0.000000000000000,0.000020000000000,0.000231000000000,0.000213000000000,0.001255000000000,0.000764000000000,0.002547000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,100.000000000000000,162.000000000000000,121.000000000000000,0.224857460689169,0.241819176684415,120.000000000000000,0.357892398925400
+0.000000000000000,0.000016000000000,0.000174000000000,0.000123000000000,0.000964000000000,0.000761000000000,0.002072000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,177.000000000000000,163.000000000000000,121.000000000000000,0.232802572049831,0.307670939545318,121.000000000000000,0.455352982415734
+0.000000000000000,0.000019000000000,0.000171000000000,0.000117000000000,0.000954000000000,0.000685000000000,0.001989000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,215.000000000000000,163.000000000000000,121.000000000000000,0.223503314093231,0.245376537152124,121.000000000000000,0.363157262109869
+0.000000000000000,0.000022000000000,0.000153000000000,0.000321000000000,0.001789000000000,0.000999000000000,0.003323000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,236.000000000000000,160.000000000000000,121.000000000000000,0.238679756461930,0.260245176933572,121.000000000000000,0.385162868349003
+0.000000000000000,0.000017000000000,0.000103000000000,0.000121000000000,0.000950000000000,0.000690000000000,0.001925000000000,1.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,253.000000000000000,156.000000000000000,121.000000000000000,0.240028083468930,0.347317483304434,119.000000000000000,0.514029864789977
+0.000000000000000,0.000026000000000,0.000155000000000,0.000141000000000,0.001421000000000,0.000834000000000,0.002605000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,225.000000000000000,203.000000000000000,121.000000000000000,0.237050818956117,0.342187690832661,118.000000000000000,0.506437791010399
+0.000000000000000,0.000031000000000,0.000105000000000,0.000133000000000,0.001151000000000,0.000787000000000,0.002244000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,224.000000000000000,192.000000000000000,121.000000000000000,0.228256287503905,0.255917399607126,118.000000000000000,0.378757763709815
+0.000000000000000,0.000029000000000,0.000173000000000,0.000116000000000,0.001046000000000,0.000670000000000,0.002067000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,237.000000000000000,174.000000000000000,121.000000000000000,0.253024524323249,0.270830988595337,117.000000000000000,0.400829860236131
+0.000000000000000,0.000032000000000,0.000206000000000,0.000201000000000,0.001159000000000,0.000767000000000,0.002415000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,243.000000000000000,168.000000000000000,121.000000000000000,0.255837803512081,0.309767343777557,117.000000000000000,0.458455710853014
+0.000000000000000,0.000029000000000,0.000102000000000,0.000134000000000,0.001078000000000,0.000785000000000,0.002160000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,247.000000000000000,176.000000000000000,121.000000000000000,0.242039161090674,0.295753112981567,120.000000000000000,0.437714633120047
+0.000000000000000,0.000018000000000,0.000174000000000,0.000117000000000,0.001043000000000,0.000946000000000,0.002334000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,284.000000000000000,173.000000000000000,121.000000000000000,0.234035474669157,0.262962395172133,120.000000000000000,0.389184330309739
+0.000000000000000,0.000042000000000,0.000251000000000,0.000203000000000,0.001816000000000,0.000807000000000,0.003189000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,323.000000000000000,168.000000000000000,121.000000000000000,0.243963461663248,0.254676688230514,118.000000000000000,0.376921484440742
+0.000000000000000,0.000022000000000,0.000128000000000,0.000121000000000,0.001084000000000,0.000899000000000,0.002287000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,346.000000000000000,164.000000000000000,121.000000000000000,0.237498671286056,0.242318617577444,120.000000000000000,0.358631550548166
+0.000000000000000,0.000042000000000,0.000198000000000,0.000115000000000,0.001158000000000,0.000720000000000,0.002269000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,360.000000000000000,174.000000000000000,121.000000000000000,0.231204372046851,0.310626801011824,119.000000000000000,0.459727694560518
+0.000000000000000,0.000020000000000,0.000128000000000,0.000280000000000,0.000998000000000,0.000883000000000,0.002349000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,371.000000000000000,154.000000000000000,121.000000000000000,0.226372262196515,0.240389813013775,118.000000000000000,0.355776916048274
+0.000000000000000,0.000020000000000,0.000125000000000,0.000116000000000,0.001040000000000,0.000676000000000,0.002025000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,383.000000000000000,163.000000000000000,121.000000000000000,0.243430360339485,0.289701992216838,116.000000000000000,0.428758939836188
+0.000000000000000,0.000028000000000,0.000186000000000,0.000201000000000,0.001357000000000,0.000862000000000,0.002663000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,351.000000000000000,189.000000000000000,121.000000000000000,0.238491256129826,0.289513389727138,119.000000000000000,0.428479837315611
+0.000000000000000,0.000024000000000,0.000104000000000,0.000131000000000,0.001197000000000,0.000689000000000,0.002178000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,351.000000000000000,187.000000000000000,121.000000000000000,0.253535521704090,0.299150905814168,119.000000000000000,0.442743330689269
+0.000000000000000,0.000013000000000,0.000128000000000,0.000142000000000,0.001157000000000,0.000751000000000,0.002226000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,360.000000000000000,182.000000000000000,121.000000000000000,0.242747326189240,0.331002657945821,119.000000000000000,0.489883908154680
+0.000000000000000,0.000041000000000,0.000225000000000,0.000214000000000,0.001614000000000,0.000892000000000,0.003073000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,420.000000000000000,166.000000000000000,121.000000000000000,0.214060199250618,0.291833988602564,120.000000000000000,0.431914284304601
+0.000000000000000,0.000016000000000,0.000113000000000,0.000126000000000,0.001256000000000,0.000871000000000,0.002419000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,467.000000000000000,189.000000000000000,121.000000000000000,0.191542639509251,0.219467558857437,120.000000000000000,0.324811977376153
+0.000000000000000,0.000022000000000,0.000110000000000,0.000116000000000,0.001163000000000,0.000737000000000,0.002178000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,491.000000000000000,176.000000000000000,121.000000000000000,0.237287779136815,0.290385733638894,119.000000000000000,0.429770876012296
+0.000000000000000,0.000016000000000,0.000108000000000,0.000532000000000,0.001507000000000,0.001176000000000,0.003390000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,503.000000000000000,195.000000000000000,121.000000000000000,0.255418654779892,0.271366103996528,119.000000000000000,0.401621830064983
+0.000000000000000,0.000023000000000,0.000128000000000,0.000118000000000,0.001247000000000,0.000704000000000,0.002255000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,519.000000000000000,192.000000000000000,121.000000000000000,0.243354932726649,0.322133765000893,118.000000000000000,0.476758002689501
+0.000000000000000,0.000028000000000,0.000117000000000,0.000194000000000,0.001487000000000,0.000763000000000,0.002640000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,537.000000000000000,182.000000000000000,121.000000000000000,0.256839812012575,0.290001379744665,120.000000000000000,0.429202006242450
+0.000000000000000,0.000021000000000,0.000192000000000,0.000135000000000,0.001268000000000,0.000737000000000,0.002390000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,551.000000000000000,186.000000000000000,121.000000000000000,0.234921062453291,0.278307687859489,121.000000000000000,0.411895400770342
+0.000000000000000,0.000023000000000,0.000110000000000,0.000128000000000,0.001205000000000,0.000853000000000,0.002375000000000,1.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,566.000000000000000,182.000000000000000,121.000000000000000,0.236213436253286,0.243545081705577,120.000000000000000,0.360446727806674
+0.000000000000000,0.000033000000000,0.000148000000000,0.000221000000000,0.001810000000000,0.000752000000000,0.003004000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,523.000000000000000,194.000000000000000,121.000000000000000,0.256770574347429,0.281629144927868,120.000000000000000,0.416811158357270
+0.000000000000000,0.000018000000000,0.000107000000000,0.000166000000000,0.001283000000000,0.000754000000000,0.002364000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,523.000000000000000,193.000000000000000,121.000000000000000,0.238569415669078,0.245006057597332,120.000000000000000,0.362608988913209
+0.000000000000000,0.000026000000000,0.000199000000000,0.000126000000000,0.001415000000000,0.000739000000000,0.002538000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,525.000000000000000,207.000000000000000,121.000000000000000,0.272522903927536,0.292694935292295,120.000000000000000,0.433188542480310
+0.000000000000000,0.000024000000000,0.000152000000000,0.000385000000000,0.001329000000000,0.000772000000000,0.002715000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,532.000000000000000,193.000000000000000,121.000000000000000,0.300157389608362,0.352279544787609,121.000000000000000,0.521373718272822
+0.000000000000000,0.000025000000000,0.000103000000000,0.000124000000000,0.001300000000000,0.000765000000000,0.002352000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,542.000000000000000,192.000000000000000,121.000000000000000,0.262925192895899,0.296136504354747,121.000000000000000,0.438282037121495
+0.000000000000000,0.000027000000000,0.000159000000000,0.000227000000000,0.001454000000000,0.000792000000000,0.002702000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,565.000000000000000,171.000000000000000,121.000000000000000,0.281949830225302,0.308467076957178,121.000000000000000,0.456531258033208
+0.000000000000000,0.000027000000000,0.000177000000000,0.000130000000000,0.001440000000000,0.000817000000000,0.002620000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,608.000000000000000,201.000000000000000,121.000000000000000,0.262127192155801,0.344178587739890,120.000000000000000,0.509384339297628
+0.000000000000000,0.000031000000000,0.000173000000000,0.000122000000000,0.001320000000000,0.000744000000000,0.002429000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,636.000000000000000,196.000000000000000,121.000000000000000,0.243118415564597,0.307364154563702,120.000000000000000,0.454898948018352
+0.000000000000000,0.000031000000000,0.000237000000000,0.000391000000000,0.002601000000000,0.000982000000000,0.004294000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,663.000000000000000,198.000000000000000,121.000000000000000,0.308234194177187,0.324201216218454,120.000000000000000,0.479817768949582
+0.000000000000000,0.000016000000000,0.000102000000000,0.000118000000000,0.001370000000000,0.000653000000000,0.002310000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,687.000000000000000,198.000000000000000,121.000000000000000,0.262171425081880,0.309537417467615,120.000000000000000,0.458115349322126
+0.000000000000000,0.000024000000000,0.000120000000000,0.000223000000000,0.001806000000000,0.000776000000000,0.002976000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,661.000000000000000,229.000000000000000,121.000000000000000,0.297772053023002,0.357203726783215,120.000000000000000,0.528661518750357
+0.000000000000000,0.000022000000000,0.000131000000000,0.000225000000000,0.001322000000000,0.000818000000000,0.002549000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,661.000000000000000,195.000000000000000,121.000000000000000,0.273836774721146,0.292258185663714,119.000000000000000,0.432542138616555
+0.000000000000000,0.000028000000000,0.000189000000000,0.000113000000000,0.001354000000000,0.000656000000000,0.002369000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,668.000000000000000,208.000000000000000,121.000000000000000,0.290291300745150,0.351263582277828,120.000000000000000,0.519870092614610
+0.000000000000000,0.000020000000000,0.000106000000000,0.000362000000000,0.002527000000000,0.000881000000000,0.003941000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,682.000000000000000,211.000000000000000,121.000000000000000,0.258959916330166,0.286061529228379,120.000000000000000,0.423371078465586
+0.000000000000000,0.000023000000000,0.000183000000000,0.000117000000000,0.001452000000000,0.000716000000000,0.002520000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,696.000000000000000,219.000000000000000,121.000000000000000,0.299843168031565,0.306288599781015,118.000000000000000,0.453307123537539
+0.000000000000000,0.000024000000000,0.000190000000000,0.000229000000000,0.001590000000000,0.000704000000000,0.002778000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,704.000000000000000,201.000000000000000,121.000000000000000,0.274895969986079,0.339774469390948,119.000000000000000,0.502866218860539
+0.000000000000000,0.000032000000000,0.000184000000000,0.000127000000000,0.001388000000000,0.000658000000000,0.002422000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,712.000000000000000,205.000000000000000,121.000000000000000,0.276261011279968,0.281429264300083,120.000000000000000,0.416515325859454
+0.000000000000000,0.000019000000000,0.000103000000000,0.000122000000000,0.001302000000000,0.000759000000000,0.002334000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,743.000000000000000,194.000000000000000,121.000000000000000,0.308426080941849,0.293337004748022,118.000000000000000,0.434138764752467
+0.000000000000000,0.000020000000000,0.000188000000000,0.000336000000000,0.002590000000000,0.000953000000000,0.004130000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,769.000000000000000,216.000000000000000,121.000000000000000,0.273447755201539,0.322902569141660,119.000000000000000,0.477895792772933
+0.000000000000000,0.000019000000000,0.000104000000000,0.000118000000000,0.001466000000000,0.000743000000000,0.002480000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,791.000000000000000,204.000000000000000,121.000000000000000,0.288081750238067,0.251959938318303,120.000000000000000,0.372900704820467
+0.000000000000000,0.000024000000000,0.000118000000000,0.000231000000000,0.001575000000000,0.000986000000000,0.003013000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,803.000000000000000,190.000000000000000,121.000000000000000,0.282457846343284,0.276454604943847,117.000000000000000,0.409152821737331
+0.000000000000000,0.000016000000000,0.000210000000000,0.000129000000000,0.001436000000000,0.000881000000000,0.002693000000000,0.000000000000000,111.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,781.000000000000000,208.000000000000000,121.000000000000000,0.294182859046983,0.317841061067694,121.000000000000000,0.470404755925379
+0.000000000000000,0.000015000000000,0.000142000000000,0.000184000000000,0.001424000000000,0.000955000000000,0.002755000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,781.000000000000000,194.000000000000000,121.000000000000000,0.243748783908456,0.304183797141307,119.000000000000000,0.450192011884479
+0.000000000000000,0.000014000000000,0.000107000000000,0.000318000000000,0.001624000000000,0.000893000000000,0.002992000000000,0.000000000000000,114.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,803.000000000000000,220.000000000000000,121.000000000000000,0.265012454662619,0.270354981784829,119.000000000000000,0.400125381617550
+0.000000000000000,0.000023000000000,0.000213000000000,0.000128000000000,0.001340000000000,0.000949000000000,0.002681000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,811.000000000000000,196.000000000000000,121.000000000000000,0.253755971722852,0.269982133879470,119.000000000000000,0.399573570359238
+0.000000000000000,0.000038000000000,0.000249000000000,0.000219000000000,0.001747000000000,0.000847000000000,0.003214000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,821.000000000000000,217.000000000000000,121.000000000000000,0.242158110541114,0.277566291448718,118.000000000000000,0.410798121492822
+0.000000000000000,0.000022000000000,0.000200000000000,0.000131000000000,0.001465000000000,0.000761000000000,0.002608000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,826.000000000000000,208.000000000000000,121.000000000000000,0.247025005586580,0.299927107710462,119.000000000000000,0.443892088763821
+0.000000000000000,0.000028000000000,0.000129000000000,0.000115000000000,0.001507000000000,0.000767000000000,0.002582000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,835.000000000000000,206.000000000000000,121.000000000000000,0.251694282278063,0.262688495423133,119.000000000000000,0.388778969531936
+0.000000000000000,0.000018000000000,0.000176000000000,0.000303000000000,0.001569000000000,0.000763000000000,0.002865000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,858.000000000000000,208.000000000000000,121.000000000000000,0.204259521447942,0.239471125876921,121.000000000000000,0.354417264227415
+0.000000000000000,0.000027000000000,0.000197000000000,0.000119000000000,0.001520000000000,0.000717000000000,0.002613000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,887.000000000000000,182.000000000000000,121.000000000000000,0.288688133937508,0.321107040561571,121.000000000000000,0.475238455753857
+0.000000000000000,0.000074000000000,0.000308000000000,0.000231000000000,0.002302000000000,0.000817000000000,0.004009000000000,1.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,895.000000000000000,169.000000000000000,121.000000000000000,0.226466756684178,0.283662806855079,120.000000000000000,0.419820962132456
+0.000000000000000,0.000032000000000,0.000120000000000,0.000146000000000,0.001779000000000,0.000719000000000,0.002820000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,887.000000000000000,201.000000000000000,121.000000000000000,0.248964133477333,0.301424345755280,121.000000000000000,0.446108027319984
+0.000000000000000,0.000031000000000,0.000209000000000,0.000347000000000,0.002379000000000,0.001268000000000,0.004286000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,881.000000000000000,197.000000000000000,121.000000000000000,0.197089987231914,0.215315450958625,121.000000000000000,0.318666844452755
+0.000000000000000,0.000022000000000,0.000183000000000,0.000138000000000,0.001523000000000,0.000904000000000,0.002805000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,889.000000000000000,208.000000000000000,121.000000000000000,0.211913610123421,0.217730864556872,121.000000000000000,0.322241701945577
+0.000000000000000,0.000023000000000,0.000188000000000,0.000135000000000,0.001539000000000,0.001010000000000,0.002927000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,912.000000000000000,191.000000000000000,121.000000000000000,0.223781553150169,0.264534665335954,119.000000000000000,0.391511288186158
+0.000000000000000,0.000024000000000,0.000111000000000,0.000561000000000,0.002319000000000,0.001083000000000,0.004153000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,959.000000000000000,191.000000000000000,121.000000000000000,0.192480342616366,0.242812113218322,120.000000000000000,0.359361932832411
+0.000000000000000,0.000023000000000,0.000113000000000,0.000129000000000,0.001473000000000,0.000780000000000,0.002555000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1010.000000000000000,186.000000000000000,121.000000000000000,0.226239188570666,0.246580462159093,121.000000000000000,0.364939080501376
+0.000000000000000,0.000025000000000,0.000147000000000,0.000235000000000,0.001877000000000,0.001081000000000,0.003415000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1042.000000000000000,191.000000000000000,121.000000000000000,0.213010810373553,0.226959616128893,121.000000000000000,0.335900212048673
+0.000000000000000,0.000019000000000,0.000180000000000,0.000123000000000,0.001581000000000,0.000797000000000,0.002856000000000,1.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1066.000000000000000,197.000000000000000,121.000000000000000,0.236500218717681,0.308339991149568,118.000000000000000,0.456343184784554
+0.000000000000000,0.000020000000000,0.000176000000000,0.000124000000000,0.001670000000000,0.001020000000000,0.003039000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1045.000000000000000,222.000000000000000,121.000000000000000,0.203403550946325,0.322649020817046,119.000000000000000,0.477520556062907
+0.000000000000000,0.000021000000000,0.000102000000000,0.000120000000000,0.001506000000000,0.000747000000000,0.002537000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1040.000000000000000,208.000000000000000,121.000000000000000,0.217457541731394,0.242874100604298,119.000000000000000,0.359453694988533
+0.000000000000000,0.000014000000000,0.000102000000000,0.000114000000000,0.001438000000000,0.000775000000000,0.002481000000000,0.000000000000000,114.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1048.000000000000000,202.000000000000000,121.000000000000000,0.237066555963892,0.363698547046353,120.000000000000000,0.538273825733024
+0.000000000000000,0.000028000000000,0.000138000000000,0.000130000000000,0.001867000000000,0.000905000000000,0.003190000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1051.000000000000000,204.000000000000000,121.000000000000000,0.204004037194742,0.217771792656962,121.000000000000000,0.322302253343558
+0.000000000000000,0.000016000000000,0.000108000000000,0.000146000000000,0.001730000000000,0.000807000000000,0.002848000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1092.000000000000000,216.000000000000000,121.000000000000000,0.189498788663577,0.218460412109291,119.000000000000000,0.323321417274184
+0.000000000000000,0.000021000000000,0.000142000000000,0.000136000000000,0.001571000000000,0.000950000000000,0.002860000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1150.000000000000000,195.000000000000000,121.000000000000000,0.234911243099688,0.229840840010708,121.000000000000000,0.340164460854319
+0.000000000000000,0.000023000000000,0.000178000000000,0.000327000000000,0.001509000000000,0.001060000000000,0.003143000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1188.000000000000000,188.000000000000000,121.000000000000000,0.238938910296195,0.242239031677996,119.000000000000000,0.358513758367035
+0.000000000000000,0.000026000000000,0.000127000000000,0.000121000000000,0.001413000000000,0.000746000000000,0.002666000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1209.000000000000000,181.000000000000000,121.000000000000000,0.231069770120829,0.351821144360750,121.000000000000000,0.520695269679250
+0.000000000000000,0.000013000000000,0.000123000000000,0.000156000000000,0.002291000000000,0.000810000000000,0.003493000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1131.000000000000000,223.000000000000000,121.000000000000000,0.232050436410629,0.243086413240903,121.000000000000000,0.359767900135068
+0.000000000000000,0.000017000000000,0.000183000000000,0.000122000000000,0.001660000000000,0.000828000000000,0.002856000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1126.000000000000000,205.000000000000000,121.000000000000000,0.217672407670591,0.232176089084970,120.000000000000000,0.343620616378729
+0.000000000000000,0.000025000000000,0.000104000000000,0.000126000000000,0.001640000000000,0.000956000000000,0.002894000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1122.000000000000000,225.000000000000000,121.000000000000000,0.210874531577464,0.231177462797771,120.000000000000000,0.342142641667492
+0.000000000000000,0.000011000000000,0.000104000000000,0.000149000000000,0.001579000000000,0.000928000000000,0.002811000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1124.000000000000000,218.000000000000000,121.000000000000000,0.199068638688048,0.231930228681434,121.000000000000000,0.343256752009726
+0.000000000000000,0.000016000000000,0.000105000000000,0.000114000000000,0.001529000000000,0.000835000000000,0.002636000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1131.000000000000000,217.000000000000000,121.000000000000000,0.199461859922069,0.201973412653913,121.000000000000000,0.298920644384362
+0.000000000000000,0.000020000000000,0.000107000000000,0.000175000000000,0.002271000000000,0.000832000000000,0.003453000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1170.000000000000000,216.000000000000000,121.000000000000000,0.220686177070489,0.284068818474203,121.000000000000000,0.420421851360250
+0.000000000000000,0.000021000000000,0.000110000000000,0.000115000000000,0.001720000000000,0.000987000000000,0.002996000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1207.000000000000000,220.000000000000000,121.000000000000000,0.211157109658996,0.207022193542394,121.000000000000000,0.306392853689555
+0.000000000000000,0.000019000000000,0.000128000000000,0.000124000000000,0.001660000000000,0.000836000000000,0.002805000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1249.000000000000000,230.000000000000000,121.000000000000000,0.227298389602018,0.273505984323976,121.000000000000000,0.404788875408716
+0.000000000000000,0.000019000000000,0.000103000000000,0.000135000000000,0.001696000000000,0.000751000000000,0.002919000000000,1.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1276.000000000000000,235.000000000000000,121.000000000000000,0.230641563123264,0.341660758731761,121.000000000000000,0.505657951678639
+0.000000000000000,0.000015000000000,0.000177000000000,0.000130000000000,0.001556000000000,0.000919000000000,0.002864000000000,0.000000000000000,106.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1118.000000000000000,225.000000000000000,121.000000000000000,0.192303276106424,0.284766422639249,121.000000000000000,0.421454308293938
+0.000000000000000,0.000013000000000,0.000138000000000,0.000178000000000,0.002041000000000,0.000992000000000,0.003412000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1112.000000000000000,223.000000000000000,121.000000000000000,0.206634709237684,0.414844586254086,120.000000000000000,0.613969997701391
+0.000000000000000,0.000015000000000,0.000134000000000,0.000127000000000,0.001905000000000,0.000764000000000,0.002995000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1116.000000000000000,248.000000000000000,121.000000000000000,0.217282816286385,0.386388750278064,121.000000000000000,0.571855388443357
+0.000000000000000,0.000016000000000,0.000128000000000,0.000127000000000,0.001825000000000,0.000925000000000,0.003066000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1115.000000000000000,245.000000000000000,121.000000000000000,0.205232081617916,0.260414398266005,121.000000000000000,0.385413312570202
+0.000000000000000,0.000020000000000,0.000107000000000,0.000141000000000,0.001807000000000,0.000923000000000,0.003060000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1141.000000000000000,234.000000000000000,121.000000000000000,0.215575249362573,0.253408200953891,120.000000000000000,0.375044163656510
+0.000000000000000,0.000019000000000,0.000128000000000,0.000120000000000,0.001568000000000,0.000884000000000,0.002762000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1162.000000000000000,189.000000000000000,121.000000000000000,0.263382144346756,0.344167879309156,121.000000000000000,0.509368468563933
+0.000000000000000,0.000023000000000,0.000131000000000,0.000373000000000,0.001757000000000,0.001182000000000,0.003515000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1191.000000000000000,211.000000000000000,121.000000000000000,0.211945557067404,0.302128504397490,121.000000000000000,0.447150188651345
+0.000000000000000,0.000025000000000,0.000154000000000,0.000124000000000,0.001844000000000,0.000803000000000,0.003004000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1216.000000000000000,227.000000000000000,121.000000000000000,0.202381916559221,0.390013881008811,120.000000000000000,0.577220555675650
+0.000000000000000,0.000015000000000,0.000109000000000,0.000129000000000,0.001708000000000,0.000890000000000,0.002894000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1231.000000000000000,213.000000000000000,121.000000000000000,0.214162701863959,0.216971594768139,120.000000000000000,0.321117988293146
+0.000000000000000,0.000019000000000,0.000106000000000,0.000141000000000,0.001738000000000,0.000788000000000,0.002834000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1244.000000000000000,223.000000000000000,121.000000000000000,0.199426583525084,0.257295226754736,121.000000000000000,0.380796925542741
+0.000000000000000,0.000015000000000,0.000104000000000,0.000127000000000,0.001749000000000,0.000817000000000,0.002854000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1244.000000000000000,237.000000000000000,121.000000000000000,0.197980287517504,0.261753748672962,121.000000000000000,0.387395536882405
+0.000000000000000,0.000022000000000,0.000120000000000,0.000159000000000,0.002300000000000,0.000963000000000,0.003628000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1233.000000000000000,228.000000000000000,121.000000000000000,0.236498759179881,0.240201532654298,121.000000000000000,0.355498268421338
+0.000000000000000,0.000017000000000,0.000181000000000,0.000128000000000,0.001959000000000,0.000872000000000,0.003382000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1223.000000000000000,248.000000000000000,121.000000000000000,0.184287569106640,0.201686577196243,121.000000000000000,0.298496127529902
+0.000000000000000,0.000013000000000,0.000178000000000,0.000136000000000,0.001684000000000,0.000852000000000,0.002943000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1105.000000000000000,230.000000000000000,121.000000000000000,0.190585604418642,0.200521952748998,120.000000000000000,0.296772510252565
+0.000000000000000,0.000019000000000,0.000106000000000,0.000143000000000,0.001668000000000,0.000981000000000,0.002955000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1104.000000000000000,218.000000000000000,121.000000000000000,0.200527511479873,0.209064190915326,120.000000000000000,0.309415015408653
+0.000000000000000,0.000014000000000,0.000104000000000,0.000123000000000,0.001517000000000,0.000849000000000,0.002647000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1144.000000000000000,201.000000000000000,121.000000000000000,0.227018266784565,0.269186250934732,121.000000000000000,0.398395648547930
+0.000000000000000,0.000017000000000,0.000178000000000,0.000348000000000,0.001620000000000,0.001141000000000,0.003354000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1182.000000000000000,206.000000000000000,121.000000000000000,0.206456021778136,0.213075158345683,121.000000000000000,0.315351251063391
+0.000000000000000,0.000026000000000,0.000108000000000,0.000150000000000,0.001619000000000,0.000821000000000,0.002770000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1214.000000000000000,197.000000000000000,121.000000000000000,0.195769620795380,0.235699009656539,120.000000000000000,0.348834556747284
+0.000000000000000,0.000018000000000,0.000178000000000,0.000127000000000,0.001593000000000,0.000826000000000,0.002776000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1234.000000000000000,206.000000000000000,121.000000000000000,0.202443810598108,0.259322628022743,121.000000000000000,0.383797505085764
+0.000000000000000,0.000018000000000,0.000105000000000,0.000140000000000,0.001539000000000,0.001030000000000,0.002870000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1255.000000000000000,195.000000000000000,121.000000000000000,0.234795060265796,0.251596569636394,120.000000000000000,0.372362919449417
+0.000000000000000,0.000017000000000,0.000175000000000,0.000125000000000,0.001584000000000,0.000833000000000,0.002923000000000,1.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1267.000000000000000,221.000000000000000,121.000000000000000,0.207163879614002,0.272216719799699,121.000000000000000,0.402880748216245
+0.000000000000000,0.000017000000000,0.000133000000000,0.000153000000000,0.002442000000000,0.000831000000000,0.003670000000000,0.000000000000000,112.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1147.000000000000000,216.000000000000000,121.000000000000000,0.170524915168911,0.199849692332816,121.000000000000000,0.295777531500515
+0.000000000000000,0.000013000000000,0.000157000000000,0.000149000000000,0.001748000000000,0.000942000000000,0.003057000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1147.000000000000000,234.000000000000000,121.000000000000000,0.189221335752284,0.197743002271810,121.000000000000000,0.292659639941872
+0.000000000000000,0.000020000000000,0.000178000000000,0.000125000000000,0.001721000000000,0.000742000000000,0.002827000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1176.000000000000000,225.000000000000000,121.000000000000000,0.198900978356214,0.245389821232497,119.000000000000000,0.363176948895773
+0.000000000000000,0.000013000000000,0.000176000000000,0.000145000000000,0.001697000000000,0.000855000000000,0.002929000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1221.000000000000000,222.000000000000000,121.000000000000000,0.196051895442564,0.262432687056220,121.000000000000000,0.388400396935210
+0.000000000000000,0.000015000000000,0.000176000000000,0.000128000000000,0.001658000000000,0.000736000000000,0.002754000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1264.000000000000000,222.000000000000000,121.000000000000000,0.188130722176495,0.226906201366918,120.000000000000000,0.335821186914497
+0.000000000000000,0.000015000000000,0.000114000000000,0.000128000000000,0.001732000000000,0.000994000000000,0.003025000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1280.000000000000000,221.000000000000000,121.000000000000000,0.195205519657246,0.208004184809569,119.000000000000000,0.307846188328575
+0.000000000000000,0.000020000000000,0.000195000000000,0.000126000000000,0.002006000000000,0.000791000000000,0.003194000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1295.000000000000000,262.000000000000000,121.000000000000000,0.204929539974912,0.216820480192029,119.000000000000000,0.320894306981155
+0.000000000000000,0.000017000000000,0.000176000000000,0.000139000000000,0.001546000000000,0.000896000000000,0.002812000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1303.000000000000000,192.000000000000000,121.000000000000000,0.226704780607630,0.257512342665443,120.000000000000000,0.381118257356317
+0.000000000000000,0.000011000000000,0.000105000000000,0.000299000000000,0.001761000000000,0.001102000000000,0.003328000000000,0.000000000000000,114.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1303.000000000000000,234.000000000000000,121.000000000000000,0.206579202251784,0.240623999885611,121.000000000000000,0.356123519730790
+0.000000000000000,0.000020000000000,0.000103000000000,0.000130000000000,0.001498000000000,0.000871000000000,0.002766000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1312.000000000000000,207.000000000000000,121.000000000000000,0.208586912880041,0.214158703806542,121.000000000000000,0.316954877506999
+0.000000000000000,0.000017000000000,0.000176000000000,0.000374000000000,0.001665000000000,0.000976000000000,0.003373000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1139.000000000000000,223.000000000000000,121.000000000000000,0.209543770412094,0.213150938854858,121.000000000000000,0.315463382345879
+0.000000000000000,0.000021000000000,0.000157000000000,0.000124000000000,0.001652000000000,0.000947000000000,0.002947000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1137.000000000000000,212.000000000000000,121.000000000000000,0.214888927283406,0.261239649375707,120.000000000000000,0.386634701996087
+0.000000000000000,0.000018000000000,0.000104000000000,0.000124000000000,0.001479000000000,0.000897000000000,0.002661000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1182.000000000000000,188.000000000000000,121.000000000000000,0.205526459876717,0.194046988139053,121.000000000000000,0.287189543909738
+0.000000000000000,0.000016000000000,0.000175000000000,0.000142000000000,0.001509000000000,0.000744000000000,0.002626000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1230.000000000000000,190.000000000000000,121.000000000000000,0.193638964960760,0.209501566643394,121.000000000000000,0.310062329059690
+0.000000000000000,0.000022000000000,0.000103000000000,0.000125000000000,0.001561000000000,0.000725000000000,0.002572000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1250.000000000000000,218.000000000000000,121.000000000000000,0.192895828207804,0.213686959411965,121.000000000000000,0.316256716855654
+0.000000000000000,0.000023000000000,0.000184000000000,0.000162000000000,0.001899000000000,0.000812000000000,0.003140000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1258.000000000000000,187.000000000000000,121.000000000000000,0.178433372351327,0.196124969530122,120.000000000000000,0.290264953456693
+0.000000000000000,0.000016000000000,0.000134000000000,0.000140000000000,0.001663000000000,0.000876000000000,0.002874000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1274.000000000000000,203.000000000000000,121.000000000000000,0.204379056805537,0.211513754780143,119.000000000000000,0.313040366095618
+0.000000000000000,0.000020000000000,0.000176000000000,0.000130000000000,0.001587000000000,0.000869000000000,0.002823000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1284.000000000000000,206.000000000000000,121.000000000000000,0.194394953473234,0.214318086264921,121.000000000000000,0.317190765131688
+0.000000000000000,0.000017000000000,0.000107000000000,0.000148000000000,0.001575000000000,0.000805000000000,0.002693000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1286.000000000000000,205.000000000000000,121.000000000000000,0.192692252511560,0.217301832678175,121.000000000000000,0.321606720966583
+0.000000000000000,0.000020000000000,0.000173000000000,0.000124000000000,0.001507000000000,0.000785000000000,0.002649000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1278.000000000000000,210.000000000000000,121.000000000000000,0.204881580167579,0.231890569564824,121.000000000000000,0.343198045458178
+0.000000000000000,0.000041000000000,0.000220000000000,0.000144000000000,0.001900000000000,0.000655000000000,0.003038000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1278.000000000000000,206.000000000000000,121.000000000000000,0.174115401194611,0.223973720218107,121.000000000000000,0.331481122502030
+0.000000000000000,0.000043000000000,0.000182000000000,0.000145000000000,0.001457000000000,0.000728000000000,0.002734000000000,1.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1272.000000000000000,191.000000000000000,121.000000000000000,0.194538477548438,0.187220557909554,121.000000000000000,0.277086431960381
+0.000000000000000,0.000020000000000,0.000182000000000,0.000129000000000,0.001451000000000,0.000813000000000,0.002657000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1148.000000000000000,207.000000000000000,121.000000000000000,0.209547468410092,0.205381330935579,119.000000000000000,0.303964378715603
+0.000000000000000,0.000037000000000,0.000255000000000,0.000211000000000,0.002943000000000,0.001198000000000,0.004740000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1146.000000000000000,192.000000000000000,121.000000000000000,0.194112090334278,0.213834860708738,121.000000000000000,0.316475596512546
+0.000000000000000,0.000021000000000,0.000119000000000,0.000128000000000,0.001482000000000,0.000836000000000,0.002622000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1196.000000000000000,188.000000000000000,121.000000000000000,0.194707145965396,0.200711676435348,120.000000000000000,0.297053280716492
+0.000000000000000,0.000018000000000,0.000128000000000,0.000353000000000,0.001530000000000,0.001123000000000,0.003198000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1222.000000000000000,182.000000000000000,121.000000000000000,0.165297929068422,0.223118899091734,120.000000000000000,0.330215962199139
+0.000000000000000,0.000019000000000,0.000133000000000,0.000124000000000,0.001554000000000,0.001016000000000,0.002890000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1250.000000000000000,187.000000000000000,121.000000000000000,0.189732092265728,0.231665076279646,121.000000000000000,0.342864305156952
+0.000000000000000,0.000018000000000,0.000195000000000,0.000130000000000,0.001387000000000,0.000871000000000,0.002639000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1278.000000000000000,167.000000000000000,121.000000000000000,0.173396135722069,0.185374303680201,121.000000000000000,0.274353961674946
+0.000000000000000,0.000015000000000,0.000105000000000,0.000165000000000,0.001566000000000,0.001052000000000,0.002950000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1292.000000000000000,196.000000000000000,121.000000000000000,0.192868888286218,0.205034141945327,120.000000000000000,0.303450551076308
+0.000000000000000,0.000014000000000,0.000102000000000,0.000127000000000,0.001521000000000,0.000791000000000,0.002592000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1302.000000000000000,199.000000000000000,121.000000000000000,0.190528673137881,0.222906754530888,121.000000000000000,0.329902009771293
+0.000000000000000,0.000021000000000,0.000215000000000,0.000167000000000,0.001934000000000,0.000805000000000,0.003204000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1308.000000000000000,190.000000000000000,121.000000000000000,0.202526348712512,0.259278841345545,120.000000000000000,0.383732682741932
+0.000000000000000,0.000022000000000,0.000156000000000,0.000126000000000,0.001696000000000,0.000788000000000,0.002834000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1308.000000000000000,190.000000000000000,121.000000000000000,0.196451913404442,0.201069768524068,120.000000000000000,0.297583282351909
+0.000000000000000,0.000025000000000,0.000103000000000,0.000132000000000,0.001543000000000,0.000902000000000,0.002745000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1305.000000000000000,194.000000000000000,121.000000000000000,0.200871633124814,0.183908150343481,120.000000000000000,0.272184068463992
+0.000000000000000,0.000025000000000,0.000102000000000,0.000150000000000,0.001478000000000,0.000854000000000,0.002788000000000,1.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1335.000000000000000,179.000000000000000,121.000000000000000,0.193823047570574,0.212329442302877,119.000000000000000,0.314247578003029
+0.000000000000000,0.000016000000000,0.000196000000000,0.000126000000000,0.001299000000000,0.000752000000000,0.002455000000000,0.000000000000000,112.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1194.000000000000000,176.000000000000000,121.000000000000000,0.207923081779578,0.229508478265270,121.000000000000000,0.339672543925396
+0.000000000000000,0.000018000000000,0.000110000000000,0.000166000000000,0.001844000000000,0.000982000000000,0.003170000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1189.000000000000000,180.000000000000000,121.000000000000000,0.163342705330096,0.249575746286056,120.000000000000000,0.369372120119674
+0.000000000000000,0.000015000000000,0.000183000000000,0.000124000000000,0.001749000000000,0.000847000000000,0.002963000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1218.000000000000000,212.000000000000000,121.000000000000000,0.173881865983057,0.194395754595030,121.000000000000000,0.287705721832844
+0.000000000000000,0.000026000000000,0.000176000000000,0.000127000000000,0.001498000000000,0.000877000000000,0.002752000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1259.000000000000000,185.000000000000000,121.000000000000000,0.155523040470466,0.162766723518840,120.000000000000000,0.240894740082153
+0.000000000000000,0.000015000000000,0.000177000000000,0.000144000000000,0.001550000000000,0.000743000000000,0.002669000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1278.000000000000000,192.000000000000000,121.000000000000000,0.152934150143871,0.272911472202902,121.000000000000000,0.403908984747820
+0.000000000000000,0.000014000000000,0.000174000000000,0.000116000000000,0.001439000000000,0.000731000000000,0.002513000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1291.000000000000000,196.000000000000000,121.000000000000000,0.188113809380047,0.184618919088813,121.000000000000000,0.273236010008633
+0.000000000000000,0.000016000000000,0.000106000000000,0.000145000000000,0.002198000000000,0.000804000000000,0.003318000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1299.000000000000000,184.000000000000000,121.000000000000000,0.189391540723312,0.210953756830538,121.000000000000000,0.312211549881851
+0.000000000000000,0.000019000000000,0.000181000000000,0.000116000000000,0.001662000000000,0.000840000000000,0.002865000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1306.000000000000000,204.000000000000000,121.000000000000000,0.184488742976474,0.215925329354907,121.000000000000000,0.319569479795696
+0.000000000000000,0.000016000000000,0.000183000000000,0.000132000000000,0.001540000000000,0.000737000000000,0.002647000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1313.000000000000000,197.000000000000000,121.000000000000000,0.164667840588859,0.233194073999862,121.000000000000000,0.345127224117476
+0.000000000000000,0.000017000000000,0.000177000000000,0.000144000000000,0.001453000000000,0.000747000000000,0.002725000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1307.000000000000000,179.000000000000000,121.000000000000000,0.190996220324968,0.267097150771334,121.000000000000000,0.395303787014294
+0.000000000000000,0.000017000000000,0.000177000000000,0.000120000000000,0.001414000000000,0.000681000000000,0.002474000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1109.000000000000000,205.000000000000000,121.000000000000000,0.175048358387321,0.227273301247532,121.000000000000000,0.336364481552976
+0.000000000000000,0.000023000000000,0.000186000000000,0.000160000000000,0.001932000000000,0.000738000000000,0.003097000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1100.000000000000000,194.000000000000000,121.000000000000000,0.173557283673669,0.191658131610694,121.000000000000000,0.283654034716855
+0.000000000000000,0.000019000000000,0.000107000000000,0.000116000000000,0.001671000000000,0.000745000000000,0.002708000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1144.000000000000000,204.000000000000000,121.000000000000000,0.158568907261423,0.243359531069700,121.000000000000000,0.360172098406900
+0.000000000000000,0.000018000000000,0.000103000000000,0.000132000000000,0.001512000000000,0.000732000000000,0.002547000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1187.000000000000000,194.000000000000000,121.000000000000000,0.146749823653506,0.164413086505490,121.000000000000000,0.243331352597904
+0.000000000000000,0.000014000000000,0.000181000000000,0.000144000000000,0.001537000000000,0.000892000000000,0.002808000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1214.000000000000000,196.000000000000000,121.000000000000000,0.131283592229348,0.155956326816494,121.000000000000000,0.230815358417731
+0.000000000000000,0.000012000000000,0.000176000000000,0.000121000000000,0.001482000000000,0.000877000000000,0.002705000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1244.000000000000000,196.000000000000000,121.000000000000000,0.159033296053125,0.194395333842714,121.000000000000000,0.287705090036121
+0.000000000000000,0.000020000000000,0.000211000000000,0.000162000000000,0.001881000000000,0.000840000000000,0.003174000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1262.000000000000000,184.000000000000000,121.000000000000000,0.163845159646448,0.177604150932044,121.000000000000000,0.262854149344864
+0.000000000000000,0.000015000000000,0.000115000000000,0.000124000000000,0.001771000000000,0.000924000000000,0.002997000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1269.000000000000000,218.000000000000000,121.000000000000000,0.167741708685618,0.187775885781358,121.000000000000000,0.277908323682058
+0.000000000000000,0.000012000000000,0.000104000000000,0.000127000000000,0.001764000000000,0.000780000000000,0.002830000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1278.000000000000000,229.000000000000000,121.000000000000000,0.186347950475868,0.251813866360726,121.000000000000000,0.372684503981683
+0.000000000000000,0.000020000000000,0.000102000000000,0.000142000000000,0.001444000000000,0.000872000000000,0.002621000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1275.000000000000000,174.000000000000000,121.000000000000000,0.166303486197133,0.189747084068580,120.000000000000000,0.280825682960661
+0.000000000000000,0.000016000000000,0.000125000000000,0.000117000000000,0.001504000000000,0.000769000000000,0.002568000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1276.000000000000000,202.000000000000000,121.000000000000000,0.167769122437661,0.195787376215580,121.000000000000000,0.289765303335874
+0.000000000000000,0.000029000000000,0.000158000000000,0.000154000000000,0.001818000000000,0.000771000000000,0.003044000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1304.000000000000000,177.000000000000000,121.000000000000000,0.158463858246833,0.191255881763862,119.000000000000000,0.283058705300228
+0.000000000000000,0.000020000000000,0.000185000000000,0.000118000000000,0.001683000000000,0.000786000000000,0.002860000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1339.000000000000000,206.000000000000000,121.000000000000000,0.161183566619254,0.178028171732775,121.000000000000000,0.263481700394391
+0.000000000000000,0.000017000000000,0.000177000000000,0.000136000000000,0.001597000000000,0.000782000000000,0.002874000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1383.000000000000000,198.000000000000000,121.000000000000000,0.142916686874711,0.162123928033689,121.000000000000000,0.239943431119633
+0.000000000000000,0.000016000000000,0.000127000000000,0.000234000000000,0.001629000000000,0.000716000000000,0.002906000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1223.000000000000000,214.000000000000000,121.000000000000000,0.154355717762879,0.218609409653489,121.000000000000000,0.323541914330688
+0.000000000000000,0.000018000000000,0.000175000000000,0.000117000000000,0.001623000000000,0.000874000000000,0.002848000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1218.000000000000000,227.000000000000000,121.000000000000000,0.174558038196839,0.199124535422790,121.000000000000000,0.294704311042227
+0.000000000000000,0.000023000000000,0.000101000000000,0.000109000000000,0.001533000000000,0.000703000000000,0.002504000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1228.000000000000000,215.000000000000000,121.000000000000000,0.137995003938898,0.152632389051692,121.000000000000000,0.225895936409808
+0.000000000000000,0.000027000000000,0.000177000000000,0.000139000000000,0.001775000000000,0.000662000000000,0.002820000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1232.000000000000000,237.000000000000000,121.000000000000000,0.135541904251767,0.167668386817417,121.000000000000000,0.248149206964557
+0.000000000000000,0.000022000000000,0.000181000000000,0.000307000000000,0.001635000000000,0.001033000000000,0.003237000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1233.000000000000000,200.000000000000000,121.000000000000000,0.163536557849499,0.206044880204936,121.000000000000000,0.304946443542624
+0.000000000000000,0.000013000000000,0.000197000000000,0.000138000000000,0.001693000000000,0.000665000000000,0.002750000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1227.000000000000000,224.000000000000000,121.000000000000000,0.153373223673703,0.199555553830275,120.000000000000000,0.295342223558018
+0.000000000000000,0.000022000000000,0.000130000000000,0.000127000000000,0.001747000000000,0.000744000000000,0.002816000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1223.000000000000000,234.000000000000000,121.000000000000000,0.139032943637894,0.186670354479152,121.000000000000000,0.276272121799386
+0.000000000000000,0.000023000000000,0.000189000000000,0.000117000000000,0.001436000000000,0.000905000000000,0.002707000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1223.000000000000000,191.000000000000000,121.000000000000000,0.139072087006321,0.170137357542569,121.000000000000000,0.251803291766507
+0.000000000000000,0.000019000000000,0.000189000000000,0.000134000000000,0.001636000000000,0.001043000000000,0.003059000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1226.000000000000000,205.000000000000000,121.000000000000000,0.147221191440148,0.219839242753776,121.000000000000000,0.325362095419246
+0.000000000000000,0.000026000000000,0.000136000000000,0.000141000000000,0.001640000000000,0.000834000000000,0.002814000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1226.000000000000000,198.000000000000000,121.000000000000000,0.134535729635055,0.177361976863745,120.000000000000000,0.262495743699553
+0.000000000000000,0.000024000000000,0.000188000000000,0.000137000000000,0.001576000000000,0.000999000000000,0.002968000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1224.000000000000000,202.000000000000000,121.000000000000000,0.150440594097383,0.170822892372034,120.000000000000000,0.252817881488855
+0.000000000000000,0.000018000000000,0.000128000000000,0.000123000000000,0.001595000000000,0.000906000000000,0.002923000000000,1.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1223.000000000000000,211.000000000000000,121.000000000000000,0.161157776486683,0.176850558454008,121.000000000000000,0.261738825952921
+0.000000000000000,0.000023000000000,0.000171000000000,0.000136000000000,0.001546000000000,0.000836000000000,0.002772000000000,0.000000000000000,112.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1094.000000000000000,211.000000000000000,121.000000000000000,0.135044752661059,0.176661372259443,121.000000000000000,0.261458838916931
+0.000000000000000,0.000016000000000,0.000180000000000,0.000129000000000,0.001692000000000,0.000849000000000,0.002919000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1089.000000000000000,203.000000000000000,121.000000000000000,0.143949354754192,0.167668782751111,120.000000000000000,0.248149788217542
+0.000000000000000,0.000017000000000,0.000108000000000,0.000139000000000,0.001568000000000,0.000704000000000,0.002578000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1166.000000000000000,200.000000000000000,121.000000000000000,0.155160670749007,0.231930876186467,120.000000000000000,0.343257712340746
+0.000000000000000,0.000035000000000,0.000227000000000,0.000132000000000,0.001664000000000,0.000790000000000,0.002968000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1216.000000000000000,195.000000000000000,121.000000000000000,0.133676069237326,0.147626375091364,120.000000000000000,0.218487058408179
+0.000000000000000,0.000015000000000,0.000128000000000,0.000120000000000,0.001551000000000,0.000732000000000,0.002590000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1232.000000000000000,204.000000000000000,121.000000000000000,0.122400626475427,0.146223124911287,121.000000000000000,0.216410216218663
+0.000000000000000,0.000020000000000,0.000245000000000,0.000118000000000,0.001618000000000,0.000770000000000,0.002815000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1238.000000000000000,219.000000000000000,121.000000000000000,0.137303784430655,0.150577933818061,121.000000000000000,0.222855351498252
+0.000000000000000,0.000016000000000,0.000214000000000,0.000131000000000,0.001570000000000,0.000888000000000,0.002875000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1239.000000000000000,183.000000000000000,121.000000000000000,0.131464957645487,0.148026075188344,121.000000000000000,0.219078597044465
+0.000000000000000,0.000018000000000,0.000103000000000,0.000139000000000,0.001556000000000,0.000929000000000,0.002796000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1234.000000000000000,197.000000000000000,121.000000000000000,0.128347170794260,0.131002024287506,121.000000000000000,0.193882998604601
+0.000000000000000,0.000034000000000,0.000281000000000,0.000139000000000,0.001662000000000,0.000725000000000,0.002956000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1226.000000000000000,194.000000000000000,121.000000000000000,0.162024538919116,0.207094523301695,121.000000000000000,0.306499905326386
+0.000000000000000,0.000019000000000,0.000198000000000,0.000119000000000,0.001621000000000,0.000844000000000,0.002973000000000,1.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1223.000000000000000,215.000000000000000,121.000000000000000,0.153235488057770,0.262436341625619,121.000000000000000,0.388405779843294
+0.000000000000000,0.000021000000000,0.000198000000000,0.000131000000000,0.001561000000000,0.000847000000000,0.002828000000000,0.000000000000000,114.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1054.000000000000000,201.000000000000000,121.000000000000000,0.157919685289792,0.167838875425546,121.000000000000000,0.248401521304057
+0.000000000000000,0.000015000000000,0.000170000000000,0.000130000000000,0.001648000000000,0.000666000000000,0.002683000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1048.000000000000000,211.000000000000000,121.000000000000000,0.142424009465490,0.209332762567431,120.000000000000000,0.309812491363346
+0.000000000000000,0.000020000000000,0.000127000000000,0.000139000000000,0.001528000000000,0.000726000000000,0.002588000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1039.000000000000000,207.000000000000000,121.000000000000000,0.140603779602412,0.159626649619649,121.000000000000000,0.236247445558979
+0.000000000000000,0.000019000000000,0.000194000000000,0.000143000000000,0.001999000000000,0.000624000000000,0.003025000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1040.000000000000000,205.000000000000000,121.000000000000000,0.147458908627836,0.237281094131071,121.000000000000000,0.351176020676474
+0.000000000000000,0.000022000000000,0.000175000000000,0.000126000000000,0.001454000000000,0.000837000000000,0.002656000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1106.000000000000000,198.000000000000000,121.000000000000000,0.125862132363062,0.146166756283214,120.000000000000000,0.216326793779291
+0.000000000000000,0.000018000000000,0.000233000000000,0.000119000000000,0.001470000000000,0.000923000000000,0.002807000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1145.000000000000000,188.000000000000000,121.000000000000000,0.148866800739627,0.175671536734951,121.000000000000000,0.259993879218198
+0.000000000000000,0.000021000000000,0.000205000000000,0.000128000000000,0.001543000000000,0.000761000000000,0.002708000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1156.000000000000000,186.000000000000000,121.000000000000000,0.127848678819928,0.170790549426913,121.000000000000000,0.252770016569082
+0.000000000000000,0.000016000000000,0.000176000000000,0.000145000000000,0.001515000000000,0.000719000000000,0.002758000000000,1.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1165.000000000000000,189.000000000000000,121.000000000000000,0.145912191034524,0.246210134411681,121.000000000000000,0.364391034751536
+0.000000000000000,0.000017000000000,0.000137000000000,0.000142000000000,0.001535000000000,0.000625000000000,0.002544000000000,0.000000000000000,112.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1007.000000000000000,197.000000000000000,121.000000000000000,0.155415123408675,0.250107101898265,121.000000000000000,0.370158504865490
+0.000000000000000,0.000018000000000,0.000187000000000,0.000129000000000,0.001485000000000,0.000745000000000,0.002609000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,997.000000000000000,196.000000000000000,121.000000000000000,0.156330994449520,0.195977327324711,121.000000000000000,0.290046452877835
+0.000000000000000,0.000027000000000,0.000216000000000,0.000122000000000,0.001463000000000,0.000715000000000,0.002603000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,994.000000000000000,202.000000000000000,121.000000000000000,0.148181823678736,0.163960783045368,121.000000000000000,0.242661951333412
+0.000000000000000,0.000021000000000,0.000148000000000,0.000131000000000,0.001726000000000,0.000788000000000,0.002871000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,990.000000000000000,214.000000000000000,121.000000000000000,0.159041550968813,0.290710356872846,121.000000000000000,0.430251294240852
+0.000000000000000,0.000014000000000,0.000109000000000,0.000146000000000,0.001782000000000,0.000832000000000,0.002935000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,989.000000000000000,237.000000000000000,121.000000000000000,0.144598980992546,0.166027958406800,121.000000000000000,0.245721363787328
+0.000000000000000,0.000033000000000,0.000225000000000,0.000141000000000,0.001562000000000,0.000724000000000,0.002797000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,989.000000000000000,200.000000000000000,121.000000000000000,0.138993500529387,0.196990361228138,121.000000000000000,0.291545731774589
+0.000000000000000,0.000018000000000,0.000102000000000,0.000130000000000,0.001577000000000,0.000768000000000,0.002776000000000,1.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1025.000000000000000,218.000000000000000,121.000000000000000,0.159928213604770,0.189984919538914,121.000000000000000,0.281177669551262
+0.000000000000000,0.000012000000000,0.000247000000000,0.000124000000000,0.001584000000000,0.000607000000000,0.002648000000000,0.000000000000000,108.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,912.000000000000000,232.000000000000000,121.000000000000000,0.169367654114597,0.221725877325533,121.000000000000000,0.328154308402856
+0.000000000000000,0.000029000000000,0.000128000000000,0.000128000000000,0.001642000000000,0.000782000000000,0.002760000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,911.000000000000000,211.000000000000000,121.000000000000000,0.147968518377667,0.154122927463339,121.000000000000000,0.228101943121478
+0.000000000000000,0.000016000000000,0.000176000000000,0.000142000000000,0.001527000000000,0.000917000000000,0.002821000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,946.000000000000000,207.000000000000000,121.000000000000000,0.137541055415491,0.157761641471241,121.000000000000000,0.233487226761447
+0.000000000000000,0.000032000000000,0.000147000000000,0.000136000000000,0.001548000000000,0.000751000000000,0.002712000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,954.000000000000000,199.000000000000000,121.000000000000000,0.162415187446687,0.197580421079148,121.000000000000000,0.292419026477691
+0.000000000000000,0.000019000000000,0.000178000000000,0.000129000000000,0.001473000000000,0.000728000000000,0.002570000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,952.000000000000000,191.000000000000000,121.000000000000000,0.171041015417160,0.194915117263576,121.000000000000000,0.288474365726664
+0.000000000000000,0.000029000000000,0.000197000000000,0.000115000000000,0.001354000000000,0.000809000000000,0.002551000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,965.000000000000000,175.000000000000000,121.000000000000000,0.154222763807780,0.285544163762687,120.000000000000000,0.422605366108563
+0.000000000000000,0.000019000000000,0.000139000000000,0.000125000000000,0.001460000000000,0.000814000000000,0.002600000000000,0.000000000000000,114.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,968.000000000000000,171.000000000000000,121.000000000000000,0.150595603214115,0.221852768512487,121.000000000000000,0.328342103660952
+0.000000000000000,0.000013000000000,0.000104000000000,0.000134000000000,0.001356000000000,0.000752000000000,0.002398000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,996.000000000000000,168.000000000000000,121.000000000000000,0.147430237486138,0.148159285827304,121.000000000000000,0.219275742894069
+0.000000000000000,0.000020000000000,0.000193000000000,0.000142000000000,0.001953000000000,0.000947000000000,0.003301000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1030.000000000000000,177.000000000000000,121.000000000000000,0.145798815037739,0.159663663944367,121.000000000000000,0.236302234970841
+0.000000000000000,0.000015000000000,0.000103000000000,0.000115000000000,0.001290000000000,0.000699000000000,0.002255000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1048.000000000000000,172.000000000000000,121.000000000000000,0.142196014156506,0.187147109561421,121.000000000000000,0.276977737652069
+0.000000000000000,0.000024000000000,0.000160000000000,0.000150000000000,0.001811000000000,0.000777000000000,0.003271000000000,1.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1069.000000000000000,202.000000000000000,121.000000000000000,0.139609449101900,0.165271225439012,121.000000000000000,0.244601415643369
+0.000000000000000,0.000018000000000,0.000179000000000,0.000151000000000,0.001532000000000,0.000737000000000,0.002693000000000,0.000000000000000,97.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,912.000000000000000,203.000000000000000,121.000000000000000,0.147947477270869,0.179555832175793,121.000000000000000,0.265742648149137
+0.000000000000000,0.000019000000000,0.000177000000000,0.000125000000000,0.001512000000000,0.000717000000000,0.002588000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,911.000000000000000,212.000000000000000,121.000000000000000,0.147575394987053,0.165266294276773,121.000000000000000,0.244594112073245
+0.000000000000000,0.000016000000000,0.000106000000000,0.000137000000000,0.001490000000000,0.000826000000000,0.002618000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,915.000000000000000,203.000000000000000,121.000000000000000,0.154152939014589,0.159181033056470,121.000000000000000,0.235587925595243
+0.000000000000000,0.000013000000000,0.000177000000000,0.000117000000000,0.001355000000000,0.000751000000000,0.002448000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,921.000000000000000,192.000000000000000,121.000000000000000,0.150692609588339,0.183809895950044,121.000000000000000,0.272038654130109
+0.000000000000000,0.000023000000000,0.000221000000000,0.000167000000000,0.001790000000000,0.000948000000000,0.003199000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,932.000000000000000,170.000000000000000,121.000000000000000,0.149963553230709,0.203273297272569,121.000000000000000,0.300844465407444
+0.000000000000000,0.000026000000000,0.000151000000000,0.000122000000000,0.001357000000000,0.000723000000000,0.002422000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,961.000000000000000,168.000000000000000,121.000000000000000,0.142178288778563,0.151033799800819,121.000000000000000,0.223530034583352
+0.000000000000000,0.000018000000000,0.000155000000000,0.000123000000000,0.001394000000000,0.000766000000000,0.002492000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,986.000000000000000,182.000000000000000,121.000000000000000,0.141207634148801,0.172667793318101,121.000000000000000,0.255548330024774
+0.000000000000000,0.000018000000000,0.000174000000000,0.000143000000000,0.001389000000000,0.000790000000000,0.002550000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1023.000000000000000,171.000000000000000,121.000000000000000,0.140177814193211,0.162740001476768,121.000000000000000,0.240855214879131
+0.000000000000000,0.000021000000000,0.000173000000000,0.000122000000000,0.001280000000000,0.000796000000000,0.002581000000000,1.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1049.000000000000000,171.000000000000000,121.000000000000000,0.138041804635574,0.175894893269503,121.000000000000000,0.260324460535807
+0.000000000000000,0.000040000000000,0.000240000000000,0.000156000000000,0.001610000000000,0.000905000000000,0.003078000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,887.000000000000000,181.000000000000000,121.000000000000000,0.128640393991057,0.132619727567557,121.000000000000000,0.196277204924269
+0.000000000000000,0.000016000000000,0.000181000000000,0.000117000000000,0.001424000000000,0.000760000000000,0.002543000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,886.000000000000000,172.000000000000000,121.000000000000000,0.138352531751683,0.243902625004102,121.000000000000000,0.360975895470416
+0.000000000000000,0.000020000000000,0.000178000000000,0.000131000000000,0.001344000000000,0.000965000000000,0.002674000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,904.000000000000000,177.000000000000000,121.000000000000000,0.132849298892330,0.165943140041726,121.000000000000000,0.245595863686171
+0.000000000000000,0.000018000000000,0.000129000000000,0.000145000000000,0.001297000000000,0.000878000000000,0.002506000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,916.000000000000000,164.000000000000000,121.000000000000000,0.139100399764650,0.179464095411716,121.000000000000000,0.265606862397320
+0.000000000000000,0.000015000000000,0.000175000000000,0.000119000000000,0.001188000000000,0.000717000000000,0.002249000000000,0.000000000000000,116.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,926.000000000000000,158.000000000000000,121.000000000000000,0.124508577433483,0.232688205210390,121.000000000000000,0.344378545000118
+0.000000000000000,0.000041000000000,0.000173000000000,0.000189000000000,0.001590000000000,0.000722000000000,0.002820000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,950.000000000000000,167.000000000000000,121.000000000000000,0.124364211160562,0.198989442345874,121.000000000000000,0.294504385287040
+0.000000000000000,0.000022000000000,0.000180000000000,0.000121000000000,0.001454000000000,0.000762000000000,0.002586000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,981.000000000000000,174.000000000000000,121.000000000000000,0.121986273502660,0.181985786779231,121.000000000000000,0.269338961458690
+0.000000000000000,0.000022000000000,0.000175000000000,0.000128000000000,0.001283000000000,0.000822000000000,0.002468000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1006.000000000000000,154.000000000000000,121.000000000000000,0.120933033868538,0.208757551950541,121.000000000000000,0.308961183186222
+0.000000000000000,0.000026000000000,0.000175000000000,0.000143000000000,0.001286000000000,0.000813000000000,0.002485000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1029.000000000000000,160.000000000000000,121.000000000000000,0.109269904153490,0.130322654376541,121.000000000000000,0.192877532026941
+0.000000000000000,0.000023000000000,0.000175000000000,0.000120000000000,0.001254000000000,0.000637000000000,0.002411000000000,1.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1040.000000000000000,160.000000000000000,121.000000000000000,0.126897062957379,0.186841162125259,121.000000000000000,0.276524916304396
+0.000000000000000,0.000021000000000,0.000140000000000,0.000122000000000,0.001471000000000,0.000691000000000,0.002520000000000,0.000000000000000,113.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,891.000000000000000,190.000000000000000,121.000000000000000,0.112694197899670,0.185521960041402,121.000000000000000,0.274572512797542
+0.000000000000000,0.000020000000000,0.000176000000000,0.000199000000000,0.001263000000000,0.000879000000000,0.002581000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,888.000000000000000,161.000000000000000,121.000000000000000,0.099793871260750,0.108940668612828,121.000000000000000,0.161232198827340
+0.000000000000000,0.000018000000000,0.000103000000000,0.000137000000000,0.001309000000000,0.000726000000000,0.002333000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,901.000000000000000,171.000000000000000,121.000000000000000,0.091727983876207,0.161347683449736,121.000000000000000,0.238794571957605
+0.000000000000000,0.000018000000000,0.000175000000000,0.000143000000000,0.001336000000000,0.000860000000000,0.002571000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,909.000000000000000,175.000000000000000,121.000000000000000,0.124196463571817,0.147059251648374,121.000000000000000,0.217647703825064
+0.000000000000000,0.000019000000000,0.000174000000000,0.000125000000000,0.001265000000000,0.000845000000000,0.002464000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,911.000000000000000,175.000000000000000,121.000000000000000,0.114265472855587,0.148521989180687,121.000000000000000,0.219812542662230
+0.000000000000000,0.000030000000000,0.000158000000000,0.000151000000000,0.001613000000000,0.000756000000000,0.002826000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,943.000000000000000,170.000000000000000,121.000000000000000,0.092242007337942,0.147927216635033,121.000000000000000,0.218932272923301
+0.000000000000000,0.000017000000000,0.000106000000000,0.000120000000000,0.001308000000000,0.001030000000000,0.002622000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,970.000000000000000,159.000000000000000,121.000000000000000,0.106941604326635,0.142972503313410,121.000000000000000,0.211599311615969
+0.000000000000000,0.000021000000000,0.000127000000000,0.000130000000000,0.001305000000000,0.000748000000000,0.002368000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,989.000000000000000,164.000000000000000,121.000000000000000,0.114683633251297,0.167145956283516,121.000000000000000,0.247376014134327
+0.000000000000000,0.000023000000000,0.000135000000000,0.000297000000000,0.001353000000000,0.000968000000000,0.002824000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1014.000000000000000,170.000000000000000,121.000000000000000,0.103049615014965,0.141054167934770,121.000000000000000,0.208760168771737
+0.000000000000000,0.000020000000000,0.000101000000000,0.000125000000000,0.001114000000000,0.000766000000000,0.002163000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1033.000000000000000,149.000000000000000,121.000000000000000,0.088395574941732,0.099895628586074,121.000000000000000,0.147845538218435
+0.000000000000000,0.000023000000000,0.000190000000000,0.000110000000000,0.001226000000000,0.000774000000000,0.002371000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1048.000000000000000,162.000000000000000,121.000000000000000,0.098469632616480,0.121321004607324,121.000000000000000,0.179555087237014
+0.000000000000000,0.000022000000000,0.000180000000000,0.000138000000000,0.001374000000000,0.001109000000000,0.002867000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1058.000000000000000,162.000000000000000,121.000000000000000,0.106165831669812,0.125397650867987,120.000000000000000,0.185588518499696
+0.000000000000000,0.000022000000000,0.000177000000000,0.000138000000000,0.001328000000000,0.000617000000000,0.002320000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1063.000000000000000,154.000000000000000,121.000000000000000,0.100439787680905,0.214221605374360,121.000000000000000,0.317047979072175
+0.000000000000000,0.000019000000000,0.000114000000000,0.000131000000000,0.001262000000000,0.000730000000000,0.002294000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1073.000000000000000,151.000000000000000,121.000000000000000,0.101286833254115,0.171921771357668,121.000000000000000,0.254444227342640
+0.000000000000000,0.000018000000000,0.000111000000000,0.000120000000000,0.001321000000000,0.000814000000000,0.002422000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1075.000000000000000,159.000000000000000,121.000000000000000,0.100562747826060,0.189141833234101,121.000000000000000,0.279929921566071
+0.000000000000000,0.000020000000000,0.000137000000000,0.000115000000000,0.001291000000000,0.000697000000000,0.002305000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1102.000000000000000,170.000000000000000,121.000000000000000,0.108513981039663,0.171230581801412,121.000000000000000,0.253421272631674
+0.000000000000000,0.000023000000000,0.000142000000000,0.000126000000000,0.001300000000000,0.000836000000000,0.002476000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1133.000000000000000,146.000000000000000,121.000000000000000,0.093525447667265,0.136448459114485,121.000000000000000,0.201943713921807
+0.000000000000000,0.000018000000000,0.000175000000000,0.000137000000000,0.001338000000000,0.000746000000000,0.002454000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1153.000000000000000,167.000000000000000,121.000000000000000,0.098830315127647,0.135984610426414,121.000000000000000,0.201257228873912
+0.000000000000000,0.000016000000000,0.000153000000000,0.000133000000000,0.001342000000000,0.000846000000000,0.002529000000000,0.000000000000000,115.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1169.000000000000000,168.000000000000000,121.000000000000000,0.104911664860530,0.116528427363267,121.000000000000000,0.172462069689336
+0.000000000000000,0.000016000000000,0.000177000000000,0.000153000000000,0.001420000000000,0.001057000000000,0.002862000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1189.000000000000000,171.000000000000000,121.000000000000000,0.112357270101045,0.155883824635645,121.000000000000000,0.230708054062211
+0.000000000000000,0.000019000000000,0.000183000000000,0.000118000000000,0.001173000000000,0.000692000000000,0.002218000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1205.000000000000000,160.000000000000000,121.000000000000000,0.114134537418471,0.211839504506888,121.000000000000000,0.313522477539393
+0.000000000000000,0.000022000000000,0.000144000000000,0.000340000000000,0.002673000000000,0.001050000000000,0.004300000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1210.000000000000000,169.000000000000000,121.000000000000000,0.118247530316238,0.220707314595291,121.000000000000000,0.326646841420566
+0.000000000000000,0.000021000000000,0.000121000000000,0.000119000000000,0.001312000000000,0.000708000000000,0.002323000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1221.000000000000000,159.000000000000000,121.000000000000000,0.108077994964789,0.189119078021565,121.000000000000000,0.279896234164773
+0.000000000000000,0.000022000000000,0.000131000000000,0.000170000000000,0.001575000000000,0.000930000000000,0.002873000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1227.000000000000000,161.000000000000000,121.000000000000000,0.111392372838571,0.157100432720142,121.000000000000000,0.232508649452340
+0.000000000000000,0.000021000000000,0.000129000000000,0.000146000000000,0.001357000000000,0.001069000000000,0.002758000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1238.000000000000000,169.000000000000000,121.000000000000000,0.115843692072703,0.148606734789863,121.000000000000000,0.219937966947781
+0.000000000000000,0.000018000000000,0.000196000000000,0.000246000000000,0.003021000000000,0.001245000000000,0.004778000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1237.000000000000000,170.000000000000000,121.000000000000000,0.110486981424295,0.140524548316829,121.000000000000000,0.207976323848749
+0.000000000000000,0.000019000000000,0.000176000000000,0.000120000000000,0.001319000000000,0.000781000000000,0.002451000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1234.000000000000000,166.000000000000000,121.000000000000000,0.116858932300554,0.142202739311708,121.000000000000000,0.210460043128204
+0.000000000000000,0.000023000000000,0.000194000000000,0.000117000000000,0.001352000000000,0.000677000000000,0.002411000000000,0.000000000000000,121.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1229.000000000000000,165.000000000000000,121.000000000000000,0.110055898076008,0.160160155663640,121.000000000000000,0.237037039832150
+0.000000000000000,0.000019000000000,0.000128000000000,0.000129000000000,0.001375000000000,0.000783000000000,0.002473000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1230.000000000000000,170.000000000000000,121.000000000000000,0.126041915882071,0.160651736154074,121.000000000000000,0.237764591667073
+0.000000000000000,0.000024000000000,0.000199000000000,0.000203000000000,0.001799000000000,0.000878000000000,0.003158000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1227.000000000000000,182.000000000000000,121.000000000000000,0.131395777815393,0.222014190834195,121.000000000000000,0.328581023909914
+0.000000000000000,0.000019000000000,0.000103000000000,0.000125000000000,0.001415000000000,0.000879000000000,0.002580000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1227.000000000000000,183.000000000000000,121.000000000000000,0.135634727713905,0.187089754575309,121.000000000000000,0.276892849444298
+0.000000000000000,0.000017000000000,0.000101000000000,0.000119000000000,0.001263000000000,0.000774000000000,0.002307000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1226.000000000000000,171.000000000000000,121.000000000000000,0.140713971719655,0.131521683676958,121.000000000000000,0.194652084756127
+0.000000000000000,0.000016000000000,0.000127000000000,0.000147000000000,0.001449000000000,0.000744000000000,0.002523000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1225.000000000000000,178.000000000000000,121.000000000000000,0.139061294677762,0.203192935815326,121.000000000000000,0.300725561264080
+0.000000000000000,0.000018000000000,0.000191000000000,0.000120000000000,0.001241000000000,0.000897000000000,0.002515000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1221.000000000000000,165.000000000000000,121.000000000000000,0.137698280788293,0.173674353597993,121.000000000000000,0.257038030883200
+0.000000000000000,0.000018000000000,0.000177000000000,0.000127000000000,0.001350000000000,0.000790000000000,0.002651000000000,1.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1209.000000000000000,165.000000000000000,121.000000000000000,0.156706447381789,0.199509549579289,121.000000000000000,0.295274141143093
+0.000000000000000,0.000016000000000,0.000153000000000,0.000147000000000,0.001501000000000,0.000734000000000,0.002643000000000,0.000000000000000,112.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1046.000000000000000,206.000000000000000,121.000000000000000,0.145254964747455,0.192623491876607,121.000000000000000,0.285082754555528
+0.000000000000000,0.000017000000000,0.000175000000000,0.000117000000000,0.001356000000000,0.000838000000000,0.002539000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1044.000000000000000,191.000000000000000,121.000000000000000,0.154344725976771,0.284210388757305,121.000000000000000,0.420631380425634
+0.000000000000000,0.000021000000000,0.000118000000000,0.000127000000000,0.001456000000000,0.000725000000000,0.002492000000000,0.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1039.000000000000000,193.000000000000000,121.000000000000000,0.154361389589804,0.236960337646421,121.000000000000000,0.350701313890299
+0.000000000000000,0.000018000000000,0.000108000000000,0.000128000000000,0.001548000000000,0.000868000000000,0.002721000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1036.000000000000000,190.000000000000000,121.000000000000000,0.144847037730729,0.155773166896081,121.000000000000000,0.230544292351477
+0.000000000000000,0.000018000000000,0.000129000000000,0.000124000000000,0.001383000000000,0.000975000000000,0.002669000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1027.000000000000000,183.000000000000000,121.000000000000000,0.145841805810321,0.158516249676071,121.000000000000000,0.234604041193658
+0.000000000000000,0.000028000000000,0.000216000000000,0.000131000000000,0.001615000000000,0.000765000000000,0.002878000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1020.000000000000000,192.000000000000000,121.000000000000000,0.144426676201549,0.156437471874113,121.000000000000000,0.231527443868863
+0.000000000000000,0.000021000000000,0.000153000000000,0.000120000000000,0.001406000000000,0.000771000000000,0.002509000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1015.000000000000000,188.000000000000000,121.000000000000000,0.147869059454709,0.168692675134760,120.000000000000000,0.249665165294811
+0.000000000000000,0.000021000000000,0.000170000000000,0.000115000000000,0.001425000000000,0.000608000000000,0.002395000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1012.000000000000000,187.000000000000000,121.000000000000000,0.168987279193652,0.289264692226178,121.000000000000000,0.428111727272626
+0.000000000000000,0.000017000000000,0.000135000000000,0.000127000000000,0.001615000000000,0.000791000000000,0.002734000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1011.000000000000000,192.000000000000000,121.000000000000000,0.170578883496370,0.166353065557405,121.000000000000000,0.246202540171952
+0.000000000000000,0.000019000000000,0.000178000000000,0.000136000000000,0.001520000000000,0.000741000000000,0.002636000000000,0.000000000000000,118.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,1005.000000000000000,203.000000000000000,121.000000000000000,0.156640038600278,0.207055055847171,121.000000000000000,0.306441476765396
+0.000000000000000,0.000033000000000,0.000177000000000,0.000135000000000,0.001641000000000,0.000861000000000,0.003288000000000,1.000000000000000,120.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,999.000000000000000,186.000000000000000,121.000000000000000,0.143274685502394,0.179257874186524,121.000000000000000,0.265301654036128
+0.000000000000000,0.000015000000000,0.000129000000000,0.000142000000000,0.001443000000000,0.000740000000000,0.002555000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,821.000000000000000,202.000000000000000,121.000000000000000,0.156881076812533,0.218447877056405,121.000000000000000,0.323302867722382
+0.000000000000000,0.000036000000000,0.000177000000000,0.000151000000000,0.001530000000000,0.000725000000000,0.002741000000000,0.000000000000000,117.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,820.000000000000000,168.000000000000000,121.000000000000000,0.155951030020751,0.213023655758012,121.000000000000000,0.315275031106667
+0.000000000000000,0.000030000000000,0.000196000000000,0.000128000000000,0.001426000000000,0.000831000000000,0.002660000000000,0.000000000000000,119.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,-1.000000000000000,816.000000000000000

CPU_version/svo/test/data/.gitignore

@@ -0,0 +1,2 @@
+.gitignore
+!.gitignore

CPU_version/svo/test/results/.gitignore

@@ -0,0 +1,2 @@
+.gitignore
+!.gitignore

CPU_version/svo/test/test_depth_filter.cpp

@@ -0,0 +1,206 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <list>
+#include <vector>
+#include <string>
+#include <boost/bind.hpp>
+#include <boost/make_shared.hpp>
+#include <boost/shared_ptr.hpp>
+#include <vikit/pinhole_camera.h>
+#include <vikit/abstract_camera.h>
+#include <vikit/atan_camera.h>
+#include <vikit/math_utils.h>
+#include <vikit/file_reader.h>
+#include <vikit/blender_utils.h>
+#include <vikit/timer.h>
+#include <iostream>
+#include <svo/feature_detection.h>
+#include <svo/depth_filter.h>
+#include <svo/frame.h>
+#include <svo/feature.h>
+#include <svo/point.h>
+#include <svo/config.h>
+#include "test_utils.h"
+
+namespace {
+
+struct ConvergedSeed {
+  int x_, y_;
+  double depth_, error_;
+  ConvergedSeed(int x, int y, double depth, double error) :
+    x_(x), y_(y), depth_(depth), error_(error)
+  {}
+};
+
+/// DepthFilter Test-Fixture
+class DepthFilterTest {
+ public:
+  DepthFilterTest();
+  virtual ~DepthFilterTest();
+  void depthFilterCb(svo::Point* point, double depth_sigma2);
+  void testReconstruction(std::string dataset_dir, std::string experiment_name);
+
+  std::list<ConvergedSeed> results_;
+  std::vector<double> errors_;
+  size_t n_converged_seeds_;
+  vk::PinholeCamera* cam_;
+  svo::DepthFilter* depth_filter_;
+  svo::FramePtr frame_ref_;
+  svo::FramePtr frame_cur_;
+  cv::Mat depth_ref_;
+};
+
+DepthFilterTest::DepthFilterTest() :
+    n_converged_seeds_(0),
+    cam_(new vk::PinholeCamera(752, 480, 315.5, 315.5, 376.0, 240.0)),
+    depth_filter_(NULL)
+{
+  errors_.reserve(1000);
+}
+
+DepthFilterTest::~DepthFilterTest()
+{
+  delete cam_;
+}
+
+void DepthFilterTest::depthFilterCb(svo::Point* point, double depth_sigma2)
+{
+  double depth = (frame_ref_->pos() - point->pos_).norm();
+  double error = fabs(depth - depth_ref_.at<float>(point->obs_.front()->px[1],
+                                                   point->obs_.front()->px[0]));
+  results_.push_back(ConvergedSeed(
+      point->obs_.front()->px[0], point->obs_.front()->px[1], depth, error));
+  errors_.push_back(error);
+  delete point->obs_.front();
+  delete point;
+  ++n_converged_seeds_;
+}
+
+void DepthFilterTest::testReconstruction(
+    std::string dataset_dir,
+    std::string experiment_name)
+{
+  vk::FileReader<vk::blender_utils::file_format::ImageNameAndPose> sequence_file_reader(dataset_dir+"/trajectory.txt");
+  std::vector<vk::blender_utils::file_format::ImageNameAndPose> sequence;
+  sequence_file_reader.skipComments();
+  if(!sequence_file_reader.next())
+    std::runtime_error("Failed to open sequence file");
+  sequence_file_reader.readAllEntries(sequence);
+  std::cout << "RUN EXPERIMENT: read " << sequence.size() << " dataset entries." << std::endl;
+  std::vector<vk::blender_utils::file_format::ImageNameAndPose>::iterator it = sequence.begin();
+  vk::Timer t;
+  std::list<size_t> n_converged_per_iteration;
+
+  svo::feature_detection::DetectorPtr feature_detector(
+      new svo::feature_detection::FastDetector(
+          cam_->width(), cam_->height(), svo::Config::gridSize(), svo::Config::nPyrLevels()));
+  svo::DepthFilter::callback_t depth_filter_cb = boost::bind(&DepthFilterTest::depthFilterCb, this, _1, _2);
+  depth_filter_ = new svo::DepthFilter(feature_detector, depth_filter_cb);
+  depth_filter_->options_.verbose = true;
+
+  for(int i=0; it != sequence.end() && i<20; ++it, ++i)
+  {
+    std::string img_name(dataset_dir+"/img/" + (*it).image_name_ + "_0.png");
+    printf("reading image: '%s'\n", img_name.c_str());
+    cv::Mat img(cv::imread(img_name, 0));
+    assert(!img.empty());
+
+    Sophus::SE3 T_w_f(it->q_, it->t_);
+    if(i == 0)
+    {
+      // create reference frame and load ground truth depthmap
+      frame_ref_ = boost::make_shared<svo::Frame>(cam_, img, 0.0);
+      frame_ref_->T_f_w_ = T_w_f.inverse();
+      depth_filter_->addKeyframe(frame_ref_, 2, 0.5);
+      vk::blender_utils::loadBlenderDepthmap(dataset_dir+"/depth/" + (*it).image_name_ + "_0.depth", *cam_, depth_ref_);
+      continue;
+    }
+
+    n_converged_seeds_ = 0;
+    frame_cur_ = boost::make_shared<svo::Frame>(cam_, img, 0.0);
+    frame_cur_->T_f_w_ = T_w_f.inverse();
+    depth_filter_->addFrame(frame_cur_);
+    n_converged_per_iteration.push_back(n_converged_seeds_);
+  }
+  printf("Experiment '%s' took %f ms\n", experiment_name.c_str(), t.stop()*1000);
+
+  // compute mean, median and variance of error in converged area
+  {
+    printf("# converged:  \t %zu (ref: 287)\n", errors_.size());
+    double sum_error = 0;
+    std::for_each(errors_.begin(), errors_.end(), [&](double& e){sum_error+=e;});
+    printf("mean error:   \t %f cm (ref: 0.080357)\n", sum_error*100/errors_.size());
+    std::vector<double>::iterator it = errors_.begin()+0.5*errors_.size();
+    std::nth_element(errors_.begin(), it, errors_.end());
+    printf("50-percentile: \t %f cm (ref: 0.062042)\n", *it*100);
+    it = errors_.begin()+0.8*errors_.size();
+    std::nth_element(errors_.begin(), it, errors_.end());
+    printf("80-percentile: \t %f cm (ref: 0.124526)\n", *it*100);
+    it = errors_.begin()+0.95*errors_.size();
+    std::nth_element(errors_.begin(), it, errors_.end());
+    printf("95-percentile: \t %f cm (ref: 0.200417)\n", *it*100);
+  }
+
+  // trace error
+  std::string trace_dir(svo::test_utils::getTraceDir());
+  std::string trace_name(trace_dir + "/depth_filter_" + experiment_name + ".txt");
+  std::ofstream ofs(trace_name.c_str());
+  for(std::list<ConvergedSeed>::iterator i=results_.begin(); i!=results_.end(); ++i)
+    ofs << i->x_ << ", " << i->y_ << ", " << fabs(i->error_) << std::endl;
+  ofs.close();
+  
+  // trace convergence rate
+  trace_name = trace_dir + "/depth_filter_" + experiment_name + "_convergence.txt";
+  ofs.open(trace_name.c_str());
+  for(std::list<size_t>::iterator it=n_converged_per_iteration.begin();
+      it!=n_converged_per_iteration.end(); ++it)
+    ofs << *it << std::endl;
+  ofs.close();
+
+  // write ply file for pointcloud visualization in Meshlab
+  trace_name = trace_dir + "/depth_filter_" + experiment_name + ".ply";
+  ofs.open(trace_name.c_str());
+  ofs << "ply" << std::endl
+      << "format ascii 1.0" << std::endl
+	  << "element vertex " << results_.size() << std::endl
+	  << "property float x" << std::endl
+	  << "property float y" << std::endl
+	  << "property float z" << std::endl
+	  << "property uchar blue" << std::endl
+	  << "property uchar green" << std::endl
+	  << "property uchar red" << std::endl
+	  << "end_header" << std::endl;
+
+  for(std::list<ConvergedSeed>::iterator i=results_.begin(); i!=results_.end(); ++i)
+  {
+	cv::Vec3b c = frame_ref_->img_pyr_[0].at<cv::Vec3b>(i->y_, i->x_);
+	Eigen::Vector3d p = cam_->cam2world(i->x_, i->y_)*i->depth_;
+	ofs << p[0] << " " << p[1] << " " << p[2] << " "
+	    << (int) c[0] << " " << (int) c[1] << " " << (int) c[2] << std::endl;
+  }
+}
+
+}  // namespace
+
+int main(int argc, char** argv)
+{
+  DepthFilterTest test;
+  std::string dataset_dir(svo::test_utils::getDatasetDir() + "/sin2_tex2_h1_v8_d");
+  std::string experiment_name("sin2_tex2_h1_v8_d");
+  test.testReconstruction(dataset_dir, experiment_name);
+  return 0;
+}

CPU_version/svo/test/test_feature_alignment.cpp

@@ -0,0 +1,124 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <string.h>
+#include <svo/global.h>
+#include <svo/feature_alignment.h>
+#include <vikit/timer.h>
+#include <vikit/aligned_mem.h>
+#include "test_utils.h"
+#if __ARM_NEON__
+#include <arm_neon.h>
+#endif
+
+using namespace Eigen;
+
+void generateRefPatchNoWarpInterpolate(const cv::Mat& img, const Vector2d& px,
+                                       cv::Mat& ref_patch_with_border)
+{
+  // compute interpolation weights
+  const int u_r = floor(px[0]);
+  const int v_r = floor(px[1]);
+  const float subpix_u = px[0]-u_r;
+  const float subpix_v = px[1]-v_r;
+  const float wTL = (1.0-subpix_u)*(1.0-subpix_v);
+  const float wTR = subpix_u * (1.0-subpix_v);
+  const float wBL = (1.0-subpix_u)*subpix_v;
+  const float wBR = subpix_u * subpix_v;
+
+  // loop through search_patch, interpolate
+  ref_patch_with_border = cv::Mat(10, 10, CV_8UC1);
+  uint8_t* patch_ptr = ref_patch_with_border.data;
+  const int stride = img.step.p[0];
+  for(int y=0; y<10; ++y)
+  {
+    uint8_t* img_ptr = (uint8_t*) img.data  + (v_r+y-4-1)*stride + u_r-4-1;
+    for(int x=0; x<10; ++x, ++patch_ptr, ++img_ptr)
+      *patch_ptr = wTL*img_ptr[0] + wTR*img_ptr[1] + wBL*img_ptr[stride] + wBR*img_ptr[stride+1];
+  }
+}
+
+int main(int argc, char **argv)
+{
+
+  std::string img_name(svo::test_utils::getDatasetDir() + "/sin2_tex2_h1_v8_d/img/frame_000002_0.png");
+  printf("Loading image '%s'\n", img_name.c_str());
+  cv::Mat img(cv::imread(img_name, 0));
+  assert(img.type() == CV_8UC1);
+
+  // TODO: test on corner/gradient features!
+  Vector2d px_true(130.2,120.3);
+
+  // create reference patch with border
+  cv::Mat ref_patch_with_border;
+  generateRefPatchNoWarpInterpolate(img, px_true, ref_patch_with_border);
+
+  // create reference patch, aligned
+  uint8_t* ref_patch = vk::aligned_mem::aligned_alloc<uint8_t>(64, 16);
+  uint8_t* ref_patch_ptr = ref_patch;
+  for(int y=1; y<9; ++y)
+  {
+    uint8_t* ref_patch_border_ptr = ref_patch_with_border.data + y*10 + 1;
+    for(int x=0; x<8; ++x, ++ref_patch_border_ptr, ++ref_patch_ptr)
+      *ref_patch_ptr = *ref_patch_border_ptr;
+  }
+
+  Vector2d px_est, px_error(-1.1, -0.8);
+  double h_inv;
+  vk::Timer t;
+  for(int i=0; i<1000; ++i)
+  {
+    px_est = px_true-px_error;
+    Vector2f dir = px_error.normalized().cast<float>();
+    svo::feature_alignment::align1D(img, dir, ref_patch_with_border.data, ref_patch, 3, px_est, h_inv);
+  }
+  Vector2d e = px_est-px_true;
+  printf("1000Xalign 1D took %fms, error = %fpx \t (ref i7-W520: 1.982000ms, 0.000033px) \n", t.stop()*1000, e.norm());
+
+  t.start();
+  for(int i=0; i<1000; ++i)
+  {
+    px_est = px_true-px_error;
+    svo::feature_alignment::align2D(img, ref_patch_with_border.data, ref_patch, 3, px_est, true);
+  }
+  e = px_est-px_true;
+  printf("1000Xalign 2D took %fms, error = %fpx \t (ref i7-W520: 2.306000ms, 0.015102px)\n", t.stop()*1000, e.norm());
+
+#ifdef __SSE2__
+  // Note, this KLT implementation is not invariant to illuminatino changes!
+  t.start();
+  for(int i=0; i<1000; ++i)
+  {
+    px_est = px_true-px_error;
+    svo::feature_alignment::align2D_SSE2(img, ref_patch_with_border.data, ref_patch, 3, px_est);
+  }
+  e = px_est-px_true;
+  printf("1000Xalign 2D SSE2 %fms, error = %fpx \t (ref i7-W520: 0.460000ms, 0.021881px)\n", t.stop()*1000, e.norm());
+#endif
+
+#ifdef __ARM_NEON__
+  t.start();
+  for(int i=0; i<1000; ++i)
+  {
+    px_est = px_true-px_error;
+    svo::feature_alignment::align2D_NEON(img, ref_patch_with_border.data, ref_patch, 3, px_est);
+  }
+  e = px_est-px_true;
+  printf("1000Xalign 2D NEON %fms, error = %fpx \t (ref Odroid-U2: ?, ?)\n", t.stop()*1000, e.norm());
+#endif
+ 
+  return 0;
+}

CPU_version/svo/test/test_feature_detection.cpp

@@ -0,0 +1,74 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <string.h>
+#include <svo/global.h>
+#include <svo/config.h>
+#include <svo/frame.h>
+#include <svo/feature_detection.h>
+#include <svo/depth_filter.h>
+#include <svo/feature.h>
+#include <vikit/timer.h>
+#include <vikit/vision.h>
+#include <vikit/abstract_camera.h>
+#include <vikit/atan_camera.h>
+#include "test_utils.h"
+
+namespace {
+
+using namespace Eigen;
+using namespace std;
+
+void testCornerDetector()
+{
+  std::string img_name(svo::test_utils::getDatasetDir() + "/sin2_tex2_h1_v8_d/img/frame_000002_0.png");
+  printf("Loading image '%s'\n", img_name.c_str());
+  cv::Mat img(cv::imread(img_name, 0));
+  assert(img.type() == CV_8UC1 && !img.empty());
+
+  vk::AbstractCamera* cam = new vk::ATANCamera(752, 480, 0.511496, 0.802603, 0.530199, 0.496011, 0.934092);
+  svo::FramePtr frame(new svo::Frame(cam, img, 0.0));
+
+  // Corner detection
+  vk::Timer t;
+  svo::Features fts;
+  svo::feature_detection::FastDetector fast_detector(
+      img.cols, img.rows, svo::Config::gridSize(), svo::Config::nPyrLevels());
+  for(int i=0; i<100; ++i)
+  {
+    fast_detector.detect(frame.get(), frame->img_pyr_, svo::Config::triangMinCornerScore(), fts);
+  }
+  printf("Fast corner detection took %f ms, %zu corners detected (ref i7-W520: 7.166360ms, 40000)\n", t.stop()*10, fts.size());
+  printf("Note, in this case, feature detection also contains the cam2world projection of the feature.\n");
+  cv::Mat img_rgb = cv::Mat(img.size(), CV_8UC3);
+  cv::cvtColor(img, img_rgb, CV_GRAY2RGB);
+  std::for_each(fts.begin(), fts.end(), [&](svo::Feature* i){
+    cv::circle(img_rgb, cv::Point2f(i->px[0], i->px[1]), 4*(i->level+1), cv::Scalar(0,255,0), 1);
+  });
+  cv::imshow("ref_img", img_rgb);
+  cv::waitKey(0);
+
+  std::for_each(fts.begin(), fts.end(), [&](svo::Feature* i){ delete i; });
+}
+
+} // namespace
+
+
+int main(int argc, char **argv)
+{
+  testCornerDetector();
+  return 0;
+}

CPU_version/svo/test/test_matcher.cpp

@@ -0,0 +1,200 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <algorithm>
+#include <vikit/pinhole_camera.h>
+#include <vikit/math_utils.h>
+#include <vikit/blender_utils.h>
+#include <svo/matcher.h>
+#include <svo/frame.h>
+#include <svo/config.h>
+#include <svo/feature.h>
+#include "test_utils.h"
+
+namespace {
+
+/// Matcher Test-Fixture
+class MatcherTest {
+ public:
+  MatcherTest()
+  {
+    cam_ = new vk::PinholeCamera(752, 480, 315.5, 315.5, 376.0, 240.0);
+
+    // load images
+    std::string dataset_dir(svo::test_utils::getDatasetDir() + "/sin2_tex2_h1_v8_d");
+    std::string img_name(dataset_dir+"/img/frame_000002_0.png");
+    printf("Loading image '%s'\n", img_name.c_str());
+    cv::Mat img_ref(cv::imread(img_name, 0));
+    img_name = std::string(dataset_dir+"/img/frame_000006_0.png");
+    printf("Loading image '%s'\n", img_name.c_str());
+    cv::Mat img_cur(cv::imread(img_name, 0));
+    assert(!img_ref.empty() && !img_cur.empty());
+
+    // create frame
+    frame_ref_ = new svo::Frame(cam_, img_ref, 1.0);
+    frame_cur_ = new svo::Frame(cam_, img_cur, 2.0);
+    ref_ftr_ = new svo::Feature(frame_ref_, Eigen::Vector2d(300, 260), 0);
+
+    // set poses
+    Eigen::Vector3d t_w_ref(0.1131, 0.1131, 2.0000);
+    Eigen::Vector3d t_w_cur(0.5673, 0.5641, 2.0000);
+    Eigen::Quaterniond q_w_ref(0.0, 0.8227, 0.2149, 0.0);
+    Eigen::Quaterniond q_w_cur(0.0, 0.8235, 0.2130, 0.0);
+    frame_ref_->T_f_w_ = Sophus::SE3(q_w_ref, t_w_ref).inverse();
+    frame_cur_->T_f_w_ = Sophus::SE3(q_w_cur, t_w_cur).inverse();
+
+    // load ground-truth depth
+    vk::blender_utils::loadBlenderDepthmap(dataset_dir+"/depth/frame_000002_0.depth", *cam_, depth_ref_);
+  }
+
+  virtual ~MatcherTest()
+  {
+    delete cam_;
+    delete frame_ref_;
+    delete frame_cur_;
+    delete ref_ftr_;
+  }
+
+  // Individual tests
+  void testEpipolarSearchFullImg();
+  void testWarpAffine();
+
+  // Objects declared here can be used by all tests
+  vk::PinholeCamera* cam_;
+  svo::Frame* frame_ref_;
+  svo::Frame* frame_cur_;
+  svo::Feature* ref_ftr_;
+  cv::Mat depth_ref_;
+};
+
+void MatcherTest::testEpipolarSearchFullImg()
+{
+  svo::Matcher matcher;
+  cv::Mat depthmap(frame_ref_->cam_->height(), frame_ref_->cam_->width(), CV_32FC1, cv::Scalar(0));
+  cv::Mat error_map(depthmap.rows, depthmap.cols, CV_32FC1, cv::Scalar(0));
+  cv::Mat mask(depthmap.rows, depthmap.cols, CV_8UC1, cv::Scalar(1));
+  double depth_range = 0.8;
+  size_t n_converged = 0;
+  double sum_error = 0;
+  std::vector<double> errors; errors.reserve(depthmap.cols*depthmap.rows);
+  for(int y=4; y<depthmap.rows-4; ++y)
+  {
+    for(int x=4; x<depthmap.cols-4; ++x)
+    {
+      svo::Feature ftr(frame_ref_, Eigen::Vector2d(x,y), 0);
+      double depth_gt = depth_ref_.at<float>(y, x);
+      double depth_estimate;
+      int res = matcher.findEpipolarMatchDirect(
+          *frame_ref_, *frame_cur_, ftr, depth_gt, fmax(depth_gt-depth_range,0.0),
+          depth_gt+depth_range, depth_estimate);
+      if(res)
+      {
+        depthmap.at<float>(y,x) = depth_estimate;
+        error_map.at<float>(y,x) = fabsf(depth_estimate-depth_gt);
+        ++n_converged;
+        sum_error += error_map.at<float>(y,x);
+        errors.push_back(error_map.at<float>(y,x));
+      }
+      else
+        mask.at<uint8_t>(y,x) = 0;
+    }
+  }
+
+  // compute mean, median and variance of error in converged area
+  printf("n converged:  \t %zu mm (ref: 216114)\n", n_converged);
+  printf("mean error:   \t %f mm (ref: 0.410084)\n", sum_error*100/n_converged);
+  std::vector<double>::iterator it = errors.begin()+0.5*errors.size();
+  std::nth_element(errors.begin(), it, errors.end());
+  printf("50-percentile: \t %f mm (ref: 0.083203)\n", *it*100);
+  it = errors.begin()+0.8*errors.size();
+  std::nth_element(errors.begin(), it, errors.end());
+  printf("80-percentile: \t %f mm (ref: 0.161824)\n", *it*100);
+  it = errors.begin()+0.95*errors.size();
+  std::nth_element(errors.begin(), it, errors.end());
+  printf("95-percentile: \t %f mm (ref: 0.263539)\n", *it*100);
+
+  // save results to file
+  std::ofstream output_stream;
+  std::string trace_dir(svo::test_utils::getTraceDir());
+  std::string output_filename(trace_dir + "/depthmap.bin");
+  output_stream.open(output_filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc);
+  output_stream.write((const char *)depthmap.data, depthmap.rows*depthmap.cols*sizeof(float));
+  output_stream.close();
+
+  output_filename.assign(trace_dir + "/depthmap_error.bin");
+  output_stream.open(output_filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc);
+  output_stream.write((const char *)error_map.data, error_map.rows*error_map.cols*sizeof(float));
+  output_stream.close();
+
+  output_filename.assign(trace_dir + "/depthmap_mask.bin");
+  output_stream.open(output_filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc);
+  output_stream.write((const char *)mask.data, mask.rows*mask.cols*sizeof(uint8_t));
+  output_stream.close();
+}
+
+void MatcherTest::testWarpAffine()
+{
+  const int halfpatch_size = 15;
+  const int patch_size=halfpatch_size*2;
+  svo::Matcher matcher;
+  double depth = 1.0;
+
+  Sophus::SE3 T_cur_ref(frame_cur_->T_f_w_*frame_ref_->T_f_w_.inverse());
+  Eigen::Vector2d px_cur(frame_cur_->cam_->world2cam(T_cur_ref*(ref_ftr_->f*depth)));
+
+  // compute reference patch
+  cv::Mat ref_patch(matcher.halfpatch_size_+1, matcher.halfpatch_size_+1,
+                    CV_8U, matcher.patch_with_border_);
+
+  Eigen::Matrix2d A_cur_ref;
+  svo::warp::getWarpMatrixAffine(
+      *cam_, *cam_, ref_ftr_->px, ref_ftr_->f, 1.0, T_cur_ref, ref_ftr_->level, A_cur_ref);
+  int level_cur = svo::warp::getBestSearchLevel(A_cur_ref, svo::Config::nPyrLevels()-1);
+  svo::warp::warpAffine(
+      A_cur_ref, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px,
+      ref_ftr_->level, level_cur, matcher.halfpatch_size_+1, matcher.patch_with_border_);
+
+  // copy current patch
+  Eigen::Vector2i pxi(px_cur[0]/(1<<level_cur)+0.5, px_cur[1]/(1<<level_cur)+0.5);
+  cv::Mat cur_patch(patch_size, patch_size, CV_8U);
+  frame_cur_->img_pyr_.at(level_cur)(cv::Range(pxi[1]-halfpatch_size,
+                                               pxi[1]+halfpatch_size),
+                                     cv::Range(pxi[0]-halfpatch_size,
+                                               pxi[0]+halfpatch_size)).copyTo(cur_patch);
+
+  // display results
+  cv::circle(frame_cur_->img_pyr_[level_cur],
+             cv::Point2f(pxi[0], pxi[1]),
+             patch_size, cv::Scalar(255), 2);
+  cv::circle(frame_ref_->img_pyr_[ref_ftr_->level],
+              cv::Point2f(ref_ftr_->px[0]/(1<<ref_ftr_->level), ref_ftr_->px[1]/(1<<ref_ftr_->level)),
+              patch_size, cv::Scalar(255), 2);
+  cv::imshow("cur_patch", cur_patch);
+  cv::imshow("ref_patch", ref_patch);
+  cv::imshow("ref_img", frame_ref_->img_pyr_[ref_ftr_->level]);
+  cv::imshow("cur_img", frame_cur_->img_pyr_[level_cur]);
+  cv::waitKey(0);
+}
+
+}  // namespace
+
+int main(int argc, char** argv)
+{
+  MatcherTest test;
+  test.testEpipolarSearchFullImg();
+//  test.testWarpAffine();
+  return 0;
+}

CPU_version/svo/test/test_pipeline.cpp

@@ -0,0 +1,98 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <svo/config.h>
+#include <svo/frame_handler_mono.h>
+#include <svo/map.h>
+#include <svo/frame.h>
+#include <vector>
+#include <string>
+#include <vikit/math_utils.h>
+#include <vikit/vision.h>
+#include <vikit/abstract_camera.h>
+#include <vikit/atan_camera.h>
+#include <vikit/pinhole_camera.h>
+#include <opencv2/opencv.hpp>
+#include <sophus/se3.h>
+#include <iostream>
+#include "test_utils.h"
+
+namespace svo {
+
+class BenchmarkNode
+{
+  vk::AbstractCamera* cam_;
+  svo::FrameHandlerMono* vo_;
+
+public:
+  BenchmarkNode();
+  ~BenchmarkNode();
+  void runFromFolder();
+};
+
+BenchmarkNode::BenchmarkNode()
+{
+  cam_ = new vk::PinholeCamera(752, 480, 315.5, 315.5, 376.0, 240.0);
+  vo_ = new svo::FrameHandlerMono(cam_);
+  vo_->start();
+}
+
+BenchmarkNode::~BenchmarkNode()
+{
+  delete vo_;
+  delete cam_;
+}
+
+void BenchmarkNode::runFromFolder()
+{
+  for(int img_id = 2; img_id < 188; ++img_id)
+  {
+    // load image
+    std::stringstream ss;
+    ss << svo::test_utils::getDatasetDir() << "/sin2_tex2_h1_v8_d/img/frame_"
+       << std::setw( 6 ) << std::setfill( '0' ) << img_id << "_0.png";
+    if(img_id == 2)
+      std::cout << "reading image " << ss.str() << std::endl;
+    cv::Mat img(cv::imread(ss.str().c_str(), 0));
+    assert(!img.empty());
+
+    // process frame
+    vo_->addImage(img, 0.01*img_id);
+
+    // display tracking quality
+    if(vo_->lastFrame() != NULL)
+    {
+    	std::cout << "Frame-Id: " << vo_->lastFrame()->id_ << " \t"
+                  << "#Features: " << vo_->lastNumObservations() << " \t"
+                  << "Proc. Time: " << vo_->lastProcessingTime()*1000 << "ms \n";
+
+    	// access the pose of the camera via vo_->lastFrame()->T_f_w_.
+    }
+  }
+}
+
+} // namespace svo
+
+int main(int argc, char** argv)
+{
+  {
+    svo::BenchmarkNode benchmark;
+    benchmark.runFromFolder();
+  }
+  printf("BenchmarkNode finished.\n");
+  return 0;
+}
+

CPU_version/svo/test/test_pose_optimizer.cpp

@@ -0,0 +1,110 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <algorithm>
+#include <vikit/pinhole_camera.h>
+#include <vikit/math_utils.h>
+#include <vikit/sample.h>
+#include <vikit/blender_utils.h>
+#include <svo/frame.h>
+#include <svo/point.h>
+#include <svo/feature.h>
+#include <svo/config.h>
+#include <svo/feature_detection.h>
+#include <svo/pose_optimizer.h>
+#include "test_utils.h"
+
+namespace {
+
+using namespace svo;
+using namespace Eigen;
+
+/// Pose Optimizer Test-Fixture
+class PoseOptimizerTest {
+ public:
+  PoseOptimizerTest()
+  {
+    cam_ = new vk::PinholeCamera(752, 480, 315.5, 315.5, 376.0, 240.0);
+
+    // load image
+    std::string dataset_dir(svo::test_utils::getDatasetDir() + "/sin2_tex2_h1_v8_d");
+    std::string img_name(dataset_dir+"/img/frame_000002_0.png");
+    printf("Loading image '%s'\n", img_name.c_str());
+    cv::Mat img(cv::imread(img_name, 0));
+    assert(!img.empty());
+
+    // create frame
+    frame_.reset(new svo::Frame(cam_, img, 1.0));
+
+    // set pose
+    Eigen::Vector3d t_w_ref(0.1131, 0.1131, 2.0000);
+    Eigen::Quaterniond q_w_ref(0.0, 0.8227, 0.2149, 0.0);
+    frame_->T_f_w_ = Sophus::SE3(q_w_ref, t_w_ref).inverse();
+
+    // load ground-truth depth
+    vk::blender_utils::loadBlenderDepthmap(dataset_dir + "/depth/frame_000002_0.depth", *cam_, depthmap_);
+
+    // detect features
+    feature_detection::FastDetector detector(
+        cam_->width(), cam_->height(), Config::gridSize(), Config::nPyrLevels());
+    detector.detect(frame_.get(), frame_->img_pyr_, Config::triangMinCornerScore(), frame_->fts_);
+    size_t n_fts = 0;
+    std::for_each(frame_->fts_.begin(), frame_->fts_.end(), [&](Feature* i){
+      Point* point(new Point(frame_->f2w(i->f*depthmap_.at<float>(i->px[1], i->px[0])), i));
+      i->point = point;
+      ++n_fts;
+    });
+    printf("Added %zu features to frame.\n", n_fts);
+  }
+
+  virtual ~PoseOptimizerTest()
+  {
+    delete cam_;
+  }
+
+  // Individual tests
+  void test(const Vector3d& pose_disturbance, double pixel_sigma2);
+
+  // Objects declared here can be used by all tests
+  vk::PinholeCamera* cam_;
+  FramePtr frame_;
+  cv::Mat depthmap_;
+};
+
+void PoseOptimizerTest::test(const Vector3d& pose_disturbance, double pixel_sigma2)
+{
+  printf("Add %f px noise to each observation\n", pixel_sigma2);
+  for(Features::iterator it=frame_->fts_.begin(); it!=frame_->fts_.end(); ++it)
+  {
+    (*it)->px += Vector2d(vk::Sample::gaussian(pixel_sigma2), vk::Sample::gaussian(pixel_sigma2));
+    (*it)->f = frame_->c2f((*it)->px);
+  }
+  frame_->T_f_w_ = frame_->T_f_w_*SE3(Matrix3d::Identity(), pose_disturbance);
+  double estimated_scale, error_init, error_final;
+  size_t num_obs;
+  pose_optimizer::optimizeGaussNewton(
+      Config::reprojThresh(), 10, true, frame_,
+      estimated_scale, error_init, error_final, num_obs);
+}
+
+}  // namespace
+
+int main(int argc, char** argv)
+{
+  PoseOptimizerTest test;
+  test.test(Eigen::Vector3d(0.2, 0.2, 0.2), 1.0);
+  return 0;
+}

CPU_version/svo/test/test_sparse_img_align.cpp

@@ -0,0 +1,163 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <vikit/pinhole_camera.h>
+#include <vikit/math_utils.h>
+#include <vikit/file_reader.h>
+#include <vikit/timer.h>
+#include <vikit/blender_utils.h>
+#include <iostream>
+#include <svo/feature_detection.h>
+#include <svo/sparse_img_align.h>
+#include <svo/frame.h>
+#include <svo/point.h>
+#include <svo/feature.h>
+#include <svo/config.h>
+#include "test_utils.h"
+
+namespace {
+
+/// SparseImgAlign Test-Fixture
+class SparseImgAlignTest {
+ public:
+
+  SparseImgAlignTest();
+  virtual ~SparseImgAlignTest();
+  void testSequence(
+      const std::string& dataset_dir,
+      const std::string& experiment_name,
+      svo::feature_detection::AbstractDetector* feature_detector);
+
+  vk::PinholeCamera* cam_;
+  svo::FramePtr frame_ref_;
+  svo::FramePtr frame_cur_;
+};
+
+SparseImgAlignTest::SparseImgAlignTest() :
+    cam_(new vk::PinholeCamera(752, 480, 315.5, 315.5, 376.0, 240.0))
+{}
+
+SparseImgAlignTest::~SparseImgAlignTest()
+{}
+
+void SparseImgAlignTest::testSequence(
+    const std::string& dataset_dir,
+    const std::string& experiment_name,
+    svo::feature_detection::AbstractDetector* feature_detector)
+{
+  vk::FileReader<vk::blender_utils::file_format::ImageNameAndPose> sequence_file_reader(dataset_dir+"/trajectory.txt");
+  std::vector<vk::blender_utils::file_format::ImageNameAndPose> sequence;
+  sequence_file_reader.skipComments();
+  if(!sequence_file_reader.next())
+    std::runtime_error("Failed to open sequence file");
+  sequence_file_reader.readAllEntries(sequence);
+  printf("RUN EXPERIMENT: read %zu dataset entries.\n", sequence.size());
+  std::vector<vk::blender_utils::file_format::ImageNameAndPose>::iterator iter = sequence.begin();
+  std::list<double> translation_error;
+
+  Sophus::SE3 T_prev_w, T_prevgt_w;
+  std::string trace_dir(svo::test_utils::getTraceDir());
+  std::string trace_name(trace_dir + "/sparse_img_align_" + experiment_name + "_trans_estimate.txt");
+  std::ofstream ofs(trace_name.c_str());
+  for(int i=0; iter != sequence.end() && i<30; ++iter, ++i)
+  {
+    // load img
+    // IMPORTANT: We need to flip the image because the Povray dataset has a
+    // negative focal length in y direction which we didn't set.
+    std::string img_name(dataset_dir+"/img/" + iter->image_name_ + "_0.png");
+    cv::Mat img(cv::imread(img_name, 0));
+    assert(!img.empty());
+
+    // load pose
+    Sophus::SE3 T_w_gt(iter->q_, iter->t_);
+    Sophus::SE3 T_gt_w = T_w_gt.inverse(); // ground-truth
+
+    if(i==0)
+    {
+      // set reference frame
+      frame_ref_.reset(new svo::Frame(cam_, img, 0.0));
+      frame_ref_->T_f_w_ = T_gt_w;
+
+      // load ground-truth depth
+      cv::Mat depthmap;
+      vk::blender_utils::loadBlenderDepthmap(
+          dataset_dir+"/depth/"+iter->image_name_+"_0.depth", *cam_, depthmap);
+
+      // extract features, generate features with 3D points
+      feature_detector->detect(
+          frame_ref_.get(), frame_ref_->img_pyr_, svo::Config::triangMinCornerScore(), frame_ref_->fts_);
+      std::for_each(frame_ref_->fts_.begin(), frame_ref_->fts_.end(), [&](svo::Feature* i) {
+        Eigen::Vector3d pt_pos_cur = i->f*depthmap.at<float>(i->px[1], i->px[0]);
+        Eigen::Vector3d pt_pos_w = frame_ref_->T_f_w_.inverse()*pt_pos_cur;
+        svo::Point* pt = new svo::Point(pt_pos_w, i);
+        i->point = pt;
+      });
+
+      printf("Added %zu 3d pts to the reference frame.\n", frame_ref_->nObs());
+      T_prev_w = frame_ref_->T_f_w_;
+      T_prevgt_w = T_gt_w;
+      continue;
+    }
+
+    frame_cur_.reset(new svo::Frame(cam_, img, 0.0));
+    //frame_cur_->T_f_w_ = frame_ref_->T_f_w_; // start at reference frame
+    frame_cur_->T_f_w_ = T_prev_w; // start at last frame
+
+    // run image align
+    vk::Timer t;
+    svo::SparseImgAlign img_align(svo::Config::kltMaxLevel(), svo::Config::kltMinLevel(),
+                                    30, svo::SparseImgAlign::GaussNewton, false, false);
+    img_align.run(frame_ref_, frame_cur_);
+
+    // compute error
+    Sophus::SE3 T_f_gt = frame_cur_->T_f_w_ * T_gt_w.inverse();
+    translation_error.push_back(T_f_gt.translation().norm());
+    printf("[%3.i] time = %f ms \t |t| = %f \t translation error = %f\n",
+           i, t.stop()*1000, (frame_ref_->T_f_w_*T_gt_w.inverse()).translation().norm(),
+           translation_error.back());
+
+    // save old pose for next iteration
+    T_prev_w = frame_cur_->T_f_w_;
+    T_prevgt_w = T_gt_w;
+
+    ofs << frame_cur_->T_f_w_.inverse().translation().transpose() << " "
+        << T_gt_w.inverse().translation().transpose() << std::endl;
+  }
+  ofs.close();
+
+  // trace error
+  trace_name = trace_dir + "/sparse_img_align_" + experiment_name + "_trans_error.txt";
+  ofs.open(trace_name.c_str());
+  for(std::list<double>::iterator it=translation_error.begin(); it!=translation_error.end(); ++it)
+    ofs << *it << std::endl;
+  ofs.close();
+}
+
+}  // namespace
+
+
+int main(int argc, char** argv)
+{
+  std::string experiment_name("flying_room_1_rig_1_fast_minlev0");
+  std::string dataset_dir(svo::test_utils::getDatasetDir() + "/sin2_tex2_h1_v8_d");
+  svo::Config::triangMinCornerScore() = 20;
+  svo::Config::kltMinLevel() = 0;
+  SparseImgAlignTest test;
+  svo::feature_detection::FastDetector detector(
+      test.cam_->width(), test.cam_->height(), svo::Config::gridSize(), svo::Config::nPyrLevels());
+  test.testSequence(dataset_dir, experiment_name, &detector);
+  return 0;
+}

CPU_version/svo/test/test_utils.h

@@ -0,0 +1,57 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO 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.
+//
+// SVO 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 this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef TEST_UTILS_H_
+#define TEST_UTILS_H_
+
+#include <string.h>
+#include <cstdlib> // for getenv
+#ifdef SVO_USE_ROS
+# include <ros/package.h>
+# include <vikit/params_helper.h>
+#endif
+
+namespace svo {
+namespace test_utils {
+
+std::string getDatasetDir()
+{
+  const char* env_dir = std::getenv("SVO_DATASET_DIR");
+#ifdef SVO_USE_ROS
+  std::string dataset_dir(ros::package::getPath("svo")+"/test/data");
+  if(env_dir != NULL)
+    dataset_dir = std::string(env_dir);
+  return dataset_dir;
+#else
+  return std::string(env_dir);
+#endif
+}
+
+std::string getTraceDir()
+{
+#ifdef SVO_USE_ROS
+  std::string default_dir(ros::package::getPath("svo")+"/test/results");
+  return vk::getParam<std::string>("svo/trace_dir", default_dir);
+#else
+  return "/tmp";
+#endif
+}
+
+} // namespace test_utils
+} // namespace svo
+
+
+#endif // TEST_UTILS_H_