Rewrite README with comprehensive project documentation

- Added detailed project overview and feature descriptions - Included complete dependency list with build instructions - Added system architecture overview and component descriptions - Documented configuration options and usage instructions - Added troubleshooting section for common issues - Included known limitations and future improvements - Improved formatting and clarity throughout Co-Authored-By: Claude Haiku 4.5 <[email protected]> Claude-Session: https://claude.ai/code/session_01GPXVpiPnb5AzgdCJ33Q6Fi
move files
2026-08-28 19:09:18 +02:00 · 2026-08-28 18:29:45 +02:00 · 2021-10-22 10:25:50 +02:00 · 2021-10-22 10:25:14 +02:00 · 2021-10-22 10:23:23 +02:00 · 2021-10-22 10:06:36 +02:00
107 changed files with 2364 additions and 2459 deletions
@@ -1,69 +0,0 @@
 SVO
 ===
 This code implements a semi-direct monocular visual odometry pipeline which has been integrated with GTSAM to infused IMU as well. The feature extraction part is implemented in OpenCL to take advantage of GPU.
 Paper svo: http://rpg.ifi.uzh.ch/docs/ICRA14_Forster.pdf
 Paper Svo+GTSAM: https://arxiv.org/abs/1512.02363
 #### Disclaimer
 SVO has been tested under ROS Groovy, Hydro and Indigo with Ubuntu 12.04, 13.04 and 14.04. This is research code, any fitness for a particular purpose is disclaimed.
 #### Licence
 The source code is released under a GPLv3 licence. A closed-source professional edition is available for commercial purposes. In this case, please contact the authors for further info.
 #### Citing
 If you use SVO in an academic context, please cite the following publication:
    @inproceedings{Forster2014ICRA,
      author = {Forster, Christian and Pizzoli, Matia and Scaramuzza, Davide},
      title = {{SVO}: Fast Semi-Direct Monocular Visual Odometry},
      booktitle = {IEEE International Conference on Robotics and Automation (ICRA)},
      year = {2014}
    }
 #### Documentation
 OpenCL documentation
 The API is documented here: http://uzh-rpg.github.io/rpg_svo/doc/
 The GTSAM documentation  https://gtsam.org/tutorials/
 #### Instructions
 Please be sure that you have the OpenCL driver installed
 First install gtsam library => https://github.com/borglab/gtsam
 Then follow the SVO instalation
 See the Wiki for more instructions. https://github.com/uzh-rpg/rpg_svo/wiki and https://gtsam.org/get_started/
 ## Setting
 You can edit the algorithm parameters in the follwoing file, please look at the config file to underestand the meaning of the parameters (CPU_version/svo/include/svo/config.h)
 CPU_version/vio_svo/param/vo_fast.yaml
 # the GPU_version
 Dose not have dependency with GTSAM, the dependencies are:
 1. OpenCV 3
 2. Eigen
 3. Sophus     https://github.com/strasdat/Sophus.git
 4. Boost
 5. OpenCL
 if make fails in the build of Sophus with the error lvalue required as left operand of assignment in so2.cpp, see this issue: https://github.com/uzh-rpg/rpg_svo/issues/237
@@ -1,253 +0,0 @@
 // 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 <vio/vision.h>
 #include <boost/bind.hpp>
 #include <boost/math/distributions/normal.hpp>
 #include <vio/global.h>
 #include <vio/global_optimizer.h>
 #include <vio/frame.h>
 #include <vio/point.h>
 #include <vio/feature.h>
 #include <vio/config.h>
 #include <g2o/solvers/structure_only/structure_only_solver.h>
 #if VIO_DEBUG
 #include <sys/types.h>
 #include <sys/stat.h>
 #endif
 namespace vio {
    BA_Glob::BA_Glob(Map& map) :map_(map),thread_(NULL)
    {
 #if VIO_DEBUG
        log_ =fopen((std::string(PROJECT_DIR)+"/depth_filter_log.txt").c_str(),"w+");
        assert(log_);
        chmod((std::string(PROJECT_DIR)+"/depth_filter_log.txt").c_str(), ACCESSPERMS);
 #endif
    }
    BA_Glob::~BA_Glob()
    {
        stopThread();
    }
    void BA_Glob::startThread()
    {
        thread_ = new boost::thread(&BA_Glob::updateLoop, this);
    }
    void BA_Glob::stopThread()
    {
        if(thread_ != NULL)
        {
            thread_->interrupt();
            usleep(5000);
            thread_->join();
            thread_ = NULL;
        }
 #if VIO_DEBUG
        fclose(log_);
 #endif
    }
    void BA_Glob::updateLoop()
    {
        while(!boost::this_thread::interruption_requested())
        {
            boost::unique_lock< boost::mutex > lk( mtx_);
            while(map_.keyframes_.empty() || new_keyframe_ == false)
                cond_.wait(lk);
 #if VIO_DEBUG
            fprintf(log_,"[%s] BA loop run \n",
                    vio::time_in_HH_MM_SS_MMM().c_str());
 #endif
            new_keyframe_=false;
            g2o::SparseOptimizer optimizer;
            optimizer.setVerbose(false);
            g2o::BlockSolver_6_3::LinearSolverType * linearSolver=new g2o::LinearSolverCholmod<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);
            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);
            }
            // init g2o
            list<EdgeContainerSE3> edges;
            list< pair<FramePtr,std::shared_ptr<Feature>> > incorrect_edges;
            ba_mux_.lock();
            // Go through all Keyframes
            size_t v_id = 0;
            for(list<FramePtr>::iterator it_kf = map_.keyframes_.begin();
                it_kf != map_.keyframes_.end(); ++it_kf)
            {
                // New Keyframe Vertex
               g2o::VertexSE3Expmap* v_kf = createG2oFrameSE3(*it_kf, v_id++, false);
                (*it_kf)->v_kf_ = v_kf;
                optimizer.addVertex(v_kf);
                for(auto&& it_ftr:(*it_kf)->fts_)
                {
                    if(it_ftr->point==NULL)continue;
                    // for each keyframe add edges to all observed mapoints
                    std::shared_ptr<Point> mp = it_ftr->point;
                    if(mp->pos_.hasNaN())continue;
                    g2o::VertexSBAPointXYZ* 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++, true);
                        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() > pow(2,Config::poseOptimThresh()/(*it_kf)->cam_->errorMultiplier2()))
                        incorrect_edges.push_back(pair<FramePtr,std::shared_ptr<Feature>>(*it_kf, it_ftr));
                    else
                    {
                        g2o::Edge_XYZ_VSC e1;
                        g2o::EdgeProjectXYZ2UV* e = createG2oEdgeSE3(v_kf, v_mp, vk::project2d(it_ftr->f),
                                                                         true,
                                                                         Config::poseOptimThresh()/(*it_kf)->cam_->errorMultiplier2()*Config::lobaRobustHuberWidth());
                        EdgeContainerSE3 edge;
                        edge.frame=(*it_kf);
                        edge.edge=e;
                        edge.feature=it_ftr;
                        edges.push_back(edge);
                        optimizer.addEdge(e);
                    }
                }
            }
            if(optimizer.vertices().size()<1)continue;
            // Optimization
            optimizer.initializeOptimization();
            optimizer.computeActiveErrors();
 /*            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)
                    points.push_back(v);
            }
            structure_only_ba.calc(points, 10);*/
 #if VIO_DEBUG
            fprintf(log_,"[%s] init error: %f \n",
                    vio::time_in_HH_MM_SS_MMM().c_str(),optimizer.activeChi2());
 #endif
            if(optimizer.optimize(vio::Config::lobaNumIter())<1)continue;
 #if VIO_DEBUG
            fprintf(log_,"[%s] end error: %f \n",
                    vio::time_in_HH_MM_SS_MMM().c_str(),optimizer.activeChi2());
 #endif
            // 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_ = SE2_5(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)
                {
                    std::shared_ptr<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,std::shared_ptr<Feature>> >::iterator it=incorrect_edges.begin();
                it!=incorrect_edges.end(); ++it)
                map_.removePtFrameRef(it->first, it->second);
            for(list<EdgeContainerSE3>::iterator it = edges.begin(); it != edges.end(); ++it)
            {
                if(it->edge->chi2() > 0.001)
                {
                    map_.removePtFrameRef(it->frame, it->feature);
                }
            }
            ba_mux_.unlock();
        }
    }
   g2o::VertexSE3Expmap*
   BA_Glob::createG2oFrameSE3(FramePtr frame, size_t id, bool fixed)
   {
 /*        if(frame->id_==0 || frame->id_==1){
            g2o::VertexSE3Expmap* v= new g2o::VertexSE3Expmap();
            v->setId(id);
            v->setToOrigin();
            v->setFixed(true);
            v->setEstimate(g2o::SE3Quat(frame->se3().unit_quaternion(), frame->se3().translation()));
        }*/
       g2o::VertexSE3Expmap* v= new g2o::VertexSE3Expmap();
       v->setId(id);
       v->setFixed(false);
       v->setEstimate(g2o::SE3Quat(frame->se3().unit_quaternion(), frame->se3().translation()));
       return v;
   }
   g2o::VertexSBAPointXYZ*
   BA_Glob::createG2oPoint(Vector3d pos,
                  size_t id,
                  bool fixed)
   {
       g2o::VertexSBAPointXYZ* v =new g2o::VertexSBAPointXYZ();
       v->setId(id);
       //v->setFixed(fixed);
       v->setMarginalized(true);
       v->setEstimate(pos);
       return v;
   }
    g2o::EdgeProjectXYZ2UV* BA_Glob::createG2oEdgeSE3( g2o::VertexSE3Expmap* v_frame,
                     g2o::VertexSBAPointXYZ* v_point,
                     const Vector2d& f_up,
                     bool robust_kernel,
                     double huber_width,
                     double weight)
   {
       g2o::EdgeProjectXYZ2UV* e= new g2o::EdgeProjectXYZ2UV;
       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() = Eigen::Matrix2d::Identity();
       g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
       rk->setDelta(huber_width);
       e->setRobustKernel(rk);
       e->setParameterId(0, 0); //old: e->setId(v_point->id());
       return e;
   }
 } // namespace vio
@@ -0,0 +1,180 @@
 # VIO - Visual Inertial Odometry
 A semi-direct monocular visual odometry pipeline with IMU and command integration using Extended Kalman Filter (EKF) for robust pose estimation. The system leverages GPU acceleration via OpenCL for real-time performance.
 ## Features
 - **Semi-Direct Visual Odometry**: Combines feature-based and direct alignment approaches for robust tracking
 - **GPU-Accelerated Processing**: OpenCL-based feature extraction and image alignment for real-time performance
 - **Multi-Sensor Fusion**: EKF integration for IMU and command data fusion with visual odometry
 - **ROS Integration**: Native ROS support for easy integration into robotic systems
 - **Graph-Based Optimization**: G2O-based global and pose optimization backends
 - **Monocular Input**: Works with standard monocular cameras
 ## System Requirements
 - **OS**: Linux (tested on Ubuntu)
 - **GPU**: OpenCL-compatible GPU with OpenCL drivers installed
 - **C++ Standard**: C++17 or later
 - **CMake**: Version 2.8.3 or later
 ## Dependencies
 The following packages are required. Specific build instructions are provided for G2O:
 1. **OpenCV** (4.x) - Computer vision library
 2. **Eigen** - Linear algebra library
 3. **G2O** - Graph optimization library
   ```bash
   # Build G2O with:
   cmake -DG2O_HAVE_OPENGL=ON -DBUILD_WITH_MARCH_NATIVE=ON ..
   # Recommended version: https://github.com/RainerKuemmerle/g2o/tree/memory_management
   ```
 4. **Boost** - C++ libraries (requires thread and system components)
 5. **OpenCL** - GPU computation framework
 6. **ROS** - Robot Operating System (for ROS integration)
   - catkin, roscpp, roslib, image_transport, cv_bridge, tf, geometry_msgs
 ## Building
 ### Prerequisites
 Ensure you have the OpenCL driver installed on your system:
 ```bash
 # For NVIDIA GPUs
 sudo apt-get install nvidia-opencl-icd
 # For AMD/Intel, install appropriate drivers
 ```
 ### Build Instructions
 ```bash
 # Create a catkin workspace (if using ROS)
 mkdir -p ~/catkin_ws/src
 cd ~/catkin_ws/src
 git clone <this-repo> vio
 cd ~/catkin_ws
 # Build
 catkin_make
 # or with specific options:
 catkin_make -DCMAKE_BUILD_TYPE=Release
 ```
 For standalone (non-ROS) builds:
 ```bash
 cd vio
 mkdir build && cd build
 cmake ..
 make -j$(nproc)
 ```
 ## Configuration
 Algorithm parameters can be configured in two main files:
 1. **Header Configuration**: `vio/include/vio/config.h`
   - Core algorithm parameters and feature extraction settings
 2. **Runtime Configuration**: `vio/param/vo_fast.yaml`
   - Fast configuration profile for real-time operation
 Additional configuration files:
 - `vio/param/px30.yaml` - Configuration for specific hardware/robotics platform
 ## Usage
 ### As a ROS Node
 ```bash
 rosrun vio vio
 ```
 ### Configuration via Parameters
 Edit `vo_fast.yaml` before running or use ROS parameter server to override settings at runtime.
 ### Output
 The system provides:
 - Estimated pose (position and orientation)
 - 3D map points
 - Tracking statistics and debug information
 Log files are written to the project directory. You can modify the logging path and debug mode in `CMakeLists.txt` (see line 89).
 ## Known Issues
 1. **Orientation Estimation**: The estimated orientation may not be fully accurate under certain conditions
 2. **Scale Ambiguity**: Semi-direct visual odometry inherently suffers from monocular scale ambiguity; the EKF sensor fusion helps but may not fully resolve it
 3. **Feature Point Accumulation**: The number of tracked feature points can increase significantly over time; limiting strategy needed to prevent performance degradation and estimation errors
 ## Architecture Overview
 ### Core Components
 - **Frame Handler** (`frame_handler_mono.cpp`): Main tracking loop for monocular sequences
 - **Feature Detection** (`feature_detection.cpp`): GPU-accelerated FAST corner detection
 - **Feature Alignment** (`feature_alignment.cpp`): Direct image alignment for sub-pixel accuracy
 - **Matcher** (`matcher.cpp`): Feature correspondence matching
 - **Reprojector** (`reprojector.cpp`): Map point reprojection and visibility management
 - **Pose Optimizer** (`pose_optimizer.cpp`): Per-frame pose optimization
 - **Global Optimizer** (`global_optimizer.cpp`): Graph-based optimization for drift correction
 - **Visualizer** (`visualizer.cpp`): Real-time visualization of results
 ### GPU Acceleration
 - **OpenCL Implementation** (`cl_class.cpp`, `sparse_img_align_gpu.cpp`): GPU kernels for feature extraction and alignment
 - **Kernel Directory** (`kernel/`): OpenCL kernel source code
 ## System Diagram
 ```
 Camera Input → Feature Detection (GPU) → Feature Alignment (GPU)
                                              ↓
                                        Frame Tracking
                                              ↓
                          ┌────────────────────┴──────────────────┐
                          ↓                                         ↓
                    Pose Optimization                        Map Management
                          ↓                                         ↓
                    EKF Sensor Fusion ←─── IMU & Command Input
                          ↓
                    Global Optimization (G2O)
                          ↓
                   Odometry Output (Pose, Map)
 ```
 ## Troubleshooting
 ### OpenCL Issues
 - Verify OpenCL drivers are installed: `clinfo`
 - Check GPU compatibility with OpenCL
 - Set `VIO_DEBUG` flag in CMakeLists.txt for detailed GPU debugging
 ### Performance Issues
 - Adjust feature point limits in configuration
 - Enable/disable GPU features in `config.h`
 - Check OpenCV and G2O build flags match the VIO build flags
 ## License
 [Add appropriate license information]
 ## Contributing
 [Add contributing guidelines if applicable]
 ## References
 - Semi-direct Visual Odometry (SVO) concept
 - EKF for sensor fusion
 - G2O framework: https://github.com/RainerKuemmerle/g2o
 - OpenCV: https://opencv.org/
 - ROS: https://www.ros.org/
@@ -1,3 +1,6 @@
 # build g2o with -DG2O_HAVE_OPENGL=ON -DBUILD_WITH_MARCH_NATIVE=ON
 # G2o version => https://github.com/RainerKuemmerle/g2o/tree/memory_management
 SET(PROJECT_NAME vio)
 PROJECT(${PROJECT_NAME})
 CMAKE_MINIMUM_REQUIRED (VERSION 2.8.3)
@@ -9,8 +12,8 @@ set(CMAKE_C_STANDARD 17)
 set(CMAKE_CXX_STANDARD 17)
 # Set definitions
-ADD_DEFINITIONS(-DSVO_TRACE)
+ADD_DEFINITIONS(-DVIO_TRACE)
-ADD_DEFINITIONS(-DSVO_USE_ROS)
+ADD_DEFINITIONS(-DVIO_USE_ROS)
 # Set build flags, set ARM_ARCHITECTURE environment variable on Odroid
@@ -41,7 +44,7 @@ add_service_files(FILES
 generate_messages(DEPENDENCIES std_msgs)
 catkin_package(
        DEPENDS Eigen OpenCV Boost
-        CATKIN_DEPENDS roscpp roslib image_transport cv_bridge tf geometry_msgs message_runtime
+        CATKIN_DEPENDS roscpp roslib image_transport cv_bridge tf geometry_msgs message_runtime getOdom stop start
        INCLUDE_DIRS include
 )
 # Include dirs
@@ -64,8 +67,8 @@ LIST(APPEND LINK_LIBS
  ${Boost_Fibers_LIBRARIES}
  ${OpenCL_LIBRARIES}
  ${catkin_LIBRARIES}
-  ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY} ${G2O_SOLVER_CHOLMOD}
+  ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY} ${G2O_SOLVER_CSPARSE} ${G2O_SOLVER_STRUCTURE_ONLY}
-  ${G2O_SOLVER_PCG}
+  ${G2O_SOLVER_PCG} ${G2O_SOLVER_CSPARSE_EXTENSION}
  ${G2O_TYPES_SBA}
  ${G2O_TYPES_SLAM3D}
  ${G2O_TYPES_ICP}
@@ -76,7 +79,7 @@ file(GLOB SRC  CONFIGURE_DEPENDS "src/*.cpp" "include/sophus/*.cpp")
-# Create svo library
+# Create VIO library
 ADD_EXECUTABLE(${PROJECT_NAME} ${SRC})
 TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${LINK_LIBS})
 set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 17)
@@ -85,6 +88,6 @@ target_compile_features(${PROJECT_NAME} PRIVATE cxx_range_for)
 ADD_DEFINITIONS(-DKERNEL_DIR=\"${PROJECT_SOURCE_DIR}/kernel\")
 ADD_DEFINITIONS(-DPROJECT_DIR=\"${PROJECT_SOURCE_DIR}\")
 ADD_DEFINITIONS(-DVIO_DEBUG=true)
-ADD_DEFINITIONS(-DG2O_USE_VENDORED_CERES=ON)
+#ADD_DEFINITIONS(-DG2O_USE_VENDORED_CERES=ON)
-ADD_DEFINITIONS(-DG2O_DELETE_IMPLICITLY_OWNED_OBJECTS=OFF)
+#ADD_DEFINITIONS(-DG2O_DELETE_IMPLICITLY_OWNED_OBJECTS=OFF)
@@ -54,7 +54,7 @@ namespace vio {
            double& final_error);
 /// Global bundle adjustment.
-/// Optimizes the whole map. Is currently not used in SVO.
+/// Optimizes the whole map. Is currently not used in VIO.
    void globalBA(Map* map);
 /// Initialize g2o with solver type, optimization strategy and camera model.
@@ -88,7 +88,7 @@ namespace vio {
            double huber_width,
            double weight = 1);
-} // namespace svo
+} // namespace VIO
 #endif //VIO_BUNDLE_ADJUSTMENT_H
@@ -1,21 +1,21 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
+// This file is part of VIO - 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
+// VIO 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
+// VIO 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_
+#ifndef VIO_CONFIG_H_
-#define SVO_CONFIG_H_
+#define VIO_CONFIG_H_
 #include <string>
 #include <stdint.h>
@@ -25,7 +25,7 @@ namespace vio {
 using std::string;
-/// Global configuration file of SVO.
+/// Global configuration file of VIO.
 /// Implements the Singleton design pattern to allow global access and to ensure
 /// that only one instance exists.
 class Config
@@ -33,23 +33,10 @@ 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; }
@@ -57,8 +44,6 @@ public:
  /// 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; }
@@ -69,14 +54,10 @@ public:
  /// 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; }
@@ -85,7 +66,6 @@ public:
  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; }
@@ -93,31 +73,15 @@ public:
  /// 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; }
@@ -168,4 +132,4 @@ private:
 } // namespace vio
-#endif // SVO_CONFIG_H_
+#endif // VIO_CONFIG_H_
@@ -1,21 +1,21 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
+// This file is part of VIO - 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
+// VIO 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
+// VIO 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_
+#ifndef VIO_FEATURE_H_
-#define SVO_FEATURE_H_
+#define VIO_FEATURE_H_
 #include <vio/frame.h>
@@ -107,4 +107,4 @@ struct Feature
 } // namespace vio
-#endif // SVO_FEATURE_H_
+#endif // VIO_FEATURE_H_
@@ -1,21 +1,21 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
+// This file is part of VIO - 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
+// VIO 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
+// VIO 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_
+#ifndef VIO_FEATURE_ALIGNMENT_H_
-#define SVO_FEATURE_ALIGNMENT_H_
+#define VIO_FEATURE_ALIGNMENT_H_
 #include <vio/global.h>
@@ -60,4 +60,4 @@ bool align2D_NEON(
 } // namespace feature_alignment
 } // namespace vio
-#endif // SVO_FEATURE_ALIGNMENT_H_
+#endif // VIO_FEATURE_ALIGNMENT_H_
@@ -1,21 +1,21 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
+// This file is part of VIO - 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
+// VIO 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
+// VIO 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_
+#ifndef VIO_FEATURE_DETECTION_H_
-#define SVO_FEATURE_DETECTION_H_
+#define VIO_FEATURE_DETECTION_H_
 #include <vio/global.h>
 #include <vio/frame.h>
@@ -110,4 +110,4 @@ public:
 } // namespace feature_detection
 } // namespace vio
-#endif // SVO_FEATURE_DETECTION_H_
+#endif // VIO_FEATURE_DETECTION_H_
@@ -1,21 +1,21 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
+// This file is part of VIO - 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
+// VIO 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
+// VIO 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_
+#ifndef VIO_FRAME_H_
-#define SVO_FRAME_H_
+#define VIO_FRAME_H_
 #include <sophus/se3.h>
 #include <vio/math_utils.h>
@@ -25,9 +25,11 @@
 #include <g2o/types/sba/types_six_dof_expmap.h>
 namespace vio {
    class Point;
    class Map;
    struct Feature;
    typedef list<std::shared_ptr<Feature>> Features;
@@ -35,7 +37,7 @@ namespace vio {
 /// A frame saves the image, the associated features and the estimated pose.
-    class Frame : boost::noncopyable
+    class Frame
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@@ -50,7 +52,7 @@ namespace vio {
        Features                      fts_;                   //!< List of features in the image.
        vector<std::shared_ptr<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?
-        g2o::VertexSE3Expmap*    v_kf_=nullptr;                  //!< Temporary pointer to the g2o node object of the keyframe.
+        std::shared_ptr<g2o::VertexSE3Expmap>         v_kf_=NULL;                  //!< 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);
@@ -183,19 +185,14 @@ namespace vio {
 //            J(1,1) = 0;
 //            J(1,2) = ((sin(theta)/z_n)*gamma)-((cos(theta)/z_n)*lamda)-(((x_n/(z_n*z_n))*gamma + (y_n/(z_n*z_n))*lamda)*n2);
        }
        /// 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(vio::Map& map,double& depth_mean, double& depth_min);
    };
 /// 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 FramePtr frame, double& depth_mean, double& depth_min);
    } // namespace frame_utils
 } // namespace vio
-#endif // SVO_FRAME_H_
+#endif // VIO_FRAME_H_
@@ -1,21 +1,21 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
+// This file is part of VIO - 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
+// VIO 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
+// VIO 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_
+#ifndef VIO_FRAME_HANDLER_BASE_H_
-#define SVO_FRAME_HANDLER_BASE_H_
+#define VIO_FRAME_HANDLER_BASE_H_
 #include <queue>
 #include <boost/noncopyable.hpp>
@@ -34,7 +34,7 @@ namespace vio
 {
 /// Base class for various VO pipelines. Manages the map and the state machine.
-class FrameHandlerBase : boost::noncopyable
+class FrameHandlerBase
 {
 public:
  enum Stage {
@@ -99,8 +99,9 @@ protected:
  /// Optimize some of the observed 3D points.
  virtual void optimizeStructure(FramePtr frame, size_t max_n_pts, int max_iter);
  virtual void posEdit(FramePtr frame);
 };
 } // namespace nslam
-#endif // SVO_FRAME_HANDLER_BASE_H_
+#endif // VIO_FRAME_HANDLER_BASE_H_
@@ -1,21 +1,21 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
+// This file is part of VIO - 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
+// VIO 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
+// VIO 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_
+#ifndef VIO_FRAME_HANDLER_H_
-#define SVO_FRAME_HANDLER_H_
+#define VIO_FRAME_HANDLER_H_
 #include <set>
 #include <vio/abstract_camera.h>
@@ -28,7 +28,7 @@
 namespace vio {
-/// Monocular Visual Odometry Pipeline as described in the SVO paper.
+/// Monocular Visual Odometry Pipeline as described in the VIO paper.
 class FrameHandlerMono : public FrameHandlerBase
 {
 public:
@@ -86,4 +86,4 @@ protected:
 } // namespace vio
-#endif // SVO_FRAME_HANDLER_H_
+#endif // VIO_FRAME_HANDLER_H_
@@ -1,21 +1,21 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
+// This file is part of VIO - 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
+// VIO 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
+// VIO 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_
+#ifndef VIO_GLOBAL_H_
-#define SVO_GLOBAL_H_
+#define VIO_GLOBAL_H_
 #include <list>
 #include <vector>
@@ -30,8 +30,8 @@
 #include <sophus/se3.h>
 #include <boost/shared_ptr.hpp>
 #include<Eigen/StdVector>
-#ifndef RPG_SVO_VIKIT_IS_VECTOR_SPECIALIZED //Guard for rpg_vikit
+#ifndef RPG_VIO_VIKIT_IS_VECTOR_SPECIALIZED //Guard for rpg_vikit
-#define RPG_SVO_VIKIT_IS_VECTOR_SPECIALIZED
+#define RPG_VIO_VIKIT_IS_VECTOR_SPECIALIZED
 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector3d)
 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2d)
 #endif
@@ -143,4 +143,4 @@ namespace vio
    typedef std::shared_ptr<Frame> FramePtr;
 } // namespace vio
-#endif // SVO_GLOBAL_H_
+#endif // VIO_GLOBAL_H_
@@ -1,21 +1,21 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
+// This file is part of VIO - 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
+// VIO 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
+// VIO 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_
+#ifndef VIO_DEPTH_FILTER_H_
-#define SVO_DEPTH_FILTER_H_
+#define VIO_DEPTH_FILTER_H_
 #include <queue>
 #include <boost/thread.hpp>
@@ -30,14 +30,18 @@
 #include <g2o/core/solver.h>
 #include <g2o/core/robust_kernel_impl.h>
 #include <g2o/core/optimization_algorithm_levenberg.h>
 /*#if defined G2O_HAVE_CHOLMOD
 #include <g2o/solvers/cholmod/linear_solver_cholmod.h>
 #elif defined G2O_HAVE_CSPARSE*/
 #include <g2o/solvers/csparse/linear_solver_csparse.h>
 /*#endif*/
 #include <g2o/solvers/structure_only/structure_only_solver.h>
 #include <g2o/core/optimization_algorithm_factory.h>
 #include <g2o/types/icp/types_icp.h>
 #include <g2o/stuff/sampler.h>
 #include <memory>
-G2O_USE_OPTIMIZATION_LIBRARY(cholmod)
+G2O_USE_OPTIMIZATION_LIBRARY(csparse)
 namespace vio {
@@ -72,31 +76,24 @@ protected:
  boost::mutex mtx_;
  boost::thread* thread_;
  Map& map_;
  size_t v_id_ = 0;
  std::unique_ptr<g2o::SparseOptimizer> optimizer_=NULL;
  std::shared_ptr<g2o::CameraParameters> cam_params_=NULL;
 #if VIO_DEBUG
  FILE* log_=nullptr;
 #endif
        /// Temporary container to hold the g2o edge with reference to frame and point.
        struct EdgeContainerSE3{
            g2o::EdgeProjectXYZ2UV*     edge;
            std::shared_ptr<Frame>          frame;
            std::shared_ptr<Feature>        feature;
        };
 /// Create a g2o vertice from a keyframe object.
-        g2o::VertexSE3Expmap* createG2oFrameSE3(
+        std::shared_ptr<g2o::VertexSE3Expmap> createG2oFrameSE3(
-                FramePtr kf,
+                FramePtr kf, bool state);
                size_t id,
                bool fixed);
    /// Creates a g2o vertice from a mappoint object.
-        g2o::VertexSBAPointXYZ* createG2oPoint(
+        std::shared_ptr<g2o::VertexPointXYZ> createG2oPoint(
-                Vector3d pos,
+                Vector3d pos);
                size_t id,
                bool fixed);
  /// Creates a g2o edge between a g2o keyframe and mappoint vertice with the provided measurement.
-        g2o::EdgeProjectXYZ2UV* createG2oEdgeSE3(
+  std::shared_ptr<g2o::EdgeProjectXYZ2UV> createG2oEdgeSE3(
-                g2o::VertexSE3Expmap* v_kf,
+          std::shared_ptr<g2o::VertexSE3Expmap> v_kf,
-                g2o::VertexSBAPointXYZ* v_mp,
+          std::shared_ptr<g2o::VertexPointXYZ>v_mp,
                const Vector2d& f_up,
                bool robust_kernel,
                double huber_width,
@@ -104,8 +101,9 @@ protected:
  /// A thread that is continuously optimizing the map.
  /// Global bundle adjustment.
  void updateLoop();
  void reset_map();
 };
 } // namespace vio
-#endif // SVO_DEPTH_FILTER_H_
+#endif // VIO_DEPTH_FILTER_H_
@@ -1,21 +1,21 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
+// This file is part of VIO - 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
+// VIO 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
+// VIO 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
+#ifndef VIO_INITIALIZATION_H
-#define SVO_INITIALIZATION_H
+#define VIO_INITIALIZATION_H
 #include <vio/global.h>
 #include <vio/cl_class.h>
@@ -115,13 +115,13 @@ protected:
        if(inlier_ekf.size()>inlier_hom.size()){
            T_cur_from_ref=res.second;
            inliers=inlier_ekf;
-            xyz_in_cur=xyz_in_hom;
+            xyz_in_cur=xyz_in_ekf;
        }else if((Homography.T_c2_from_c1.translation()-res.second.se3().translation()).norm()<0.1 && inlier_ekf.size()<inlier_hom.size()){
            auto euler =Homography.T_c2_from_c1.unit_quaternion().normalized().toRotationMatrix().eulerAngles(0,1,2);
            res=ukf_->UpdateVO(Homography.T_c2_from_c1.translation().x(),Homography.T_c2_from_c1.translation().z(),euler(1));
            T_cur_from_ref=res.second;
            inliers=inlier_hom;
-            xyz_in_cur=xyz_in_ekf;
+            xyz_in_cur=xyz_in_hom;
        }
    }
@@ -154,4 +154,4 @@ void trackKlt(
 } // namespace initialization
 } // namespace vio
-#endif // SVO_INITIALIZATION_H
+#endif // VIO_INITIALIZATION_H
@@ -1,21 +1,21 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
+// This file is part of VIO - 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
+// VIO 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
+// VIO 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_
+#ifndef VIO_MAP_H_
-#define SVO_MAP_H_
+#define VIO_MAP_H_
 #include <queue>
 #include <boost/noncopyable.hpp>
@@ -28,7 +28,7 @@
 namespace vio {
 /// Map object which saves all keyframes which are in a map.
-class Map : boost::noncopyable
+class Map
 {
 public:
  list< FramePtr > keyframes_;          //!< List of keyframes in the map.
@@ -100,4 +100,4 @@ void pointValidation(std::shared_ptr<Point> point, int id);
 } // namespace map_debug
 } // namespace vio
-#endif // SVO_MAP_H_
+#endif // VIO_MAP_H_
@@ -1,21 +1,21 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
+// This file is part of VIO - 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
+// VIO 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
+// VIO 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_
+#ifndef VIO_MATCHER_H_
-#define SVO_MATCHER_H_
+#define VIO_MATCHER_H_
 #include <vio/global.h>
@@ -146,4 +146,4 @@ public:
 } // namespace vio
-#endif // SVO_MATCHER_H_
+#endif // VIO_MATCHER_H_
@@ -133,7 +133,7 @@ template<class T>
 T getMean(vector<T>& data_vec,T& min=0,T& max=0)
 {
    assert(!data_vec.empty());
-    T sum=0;
+    T sum=0.0;
    min=std::numeric_limits<T>::max();
    max=std::numeric_limits<T>::min();
    for(auto&& i:data_vec){
@@ -1,27 +1,28 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
+// This file is part of VIO - 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
+// VIO 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
+// VIO 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_
+#ifndef VIO_POINT_H_
-#define SVO_POINT_H_
+#define VIO_POINT_H_
 #include <boost/noncopyable.hpp>
 #include <vio/global.h>
 #include <boost/thread.hpp>
 #include <boost/function.hpp>
 #include <g2o/types/sba/types_six_dof_expmap.h>
 /*#include <g2o/types/icp/types_icp.h>*/
 static boost::mutex point_mut_;
 namespace vio {
@@ -31,7 +32,7 @@ class Feature;
 typedef Matrix<double, 2, 3> Matrix23d;
 /// A 3D point on the surface of the scene.
-class Point : boost::noncopyable
+class Point
 {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@@ -48,7 +49,7 @@ public:
  Vector3d                    pos_;                     //!< 3d pos of the point in the world coordinate frame.
  list<std::shared_ptr<Feature>>              obs_;                     //!< References to keyframes which observe the point.
  size_t                      n_obs_;                   //!< Number of obervations: Keyframes AND successful reprojections in intermediate frames.
-  g2o::VertexSBAPointXYZ*     v_pt_=nullptr;                    //!< Temporary pointer to the point-vertex in g2o during bundle adjustment.
+  std::shared_ptr<g2o::VertexPointXYZ>     v_pt_=NULL;                    //!< Temporary pointer to the point-vertex in g2o during bundle adjustment.
  int                         last_frame_overlap_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.
@@ -92,8 +93,8 @@ public:
        double x_n = p_in_f.x();
        double y_n = p_in_f.y();
        double z_n = p_in_f.z();
-        double x_c =  fram_t_f_w.se3().translation().x();
+        double x_c =  fram_t_f_w.se2().translation()(0);
-        double z_c = fram_t_f_w.se3().translation().y();
+        double z_c = fram_t_f_w.se2().translation()(1);
        double theta = fram_t_f_w.pitch();
        double alpha = (fx*(theta/r))-(fx*((x_n*x_n)/(r*r))*theta)+((1+3*s*theta*theta)/((r*r)+1))*((fx*x_n*x_n)/(r*r));
@@ -115,9 +116,24 @@ public:
        point_jac(1, 2) = ((1/z_n)*gamma*n1)-(((x_n/(z_n*z_n))*gamma + (y_n/(z_n*z_n))*lamda)*n2);
        //point_jac = - point_jac * R_f_w;
    }
    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 vio
-#endif // SVO_POINT_H_
+#endif // VIO_POINT_H_
@@ -1,21 +1,21 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
+// This file is part of VIO - 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
+// VIO 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
+// VIO 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_
+#ifndef VIO_POSE_OPTIMIZER_H_
-#define SVO_POSE_OPTIMIZER_H_
+#define VIO_POSE_OPTIMIZER_H_
 #include <vio/global.h>
 #include <vio/map.h>
@@ -48,4 +48,4 @@ void optimizeGaussNewton(
 } // namespace pose_optimizer
 } // namespace vio
-#endif // SVO_POSE_OPTIMIZER_H_
+#endif // VIO_POSE_OPTIMIZER_H_
@@ -1,21 +1,21 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
+// This file is part of VIO - 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
+// VIO 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
+// VIO 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_
+#ifndef VIO_REPROJECTION_H_
-#define SVO_REPROJECTION_H_
+#define VIO_REPROJECTION_H_
 #include <vio/global.h>
 #include <vio/matcher.h>
@@ -75,9 +75,8 @@ private:
  /// will search a maching feature in the image.
  struct Candidate {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    std::shared_ptr<Point> pt;       //!< 3D point.
    Vector2d px;     //!< projected 2D pixel location.
-    Candidate(std::shared_ptr<Point> pt, Vector2d& px) : pt(pt), px(px) {}
+    Candidate(Vector2d& px) : px(px) {}
  };
  typedef std::list<Candidate > Cell;
  typedef std::vector<Cell*> CandidateGrid;
@@ -102,4 +101,4 @@ private:
 } // namespace vio
-#endif // SVO_REPROJECTION_H_
+#endif // VIO_REPROJECTION_H_
@@ -1,21 +1,21 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
+// This file is part of VIO - 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
+// VIO 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
+// VIO 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_
+#ifndef VIO_SPARSE_IMG_ALIGN_H_
-#define SVO_SPARSE_IMG_ALIGN_H_
+#define VIO_SPARSE_IMG_ALIGN_H_
 #include <vio/nlls_solver.h>
 #include <vio/global.h>
@@ -82,4 +82,4 @@ protected:
 } // namespace vio
-#endif // SVO_SPARSE_IMG_ALIGN_H_
+#endif // VIO_SPARSE_IMG_ALIGN_H_
@@ -1,13 +1,13 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
+// This file is part of VIO - 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
+// VIO 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
+// VIO 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.
 //
@@ -1,21 +1,21 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
+// This file is part of VIO - 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
+// VIO 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
+// VIO 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_VISUALIZER_H_
+#ifndef VIO_VISUALIZER_H_
-#define SVO_VISUALIZER_H_
+#define VIO_VISUALIZER_H_
 #include <queue>
 #include <ros/ros.h>
@@ -59,4 +59,4 @@ public:
 } // end namespace vio
-#endif /* SVO_VISUALIZER_H_ */
+#endif /* VIO_VISUALIZER_H_ */
@@ -1,16 +1,16 @@
 <launch>
-  <node pkg="vio_svo" type="vo" name="vio" clear_params="true" output="screen">
+  <node pkg="vio_VIO" type="vo" name="vio" clear_params="true" output="screen">
    <!-- Camera topic to subscribe to -->
    <param name="cam_topic" value="/camera/image_raw" type="str" />
    <param name="imu_topic" value="/SensorNode/imu" type="str" />
    <!-- Camera calibration file -->
-    <rosparam file="$(find vio_svo)/param/camera_atan.yaml" />
+    <rosparam file="$(find vio_VIO)/param/camera_atan.yaml" />
    <!-- Default parameter settings: choose between vo_fast and vo_accurate -->
-    <rosparam file="$(find vio_svo)/param/vo_fast.yaml" />
+    <rosparam file="$(find vio_VIO)/param/vo_fast.yaml" />
    <!-- Initial camera orientation, make it point downwards -->
    <param name="init_rx" value="3.14" />
@@ -1,16 +1,16 @@
 <launch>
-  <node pkg="vio_svo" type="vo" name="vio" clear_params="true" output="screen">
+  <node pkg="vio_VIO" type="vo" name="vio" clear_params="true" output="screen">
    <!-- Camera topic to subscribe to -->
    <param name="cam_topic" value="/image_converter/output_image " type="str" />
    <param name="imu_topic" value="/imu_unity/imu" type="str" />
    <!-- Camera calibration file -->
-    <rosparam file="$(find vio_svo)/param/unity.yaml" />
+    <rosparam file="$(find vio_VIO)/param/unity.yaml" />
    <!-- Default parameter settings: choose between vo_fast and vo_accurate -->
-    <rosparam file="$(find vio_svo)/param/vo_fast.yaml" />
+    <rosparam file="$(find vio_VIO)/param/vo_fast.yaml" />
    <!-- Initial camera orientation, make it point downwards -->
    <param name="init_rx" value="0.00" />
@@ -0,0 +1,272 @@
 [10:20:40.613] New key frame 
 [10:20:40.613] BA loop run 
 [10:20:41.976] New key frame 
 [10:20:41.977] BA loop run 
 [10:20:42.166] New key frame 
 [10:20:42.166] BA loop run 
 [10:20:43.810] New key frame 
 [10:20:43.810] BA loop run 
 [10:20:44.089] New key frame 
 [10:20:44.089] BA loop run 
 [10:20:45.872] New key frame 
 [10:20:45.872] BA loop run 
 [10:20:46.135] New key frame 
 [10:20:46.135] BA loop run 
 [10:20:46.138] init error: 111.515436 
 [10:20:46.141] end error: 1.729429 
 [10:20:49.427] New key frame 
 [10:20:49.428] BA loop run 
 [10:20:49.433] init error: 254.786029 
 [10:20:49.443] end error: 15.672577 
 [10:20:49.832] New key frame 
 [10:20:49.832] BA loop run 
 [10:20:49.838] init error: 249.475397 
 [10:20:49.849] end error: 49.278147 
 [10:20:50.532] New key frame 
 [10:20:50.532] BA loop run 
 [10:20:50.538] init error: 254.368373 
 [10:20:50.552] end error: 45.489316 
 [10:20:51.395] New key frame 
 [10:20:51.396] BA loop run 
 [10:20:51.403] init error: 252.991840 
 [10:20:51.417] end error: 26.570105 
 [10:20:52.201] New key frame 
 [10:20:52.201] BA loop run 
 [10:20:52.207] init error: 271.855495 
 [10:20:52.220] end error: 29.310106 
 [10:20:53.086] New key frame 
 [10:20:53.086] BA loop run 
 [10:20:53.093] init error: 274.895253 
 [10:20:53.104] end error: 30.065412 
 [10:20:53.867] New key frame 
 [10:20:53.867] BA loop run 
 [10:20:53.876] init error: 334.202889 
 [10:20:53.889] end error: 64.227123 
 [10:20:54.625] New key frame 
 [10:20:54.625] BA loop run 
 [10:20:54.631] init error: 193.785127 
 [10:20:54.641] end error: 49.594006 
 [10:20:55.270] New key frame 
 [10:20:55.271] BA loop run 
 [10:20:55.278] init error: 298.736841 
 [10:20:55.290] end error: 41.156798 
 [10:20:56.208] New key frame 
 [10:20:56.208] BA loop run 
 [10:20:56.218] init error: 367.881322 
 [10:20:56.234] end error: 44.782346 
 [10:20:57.149] New key frame 
 [10:20:57.149] BA loop run 
 [10:20:57.159] init error: 2983.504957 
 [10:20:57.177] end error: 54.766284 
 [10:20:58.334] New key frame 
 [10:20:58.334] BA loop run 
 [10:20:58.346] init error: 369.850114 
 [10:20:58.370] end error: 51.997171 
 [10:20:59.855] New key frame 
 [10:20:59.855] BA loop run 
 [10:20:59.868] init error: 481.237189 
 [10:20:59.889] end error: 72.018658 
 [10:21:01.060] New key frame 
 [10:21:01.060] BA loop run 
 [10:21:01.074] init error: 399.810786 
 [10:21:01.096] end error: 75.617754 
 [10:21:02.464] New key frame 
 [10:21:02.464] BA loop run 
 [10:21:02.474] init error: 475.060363 
 [10:21:02.493] end error: 79.983876 
 [10:21:03.877] New key frame 
 [10:21:03.877] BA loop run 
 [10:21:03.887] init error: 457.014129 
 [10:21:03.906] end error: 83.459347 
 [10:21:05.323] New key frame 
 [10:21:05.323] BA loop run 
 [10:21:05.341] init error: 747.490295 
 [10:21:05.372] end error: 132.027915 
 [10:21:07.316] New key frame 
 [10:21:07.316] BA loop run 
 [10:21:07.331] init error: 822.589732 
 [10:21:07.359] end error: 926.803751 
 [10:21:08.623] New key frame 
 [10:21:08.623] BA loop run 
 [10:21:08.637] init error: 885.983129 
 [10:21:08.665] end error: 534.120875 
 [10:21:09.864] New key frame 
 [10:21:09.864] BA loop run 
 [10:21:09.883] init error: 864.099098 
 [10:21:09.910] end error: 1925.142176 
 [10:21:11.044] New key frame 
 [10:21:11.044] BA loop run 
 [10:21:11.061] init error: 285204.654693 
 [10:21:11.089] end error: 1542.316455 
 [10:21:13.905] New key frame 
 [10:21:13.905] BA loop run 
 [10:21:13.919] init error: 5800.269291 
 [10:21:13.945] end error: 939.296280 
 [10:21:20.938] New key frame 
 [10:21:20.938] BA loop run 
 [10:21:20.952] init error: 17686.291865 
 [10:21:20.982] end error: 1993.793895 
 [10:21:24.731] New key frame 
 [10:21:24.732] BA loop run 
 [10:21:24.748] init error: 175503.496642 
 [10:21:24.783] end error: 7875.903157 
 [10:21:30.048] New key frame 
 [10:21:30.048] BA loop run 
 [10:21:30.069] init error: 10923372.480944 
 [10:21:30.109] end error: 2767.293737 
 [10:21:32.819] New key frame 
 [10:21:32.819] BA loop run 
 [10:21:32.840] init error: 4407303.714091 
 [10:21:32.877] end error: 2406.055787 
 [10:21:35.019] New key frame 
 [10:21:35.019] BA loop run 
 [10:21:35.038] init error: 75150489.050763 
 [10:21:35.069] end error: 3842.685517 
 [10:21:40.769] New key frame 
 [10:21:40.769] BA loop run 
 [10:21:40.786] init error: 129925.548703 
 [10:21:40.812] end error: 2806.264523 
 [10:21:43.440] New key frame 
 [10:21:43.440] BA loop run 
 [10:21:43.453] init error: 16329915.161626 
 [10:21:43.472] end error: 2300.690079 
 [10:21:45.879] New key frame 
 [10:21:45.879] BA loop run 
 [10:21:45.893] init error: 150162.722231 
 [10:21:45.912] end error: 1889.360943 
 [10:21:47.906] New key frame 
 [10:21:47.906] BA loop run 
 [10:21:47.922] init error: 58310.067507 
 [10:21:47.942] end error: 1254.251346 
 [10:21:50.032] New key frame 
 [10:21:50.032] BA loop run 
 [10:21:50.047] init error: 779404.826745 
 [10:21:50.067] end error: 1328.589741 
 [10:21:52.256] New key frame 
 [10:21:52.256] BA loop run 
 [10:21:52.269] init error: 358418.400359 
 [10:21:52.286] end error: 1413.448003 
 [10:21:54.031] New key frame 
 [10:21:54.031] BA loop run 
 [10:21:54.042] init error: 156363.448534 
 [10:21:54.056] end error: 3676.950800 
 [10:21:55.370] New key frame 
 [10:21:55.370] BA loop run 
 [10:21:55.380] init error: 1109576.860168 
 [10:21:55.392] end error: 1065.199685 
 [10:21:56.652] New key frame 
 [10:21:56.652] BA loop run 
 [10:21:56.663] init error: 35530.244947 
 [10:21:56.675] end error: 1150.186793 
 [10:21:57.840] New key frame 
 [10:21:57.841] BA loop run 
 [10:21:57.851] init error: 10924.093702 
 [10:21:57.863] end error: 479.159296 
 [10:21:58.980] New key frame 
 [10:21:58.980] BA loop run 
 [10:21:58.991] init error: 8503.011338 
 [10:21:59.001] end error: 655.495939 
 [10:22:00.300] New key frame 
 [10:22:00.300] BA loop run 
 [10:22:00.309] init error: 194851.488259 
 [10:22:00.318] end error: 701.052402 
 [10:22:01.076] New key frame 
 [10:22:01.076] BA loop run 
 [10:22:01.083] init error: 127681.981590 
 [10:22:01.091] end error: 661.633557 
 [10:22:01.867] New key frame 
 [10:22:01.867] BA loop run 
 [10:22:01.875] init error: 20426.846409 
 [10:22:01.881] end error: 198.901288 
 [10:22:02.618] New key frame 
 [10:22:02.618] BA loop run 
 [10:22:02.625] init error: 4718850.400845 
 [10:22:02.630] end error: 421.309123 
 [10:22:03.208] New key frame 
 [10:22:03.208] BA loop run 
 [10:22:03.214] init error: 22146604.173581 
 [10:22:03.220] end error: 911.872444 
 [10:22:03.738] New key frame 
 [10:22:03.739] BA loop run 
 [10:22:03.745] init error: 21930.877091 
 [10:22:03.750] end error: 1494.255573 
 [10:22:04.286] New key frame 
 [10:22:04.286] BA loop run 
 [10:22:04.292] init error: 2517.970856 
 [10:22:04.296] end error: 320.399692 
 [10:22:04.682] New key frame 
 [10:22:04.682] BA loop run 
 [10:22:04.687] init error: 11071.056653 
 [10:22:04.692] end error: 207.054228 
 [10:22:05.035] New key frame 
 [10:22:05.035] BA loop run 
 [10:22:05.040] init error: 7824.538956 
 [10:22:05.044] end error: 215.049708 
 [10:22:05.523] New key frame 
 [10:22:05.523] BA loop run 
 [10:22:05.528] init error: 1408508.381403 
 [10:22:05.533] end error: 2079.123600 
 [10:22:06.130] New key frame 
 [10:22:06.130] BA loop run 
 [10:22:06.137] init error: 28374.682691 
 [10:22:06.143] end error: 265.084628 
 [10:22:06.810] New key frame 
 [10:22:06.811] BA loop run 
 [10:22:06.818] init error: 92146.833883 
 [10:22:06.826] end error: 363.156574 
 [10:22:07.437] New key frame 
 [10:22:07.437] BA loop run 
 [10:22:07.443] init error: 812.305994 
 [10:22:07.449] end error: 366.001259 
 [10:22:08.064] New key frame 
 [10:22:08.064] BA loop run 
 [10:22:08.071] init error: 35646.460541 
 [10:22:08.078] end error: 15622.440773 
 [10:22:08.625] New key frame 
 [10:22:08.625] BA loop run 
 [10:22:08.634] init error: 15057.402516 
 [10:22:08.641] end error: 487.835605 
 [10:22:09.281] New key frame 
 [10:22:09.281] BA loop run 
 [10:22:09.288] init error: 865.462673 
 [10:22:09.294] end error: 337.904637 
 [10:22:09.796] New key frame 
 [10:22:09.796] BA loop run 
 [10:22:09.804] init error: 15216.998779 
 [10:22:09.815] end error: 278.006301 
 [10:22:10.339] New key frame 
 [10:22:10.339] BA loop run 
 [10:22:10.346] init error: 2806.291024 
 [10:22:10.354] end error: 328.467991 
 [10:22:12.654] New key frame 
 [10:22:12.654] BA loop run 
 [10:22:12.663] init error: 68848.199953 
 [10:22:12.676] end error: 509.210944 
 [10:22:13.388] New key frame 
 [10:22:13.388] BA loop run 
 [10:22:13.403] init error: 4722243.529127 
 [10:22:13.422] end error: 2687.242929 
 [10:22:14.115] New key frame 
 [10:22:14.115] BA loop run 
 [10:22:14.131] init error: 19432.541420 
 [10:22:14.153] end error: 335.205556 
 [10:22:15.030] New key frame 
 [10:22:15.030] BA loop run 
 [10:22:15.049] init error: 74414.967515 
 [10:22:15.077] end error: 802.217198 
 [10:22:15.815] New key frame 
 [10:22:15.815] BA loop run 
 [10:22:15.834] init error: 211864.358318 
 [10:22:15.861] end error: 861.614087 
 [10:22:16.708] New key frame 
 [10:22:16.708] BA loop run 
 [10:22:16.727] init error: 31442.228519 
 [10:22:16.756] end error: 1922.391569 
 [10:22:17.579] New key frame 
 [10:22:17.579] BA loop run 
 [10:22:17.599] init error: 219444.324630 
 [10:22:17.628] end error: 2139.696878 
 [10:22:18.674] New key frame 
 [10:22:18.674] BA loop run 
 [10:22:18.697] init error: 103016.190886 
 [10:22:18.730] end error: 2421.178836 
@@ -31,4 +31,7 @@
  <run_depend>tf</run_depend>
  <run_depend>geometry_msgs</run_depend>
  <run_depend>message_runtime</run_depend>
  <run_depend>getOdom</run_depend>
  <run_depend>start</run_depend>
  <run_depend>stop</run_depend>
 </package>
@@ -1,6 +1,6 @@
 vio:
  grid_size: 8            #Feature grid size of a cell in [px].
-  max_n_kfs: 20            #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.
+  max_n_kfs: 30            #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.
  loba_num_iter: 10         #Number of iterations in the local bundle adjustment.
  quality_min_fts: 10      #If the number of tracked features drops below this threshold. Tracking quality is bad.
  quality_max_drop_fts: 90 #If within one frame, this amount of features are dropped. Tracking quality is bad.
@@ -8,11 +8,10 @@ vio:
  init_min_disparity: 10
  structureoptim_max_pts: 50
  kfselect_mindist: 0.01
-  poseoptim_thresh: 8.0
+  poseoptim_thresh: 0.25
  init_min_inliers: 40
  ACC_ekf: 0.2          #acc white noise in continuous in EKF
-  GYO_ekf: 0.5         #gyro white noise in continuous in EKF
+  GYO_ekf: 0.1         #gyro white noise in continuous in EKF
  VO_ekf_translation: 1.0              #VO noise  in EKF
-  VO_ekf_orientation: 1.5              #VO noise  in EKF
+  VO_ekf_orientation: 1.0              #VO noise  in EKF
  CMD_ekf_translation: 0.1  #control vector noise in EKF
  CMD_ekf_orientation: 0.01  #control vector noise in EKF
@@ -5,16 +5,16 @@
 #include "vio/bundle_adjustment.h"
-// This file is part of SVO - Semi-direct Visual Odometry.
+// This file is part of VIO - 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
+// VIO 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
+// VIO 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.
 //
@@ -436,5 +436,5 @@ namespace vio {
            return e;
        }
-} // namespace svo
+} // namespace VIO
 */
@@ -1,13 +1,13 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
+// This file is part of VIO - 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
+// VIO 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
+// VIO 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.
 //
@@ -20,7 +20,7 @@
 namespace vio {
 Config::Config() :
-    trace_name(vk::getParam<string>("vio/trace_name", "svo")),
+    trace_name(vk::getParam<string>("vio/trace_name", "VIO")),
    trace_dir(vk::getParam<string>("vio/trace_dir", "/tmp")),
    n_pyr_levels(vk::getParam<int>("vio/n_pyr_levels", 3)),
    use_imu(vk::getParam<bool>("vio/use_imu", false)),
@@ -36,7 +36,7 @@ Config::Config() :
    poseoptim_thresh(vk::getParam<double>("vio/poseoptim_thresh", 2.0)),
    poseoptim_num_iter(vk::getParam<int>("vio/poseoptim_num_iter", 10)),
    structureoptim_max_pts(vk::getParam<int>("vio/structureoptim_max_pts", 20)),
-    structureoptim_num_iter(vk::getParam<int>("vio/structureoptim_num_iter", 5)),
+    structureoptim_num_iter(vk::getParam<int>("vio/structureoptim_num_iter", 10)),
    loba_thresh(vk::getParam<double>("vio/loba_thresh", 2.0)),
    loba_robust_huber_width(vk::getParam<double>("vio/loba_robust_huber_width", 1.0)),
    loba_num_iter(vk::getParam<int>("vio/loba_num_iter", 0)),
@@ -1,13 +1,13 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
+// This file is part of VIO - 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
+// VIO 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
+// VIO 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.
 //
@@ -1,13 +1,13 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
+// This file is part of VIO - 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
+// VIO 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
+// VIO 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.
 //
@@ -73,7 +73,6 @@ void FastDetector::detect(
    list<shared_ptr<Feature>>& fts)
    {
  std::vector<cv::KeyPoint> keypoints;
  fts.clear();
  for(int L=0; L<n_pyr_levels_; ++L)
  {
    if(L>img_pyr.size())return;
@@ -116,8 +115,7 @@ void FastDetector::detect(
    free(fast_corners);
  }
  if(keypoints.size()<1){
-      ROS_ERROR("GPU Driver crash try again!");
+      assert(0 && "GPU Driver crash try again!");
      assert(0);
  }
  cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 22.0f, 4);
  cv::Mat descriptor;
@@ -1,13 +1,13 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
+// This file is part of VIO - 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
+// VIO 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
+// VIO 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.
 //
@@ -21,6 +21,7 @@
 #include <vio/config.h>
 #include <vio/math_utils.h>
 #include <vio/vision.h>
 #include <vio/map.h>
 namespace vio {
@@ -53,7 +54,7 @@ void Frame::initFrame(const cv::Mat& img)
  for(auto&& ftr:key_pts_)ftr.reset();
  // Build Image Pyramid
-  frame_utils::createImgPyramid(img, max(Config::nPyrLevels(), Config::kltMaxLevel()+1), img_pyr_);
+   createImgPyramid(img, max(Config::nPyrLevels(), Config::kltMaxLevel()+1), img_pyr_);
 }
 void Frame::setKeyframe()
@@ -154,11 +155,7 @@ bool Frame::isVisible(const Vector3d& xyz_w) const
  return false;
 }
-
+void Frame::createImgPyramid(const cv::Mat& img_level_0, int n_levels, ImgPyr& pyr)
 /// 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;
@@ -169,16 +166,23 @@ void createImgPyramid(const cv::Mat& img_level_0, int n_levels, ImgPyr& pyr)
  }
 }
-bool getSceneDepth(const FramePtr frame, double& depth_mean, double& depth_min)
+bool Frame::getSceneDepth(vio::Map& map,double& depth_mean, double& depth_min)
 {
  vector<double> depth_vec;
-  for(auto it=frame->fts_.begin(), ite=frame->fts_.end(); it!=ite; ++it)
+  for(auto it=fts_.begin(); it!=fts_.end();)
  {
-    if((*it)->point != NULL)
+      if((*it)->point==NULL){
-    {
+          ++it;
-        double z = frame->w2f((*it)->point->pos_).z();
+          continue;
-        depth_vec.push_back(z);
+      }
-    }
+      double z=w2f((*it)->point->pos_).z();
      if((*it)->point->pos_.hasNaN() || (*it)->point->pos_.norm()==0. || z<0.05 || z > 20.0){
          map.safeDeletePoint((*it)->point);
          it = fts_.erase(it);
          continue;
      }
      depth_vec.push_back(z);
      ++it;
  }
  if(depth_vec.empty())
  {
@@ -189,5 +193,4 @@ bool getSceneDepth(const FramePtr frame, double& depth_mean, double& depth_min)
  return true;
 }
 } // namespace frame_utils
 } // namespace vio
@@ -1,13 +1,13 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
+// This file is part of VIO - 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
+// VIO 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
+// VIO 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.
 //
@@ -54,7 +54,6 @@ bool FrameHandlerBase::startFrameProcessingCommon(const double timestamp)
  if(stage_ == STAGE_PAUSED)
    return false;
  // some cleanup from last iteration, can't do before because of visualization
  map_.emptyTrash();
  return true;
 }
@@ -98,17 +97,40 @@ void FrameHandlerBase::optimizeStructure(
    size_t max_n_pts,
    int max_iter)
 {
  deque<std::shared_ptr<Point>> pts;
  for(auto&& it:frame->fts_){
-      if(it->point!=NULL)pts.push_back(it->point);
+      if(it->point==NULL)continue;
-  }
+      if(it->point->obs_.size()<2){
-  //max_n_pts = min(max_n_pts, pts.size());
+          it->point->last_frame_overlap_id_= frame->id_;
-  //nth_element(pts.begin(), pts.begin() + max_n_pts, pts.end(), ptLastOptimComparator);
+          continue;
-  for(auto&& pt:pts){
+      }
-      pt->optimize(max_iter);
+      it->point->optimize(max_iter);
-      pt->last_frame_overlap_id_= frame->id_;
+      it->point->last_frame_overlap_id_= frame->id_;
  }
 }
-
+void FrameHandlerBase::posEdit(
            FramePtr frame)
    {
        for(auto&& it:frame->fts_){
            if(it->point!=NULL && !it->point->pos_.hasNaN() && it->point->pos_.norm() !=0.){
                if(it->point->obs_.size()==2){
                    Eigen::Matrix<double,4,4> A,frame_a,frame_b;
                    frame_a=it->point->obs_.front()->frame->se3().matrix();
                    frame_b=it->point->obs_.back()->frame->se3().matrix();
                    A.row(0) = it->point->obs_.front()->f(0) * frame_a.row(2) - it->point->obs_.front()->f(2) * frame_a.row(0);
                    A.row(1) = it->point->obs_.front()->f(1) * frame_a.row(2) - it->point->obs_.front()->f(2) * frame_a.row(1);
                    A.row(2) = it->point->obs_.back()->f(0) * frame_b.row(2) - it->point->obs_.back()->f(2) * frame_b.row(0);
                    A.row(3) = it->point->obs_.back()->f(1) * frame_b.row(2) - it->point->obs_.back()->f(2) * frame_b.row(1);
                    // Aを特異値分解する (A = U S Vt)
                    // https://eigen.tuxfamily.org/dox/classEigen_1_1JacobiSVD.html
                    Eigen::JacobiSVD<Eigen::Matrix<double,4,4>> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
                    const Eigen::Matrix<double,4,1> singular_vector = svd.matrixV().block<4, 1>(0, 3);
                    it->point->pos_=singular_vector.block<3, 1>(0, 0) / singular_vector(3);
                }
                //if(frame->w2f(it->point->pos_).z()<1e-9)map_.safeDeletePoint(it->point);
            }else{
                map_.safeDeletePoint(it->point);
            }
        }
    }
 } // namespace vio
@@ -1,13 +1,13 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
+// This file is part of VIO - 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
+// VIO 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
+// VIO 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.
 //
@@ -96,9 +96,9 @@ FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame()
  if(klt_homography_init_->addFirstFrame(new_frame_) == initialization::FAILURE){
      return RESULT_NO_KEYFRAME;
  }
-  new_frame_->setKeyframe();
+  //new_frame_->setKeyframe();
-  map_.reset();
+  //map_.reset();
-  map_.addKeyframe(new_frame_);
+  //map_.addKeyframe(new_frame_);
  stage_ = STAGE_SECOND_FRAME;
 #if VIO_DEBUG
    fprintf(log_,"[%s] Init: Selected first frame. \n",vio::time_in_HH_MM_SS_MMM().c_str());
@@ -122,17 +122,17 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
      return RESULT_FAILURE;
  }
  new_frame_->setKeyframe();
  double depth_mean, depth_min;
  frame_utils::getSceneDepth(new_frame_, depth_mean, depth_min);
  // add frame to map
  map_.addKeyframe(new_frame_);
  stage_ = STAGE_DEFAULT_FRAME;
  klt_homography_init_->reset();
  double depth_mean, depth_min;
  new_frame_->getSceneDepth(map_,depth_mean, depth_min);
  // add frame to map
 #if VIO_DEBUG
    fprintf(log_,"[%s] Init: Selected Second frame. \t The number of features: %d depth mean:%f min:%f\n",
                       vio::time_in_HH_MM_SS_MMM().c_str(),new_frame_->fts_.size(),depth_mean,depth_min);
 #endif
-  ba_glob_->new_key_frame();
+  //ba_glob_->new_key_frame();
  ROS_INFO("VIO initialized :)");
  ROS_INFO("Running ...");
  return RESULT_IS_KEYFRAME;
@@ -158,7 +158,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
  size_t sfba_n_edges_final=0;
  double sfba_thresh, sfba_error_init, sfba_error_final;
  pose_optimizer::optimizeGaussNewton(
-            Config::poseOptimThresh(),
+            10,
            new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final,map_,log_);
 #if VIO_DEBUG
    fprintf(log_,"[%s] After pose optimization, distance between ekf and vo x:%f ,z=%f,angle between two frames:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
@@ -180,10 +180,14 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
            new_frame_->fts_.size(),new_frame_->nObs());
 #endif
  double depth_mean=0.0, depth_min=0.0;
-  frame_utils::getSceneDepth(new_frame_, depth_mean, depth_min);
+    new_frame_->getSceneDepth(map_, depth_mean, depth_min);
-  if(depth_mean>10.0)return RESULT_NO_KEYFRAME;
+#if VIO_DEBUG
-
+    fprintf(log_,"[%s] frame Scene Depth mean:%f ,depth min:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
            depth_mean,
            depth_min);
 #endif
  optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
  // select keyframe
  if(!needNewKf())//edited
@@ -199,7 +203,11 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
 #endif
  // new keyframe selected
  for(auto&& it:new_frame_->fts_){
-      if(it->point != NULL)it->point->addFrameRef(it);
+      if(it->point != NULL && !it->point->pos_.hasNaN() && it->point->pos_.norm() !=0.){
          it->point->addFrameRef(it);
      }else{
          map_.safeDeletePoint(it->point);
      }
  }
  // if limited number of keyframes, remove the one furthest apart
@@ -210,9 +218,14 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
  }
  // add keyframe to map
  map_.addKeyframe(new_frame_);
-  map_.checkKeyFrames();
+  if(map_.checkKeyFrames()){
-  ba_glob_->new_key_frame();
+      ba_glob_->new_key_frame();
-  return RESULT_IS_KEYFRAME;
+/*      std::unique_ptr<feature_detection::FastDetector> detector=std::make_unique<feature_detection::FastDetector>(
              new_frame_->img().cols, new_frame_->img().rows, Config::gridSize(), gpu_fast_,Config::nPyrLevels());
      detector->detect(new_frame_, new_frame_->img_pyr_, Config::triangMinCornerScore(), new_frame_->fts_);*/
      return RESULT_IS_KEYFRAME;
  }
  return RESULT_FAILURE;
 }
@@ -0,0 +1,230 @@
 // This file is part of VIO - Semi-direct Visual Odometry.
 //
 // Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
 // (Robotics and Perception Group, University of Zurich, Switzerland).
 //
 // VIO 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.
 //
 // VIO 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 <vio/vision.h>
 #include <boost/bind.hpp>
 #include <boost/math/distributions/normal.hpp>
 #include <vio/global.h>
 #include <vio/global_optimizer.h>
 #include <vio/frame.h>
 #include <vio/point.h>
 #include <vio/feature.h>
 #include <vio/config.h>
 #include <g2o/solvers/structure_only/structure_only_solver.h>
 #if VIO_DEBUG
 #include <sys/types.h>
 #include <sys/stat.h>
 #endif
 namespace vio {
    BA_Glob::BA_Glob(Map& map) :map_(map),thread_(NULL)
    {
        optimizer_=std::make_unique<g2o::SparseOptimizer>();
        optimizer_->setVerbose(false);
        /*g2o::BlockSolver_6_3::LinearSolverType * linearSolver=new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>();*/
        std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver=g2o::make_unique<g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType>>();
        std::unique_ptr<g2o::OptimizationAlgorithm> solver(new g2o::OptimizationAlgorithmLevenberg(
                g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver))));
        optimizer_->setAlgorithm(std::move(solver));
        // setup camera
        cam_params_ =std::make_shared<g2o::CameraParameters>(1.0, Vector2d(0.,0.), 0.);
        cam_params_->setId(0);
        if (!optimizer_->addParameter(cam_params_)) {
            assert(false && "Camera initialization in BA");
        }
 #if VIO_DEBUG
        log_ =fopen((std::string(PROJECT_DIR)+"/loop_closure_log.txt").c_str(),"w+");
        assert(log_);
        chmod((std::string(PROJECT_DIR)+"/loop_closure_log.txt").c_str(), ACCESSPERMS);
 #endif
    }
    BA_Glob::~BA_Glob()
    {
        stopThread();
    }
    void BA_Glob::startThread()
    {
        thread_ = new boost::thread(&BA_Glob::updateLoop, this);
    }
    void BA_Glob::stopThread()
    {
        if(thread_ != NULL)
        {
            thread_->interrupt();
            usleep(5000);
            thread_->join();
            thread_ = NULL;
        }
 #if VIO_DEBUG
        fclose(log_);
 #endif
    }
    void BA_Glob::updateLoop()
    {
        while(!boost::this_thread::interruption_requested())
        {
            boost::unique_lock< boost::mutex > lk( mtx_);
            while(map_.keyframes_.empty() || new_keyframe_ == false)
                cond_.wait(lk);
 #if VIO_DEBUG
            fprintf(log_,"[%s] BA loop run \n",
                    vio::time_in_HH_MM_SS_MMM().c_str());
 #endif
            new_keyframe_=false;
            // init g2o
            g2o::OptimizableGraph::VertexContainer points;
            ba_mux_.lock();
            // Go through all Keyframes
            v_id_ = 0;
            auto end=map_.keyframes_.end();
            auto end_1=end--;
            for(auto it_kf=map_.keyframes_.begin();it_kf!=map_.keyframes_.end();++it_kf)
            {
                // New Keyframe Vertex
                if(it_kf !=map_.keyframes_.end() && it_kf !=end && it_kf !=end_1){
                    (*it_kf)->v_kf_ = createG2oFrameSE3(*it_kf,true);
                }else{
                    (*it_kf)->v_kf_ = createG2oFrameSE3(*it_kf,false);
                }
                optimizer_->addVertex((*it_kf)->v_kf_);
                for(auto& it_ftr:(*it_kf)->fts_)
                {
                    if(it_ftr->point==NULL)continue;
                    if(it_ftr->point->type_ != vio::Point::TYPE_GOOD)continue;
                    // for each keyframe add edges to all observed mapoints
                    if(it_ftr->point->pos_.hasNaN())continue;
                    if(it_ftr->point->pos_.norm()==0.)continue;
                    if(it_ftr->point->v_pt_ == NULL)
                    {
                        // mappoint-vertex doesn't exist yet. create a new one:
                        it_ftr->point->v_pt_ = createG2oPoint(it_ftr->point->pos_);
                        optimizer_->addVertex(it_ftr->point->v_pt_);
                    }
                    optimizer_->addEdge(createG2oEdgeSE3((*it_kf)->v_kf_, it_ftr->point->v_pt_, vk::project2d(it_ftr->f),
                                                                         true,
                                                                         Config::poseOptimThresh()/(*it_kf)->cam_->errorMultiplier2()*Config::lobaRobustHuberWidth()));
                }
            }
            for (g2o::OptimizableGraph::VertexIDMap::const_iterator it = optimizer_->vertices().begin();
                 it != optimizer_->vertices().end(); ++it) {
                auto v = std::static_pointer_cast<g2o::OptimizableGraph::Vertex>(it->second);
                if (v->dimension() == 3 && v->edges().size()>2) points.push_back(v);
            }
            // Optimization
            if(points.empty()){
                optimizer_->clear();
                ba_mux_.unlock();
                continue;
            }
            optimizer_->initializeOptimization();
            optimizer_->computeActiveErrors();
            g2o::StructureOnlySolver<3> structure_only_ba;
            structure_only_ba.calc(points, vio::Config::lobaNumIter());
 #if VIO_DEBUG
            fprintf(log_,"[%s] init error: %f \n",
                    vio::time_in_HH_MM_SS_MMM().c_str(),optimizer_->activeChi2());
 #endif
            if(optimizer_->optimize(vio::Config::lobaNumIter())<1){
                optimizer_->clear();
                ba_mux_.unlock();
                continue;
            }
 #if VIO_DEBUG
            fprintf(log_,"[%s] end error: %f \n",
                    vio::time_in_HH_MM_SS_MMM().c_str(),optimizer_->activeChi2());
 #endif
            // 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_ = SE2_5(SE3((*it_kf)->v_kf_->estimate().rotation().toRotationMatrix(),
                                        (*it_kf)->v_kf_->estimate().translation()));
                for(Features::iterator it_ftr=(*it_kf)->fts_.begin(); it_ftr!=(*it_kf)->fts_.end(); ++it_ftr)
                {
                    if((*it_ftr)->point == NULL)
                        continue;
                    if((*it_ftr)->point->v_pt_ == NULL)
                        continue;       // mp was updated before
                    (*it_ftr)->point->pos_ = (*it_ftr)->point->v_pt_->estimate();
                    (*it_ftr)->point->v_pt_.reset();
                }
            }
            optimizer_->clear();
            ba_mux_.unlock();
        }
    }
   std::shared_ptr<g2o::VertexSE3Expmap>
   BA_Glob::createG2oFrameSE3(FramePtr frame, bool state)
   {
       std::shared_ptr<g2o::VertexSE3Expmap> v= std::make_shared<g2o::VertexSE3Expmap>();
       ++v_id_;
       v->setId(v_id_);
       // not all frames are fixed
       v->setFixed(state);
       v->setEstimate(g2o::SE3Quat(frame->se3().unit_quaternion(), frame->se3().translation()));
       return v;
   }
   std::shared_ptr<g2o::VertexPointXYZ>
   BA_Glob::createG2oPoint(Vector3d pos)
   {
       ++v_id_;
       std::shared_ptr<g2o::VertexPointXYZ> v =std::make_shared<g2o::VertexPointXYZ>();
       v->setId(v_id_);
       //v->setFixed(false);
       v->setMarginalized(true);
       v->setEstimate(pos);
       return v;
   }
    std::shared_ptr<g2o::EdgeProjectXYZ2UV> BA_Glob::createG2oEdgeSE3(
            std::shared_ptr<g2o::VertexSE3Expmap> v_frame,
            std::shared_ptr<g2o::VertexPointXYZ> v_point,
                     const Vector2d& f_up,
                     bool robust_kernel,
                     double huber_width,
                     double weight)
   {
       std::shared_ptr<g2o::EdgeProjectXYZ2UV> e= std::make_shared<g2o::EdgeProjectXYZ2UV>();
       e->vertices()[0]=v_point;
       e->vertices()[1]=v_frame;
       e->setMeasurement(f_up);
       e->information() = weight*Eigen::Matrix2d::Identity();
       g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
       rk->setDelta(huber_width);
       e->setRobustKernel(rk);
       e->setParameterId(0, 0); //old: e->setId(v_point->id());
       return e;
   }
   void BA_Glob::reset_map(){
        for(auto&& f:map_.keyframes_){
            f->v_kf_.reset();
            for(auto p:f->fts_)
                if(p->point!=NULL)
                    p->point->v_pt_.reset();
        }
    }
 } // namespace vio
@@ -1,13 +1,13 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
+// This file is part of VIO - 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
+// VIO 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
+// VIO 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.
 //
@@ -75,7 +75,7 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
    }
  computeHomography(frame_cur,
       features_ref_, px_cur_,
-      frame_ref_->cam_->errorMultiplier2(), Config::poseOptimThresh(),
+      frame_ref_->cam_->errorMultiplier2(), 6.0,
      inliers_, xyz_in_cur_, T_cur_from_ref_);
  if(inliers_.size() < Config::initMinInliers()){
 #if VIO_DEBUG
@@ -96,12 +96,12 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
          if(frame_cur->cam_->isInFrame(Vector2d(px_cur_.at(f.index).x,px_cur_.at(f.index).y).cast<int>(), 10) &&
             frame_ref_->cam_->isInFrame(f.item->px.cast<int>(), 10)){
              Vector3d pos = xyz_in_cur_.at(f.index);
              std::shared_ptr<Point> new_point = std::make_shared<Point>(pos);
              std::shared_ptr<Feature> ftr_cur=std::make_shared<Feature>(frame_cur, new_point, Vector2d(px_cur_.at(f.index).x,px_cur_.at(f.index).y),
                                                                         frame_cur->c2f(px_cur_.at(f.index).x,px_cur_.at(f.index).y), f.item->score,f.item->level,f.item->descriptor);
              frame_cur->addFeature(ftr_cur);
              new_point->addFrameRef(ftr_cur);
              new_point->type_=vio::Point::TYPE_GOOD;
              std::shared_ptr<Feature> ftr_ref=std::make_shared<Feature>(frame_ref_, new_point, f.item->px, f.item->f, f.item->score,f.item->level,f.item->descriptor);
              frame_ref_->addFeature(ftr_ref);
@@ -1,13 +1,13 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
+// This file is part of VIO - 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
+// VIO 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
+// VIO 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.
 //
@@ -57,7 +57,7 @@ bool Map::safeDeleteFrame(FramePtr frame)
  if(found)
    return true;
-  //SVO_ERROR_STREAM("Tried to delete Keyframe in map which was not there.");
+  //VIO_ERROR_STREAM("Tried to delete Keyframe in map which was not there.");
  return false;
 }
@@ -79,15 +79,24 @@ void Map::safeDeletePoint(std::shared_ptr<Point> pt)
 {
  boost::unique_lock<boost::mutex> lock(point_mut_);
  // Delete references to mappoints in all keyframes
  if(pt == NULL)return;
  if(pt->obs_.size()>1){
      for(auto&& ftr:pt->obs_){
-          if(ftr->frame)ftr->frame->removeKeyPoint(ftr);
+          if(ftr->frame!=nullptr){
              if(ftr->frame->is_keyframe_){
                  ftr->frame->removeKeyPoint(ftr);
              }
          }
          if(ftr->point!=nullptr)ftr->point.reset();
      }
  }else{
-      if(!pt->obs_.empty())if(pt->obs_.back()->point!=nullptr)pt->obs_.back()->point.reset();
+      if(!pt->obs_.empty()){
          if(pt->obs_.front()->point!=nullptr){
              pt->obs_.back()->point.reset();
          }
      }
  }
-  pt->obs_.clear();
+  if(!pt->obs_.empty())pt->obs_.clear();
  // Delete mappoint
  deletePoint(pt);
 }
@@ -1,13 +1,13 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
+// This file is part of VIO - 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
+// VIO 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
+// VIO 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.
 //
@@ -97,7 +97,7 @@ computeInliers(const vector<Vector3d>& features2, // c2
        xyz_vec.push_back(triangulateFeatureNonLin(R, t, features1[j]/*reference*/, features2[j]/*current*/ ));
        double e1 = reprojError(features1[j], xyz_vec.back(), error_multiplier2);
        double e2 = reprojError(features2[j], SE3(R,t).inverse()*xyz_vec.back(), error_multiplier2);
-        if(e1 > reproj_thresh || e2 > reproj_thresh || xyz_vec.back().norm()==0.0)
+        if(e1 > reproj_thresh || e2 > reproj_thresh || xyz_vec.back().norm()==0.0 ||xyz_vec.back().z()==0. ||xyz_vec.back().hasNaN())
            outliers.push_back(j);
        else
        {
@@ -1,13 +1,13 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
+// This file is part of VIO - 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
+// VIO 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
+// VIO 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.
 //
@@ -19,6 +19,7 @@
 #include <vio/point.h>
 #include <vio/frame.h>
 #include <vio/feature.h>
 #include <vio/config.h>
 namespace vio {
@@ -117,7 +118,7 @@ bool Point::getCloseViewObs(const Vector2d& framepos, std::shared_ptr<Feature>&
  if(ftr== nullptr)return false;
  return true;
 }
-
+///TODO look at point optimization needs to be better
 void Point::optimize(const size_t n_iter)
 {
  boost::unique_lock<boost::mutex> lock(point_mut_);
@@ -135,10 +136,11 @@ void Point::optimize(const size_t n_iter)
    for(auto it=obs_.begin(); it!=obs_.end(); ++it)
    {
      Matrix23d J;
-      const Vector3d p_in_f((*it)->frame->se3()*pos_);
+      const Vector3d p_in_f((*it)->frame->w2f(pos_));
-      Point::jacobian_xyz2uv_(p_in_f, (*it)->frame->se3().rotation_matrix(), J, (*it)->frame->cam_->params(), (*it)->frame->T_f_w_);
+      jacobian_xyz2uv_(p_in_f, (*it)->frame->se3().rotation_matrix(), J, (*it)->frame->cam_->params(), (*it)->frame->T_f_w_);
-      const Vector2d e(vk::project2d((*it)->f) - vk::project2d(p_in_f));
+      //jacobian_xyz2uv(p_in_f,(*it)->frame->se3().rotation_matrix(),J);
-      new_chi2 += e.squaredNorm();
+      const Vector2d e=vk::project2d((*it)->f) - vk::project2d(p_in_f)/(1<<(*it)->level);
      new_chi2 += e.norm();
      A.noalias() += J.transpose() * J;
      b.noalias() -= J.transpose() * e;
    }
@@ -163,14 +165,15 @@ void Point::optimize(const size_t n_iter)
    if(vk::norm_max(dp) <= EPS)
      break;
  }
  n_failed_reproj_=0;
  for(auto it=obs_.begin(); it!=obs_.end(); ++it) {
-            Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d((*it)->frame->se3() * pos_));
+            Vector2d e = vk::project2d((*it)->f) - vk::project2d((*it)->frame->w2f(pos_));
            e /= (1<<(*it)->level);
-            if (e.norm() > 3.0/ (*it)->frame->cam_->errorMultiplier2())
+            if (e.squaredNorm() > 2.0*vio::Config::poseOptimThresh()/ (*it)->frame->cam_->errorMultiplier2())
                n_failed_reproj_++;
  }
-
+  if(n_failed_reproj_<0.50*obs_.size())
-
+      type_=TYPE_GOOD;
 }
 } // namespace vio
@@ -1,13 +1,13 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
+// This file is part of VIO - 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
+// VIO 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
+// VIO 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.
 //
@@ -22,6 +22,7 @@
 #include <vio/robust_cost.h>
 #include <vio/math_utils.h>
 #include <sophus/se2.h>
 #include <vio/config.h>
 namespace vio {
 namespace pose_optimizer {
@@ -48,14 +49,27 @@ void optimizeGaussNewton(
  std::vector<float> errors;
  for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); /*++it*/)
  {
-    if((*it)->point == NULL)
+      if((*it)->point == NULL) {
-      continue;
+          it++;// = frame->fts_.erase(it);
          continue;
      }
      double z=frame->w2f((*it)->point->pos_).z();
      if((*it)->point->pos_.hasNaN() || (*it)->point->pos_.norm()==0. || z<0.05 || z > 20.0){
          map.safeDeletePoint((*it)->point);
          it = frame->fts_.erase(it);
          continue;
      }
      if((*it)->point->type_==vio::Point::TYPE_UNKNOWN){
          it++;
          continue;
      }
    //Reprojection error
    Vector2d e = vk::project2d((*it)->f)
               - vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));
    if(std::isnan(e.norm())){
        map.safeDeletePoint((*it)->point);
        it = frame->fts_.erase(it);
        continue;
    }
    e *= 1.0 / (1<<(*it)->level);
    chi2_vec_init.push_back(e.norm()); // just for debug
@@ -68,7 +82,6 @@ void optimizeGaussNewton(
  vk::robust_cost::MADScaleEstimator scale_estimator;
  estimated_scale = scale_estimator.compute(errors);
  num_obs = errors.size();
  double scale = estimated_scale;
  for(size_t iter=0; iter<n_iter; iter++)
  {
@@ -82,12 +95,11 @@ void optimizeGaussNewton(
    // compute residual
    for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
    {
-      if((*it)->point == NULL)
+      if((*it)->point==NULL)continue;
-        continue;
+      if((*it)->point->type_==vio::Point::TYPE_UNKNOWN)continue;
      Matrix23d J;
      Vector3d xyz_f(Vector3d(frame->se3()*(*it)->point->pos_));
      frame->jacobian_xyz2uv_((*it)->f,(*it)->point->pos_,J);
-      Vector2d e = vk::project2d((*it)->f) - vk::project2d(xyz_f);
+      Vector2d e = vk::project2d((*it)->f) - vk::project2d(frame->w2f((*it)->point->pos_));
      double sqrt_inv_cov = 1.0 / (1<<(*it)->level);
      e *= sqrt_inv_cov;
      J *= sqrt_inv_cov;
@@ -117,28 +129,32 @@ void optimizeGaussNewton(
    if(vk::norm_max(dT) <= EPS)
      break;
  }
-  //point_mut_.lock();
+  num_obs=0;
  size_t n_deleted_refs = 0;
  for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); /*++it*/)
  {
    if((*it)->point == NULL) {
-        it++;
+        it++;// = frame->fts_.erase(it);
        continue;
    }
-    Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));
+    if((*it)->point->pos_.hasNaN() || (*it)->point->pos_.norm()==0.){
          map.safeDeletePoint((*it)->point);
          it = frame->fts_.erase(it);
          continue;
    }
    Vector2d e = vk::project2d((*it)->f) - vk::project2d(frame->w2f((*it)->point->pos_));
    e /= (1<<(*it)->level);
    chi2_vec_final.push_back(e.norm());
-    if(e.norm() >  1.0 / frame->cam_->errorMultiplier2())
+    if(e.norm() >  vio::Config::poseOptimThresh() / frame->cam_->errorMultiplier2())
    {
      map.safeDeletePoint((*it)->point);
      it = frame->fts_.erase(it);
-      ++n_deleted_refs;
+    }else{
-    }
+        (*it)->point->type_=vio::Point::TYPE_CANDIDATE;
-    else
+        ++num_obs;
        it++;
    }
  }
  //point_mut_.unlock();
  num_obs -= n_deleted_refs;
 #if VIO_DEBUG
    error_init=0.0;
    error_final=0.0;
@@ -146,8 +162,8 @@ void optimizeGaussNewton(
        error_init /=chi2_vec_init.size();
    if(!chi2_vec_final.empty())for(auto&& i:chi2_vec_final)error_final+=i;
        error_final /= chi2_vec_final.size();
-    fprintf(log,"[%s] n deleted obs = %d \t n obs with reprojection error less than 2.0 / frame->cam_->errorMultiplier2() =%d \t error init =%f \t error end=%f\n",
+    fprintf(log,"[%s]  n obs with reprojection error less than 1.0 / frame->cam_->errorMultiplier2() =%d \t error init =%f \t error end=%f\n",
-            vio::time_in_HH_MM_SS_MMM().c_str(),n_deleted_refs,num_obs,error_init,error_final);
+            vio::time_in_HH_MM_SS_MMM().c_str(),num_obs,error_init,error_final);
 #endif
 }
@@ -1,13 +1,13 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
+// This file is part of VIO - 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
+// VIO 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
+// VIO 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.
 //
@@ -103,14 +103,11 @@ namespace vio {
                                                                    last_frame->T_f_w_.se2().translation()).norm()));
        close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <
                       boost::bind(&std::pair<FramePtr, double>::second, _2));
        std::unique_ptr<SparseImgAlignGpu> img_align=std::make_unique<SparseImgAlignGpu>(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_);
        for (auto &&it_frame:_for(close_kfs)) {
            img_align->run(it_frame.item.first, frame, log_);
        }
        overlap_kfs.reserve(options_.max_n_kfs);
        std::unique_ptr<SparseImgAlignGpu> img_align=std::make_unique<SparseImgAlignGpu>(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_);
        std::vector<int> added_keypoints;
        int points_count=0;
        for (auto &&it_frame:_for(close_kfs)) {
            int points_count=0;
            if (it_frame.index > options_.max_n_kfs)continue;
            overlap_kfs.push_back(pair<FramePtr, size_t>(it_frame.item.first, 0));
            list<std::shared_ptr<Feature>>::iterator it_ref=it_frame.item.first->fts_.begin();
@@ -140,37 +137,51 @@ namespace vio {
                        // pose with respect to reference frame
                        Vector3d pos=vk::triangulateFeatureNonLin(T_ref_cur.rotation_matrix(),T_ref_cur.translation(),
                                                                  (*it_ref)->f,frame->c2f(px));
-                        if(pos.norm()==0.0)continue;
+                        if(pos.norm()==0. || pos.hasNaN() || pos.z() < 0.01)continue;
                        // point in world frame
-                        std::shared_ptr<Point> new_point=std::make_shared<Point>(it_frame.item.first->se3()*pos,(*it_ref));
+                        (*it_ref)->point=std::make_shared<Point>(it_frame.item.first->se3()*pos,(*it_ref));
                        if(!matcher_.findMatchDirect(*new_point, *frame, px))continue;
                        if(!matcher_.findMatchDirect(*(*it_ref)->point, *frame, px)){
                            (*it_ref)->point.reset();
                            continue;
                        }
                        frame->addFeature(std::make_shared<Feature>(frame,
-                                                                    new_point,
+                                                                    (*it_ref)->point,
                                                                    px,(*it_cur)->level,(*it_cur)->score,(*it_cur)->descriptor));
-                        new_point->addFrameRef(frame->fts_.back());
+                        (*it_ref)->point->addFrameRef(frame->fts_.back());
                        added_keypoints.push_back(match.trainIdx);
-                        frame->fts_.back()->point->last_frame_overlap_id_=it_frame.item.first->id_;
+                        (*it_ref)->point->last_frame_overlap_id_=it_frame.item.first->id_;
-                        grid_.cells.at(k)->push_back(Candidate(frame->fts_.back()->point, px));
+                        (*it_ref)->point->type_=vio::Point::TYPE_UNKNOWN;
                        grid_.cells.at(k)->push_back(Candidate( px));
                        overlap_kfs.back().second++;
                        ++points_count;
                    }else{
                        if(!matcher_.findMatchDirect(*(*it_ref)->point, *frame, px))continue;
                        (*it_ref)->point->last_frame_overlap_id_ = frame->id_;
                        frame->addFeature(std::make_shared<Feature>(frame,
                                                                    (*it_ref)->point,
                                                                    px,(*it_cur)->level,(*it_cur)->score,(*it_cur)->descriptor));
                        (*it_ref)->point->addFrameRef(frame->fts_.back());
                        (*it_ref)->point->type_=vio::Point::TYPE_CANDIDATE;
                        added_keypoints.push_back(match.trainIdx);
-                        grid_.cells.at(k)->push_back(Candidate(frame->fts_.back()->point, px));
+                        grid_.cells.at(k)->push_back(Candidate( px));
                        overlap_kfs.back().second++;
                        ++points_count;
                    }
                }
                ++it_ref;
            }
-            if(points_count>300)return;
+            if(points_count>10)img_align->run(it_frame.item.first, frame, log_);
        }
        for(auto&& p:keypoints){
            int k = static_cast<int>(p->px.y() / grid_.cell_size) *
                          grid_.grid_n_cols
                          + static_cast<int>(p->px.x() / grid_.cell_size);
            if(grid_.cells.at(k)->size()<0.5*Config::gridSize()) {
                frame->addFeature(std::make_shared<Feature>(frame, p->px, p->level, p->score, p->descriptor));
                grid_.cells.at(k)->push_back(Candidate( p->px));
            }
        }
 /*        std::cerr<<"here\n";
        exit(0);*/
@@ -1,13 +1,13 @@
-// This file is part of SVO - Semi-direct Visual Odometry.
+// This file is part of VIO - 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
+// VIO 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
+// VIO 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.
 //
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
msjidandClaude Haiku 4.5	a4bbac7542	Rewrite README with comprehensive project documentation - Added detailed project overview and feature descriptions - Included complete dependency list with build instructions - Added system architecture overview and component descriptions - Documented configuration options and usage instructions - Added troubleshooting section for common issues - Included known limitations and future improvements - Improved formatting and clarity throughout Co-Authored-By: Claude Haiku 4.5 <[email protected]> Claude-Session: https://claude.ai/code/session_01GPXVpiPnb5AzgdCJ33Q6Fi	2026-08-28 19:09:18 +02:00
majid	40df37d770	move files	2026-08-28 18:29:45 +02:00
Majid Geravand	44981569e7	Update README.md	2021-10-22 10:25:50 +02:00
Majid Geravand	de211098f2	Update README.md	2021-10-22 10:25:14 +02:00
gfuse	5b7b1fe1f9	change log name	2021-10-22 10:23:23 +02:00
gfuse	57705ff17b	Merge remote-tracking branch 'client/master'	2021-10-22 10:06:36 +02:00
gfuse	e6dc14332f	delete Doxy	2021-10-22 10:05:55 +02:00
Majid Geravand	679a32f64e	Update README.md	2021-10-22 10:03:13 +02:00
Majid Geravand	d3c857fd06	edit readme	2021-10-22 09:58:31 +02:00
gfuse	425e80c4db	calibration	2021-10-22 09:44:48 +02:00
gfuse	cecb6d9ea4	add feature on each key frame	2021-10-22 08:01:25 +02:00
gfuse	401fae85c3	add a feature detection on each key frame	2021-10-20 18:36:41 +02:00
gfuse	841c1b9465	change the reprojection debugged	2021-10-20 17:27:50 +02:00
gfuse	11c230af3f	change the reprojection	2021-10-20 15:39:51 +02:00
gfuse	ef14e33bdf	do calibration and edit	2021-10-18 14:49:40 +02:00
gfuse	16637a4f0c	solve the seg errors and chnage the g2o version to manegment_memory beranch	2021-10-16 19:12:26 +02:00
gfuse	ea5573b455	change vertecses loop from forward into refrance	2021-10-16 12:07:33 +02:00
gfuse	c69cb17521	change ba loop	2021-10-16 10:34:03 +02:00
gfuse	cd209204bf	change the ba pipline to avoid a vertex with not edge	2021-10-15 17:52:08 +02:00
gfuse	da1242746d	detect an issue: ba should not correct camera positions	2021-10-14 19:45:18 +02:00
gfuse	86ab38e13f	best calibration without ba	2021-10-14 19:17:09 +02:00
gfuse	a4e5c86932	one loop without bandle	2021-10-13 21:04:59 +02:00
gfuse	7395e10523	debug depth mines	2021-10-13 21:00:05 +02:00
gfuse	08a0bbe124	debug depth mines	2021-10-13 19:55:36 +02:00
gfuse	cd4418eda9	debug depth mines	2021-10-13 19:30:20 +02:00
gfuse	84d227cdcd	debug some issues	2021-10-13 18:45:00 +02:00
gfuse	00145478ef	add editPoints	2021-10-13 15:19:28 +02:00
gfuse	612963faf1	do calibration	2021-10-13 09:16:36 +02:00
gfuse	aa9143b875	get an unstable currect result	2021-10-12 23:28:30 +02:00
gfuse	094c9f4282	add landmark type	2021-10-12 20:41:15 +02:00
gfuse	bfecc6bda7	debug some issues	2021-10-12 18:29:07 +02:00
gfuse	65ade550be	debug some issues	2021-10-12 17:02:39 +02:00