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

GPU_version/README.md

@@ -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
-
-
-
-

GPU_version/vio/depth_filter_log.txt

@@ -1,6 +0,0 @@
-[17:12:03.647] New key frame 
-[17:12:03.647] BA loop 
-[17:12:44.004] New key frame 
-[17:12:44.004] BA loop 
-[17:12:44.192] New key frame 
-[17:12:44.192] BA loop 

GPU_version/vio/include/vio/cl_class.h

@@ -1,199 +0,0 @@
-//
-// Created by root on 4/27/21.
-//
-
-#ifndef VIO_OPENCL_CL_CLASS_H
-#define VIO_OPENCL_CL_CLASS_H
-#include <vio/cl.hpp>
-#include <iostream>
-#include <vector>
-#include <algorithm>
-#include <fstream>
-#include <exception>
-#include <CL/opencl.h>
-#include <opencv2/opencv.hpp>
-#include <vio/abstract_camera.h>
-
-class kernel{
-public:
-    kernel(cl::Program* program,std::string name){
-        cl_int error;
-        _kernel = new cl::Kernel(*program,name.c_str(),&error);
-        assert(error== CL_SUCCESS);
-    };
-    ~kernel(){
-        for(auto&& i:_images)i.first.setDestructorCallback((void (*)(_cl_mem *, void *))notify, NULL);
-        for(auto&& i:_buffers)i.first.first.setDestructorCallback((void (*)(_cl_mem *, void *))notify, NULL);
-        _buffers.clear();
-        _images.clear();
-    };
-    template<typename T>
-    int32_t write(size_t id/*buffer ID*/,T* buf,cl::CommandQueue* queue,cl::Context* context,size_t buf_size){
-        assert(buf);
-        cl_int error;
-        _buffers.push_back(std::pair<std::pair<cl::Buffer,size_t>,size_t>(std::pair<cl::Buffer,size_t>(cl::Buffer(*context, CL_MEM_READ_WRITE | CL_MEM_USE_HOST_PTR
-                , sizeof(T) * buf_size,buf,&error),
-                                                                                                       buf_size),id));
-        cl_int err=_kernel->setArg(id,_buffers.back().first.first);
-        assert(err == CL_SUCCESS);
-        assert(error == CL_SUCCESS);
-        return error;
-
-    };
-    int32_t write(size_t id/*buffer ID*/,cv::Mat& buf,cl::Context* context){
-        assert(!buf.empty());
-        cl_int error;
-        _images.push_back(std::pair<cl::Image2D,size_t>(cl::Image2D(*context, CL_MEM_READ_ONLY | CL_MEM_USE_HOST_PTR,
-                                                                    cl::ImageFormat(CL_R, CL_UNSIGNED_INT8),
-                                                                    buf.size().width,
-                                                                    buf.size().height,
-                                                                    0,
-                                                                    reinterpret_cast<uchar*>(buf.data),&error),id));
-        cl_int err=_kernel->setArg(id,_images.back().first);
-        assert(err == CL_SUCCESS);
-        assert(error == CL_SUCCESS);
-        return error;
-    };
-
-    template<typename T>
-    int32_t reload(size_t id,T* buf,cl::CommandQueue* queue){
-        assert(buf);
-        cl_int error;
-        for(auto&& i:_buffers)if(i.second==id){
-                cl::Event event;
-                T* Map_buf=(T*)queue->enqueueMapBuffer(i.first.first,CL_NON_BLOCKING,CL_MAP_WRITE,0,sizeof(T) * i.first.second,NULL,&event,&error);
-                event.wait();
-                memcpy(Map_buf,buf,sizeof(T) * i.first.second);
-                assert(queue->enqueueUnmapMemObject(i.first.first,Map_buf,NULL,&event)==CL_SUCCESS);
-                event.wait();
-
-        }
-        assert(error == CL_SUCCESS);
-        return error;
-    };
-    int32_t reload(size_t id,cv::Mat& buf,cl::Context* context){
-        assert(!buf.empty());
-        cl_int error;
-        try{
-            for(auto&& i:_images)if(i.second==id){
-                    i.first.setDestructorCallback((void (*)(_cl_mem *, void *))notify, NULL);
-                    i.first=cl::Image2D(*context, CL_MEM_READ_ONLY | CL_MEM_USE_HOST_PTR,
-                                        cl::ImageFormat(CL_R, CL_UNSIGNED_INT8),
-                                        buf.size().width,
-                                        buf.size().height,
-                                        0,
-                                        reinterpret_cast<uchar*>(buf.data),&error);
-                    cl_int err=_kernel->setArg(id,i.first);
-                    assert(err == CL_SUCCESS);
-                }
-            assert(error == CL_SUCCESS);
-            return error;
-        }catch (std::exception& err){
-            std::cerr<<"Reload buffer on GPU failed\n"<<err.what()<<'\n';
-            return CL_MAP_FAILURE;
-        }
-    };
-    cl::Kernel* _kernel = NULL;
-    std::pair<cl::Buffer,size_t> read(size_t id){
-        for(auto i:_buffers)if(i.second==id)return i.first;
-    }
-    cl::Image2D read_img(size_t id){
-        for(auto i:_images)if(i.second==id)return i.first;
-    }
-private:
-    static void notify(cl_mem *, void * user_data) {
-        //std::cerr << "Memory object was deleted." << std::endl;
-    }
-    std::vector<std::pair<std::pair<cl::Buffer,size_t>,size_t>> _buffers;
-    std::vector<std::pair<cl::Image2D,size_t>> _images;
-
-};
-class read_cl{
-public:
-    read_cl(std::string path){
-        src_str=(char *)malloc(0x100000);
-        FILE *fp;
-        fp = fopen(path.c_str(), "r");
-        size = fread(src_str, 1, 0x100000, fp);
-        fclose(fp);
-    }
-    void print(){
-        std::cerr<<src_str<<"\n";
-    }
-    char *src_str = nullptr;
-    size_t size;
-};
-class opencl{
-public:
-    opencl(vk::AbstractCamera* cam);
-    ~opencl();
-    int32_t make_kernel(std::string name){_kernels.push_back(kernel(program,name));};
-    template<typename T>
-    bool write_buf(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/, int size,T* buf) {
-            if(_kernels.at(id1).write(id2,buf,queue,context,size)!= CL_SUCCESS)return false;
-            return true;
-    }
-    bool write_buf(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,cv::Mat& buf) {
-            if(_kernels.at(id1).write(id2,buf,context)!= CL_SUCCESS)return false;
-            return true;
-    }
-    template<typename T>
-    bool write_buf(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,T&& value){
-        if(_kernels.at(id1)._kernel->setArg(id2,value)!= CL_SUCCESS)return false;
-        return true;
-    };
-    template<typename T>
-    bool reload_buf(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,T* buf) {
-            if(_kernels.at(id1).reload(id2,buf,queue)!= CL_SUCCESS)return false;
-            return true;
-    }
-    bool reload_buf(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,cv::Mat& buf) {
-            if(_kernels.at(id1).reload(id2,buf,context)!= CL_SUCCESS)return false;
-            return true;
-    }
-    cl_int run(size_t id1/*kernal ID*/,std::size_t  x=1,std::size_t y=1,std::size_t z=1) {
-        cl_int err=0;
-        cl::Event event;
-        if(queue->flush()==CL_OUT_OF_HOST_MEMORY){
-            std::cerr<<"GPU out of memory goodbye:)\n";
-            exit(0);
-        }
-        if(z>1 && y>1){
-            err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x,y,z)/*Global*/, cl::NullRange/*local*/,NULL,&event);
-        }else if(z<2 && y>1){
-            err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x,y)/*Global*/, cl::NullRange/*local*/,NULL,&event);
-        }else{
-            err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x)/*Global*/, cl::NullRange/*local*/,NULL,&event);
-        };
-        assert(err==CL_SUCCESS);
-        event.wait();
-        if(queue->finish()==CL_OUT_OF_HOST_MEMORY){
-            std::cerr<<"GPU out of memory goodbye:)\n";
-            exit(0);
-        }
-        return 1;
-
-    }
-    template<typename T>
-    void read(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,size_t size/*size*/, T* out){
-        assert(out);
-        cl_int error;
-        cl::Event event;
-        T* Map_buf=(T*)queue->enqueueMapBuffer(_kernels.at(id1).read(id2).first,CL_NON_BLOCKING,CL_MAP_READ,0,sizeof(T) * size,NULL,&event,&error);
-        assert(error == CL_SUCCESS);
-        event.wait();
-        memcpy(out,Map_buf,sizeof(T) * size);
-        if(queue->enqueueUnmapMemObject(_kernels.at(id1).read(id2).first,Map_buf,NULL,&event)!=CL_SUCCESS)std::cerr<<"Read buffer on GPU failed"<<'\n';
-        event.wait();
-    }
-    void clear_buf();
-private:
-    std::vector<kernel> _kernels;
-    cl::Context* context = nullptr;
-    cl::Device* device = nullptr;
-    cl::Program* program = nullptr;
-    cl::CommandQueue* queue= nullptr;
-    vk::AbstractCamera* cam= nullptr;
-};
-
-#endif //VIO_OPENCL_CL_CLASS_H

GPU_version/vio/param/vo_fast.yaml

@@ -1,16 +0,0 @@
-svo:
-  grid_size: 30            #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.
-  loba_num_iter: 0         #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.
-  init_min_tracked: 100
-  init_min_disparity: 10
-  structureoptim_max_pts: 50
-  kfselect_mindist: 0.01
-  poseoptim_thresh: 8.0
-  init_min_inliers: 30
-  ACC_noise: 0.5          #acc white noise in continuous in EKF
-  GYO_noise: 0.5         #gyro white noise in continuous in EKF
-  svo_ekf: 0.05              #Svo noise that will be devided by number of Reprojected points after opmization in EKF
-  Cmd_Cov: 1.0  #control vector noise in EKF

GPU_version/vio/src/config.cpp

@@ -1,73 +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 <vio/params_helper.h>
-#include <vio/config.h>
-
-namespace vio {
-
-Config::Config() :
-    trace_name(vk::getParam<string>("svo/trace_name", "svo")),
-    trace_dir(vk::getParam<string>("svo/trace_dir", "/tmp")),
-    n_pyr_levels(vk::getParam<int>("svo/n_pyr_levels", 3)),
-    use_imu(vk::getParam<bool>("svo/use_imu", false)),
-    core_n_kfs(vk::getParam<int>("svo/core_n_kfs", 3)),
-    map_scale(vk::getParam<double>("svo/map_scale", 1.0)),
-    grid_size(vk::getParam<int>("svo/grid_size", 30)),
-    init_min_disparity(vk::getParam<double>("svo/init_min_disparity", 50.0)),
-    init_min_tracked(vk::getParam<int>("svo/init_min_tracked", 50)),
-    init_min_inliers(vk::getParam<int>("svo/init_min_inliers", 40)),
-    klt_max_level(vk::getParam<int>("svo/klt_max_level", 4)),
-    klt_min_level(vk::getParam<int>("svo/klt_min_level", 2)),
-    reproj_thresh(vk::getParam<double>("svo/reproj_thresh", 2.0)),
-    poseoptim_thresh(vk::getParam<double>("svo/poseoptim_thresh", 2.0)),
-    poseoptim_num_iter(vk::getParam<int>("svo/poseoptim_num_iter", 10)),
-    structureoptim_max_pts(vk::getParam<int>("svo/structureoptim_max_pts", 20)),
-    structureoptim_num_iter(vk::getParam<int>("svo/structureoptim_num_iter", 5)),
-    loba_thresh(vk::getParam<double>("svo/loba_thresh", 2.0)),
-    loba_robust_huber_width(vk::getParam<double>("svo/loba_robust_huber_width", 1.0)),
-    loba_num_iter(vk::getParam<int>("svo/loba_num_iter", 0)),
-    kfselect_mindist(vk::getParam<double>("svo/kfselect_mindist", 0.12)),
-    triang_min_corner_score(vk::getParam<double>("svo/triang_min_corner_score", 20.0)),
-    triang_half_patch_size(vk::getParam<int>("svo/triang_half_patch_size", 4)),
-    subpix_n_iter(vk::getParam<int>("svo/subpix_n_iter", 10)),
-    max_n_kfs(vk::getParam<int>("svo/max_n_kfs", 10)),
-    img_imu_delay(vk::getParam<double>("svo/img_imu_delay", 0.0)),
-    max_fts(vk::getParam<int>("svo/max_fts", 120)),
-    quality_min_fts(vk::getParam<int>("svo/quality_min_fts", 50)),
-    quality_max_drop_fts(vk::getParam<int>("svo/quality_max_drop_fts", 40)),
-    ACC_noise(vk::getParam<double>("svo/ACC_noise", 1e-4)),
-    GYO_noise(vk::getParam<double>("svo/GYO_noise", 1e-4)),
-    iuc(vk::getParam<double>("svo/IUC", 1e-1)),
-    abc(vk::getParam<double>("svo/ABC", 1e-1)),
-    gbc(vk::getParam<double>("svo/GBC", 1e-1)),
-    ebp(vk::getParam<double>("svo/EBP", 1e-2)),
-    nrp(vk::getParam<double>("svo/NRP", 1e-5)),
-    svo_ekf(vk::getParam<double>("svo/svo_ekf", 30)),
-    acc_x_off(vk::getParam<double>("svo/Acceleration_offset_x", 0.0)),
-    acc_y_off(vk::getParam<double>("svo/Acceleration_offset_y", 0.0)),
-    gyr_z_off(vk::getParam<double>("svo/Gyroscope_offset_z", 0.0)),
-    cmd_cov(vk::getParam<double>("svo/Cmd_Cov", 0.0))
-{}
-
-Config& Config::getInstance()
-{
-  static Config instance; // Instantiated on first use and guaranteed to be destroyed
-  return instance;
-}
-
-} // namespace vio
-

GPU_version/vio/src/global_optimizer.cpp

@@ -1,257 +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>
-#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 \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;
-            // Go through all Keyframes
-            size_t v_id = 0;
-            point_mut_.lock();
-            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_)
-                {
-                    // for each keyframe add edges to all observed mapoints
-                    std::shared_ptr<Point> mp = it_ftr->point;
-                    if(mp == NULL)
-                        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++, false);
-                        mp->v_pt_ = v_mp;
-                        optimizer.addVertex(v_mp);
-                    }
-
-                    // Due to merging of mappoints it is possible that references in kfs suddenly
-                    // have a very large reprojection error which may result in distorted results.
-                    Vector2d error = vk::project2d(it_ftr->f) - vk::project2d((*it_kf)->w2f(mp->pos_));
-                    if(error.squaredNorm() > pow(2,Config::poseOptimThresh()/(*it_kf)->cam_->errorMultiplier2()))
-                        incorrect_edges.push_back(pair<FramePtr,std::shared_ptr<Feature>>(*it_kf, it_ftr));
-                    else
-                    {
-                        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);
-                    }
-                }
-            }
-
-            // Optimization
-            double init_error=0.0, final_error=0.0;
-            if(Config::lobaNumIter() > 0)
-                runSparseBAOptimizer( Config::lobaNumIter(), init_error, final_error);
-
-            // Update Keyframe and MapPoint Positions
-            for(list<FramePtr>::iterator it_kf = map_.keyframes_.begin();
-                it_kf != map_.keyframes_.end(); ++it_kf)
-            {
-                (*it_kf)->T_f_w_ = SE3( (*it_kf)->v_kf_->estimate().rotation(),
-                                        (*it_kf)->v_kf_->estimate().translation());
-                (*it_kf)->v_kf_ = NULL;
-                for(Features::iterator it_ftr=(*it_kf)->fts_.begin(); it_ftr!=(*it_kf)->fts_.end(); ++it_ftr)
-                {
-                    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);
-                }
-            }
-            point_mut_.unlock();
-        }
-    }
-
-   void BA_Glob::setupG2o()
-   {
-       // TODO: What's happening with all this HEAP stuff? Memory Leak?
-/*       optimizer_=std::make_shared<g2o::SparseOptimizer>();
-       optimizer_->setVerbose(false);
-       auto linearSolver = g2o::make_unique<g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>>();
-       g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(
-               g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver)));
-
-       optimizer_->setAlgorithm(solver);
-
-       // setup camera
-       std::shared_ptr<g2o::CameraParameters> cam_params = std::make_shared<g2o::CameraParameters>(1.0, Vector2d(0.,0.), 0.);
-       cam_params->setId(0);
-       if (!optimizer_->addParameter(cam_params.get())) {
-           assert(false);*/
-      // }
-   }
-
-   void
-   BA_Glob::runSparseBAOptimizer(unsigned int num_iter,
-                        double& init_error, double& final_error)
-   {
-       optimizer_->initializeOptimization();
-       optimizer_->computeActiveErrors();
-       init_error = optimizer_->activeChi2();
-       optimizer_->optimize(num_iter);
-       final_error = optimizer_->activeChi2();
-   }
-
-   g2o::VertexSE3Expmap*
-   BA_Glob::createG2oFrameSE3(FramePtr frame, size_t id, bool fixed)
-   {
-       g2o::VertexSE3Expmap* v= new g2o::VertexSE3Expmap();
-       v->setId(id);
-       v->setFixed(fixed);
-       v->setEstimate(g2o::SE3Quat(frame->T_f_w_.se3().unit_quaternion(), frame->T_f_w_.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->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(Vector2d(static_cast<int>(g2o::Sampler::uniformRand(0, 640)),
-                                  static_cast<int>(g2o::Sampler::uniformRand(0, 480))));
-       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

README.md

@@ -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/

vio/CMakeLists.txt

@@ -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(-DSVO_USE_ROS)
+ADD_DEFINITIONS(-DVIO_TRACE)
+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,10 +67,11 @@ LIST(APPEND LINK_LIBS
   ${Boost_Fibers_LIBRARIES}
   ${OpenCL_LIBRARIES}
   ${catkin_LIBRARIES}
-  ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY} ${G2O_SOLVER_CHOLMOD}
-  ${G2O_SOLVER_PCG}
+  ${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY} ${G2O_SOLVER_CSPARSE} ${G2O_SOLVER_STRUCTURE_ONLY}
+  ${G2O_SOLVER_PCG} ${G2O_SOLVER_CSPARSE_EXTENSION}
   ${G2O_TYPES_SBA}
   ${G2O_TYPES_SLAM3D}
+  ${G2O_TYPES_ICP}
   cholmod cxsparse
 )
 
@@ -75,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)
@@ -84,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_DELETE_IMPLICITLY_OWNED_OBJECTS=OFF)
+#ADD_DEFINITIONS(-DG2O_USE_VENDORED_CERES=ON)
+#ADD_DEFINITIONS(-DG2O_DELETE_IMPLICITLY_OWNED_OBJECTS=OFF)
 

vio/include/sophus/so3.cpp

@@ -161,7 +161,7 @@ Vector3d SO3
           two_atan_nbyw_by_n = -M_PI/n;
         }
       }
-      two_atan_nbyw_by_n = 2*atan(n/w)/n;
+      two_atan_nbyw_by_n = 2*atan2(n,w)/n;
     }
 
     *theta = two_atan_nbyw_by_n*n;

vio/include/vio/atan_camera.h

@@ -43,8 +43,12 @@ private:
   {
     if(r < 0.001 || s_ == 0.0)
       return 1.0;
-    else
-      return (s_inv_* atan(r * tans_) / r);
+    else{
+        float y=2*r*sin(0.5 * (float)s_);
+        float x=cos(0.5 * (float)s_);
+        return (atan2(y,x) / (r * (float)s_));
+        //return (s_inv_* atan(r * tans_) / r);
+    }
   };
 
   //! Inverse radial distortion: returns un-distorted radius from distorted.

vio/include/vio/bundle_adjustment.h

@@ -15,7 +15,7 @@ namespace g2o {
 
 namespace vio {
 
-    typedef g2o::EdgeProjectXYZ2UV g2oEdgeSE3;
+    typedef EdgeProjectXYZ2UV g2oEdgeSE3;
     typedef g2o::VertexSE2Expmap g2oFrameSE2;
     typedef g2o::VertexSBAPointXYZ g2oPoint;
 
@@ -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

vio/include/vio/cl_class.h

@@ -0,0 +1,181 @@
+//
+// Created by root on 4/27/21.
+//
+
+#ifndef VIO_OPENCL_CL_CLASS_H
+#define VIO_OPENCL_CL_CLASS_H
+#include <vio/cl.hpp>
+#include <iostream>
+#include <vector>
+#include <algorithm>
+#include <fstream>
+#include <exception>
+#include <CL/opencl.h>
+#include <opencv2/opencv.hpp>
+#include <vio/abstract_camera.h>
+#include <vio/for_it.hpp>
+
+class kernel{
+public:
+    kernel(cl::Program* program,std::string name){
+        cl_int error;
+        _kernel = new cl::Kernel(*program,name.c_str(),&error);
+        assert(error== CL_SUCCESS);
+    };
+    ~kernel(){
+        for(auto&& i:_images)i.first->setDestructorCallback((void (*)(_cl_mem *, void *))notify, NULL);
+        for(auto&& i:_buffers)i.first->setDestructorCallback((void (*)(_cl_mem *, void *))notify, NULL);
+        _buffers.clear();
+        _images.clear();
+    };
+    template<typename T>
+    void load(size_t id/*buffer ID*/,T* buf,cl::CommandQueue* queue,cl::Context* context,size_t buf_size){
+        assert(buf);
+        cl_int error;
+        _buffers.push_back(std::pair<std::shared_ptr<cl::Buffer>,size_t>(std::make_shared<cl::Buffer>(*context, CL_MEM_READ_WRITE | CL_MEM_USE_HOST_PTR, sizeof(T) * buf_size,buf,&error),id));
+        assert(error == CL_SUCCESS);
+        cl_int err=_kernel->setArg(id,*_buffers.back().first);
+        assert(err == CL_SUCCESS);
+
+    };
+    void load(size_t id/*buffer ID*/,cv::Mat& buf,cl::Context* context){
+        assert(!buf.empty());
+        cl_int error;
+        _images.push_back(std::pair<std::shared_ptr<cl::Image2D>,size_t>(std::make_shared<cl::Image2D>(*context, CL_MEM_READ_ONLY | CL_MEM_USE_HOST_PTR,
+                                                                    cl::ImageFormat(CL_R, CL_UNSIGNED_INT8),
+                                                                    buf.size().width,
+                                                                    buf.size().height,
+                                                                    0,
+                                                                    reinterpret_cast<uchar*>(buf.data),&error),id));
+        assert(error == CL_SUCCESS);
+        cl_int err=_kernel->setArg(id,*_images.back().first);
+        assert(err == CL_SUCCESS);
+    };
+    template<typename T>
+    void reload(size_t id,T* buf,cl::CommandQueue* queue,size_t size){
+        assert(buf);
+        cl_int error;
+        for(auto&& i:_buffers)if(i.second==id){
+                cl::Event event;
+                T* Map_buf=(T*)queue->enqueueMapBuffer(*i.first,CL_NON_BLOCKING,CL_MAP_WRITE,0,sizeof(T) * size,NULL,&event,&error);
+                event.wait();
+                memcpy(Map_buf,buf,sizeof(T) * size);
+                assert(queue->enqueueUnmapMemObject(*i.first,Map_buf,NULL,&event)==CL_SUCCESS);
+                event.wait();
+
+            }
+        assert(error == CL_SUCCESS);
+    };
+    void release(size_t id/*buffer ID*/){
+        for(auto it=_buffers.begin();it!=_buffers.end();++it){
+            if((*it).second==id){
+                (*it).first->setDestructorCallback((void (*)(_cl_mem *, void *))notify, NULL);
+                (*it).first.reset();
+                _buffers.erase(it);
+                return;
+            }
+        }
+        for(auto it=_images.begin();it!=_images.end();++it){
+            if((*it).second==id){
+                (*it).first->setDestructorCallback((void (*)(_cl_mem *, void *))notify, NULL);
+                (*it).first.reset();
+                _images.erase(it);
+                return;
+            }
+        }
+    };
+    void release(){
+        _buffers.clear();
+        _images.clear();
+    }
+    cl::Kernel* _kernel = NULL;
+    std::shared_ptr<cl::Buffer> read(size_t id){
+        for(auto&& i:_buffers)if(i.second==id)return i.first;
+    }
+private:
+    static void notify(cl_mem *, void * user_data) {
+    }
+    std::list<std::pair<std::shared_ptr<cl::Buffer>,size_t>> _buffers;
+    std::list<std::pair<std::shared_ptr<cl::Image2D>,size_t>> _images;
+
+};
+class read_cl{
+public:
+    read_cl(std::string path){
+        src_str=(char *)calloc(0x100000, sizeof(char));
+        FILE *fp;
+        fp = fopen(path.c_str(), "r");
+        size = fread(src_str, 1, 0x100000, fp);
+        fclose(fp);
+    }
+    void print(){
+        std::cerr<<src_str<<"\n";
+    }
+    char *src_str = nullptr;
+    size_t size;
+};
+class opencl{
+public:
+    opencl(vk::AbstractCamera* cam);
+    ~opencl();
+    int32_t make_kernel(std::string name){_kernels.push_back(kernel(program,name));};
+    template<typename T>
+    void load(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/, int size,T* buf) {
+        _kernels.at(id1).load(id2,buf,queue,context,size);
+    }
+    void load(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,cv::Mat& buf) {
+        _kernels.at(id1).load(id2,buf,context);
+    }
+    template<typename T>
+    void load(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,T&& value){
+        _kernels.at(id1)._kernel->setArg(id2,value);
+    };
+    template<typename T>
+    void reload(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/, size_t size,T* buf){
+        _kernels.at(id1).reload(id2,buf,queue,size);
+    };
+    cl_int run(size_t id1/*kernal ID*/,std::size_t  x=1,std::size_t y=1,std::size_t z=1) {
+        cl_int err=0;
+        cl::Event event;
+        assert(queue->flush()==CL_SUCCESS);
+        if(z>1 && y>1){
+            err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x,y,z)/*Global*/, cl::NullRange/*local*/,NULL,&event);
+        }else if(z<2 && y>1){
+            err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x,y)/*Global*/, cl::NullRange/*local*/,NULL,&event);
+        }else{
+            err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x)/*Global*/, cl::NullRange/*local*/,NULL,&event);
+        };
+        assert(err==CL_SUCCESS);
+        event.wait();
+        assert(queue->finish()==CL_SUCCESS);
+        return 1;
+
+    }
+    template<typename T>
+    void read(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,size_t size/*size*/, T* out){
+        assert(out);
+        cl_int error;
+        cl::Event event;
+        T* Map_buf=(T*)queue->enqueueMapBuffer(*_kernels.at(id1).read(id2),CL_NON_BLOCKING,CL_MAP_READ,0,sizeof(T) * size,NULL,&event,&error);
+        assert(error == CL_SUCCESS);
+        event.wait();
+        memcpy(out,Map_buf,sizeof(T) * size);
+        assert(queue->enqueueUnmapMemObject(*_kernels.at(id1).read(id2),Map_buf,NULL,&event)==CL_SUCCESS);
+        event.wait();
+    }
+    void clear_buf(){
+        for(auto k:_kernels)k.release();
+    };
+    void release(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/){
+        _kernels.at(id1).release(id2);
+    }
+private:
+    std::vector<kernel> _kernels;
+    cl::Context* context = nullptr;
+    cl::Device* device = nullptr;
+    cl::Program* program = nullptr;
+    cl::CommandQueue* queue= nullptr;
+    vk::AbstractCamera* cam= nullptr;
+};
+
+#endif //VIO_OPENCL_CL_CLASS_H

vio/include/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_CONFIG_H_
-#define SVO_CONFIG_H_
+#ifndef VIO_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,52 +73,25 @@ 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; }
 
   /// Gro white noise in continuous.
   static double& GYO_Noise() { return getInstance().GYO_noise; }
-  /// integration uncertainty continuous
-  static double& IUC() { return getInstance().iuc; }
-    /// acc bias in continuous
-  static double& ABC() { return getInstance().abc; }
-    /// gyro bias in continuous
-  static double& GBC() { return getInstance().gbc; }
-    /// error in the bias used for preintegration
-  static double& EBP() { return getInstance().ebp; }
-    /// the noise used for projection
-  static double& NRP() { return getInstance().nrp; }
-  static double& Svo_Ekf() { return getInstance().svo_ekf; }
-  static double& X_off() { return getInstance().acc_x_off; }
-  static double& Y_off() { return getInstance().acc_y_off; }
-  static double& Teta_off() { return getInstance().gyr_z_off; }
-  static double& Cmd_Cov() { return getInstance().cmd_cov; }
+  static double& VO_ekf_t() { return getInstance().vo_ekf_t; }
+  static double& VO_ekf_o() { return getInstance().vo_ekf_o; }
+  static double& Cmd_ekf_t() { return getInstance().cmd_ekf_t; }
+  static double& Cmd_ekf_o() { return getInstance().cmd_ekf_o; }
 
 
 private:
@@ -174,9 +127,9 @@ private:
   size_t max_fts;
   size_t quality_min_fts;
   int quality_max_drop_fts;
-  double nrp,ebp,gbc,abc,iuc,ACC_noise,GYO_noise,svo_ekf,acc_x_off,acc_y_off,gyr_z_off,cmd_cov;
+  double ACC_noise,GYO_noise,vo_ekf_t,vo_ekf_o,cmd_ekf_t,cmd_ekf_o;
 };
 
 } // namespace vio
 
-#endif // SVO_CONFIG_H_
+#endif // VIO_CONFIG_H_

vio/include/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_H_
-#define SVO_FEATURE_H_
+#ifndef VIO_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_

vio/include/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_ALIGNMENT_H_
-#define SVO_FEATURE_ALIGNMENT_H_
+#ifndef VIO_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_

vio/include/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_FEATURE_DETECTION_H_
-#define SVO_FEATURE_DETECTION_H_
+#ifndef VIO_FEATURE_DETECTION_H_
+#define VIO_FEATURE_DETECTION_H_
 
 #include <vio/global.h>
 #include <vio/frame.h>
@@ -72,10 +72,10 @@ public:
 protected:
 
   static const int border_ = 8; //!< no feature should be within 8px of border.
-  const int cell_size_;
-  const int n_pyr_levels_;
-  const int grid_n_cols_;
-  const int grid_n_rows_;
+  int cell_size_;
+  int n_pyr_levels_;
+  int grid_n_cols_;
+  int grid_n_rows_;
   vector<bool> grid_occupancy_;
 
   inline int getCellIndex(int x, int y, int level)
@@ -110,4 +110,4 @@ public:
 } // namespace feature_detection
 } // namespace vio
 
-#endif // SVO_FEATURE_DETECTION_H_
+#endif // VIO_FEATURE_DETECTION_H_

vio/include/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_H_
-#define SVO_FRAME_H_
+#ifndef VIO_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);
@@ -93,7 +95,7 @@ namespace vio {
             return cam_->world2cam( T_f_w_.se3().inverse()*xyz_w);
         }
         /// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c).
-        inline Vector2d w2px(const Vector3d& xyz_w) const { return cam_->world2cam( xyz_w ); }
+        inline Vector2d w2px(const Vector3d& xyz_w) const { return cam_->world2cam( w2f(xyz_w) ); }
 
         /// Transforms pixel coordinates (c) to frame unit sphere coordinates (f).
         inline Vector3d c2f(const Vector2d& px) const { return cam_->cam2world(px[0], px[1]); }
@@ -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_

vio/include/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_BASE_H_
-#define SVO_FRAME_HANDLER_BASE_H_
+#ifndef VIO_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_

vio/include/vio/frame_handler_mono.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_
-#define SVO_FRAME_HANDLER_H_
+#ifndef VIO_FRAME_HANDLER_H_
+#define VIO_FRAME_HANDLER_H_
 
 #include <set>
 #include <vio/abstract_camera.h>
@@ -24,17 +24,15 @@
 #include <vio/initialization.h>
 #include <vio/ukf.h>
 #include <vio/cl_class.h>
-#include <vio/sparse_img_align_gpu.h>
 #include <vio/global_optimizer.h>
 
 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:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-  
   FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,3,1>& init);
   virtual ~FrameHandlerMono();
 
@@ -88,4 +86,4 @@ protected:
 
 } // namespace vio
 
-#endif // SVO_FRAME_HANDLER_H_
+#endif // VIO_FRAME_HANDLER_H_

vio/include/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_GLOBAL_H_
-#define SVO_GLOBAL_H_
+#ifndef VIO_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
-#define RPG_SVO_VIKIT_IS_VECTOR_SPECIALIZED
+#ifndef RPG_VIO_VIKIT_IS_VECTOR_SPECIALIZED //Guard for rpg_vikit
+#define RPG_VIO_VIKIT_IS_VECTOR_SPECIALIZED
 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector3d)
 EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2d)
 #endif
@@ -104,12 +104,16 @@ namespace vio
             assert(T2_!= nullptr);
             return *T2_;
         }
-        SE2 inverse() const{
+/*        SE2 inverse() const{
             assert(T2_!= nullptr);
             double pitch=atan2(T2_->so2().unit_complex().imag(),T2_->so2().unit_complex().real());
             SE2 tem=SE2(pitch+M_PI,-1.0*T2_->translation());
             return tem;
-        }
+        }*/
+/*        SE2_5 inverse_h() const{
+            assert(T2_!= nullptr);
+            return SE2_5(T2_->inverse());
+        }*/
         // Rotation around y
         double pitch()const{
             assert(T2_!= nullptr);
@@ -139,4 +143,4 @@ namespace vio
     typedef std::shared_ptr<Frame> FramePtr;
 } // namespace vio
 
-#endif // SVO_GLOBAL_H_
+#endif // VIO_GLOBAL_H_

vio/include/vio/global_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_DEPTH_FILTER_H_
-#define SVO_DEPTH_FILTER_H_
+#ifndef VIO_DEPTH_FILTER_H_
+#define VIO_DEPTH_FILTER_H_
 
 #include <queue>
 #include <boost/thread.hpp>
@@ -30,15 +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>
-#include <g2o/solvers/dense/linear_solver_dense.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 {
@@ -61,53 +64,36 @@ public:
       boost::unique_lock< boost::mutex > lk( mtx_);
       new_keyframe_=true;
       cond_.notify_one();
-      std::cerr<<"new_key_frame\n";
 #if VIO_DEBUG
       fprintf(log_,"[%s] New key frame \n",
               vio::time_in_HH_MM_SS_MMM().c_str());
 #endif
   }
+ boost::mutex ba_mux_;
 protected:
   bool new_keyframe_=false;
-  std::shared_ptr<g2o::OptimizationAlgorithmLevenberg> Levenberg_;
-  std::shared_ptr<g2o::SparseOptimizer> optimizer_;
   boost::condition_variable cond_;
   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;
-        };
-
-/// Initialize g2o with solver type, optimization strategy and camera model.
-        void setupG2o();
-        /// Run the optimization on the provided graph.
-        void runSparseBAOptimizer(
-                unsigned int num_iter,
-                double& init_error,
-                double& final_error);
 
 /// Create a g2o vertice from a keyframe object.
-        g2o::VertexSE3Expmap* createG2oFrameSE3(
-                FramePtr kf,
-                size_t id,
-                bool fixed);
+        std::shared_ptr<g2o::VertexSE3Expmap> createG2oFrameSE3(
+                FramePtr kf, bool state);
     /// Creates a g2o vertice from a mappoint object.
-        g2o::VertexSBAPointXYZ* createG2oPoint(
-                Vector3d pos,
-                size_t id,
-                bool fixed);
+        std::shared_ptr<g2o::VertexPointXYZ> createG2oPoint(
+                Vector3d pos);
   /// Creates a g2o edge between a g2o keyframe and mappoint vertice with the provided measurement.
-        g2o::EdgeProjectXYZ2UV* createG2oEdgeSE3(
-                g2o::VertexSE3Expmap* v_kf,
-                g2o::VertexSBAPointXYZ* v_mp,
+  std::shared_ptr<g2o::EdgeProjectXYZ2UV> createG2oEdgeSE3(
+          std::shared_ptr<g2o::VertexSE3Expmap> v_kf,
+          std::shared_ptr<g2o::VertexPointXYZ>v_mp,
                 const Vector2d& f_up,
                 bool robust_kernel,
                 double huber_width,
@@ -115,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_

vio/include/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_INITIALIZATION_H
-#define SVO_INITIALIZATION_H
+#ifndef VIO_INITIALIZATION_H
+#define VIO_INITIALIZATION_H
 
 #include <vio/global.h>
 #include <vio/cl_class.h>
@@ -73,6 +73,8 @@ protected:
         vector<Vector2d> uv_cur(features_ref.size());
         vector<Vector3d> f_ref(features_ref.size());
         vector<Vector3d> f_cur(features_ref.size());
+        vector<Vector3d> xyz_in_hom(features_ref.size());
+        vector<Vector3d> xyz_in_ekf(features_ref.size());
         for(auto&& ftr:_for(features_ref)){
             uv_ref.at(ftr.index) = ftr.item->px;
             uv_cur.at(ftr.index) = Vector2d(px_cur_.at(ftr.index).x,px_cur_.at(ftr.index).y);
@@ -84,33 +86,56 @@ protected:
         vector<int> outliers_hom, inlier_hom;
         vector<int> outliers_ekf,inlier_ekf;
         assert(ukf_!= nullptr);
-/*#if VIO_DEBUG
-        for(int i=0;i<f_cur.size();++i)fprintf(log_,"[%s] cur: x=%f, y=%f, z=%f ref: x=%f y=%f z=%f\n",
-              vio::time_in_HH_MM_SS_MMM().c_str(),
-              f_cur.at(i).x(),f_cur.at(i).y(),f_cur.at(i).z(),
-              f_ref.at(i).x(),f_ref.at(i).y(),f_ref.at(i).z());
-#endif*/
         vk::computeInliers(f_cur, f_ref,
                            Homography.T_c2_from_c1.rotation_matrix(), Homography.T_c2_from_c1.translation(),
                            reprojection_threshold, focal_length,
-                           xyz_in_cur, inlier_hom, outliers_hom);
+                           xyz_in_hom, inlier_hom, outliers_hom);
+/*        std::cerr<<xyz_in_hom.size()<<'\n';*/
+/*#if VIO_DEBUG
+      for(int i=0;i<f_cur.size();++i)fprintf(log_,"[%s] curhomog: x=%f, y=%f, z=%f ref: x=%f y=%f z=%f, inlier: %f, %f, %f\n",
+                                             vio::time_in_HH_MM_SS_MMM().c_str(),
+                                             f_cur.at(i).x(),f_cur.at(i).y(),f_cur.at(i).z(),
+                                             f_ref.at(i).x(),f_ref.at(i).y(),f_ref.at(i).z(),
+                                             xyz_in_hom.at(i).x(),xyz_in_hom.at(i).y(),xyz_in_hom.at(i).z());
+#endif*/
         auto res=ukf_->get_location();
         vk::computeInliers(f_cur, f_ref,
                            res.second.se3().rotation_matrix(), res.second.se3().translation(),
                                reprojection_threshold, focal_length,
-                               xyz_in_cur, inlier_ekf, outliers_ekf);
+                           xyz_in_ekf, inlier_ekf, outliers_ekf);
 
+/*      std::cerr<<xyz_in_ekf.size()<<'\n';*/
+/*#if VIO_DEBUG
+      for(int i=0;i<f_cur.size();++i)fprintf(log_,"[%s] cur ukf: x=%f, y=%f, z=%f ref: x=%f y=%f z=%f inlier: %f, %f, %f\n",
+                                             vio::time_in_HH_MM_SS_MMM().c_str(),
+                                             f_cur.at(i).x(),f_cur.at(i).y(),f_cur.at(i).z(),
+                                             f_ref.at(i).x(),f_ref.at(i).y(),f_ref.at(i).z(),
+                                             xyz_in_ekf.at(i).x(),xyz_in_ekf.at(i).y(),xyz_in_ekf.at(i).z());
+#endif*/
         if(inlier_ekf.size()>inlier_hom.size()){
             T_cur_from_ref=res.second;
             inliers=inlier_ekf;
+            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_->UpdateSvo(Homography.T_c2_from_c1.translation().x(),Homography.T_c2_from_c1.translation().z(),euler(1));
+            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_hom;
         }
 
     }
+    void debug(FramePtr ref,FramePtr cur){
+        cv::Mat reference,current;
+        cv::cvtColor(ref->img(),reference,cv::COLOR_GRAY2RGB);
+        cv::cvtColor(cur->img(),current,cv::COLOR_GRAY2RGB);
+        for(auto&& f:ref->fts_)cv::circle(reference, cv::Point(f->px.x(),f->px.y() ), 2, cv::Scalar(0, 255,0),2);
+        for(auto&& f:cur->fts_)cv::circle(current, cv::Point(f->px.x(),f->px.y() ), 2, cv::Scalar(0, 255,0),2);
+        cv::imshow("Reference",reference);
+        cv::imshow("Current",current);
+        cv::waitKey();
+        exit(0);
+    }
 };
 
 /// Detect Fast corners in the image.
@@ -129,4 +154,4 @@ void trackKlt(
 } // namespace initialization
 } // namespace vio
 
-#endif // SVO_INITIALIZATION_H
+#endif // VIO_INITIALIZATION_H

vio/include/vio/map.h

@@ -1,32 +1,34 @@
-// 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_
-#define SVO_MAP_H_
+#ifndef VIO_MAP_H_
+#define VIO_MAP_H_
 
 #include <queue>
 #include <boost/noncopyable.hpp>
 #include <boost/thread.hpp>
 #include <vio/global.h>
 #include <vio/point.h>
+#include <vio/frame.h>
+#include <vio/feature.h>
 
 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.
@@ -54,16 +56,36 @@ public:
   void addKeyframe(FramePtr new_keyframe);
 
   /// Given a frame, return all keyframes which have an overlapping field of view.
-  void getCloseKeyframes(const FramePtr& frame, list< pair<FramePtr,double> >& close_kfs) const;
+  void getCloseKeyframes(const FramePtr& frame, list< pair<FramePtr,double> >& close_kfs){
+      for(auto kf : keyframes_)
+      {
+          // check if kf has overlaping field of view with frame, use therefore KeyPoints
+          for(auto keypoint : kf->key_pts_)
+          {
+              if(keypoint == nullptr)
+                  continue;
+              if(keypoint->point==NULL)continue;
+              if(frame->isVisible(keypoint->point->pos_))
+              {
+                  close_kfs.push_back(
+                          std::make_pair(
+                                  kf, (frame->T_f_w_.se2().translation()-kf->T_f_w_.se2().translation()).norm()));
+                  break; // this keyframe has an overlapping field of view -> add to close_kfs
+              }
+          }
+      }
+  }
 
   /// Return the keyframe which is furthest apart from pos.
-  FramePtr getFurthestKeyframe(const Vector2d& pos) const;
+  FramePtr getFurthestKeyframe(const Vector2d& pos);
 
   /// Empty trash bin of deleted keyframes and map points. We don't delete the
   /// points immediately to ensure proper cleanup and to provide the visualizer
   /// a list of objects which must be removed.
   void emptyTrash();
 
+  bool checkKeyFrames();
+
 
   /// Return the number of keyframes in the map
   inline size_t size() const { return keyframes_.size(); }
@@ -78,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_

vio/include/vio/matcher.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_
-#define SVO_MATCHER_H_
+#ifndef VIO_MATCHER_H_
+#define VIO_MATCHER_H_
 
 #include <vio/global.h>
 
@@ -66,8 +66,8 @@ class Matcher
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
-  static const int halfpatch_size_ = 8;
-  static const int patch_size_ = 16;
+  static const int halfpatch_size_ = 4;
+  static const int patch_size_ = 8;
 
   typedef vk::patch_score::ZMSSD<halfpatch_size_> PatchScore;
 
@@ -93,6 +93,7 @@ public:
 
   uint8_t patch_[patch_size_*patch_size_] __attribute__ ((aligned (16)));
   uint8_t patch_with_border_[(patch_size_+2)*(patch_size_+2)] __attribute__ ((aligned (16)));
+  uint8_t patch_with_border_cur_[(patch_size_+2)*(patch_size_+2)] __attribute__ ((aligned (16)));
   Matrix2d A_cur_ref_;          //!< affine warp matrix
   Vector2d epi_dir_;
   double epi_length_;           //!< length of epipolar line segment in pixels (only used for epipolar search)
@@ -124,7 +125,7 @@ public:
 
   void createPatchFromPatchWithBorder();
   uint i=0;
-  void debug(cv::Mat ref,cv::Mat cur, Vector2d ref_px,Vector2d cur_px_in,Vector2d cur_px_out,bool res,uint8_t* patch,uint8_t* patch_border){
+  void debug(cv::Mat ref,cv::Mat cur, Vector2d ref_px,Vector2d cur_px_in,Vector2d cur_px_out,bool res,uint8_t* patch,uint8_t* patch_border, double depth){
       cv::Mat Ref,Cur,Patch,Patch_b;
       cv::cvtColor(cv::Mat(patch_size_,patch_size_,CV_8UC1,patch),Patch,cv::COLOR_GRAY2RGB);
       cv::cvtColor(cv::Mat(patch_size_+2,patch_size_+2,CV_8UC1,patch_border),Patch_b,cv::COLOR_GRAY2RGB);
@@ -133,7 +134,8 @@ public:
       cv::circle(Ref, cv::Point(ref_px.x(),ref_px.y() ), 2, cv::Scalar(0, 0,255),2);
       cv::circle(Cur, cv::Point(cur_px_in.x(),cur_px_in.y() ), 2, cv::Scalar(0, 0,255),2);
       cv::circle(Cur, cv::Point(cur_px_out.x(),cur_px_out.y() ), 2, cv::Scalar(0, 255,0),2);
-      cv::putText(Cur,res? "Accept": "reject",cv::Point(10,10),cv::FONT_HERSHEY_COMPLEX_SMALL,1.0,cv::Scalar(0,255,0),1,cv:: LINE_AA);
+      cv::putText(Cur,res? "Accept": "reject",cv::Point(10,30),cv::FONT_HERSHEY_COMPLEX_SMALL,1.0,cv::Scalar(0,255,0),1,cv:: LINE_AA);
+      cv::putText(Cur,std::to_string(depth),cv::Point(100,30),cv::FONT_HERSHEY_COMPLEX_SMALL,1.0,cv::Scalar(0,255,0),1,cv:: LINE_AA);
       cv::imwrite(std::string(PROJECT_DIR)+"/Ref"+std::to_string(i)+".png",Ref);
       cv::imwrite(std::string(PROJECT_DIR)+"/Cur"+std::to_string(i)+".png",Cur);
       cv::imwrite(std::string(PROJECT_DIR)+"/patch"+std::to_string(i)+".png",Patch);
@@ -144,4 +146,4 @@ public:
 
 } // namespace vio
 
-#endif // SVO_MATCHER_H_
+#endif // VIO_MATCHER_H_

vio/include/vio/math_utils.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){

vio/include/vio/nlls_solver_impl.hpp

@@ -37,7 +37,7 @@ void vk::NLLSSolver<D, T>::optimizeGaussNewton()
     if(!solve())
     {
       // matrix was singular and could not be computed
-      if(verbose_)std::cout << "Matrix is close to singular! Stop Optimizing." << std::endl;
+      if(verbose_)std::cerr << "Matrix is close to singular! Stop Optimizing." << std::endl;
       stop_ = true;
     }
     // check if error increased since last optimization
@@ -45,7 +45,7 @@ void vk::NLLSSolver<D, T>::optimizeGaussNewton()
     {
       if(verbose_)
       {
-        std::cout << "It. " << iter_
+        std::cerr << "It. " << iter_
                   << "\t Failure"
                   << "\t new_chi2 = " << new_chi2
                   << "\t Error increased. Stop optimizing.\n";
@@ -57,7 +57,7 @@ void vk::NLLSSolver<D, T>::optimizeGaussNewton()
     chi2_ = new_chi2;
     if(verbose_)
     {
-      std::cout << "It. " << iter_
+      std::cerr << "It. " << iter_
                 << "\t Success"
                 << "\t new_chi2 = " << new_chi2
                 << "\t x_norm = " << vk::norm_max(x_)<<'\n';

vio/include/vio/point.h

@@ -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_
-#define SVO_POINT_H_
+#ifndef VIO_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
@@ -46,17 +47,12 @@ public:
   static int                  point_counter_;           //!< Counts the number of created points. Used to set the unique id.
   int                         id_;                      //!< Unique ID of the point.
   Vector3d                    pos_;                     //!< 3d pos of the point in the world coordinate frame.
-  Vector3d                    normal_;                  //!< Surface normal at point.
-  Matrix3d                    normal_information_;      //!< Inverse covariance matrix of normal estimation.
-  bool                        normal_set_;              //!< Flag whether the surface normal was estimated or not.
   list<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.
-  int                         last_published_ts_;       //!< Timestamp of last publishing.
+  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.
-  int                         n_succeeded_reproj_;      //!< Number of succeeded reprojections. Used to assess the quality of the point.
   int                         last_structure_optim_;    //!< Timestamp of last point optimization
 
   Point(const Vector3d& pos);
@@ -69,8 +65,6 @@ public:
   /// Remove reference to a frame.
   bool deleteFrameRef(FramePtr frame);
 
-  /// Initialize point normal. The inital estimate will point towards the frame.
-  void initNormal();
 
   /// Check whether mappoint has reference to a frame.
   std::shared_ptr<Feature> findFrameRef(FramePtr frame);
@@ -85,22 +79,6 @@ public:
   void optimize(const size_t n_iter);
 
   /// Jacobian of point projection on unit plane (focal length = 1) in frame (f).
-  inline static void jacobian_xyz2uv(
-      const Vector3d& p_in_f,
-      const Matrix3d& R_f_w,
-      Matrix23d& point_jac)
-  {
-    const double z_inv = 1.0/p_in_f[2];
-    const double z_inv_sq = z_inv*z_inv;
-    point_jac(0, 0) = z_inv;
-    point_jac(0, 1) = 0.0;
-    point_jac(0, 2) = -p_in_f[0] * z_inv_sq;
-    point_jac(1, 0) = 0.0;
-    point_jac(1, 1) = z_inv;
-    point_jac(1, 2) = -p_in_f[1] * z_inv_sq;
-    point_jac = - point_jac * R_f_w;
-  }
-
     void jacobian_xyz2uv_(
             const Vector3d& p_in_f,
             const Matrix3d& R_f_w,
@@ -115,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 z_c = fram_t_f_w.se3().translation().y();
+        double x_c =  fram_t_f_w.se2().translation()(0);
+        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));
@@ -138,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_

vio/include/vio/pose_optimizer.h

@@ -1,23 +1,24 @@
-// 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_
-#define SVO_POSE_OPTIMIZER_H_
+#ifndef VIO_POSE_OPTIMIZER_H_
+#define VIO_POSE_OPTIMIZER_H_
 
 #include <vio/global.h>
+#include <vio/map.h>
 
 namespace vio {
 
@@ -35,17 +36,16 @@ class Point;
 namespace pose_optimizer {
 
 void optimizeGaussNewton(
-    const double reproj_thresh,
     const size_t n_iter,
-    const bool verbose,
     FramePtr& frame,
     double& estimated_scale,
     double& error_init,
     double& error_final,
     size_t& num_obs,
+    vio::Map& map,
     FILE* log);
 
 } // namespace pose_optimizer
 } // namespace vio
 
-#endif // SVO_POSE_OPTIMIZER_H_
+#endif // VIO_POSE_OPTIMIZER_H_

vio/include/vio/reprojector.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_
-#define SVO_REPROJECTION_H_
+#ifndef VIO_REPROJECTION_H_
+#define VIO_REPROJECTION_H_
 
 #include <vio/global.h>
 #include <vio/matcher.h>
@@ -61,8 +61,7 @@ public:
 
   /// Project points from the map into the image. First finds keyframes with
   /// overlapping field of view and projects only those map-points.
-
-  void reprojectMap2(
+  void reprojectMap(
       FramePtr frame,
       FramePtr last_frame,
       std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs,
@@ -76,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;
@@ -97,12 +95,10 @@ private:
   Matcher matcher_;
   Map& map_;
 
-  static bool pointQualityComparator(Candidate& lhs, Candidate& rhs);
   void initializeGrid(vk::AbstractCamera* cam);
   void resetGrid();
-  bool reprojectCell(Cell& cell, FramePtr frame, FILE* log_);
 };
 
 } // namespace vio
 
-#endif // SVO_REPROJECTION_H_
+#endif // VIO_REPROJECTION_H_

vio/include/vio/sparse_img_align.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_
-#define SVO_SPARSE_IMG_ALIGN_H_
+#ifndef VIO_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_

vio/include/vio/sparse_img_align_gpu.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.
 //

vio/include/vio/ukf.h

@@ -10,6 +10,8 @@
 #include <Eigen/Dense>
 #include <boost/thread.hpp>
 #include <boost/function.hpp>
+#include <chrono>
+#include <ctime>
 class Base{
 public:
     Base(const Eigen::Matrix<double, 3, 1> &init) {
@@ -23,45 +25,39 @@ public:
     virtual ~Base(){
     };
     void predict(double dx,double dy,double dpitch,double time){
-        double dt=time-t_cmd;
-        if(t_cmd==0.0){
-            t_cmd=time;
-            return;
-        }
-        t_cmd=time;
+        double dt=0.2;
         state_h_(2)=state_(2)+dt*dpitch;
-        state_h_(1)=state_(1)+dt*2.0*(dy*cos(state_(2))-dx*sin(state_(2)));
+        state_h_(1)=state_(1)+dt*(dy*cos(state_(2))-dx*sin(state_(2)));
         state_h_(0)=state_(0)+dt*(dx*cos(state_(2))+dy*sin(state_(2)));
         state_h_(3)=dpitch;
         Eigen::Matrix<double,4,4> R;
-        R<<vio::Config::Cmd_Cov(),0,0,0,
-           0,vio::Config::Cmd_Cov(),0,0,
-           0,0,vio::Config::Cmd_Cov(),0,
-           0,0,0,vio::Config::Cmd_Cov();
+        R<<vio::Config::Cmd_ekf_t(),0,0,0,
+           0,vio::Config::Cmd_ekf_t(),0,0,
+           0,0,vio::Config::Cmd_ekf_o(),0,
+           0,0,0,vio::Config::Cmd_ekf_o();
         Eigen::Matrix<double,4,4> G;
         G<<1.0,0,dt*(-dx*sin(state_(2))+dy*cos(state_(2))),0.0,
-           0,1.0,dt*2.0*(-dy*sin(state_(2))-dx*cos(state_(2))),0.0,
+           0,1.0,dt*(-dy*sin(state_(2))-dx*cos(state_(2))),0.0,
            0,0,1.0,dt,
            0,0,0,1.0;
         cov_h_=G*cov_*G.transpose()+R;
+        setTime();
     }
     void correct(double ddx, double ddy, double dpitch, double time){
-        double dt=time-t_imu;
-        if(t_imu==0.0){
-            t_imu=time;
-            return;
-        }
-        t_imu=time;
+        auto imu_end = std::chrono::steady_clock::now();
+        double dt=1e-9*(imu_end-vio_t).count();
+        vio_t=std::chrono::steady_clock::now();
         Eigen::Matrix<double,4,4> H;
         Eigen::Matrix<double,4,4> Q;
         Eigen::Matrix<double,4,1> E;
         Eigen::Matrix<double,4,4> k;
+        //update
         E(0)=state_(0)+pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2)))-state_h_(0);
-        E(1)=state_(1)+2.0*pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2)))-state_h_(1);
+        E(1)=state_(1)+pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2)))-state_h_(1);
         E(2)=state_(2)+dt*state_(3)-state_h_(2);
         E(3)=dpitch-state_h_(3);
         H<<1.0,0,pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2))),0.0,
-           0,1.0,-2.0*pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2))),0.0,
+           0,1.0,-pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2))),0.0,
            0,0,1.0,dt,
            0,0,0,1.0;
         Q<<vio::Config::ACC_Noise(),0,0,0,
@@ -71,8 +67,9 @@ public:
         k=cov_h_*H.transpose()*(H*cov_h_*H.transpose()+Q).inverse();
         state_=state_h_+k*E;
         cov_=(Eigen::MatrixXd::Identity(4,4)-k*H)*cov_h_;
+        //predict
         state_h_(2)=state_(2)+dt*state_(3);
-        state_h_(1)=state_(1)+2.0*pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2)));
+        state_h_(1)=state_(1)+pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2)));
         state_h_(0)=state_(0)+pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2)));
         state_h_(3)=state_(3);
         Eigen::Matrix<double,4,4> R;
@@ -82,7 +79,7 @@ public:
                 0,0,0,vio::Config::GYO_Noise();
         Eigen::Matrix<double,4,4> G;
         G<<1.0,0,pow(dt,2)*(ddy*cos(state_(2))-ddx*sin(state_(2))),0,
-           0,1.0,2.0*pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2))),0,
+           0,1.0,pow(dt,2)*(ddx*cos(state_(2))+ddy*sin(state_(2))),0,
            0,0,1.0,dt,
            0,0,0,1.0;
         cov_h_=G*cov_*G.transpose()+R;
@@ -96,21 +93,23 @@ public:
         H<<1.0,0,0,0,
            0,1.0,0,0,
            0,0,1.0,0;
-        Q<<vio::Config::Svo_Ekf(),0,0,
-           0,vio::Config::Svo_Ekf(),0,
-           0,0,vio::Config::Svo_Ekf();
+        Q<<vio::Config::VO_ekf_t(),0,0,
+           0,vio::Config::VO_ekf_t(),0,
+           0,0,vio::Config::VO_ekf_o();
         E<<x-state_h_(0),z-state_h_(1),pitch-state_h_(2);
         k=cov_h_*H.transpose()*(H*cov_h_*H.transpose()+Q).inverse();
         state_=state_h_+(k*E);
         cov_=(Eigen::MatrixXd::Identity(4,4)-k*H)*cov_h_;
     }
+    void setTime(){
+        vio_t=std::chrono::steady_clock::now();
+    }
     Eigen::Matrix<double,4,4> cov_;
     Eigen::Matrix<double, 4, 1> state_;
 private:
-    double t_cmd=0.0;
+    std::chrono::time_point<std::chrono::steady_clock,std::chrono::duration<double>> vio_t;
     Eigen::Matrix<double,4,4> cov_h_;
     Eigen::Matrix<double, 4, 1> state_h_;//state{x,y,pitch( rotation around z),dyaw} in world frame
-    double t_imu=0.0;
 
 };
 
@@ -122,11 +121,11 @@ public:
         delete filter_;
     };
     void UpdateIMU(double x/*in imu frame*/,double y/*in imu frame*/,double theta/*in imu frame*/,const ros::Time& time) {
-        if(abs(x)<1.0)x=pow(x,3);//picked up from your code
-        if(abs(y)<1.0)y=pow(y,3);//picked up from your code
+       /* if(abs(x)<1.0)x=pow(x,3);//picked up from your code
+        if(abs(y)<1.0)y=pow(y,3);//picked up from your code*/
         ++imu_syn_count_;
-        if(imu_syn_[imu_syn_count_ % 7]){
-            imu_syn_[imu_syn_count_ % 7]=false;
+        if(imu_syn_[imu_syn_count_ % 10]){
+            imu_syn_[imu_syn_count_ % 10]=false;
         }else{
             return;
         }
@@ -136,8 +135,7 @@ public:
     };
     //IMU frame y front, x right, z up -> left hands (theta counts from y)
     void UpdateCmd(double x/*in imu frame*/,double y/*in imu frame*/,double theta/*in imu frame*/,const ros::Time& time) {
-        cam_syn_[cmd_syn_count_ % 7]=true;
-        imu_syn_[cmd_syn_count_ % 7]=true;
+        imu_syn_[cmd_syn_count_ % 10]=true;
         ++cmd_syn_count_;
         if(cmd_syn_count_>50)cmd_syn_count_=0;
         boost::unique_lock<boost::mutex> lock(ekf_mut_);
@@ -145,7 +143,7 @@ public:
     };
     //Camera frame z back, x right, y down -> left hands (pitch counts from x)correct
     //Notice T_F_W is the position of the first frame with respect to the new frame while they are looking at one feature
-    std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> UpdateSvo(double x/*in camera frame*/,double z/*in camera frame*/,double pitch/*in camera frame*/) {
+    std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> UpdateVO(double x/*in camera frame*/,double z/*in camera frame*/,double pitch/*in camera frame*/) {
         boost::unique_lock<boost::mutex> lock(ekf_mut_);
         filter_->correct(x,z,-1.0*pitch);
         Eigen::Matrix<double,3,3> cov;
@@ -162,10 +160,12 @@ public:
                 filter_->cov_(2,0),filter_->cov_(2,1),filter_->cov_(2,2);
         return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(cov,vio::SE2_5(filter_->state_(0),filter_->state_(1),-1.0*(filter_->state_(2))));
     }
+    void setImuTime(){
+        filter_->setTime();
+    }
     size_t cmd_syn_count_=0;
     size_t imu_syn_count_=2;
-    bool cam_syn_[7]={false,false,false,false,false,false,false};
-    bool imu_syn_[7]={false,false,false,false,false,false,false};
+    bool imu_syn_[10]={false,false,false,false,false,false,false,false,false,false};
 private:
      Base* filter_= nullptr;
     boost::mutex ekf_mut_;

vio/include/vio/visualizer.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_VISUALIZER_H_
-#define SVO_VISUALIZER_H_
+#ifndef VIO_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_ */

vio/launch/Px30.launch

@@ -1,6 +1,6 @@
 <launch>
 
-  <!-- node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" / -->
+  <node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" />
 
     <!-- Camera topic to subscribe to -->
     <param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" />

vio/launch/test.launch

@@ -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" />

vio/launch/unity.launch

@@ -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" />

vio/loop_closure_log.txt

@@ -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 

vio/package.xml

@@ -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>

vio/param/vo_fast.yaml

@@ -0,0 +1,17 @@
+vio:
+  grid_size: 8            #Feature grid size of a cell in [px].
+  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.
+  init_min_tracked: 120
+  init_min_disparity: 10
+  structureoptim_max_pts: 50
+  kfselect_mindist: 0.01
+  poseoptim_thresh: 0.25
+  ACC_ekf: 0.2          #acc 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.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

vio/src/bundle_adjustment.cpp

@@ -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
 */

vio/src/cl_class.cpp

@@ -20,6 +20,7 @@ opencl::opencl(vk::AbstractCamera* cam):cam(cam) {
         std::cout << " No devices found. Check OpenCL installation!\n";
         exit(1);
     }
+
     device = new cl::Device(all_devices[0]);
     std::cout << "CL_DEVICE_NAME: " << device->getInfo<CL_DEVICE_NAME>() <<'\n'
               << "CL_DRIVER_VERSION: " <<device->getInfo<CL_DRIVER_VERSION>()<<'\n'
@@ -37,7 +38,7 @@ opencl::opencl(vk::AbstractCamera* cam):cam(cam) {
     sources.push_back({ compute_residual.src_str, compute_residual.size });
     program=new cl::Program(*context, sources);
     double* camera=cam->params();
-    std::string options="-DFAST_THRESH=40 -DPATCH_SIZE=2 -DPATCH_HALFSIZE=1 -DF_X="+ std::to_string(camera[0]) +
+    std::string options="-DFAST_THRESH=40 -DPATCH_SIZE=8 -DPATCH_HALFSIZE=4 -DF_X="+ std::to_string(camera[0]) +
                         " -DF_Y="+std::to_string(camera[1])+
                         " -DC_X="+std::to_string(camera[2])+
                         " -DC_Y="+std::to_string(camera[3])+
@@ -51,12 +52,10 @@ opencl::opencl(vk::AbstractCamera* cam):cam(cam) {
     queue=new cl::CommandQueue(*context,*device,CL_QUEUE_OUT_OF_ORDER_EXEC_MODE_ENABLE,NULL);
 }
 opencl::~opencl() {
+    clear_buf();
+    queue->finish();
+    delete program;
     delete context;
     delete device;
 }
-void opencl::clear_buf() {
-    queue->finish();
-    _kernels.clear();
-    delete program;
-}
 

vio/src/config.cpp

@@ -0,0 +1,67 @@
+// 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 <vio/params_helper.h>
+#include <vio/config.h>
+
+namespace vio {
+
+Config::Config() :
+    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)),
+    core_n_kfs(vk::getParam<int>("vio/core_n_kfs", 3)),
+    map_scale(vk::getParam<double>("vio/map_scale", 1.0)),
+    grid_size(vk::getParam<int>("vio/grid_size", 10)),
+    init_min_disparity(vk::getParam<double>("vio/init_min_disparity", 50.0)),
+    init_min_tracked(vk::getParam<int>("vio/init_min_tracked", 50)),
+    init_min_inliers(vk::getParam<int>("vio/init_min_inliers", 40)),
+    klt_max_level(vk::getParam<int>("vio/klt_max_level", 4)),
+    klt_min_level(vk::getParam<int>("vio/klt_min_level", 2)),
+    reproj_thresh(vk::getParam<double>("vio/reproj_thresh", 2.0)),
+    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", 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)),
+    kfselect_mindist(vk::getParam<double>("vio/kfselect_mindist", 0.12)),
+    triang_min_corner_score(vk::getParam<double>("vio/triang_min_corner_score", 20.0)),
+    triang_half_patch_size(vk::getParam<int>("vio/triang_half_patch_size", 4)),
+    subpix_n_iter(vk::getParam<int>("vio/subpix_n_iter", 10)),
+    max_n_kfs(vk::getParam<int>("vio/max_n_kfs", 10)),
+    img_imu_delay(vk::getParam<double>("vio/img_imu_delay", 0.0)),
+    max_fts(vk::getParam<int>("vio/max_fts", 120)),
+    quality_min_fts(vk::getParam<int>("vio/quality_min_fts", 50)),
+    quality_max_drop_fts(vk::getParam<int>("vio/quality_max_drop_fts", 40)),
+    ACC_noise(vk::getParam<double>("vio/ACC_ekf", 1e-4)),
+    GYO_noise(vk::getParam<double>("vio/GYO_ekf", 1e-4)),
+    vo_ekf_t(vk::getParam<double>("vio/VO_ekf_translation", 1e-4)),
+    vo_ekf_o(vk::getParam<double>("vio/VO_ekf_orientation", 1e-4)),
+    cmd_ekf_t(vk::getParam<double>("vio/CMD_ekf_translation", 1e-4)),
+    cmd_ekf_o(vk::getParam<double>("vio/CMD_ekf_orientation", 1e-4))
+{}
+
+Config& Config::getInstance()
+{
+  static Config instance; // Instantiated on first use and guaranteed to be destroyed
+  return instance;
+}
+
+} // namespace vio
+

vio/src/feature_alignment.cpp

@@ -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.
 //
@@ -69,7 +69,7 @@ bool align1D(
   float v = cur_px_estimate.y();
 
   // termination condition
-  const float min_update_squared = 0.001;
+  const float min_update_squared = 1.0;
   const int cur_step = cur_img.step.p[0];
   float chi2 = 0;
   Vector2f update; update.setZero();
@@ -163,9 +163,9 @@ bool align2D(
     return align2D_NEON(cur_img, ref_patch_with_border, ref_patch, n_iter, cur_px_estimate);
 #endif
 
-  const int halfpatch_size_ = 8;
-  const int patch_size_ = 16;
-  const int patch_area_ = 256;
+  const int halfpatch_size_ = 4;
+  const int patch_size_ = 8;
+  const int patch_area_ = 64;
   bool converged=false;
 
   // compute derivative of template and prepare inverse compositional
@@ -202,7 +202,7 @@ bool align2D(
   bool sign=true;// +
 
   // termination condition
-  const float min_update_squared = 1.0;//0.001
+  const float min_update_squared = 0.1;//0.001
   const int cur_step = cur_img.step.p[0];
 //  float chi2 = 0;
   Vector3f update; update.setZero();
@@ -337,7 +337,7 @@ bool align2D_SSE2(
   float v = cur_px_estimate.y();
 
   // termination condition
-  const float min_update_squared = 0.001;
+  const float min_update_squared = 1.0;
   const int cur_step = cur_img.step.p[0];
   const float Dinv = 1.0f/(A11*A22 - A12*A12); // we are missing an extra factor 16
   float chi2 = 0;
@@ -502,7 +502,7 @@ bool align2D_NEON (
   float v = cur_px_estimate.y();
 
   // termination condition
-  const float min_update_squared = 0.001;
+  const float min_update_squared = 1.0;
   const int cur_step = cur_img.step.p[0];
   Vector3f update;
   Vector3f Jres;

vio/src/feature_detection.cpp

@@ -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.
 //
@@ -70,62 +70,60 @@ void FastDetector::detect(
     std::shared_ptr<Frame> frame,
     const ImgPyr& img_pyr,
     const double detection_threshold,
-    list<shared_ptr<Feature>>& fts){
-  Corners corners(grid_n_cols_*grid_n_rows_, Corner(0,0,0.0,0,0.0f));
+    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;
-    const int scale = (1<<L);
+    int scale = (1<<L);
     cv::Mat img=img_pyr.at(L);
-    if(!gpu_fast_->reload_buf(0,0,img)){
-        ROS_ERROR("Failed to write into GPU goodbye :)");
-        exit(0);
-    };
+    gpu_fast_->load(0,0,img);
+    cl_int2* fast_corner=(cl_int2*)calloc(2000, sizeof(cl_int2));
+    gpu_fast_->load(0,1,2000,fast_corner);
     cl_int icorner[1]={0};
-    if(!gpu_fast_->reload_buf(0,2,icorner)){
-          ROS_ERROR("Failed to write into GPU goodbye :)");
-          exit(0);
-      };
+    gpu_fast_->load(0,2,1,icorner);
     gpu_fast_->run(0,img.cols,img.rows);
     cl_int count[1]={0};
     gpu_fast_->read(0,2,1,count);
-    cl_int2 fast_corners[350000]={0};
-    gpu_fast_->read(0,1,350000,fast_corners);
     if(count[0]<1 ){
-          ROS_ERROR("Can not communicate with GPU");
-          exit(0);
+        ROS_ERROR("Can not communicate with GPU");
+        exit(0);
     }
-    for(uint i=0;i<count[0];++i)
+    const int size=count[0];
+    if(size<1 || size > 2000){
+        gpu_fast_->release(0,0);
+        gpu_fast_->release(0,1);
+        gpu_fast_->release(0,2);
+        free(fast_corner);
+        return;
+    }
+    cl_int2* fast_corners=(cl_int2*)calloc(size, sizeof(cl_int2));
+    gpu_fast_->read(0,1,size,fast_corners);
+    for(uint i=0;i<size;++i)
     {
-      if(fast_corners==NULL)return;
-      if(fast_corners[i].x<1 || fast_corners[i].x>img_pyr[L].cols || fast_corners[i].y>img_pyr[L].rows || fast_corners[i].y<1){
+      if(fast_corners[i].x<5 || fast_corners[i].x>img_pyr[L].cols-5 || fast_corners[i].y>img_pyr[L].rows-5 || fast_corners[i].y<5){
           continue;
       }
-      const int k = static_cast<int>((fast_corners[i].y*scale)/cell_size_)*grid_n_cols_
-                  + static_cast<int>((fast_corners[i].x*scale)/cell_size_);
-
-      if(grid_occupancy_[k] || k > corners.size()){
-          continue;
-      }
-      const float score = vk::shiTomasiScore(img_pyr[L], fast_corners[i].x, fast_corners[i].y);
-      if(score > corners.at(k).score){
-          corners.at(k) = Corner(fast_corners[i].x, fast_corners[i].y, score, L, 0.0f);
-          keypoints.push_back(cv::KeyPoint(fast_corners[i].x*scale, fast_corners[i].y*scale, 7.f,-1,score));
-      }
+      float score = vk::shiTomasiScore(img_pyr[L], fast_corners[i].x, fast_corners[i].y);
+      keypoints.push_back(cv::KeyPoint(fast_corners[i].x*scale, fast_corners[i].y*scale, 7.f,-1,score));
     }
+    gpu_fast_->release(0,0);
+    gpu_fast_->release(0,1);
+    gpu_fast_->release(0,2);
+    free(fast_corner);
+    free(fast_corners);
   }
   if(keypoints.size()<1){
-      ROS_ERROR("GPU Driver crash try again!");
-      assert(false);
+      assert(0 && "GPU Driver crash try again!");
   }
-  cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 50.0f, 4);
+  cv::Ptr<cv::xfeatures2d::FREAK> extractor = cv::xfeatures2d::FREAK::create(true, true, 22.0f, 4);
   cv::Mat descriptor;
   extractor->compute(frame->img(), keypoints, descriptor);
-  for(auto&& p:_for(keypoints))fts.push_back(make_shared<Feature>(frame, Vector2d(p.item.pt.x, p.item.pt.y), p.item.response ,0,descriptor.data+(p.index*64)));
-  resetGrid();
-};
+  for(auto&& p:_for(keypoints)){
+      fts.push_back(make_shared<Feature>(frame, Vector2d(p.item.pt.x, p.item.pt.y), p.item.response ,0,descriptor.data+(p.index*64)));
+  }
+}
 
 } // namespace feature_detection
 } // namespace vio

vio/src/frame.cpp

@@ -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()
@@ -143,8 +144,8 @@ void Frame::removeKeyPoint(std::shared_ptr<Feature> ftr)
 
 bool Frame::isVisible(const Vector3d& xyz_w) const
 {
-    if(!id_)return false;
-    assert(!xyz_w.hasNaN());
+  if(!id_)return false;
+  if(xyz_w.hasNaN())return false;
   Vector3d xyz_f = this->se3().inverse()*xyz_w;
   if(xyz_f.z() < 0.0)
     return false; // point is behind the camera
@@ -154,11 +155,7 @@ bool Frame::isVisible(const Vector3d& xyz_w) const
   return false;
 }
 
-
-/// Utility functions for the Frame class
-namespace frame_utils {
-
-void createImgPyramid(const cv::Mat& img_level_0, int n_levels, ImgPyr& pyr)
+void Frame::createImgPyramid(const cv::Mat& img_level_0, int n_levels, ImgPyr& pyr)
 {
   pyr.resize(n_levels);
   pyr[0] = img_level_0;
@@ -169,31 +166,31 @@ 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;
-  //depth_vec.reserve(frame.fts_.size());
-  depth_min = std::numeric_limits<double>::max();
-  for(auto it=frame->fts_.begin(), ite=frame->fts_.end(); it!=ite; ++it)
+  for(auto it=fts_.begin(); it!=fts_.end();)
   {
-    if((*it)->point != NULL)
-    {
-        Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));
-        if((e /= (1<<(*it)->level)).norm()>0.5)continue;
-        double z = frame->w2f((*it)->point->pos_).z();
-        depth_vec.push_back(z);
-        depth_min = fmin(z, depth_min);
-
-    }
+      if((*it)->point==NULL){
+          ++it;
+          continue;
+      }
+      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())
   {
     return false;
   }
-  double min,max;
-  depth_mean = vk::getMean(depth_vec,min,max);
+  double max;
+  depth_mean = vk::getMean(depth_vec,depth_min,max);
   return true;
 }
 
-} // namespace frame_utils
 } // namespace vio

vio/src/frame_handler_base.cpp

@@ -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);
-  }
-  //max_n_pts = min(max_n_pts, pts.size());
-  //nth_element(pts.begin(), pts.begin() + max_n_pts, pts.end(), ptLastOptimComparator);
-  for(auto&& pt:pts){
-      pt->optimize(max_iter);
-      pt->last_frame_overlap_id_= frame->id_;
+      if(it->point==NULL)continue;
+      if(it->point->obs_.size()<2){
+          it->point->last_frame_overlap_id_= frame->id_;
+          continue;
+      }
+      it->point->optimize(max_iter);
+      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

vio/src/frame_handler_mono.cpp

@@ -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,7 +21,6 @@
 #include <vio/feature.h>
 #include <vio/point.h>
 #include <vio/pose_optimizer.h>
-#include <vio/sparse_img_align.h>
 #include <vio/global_optimizer.h>
 #include <vio/for_it.hpp>
 #include <assert.h>
@@ -40,37 +39,7 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,
 {
     gpu_fast_= new opencl(cam_);
     gpu_fast_->make_kernel("fast_gray");
-    cv::Mat img=cv::Mat(cv::Size(700, 500),CV_8UC1);
-    std::vector<bool> errors;
-    errors.push_back(gpu_fast_->write_buf(0,0,img));
-    cl_int2 corners_[350000]={0};
-    errors.push_back(gpu_fast_->write_buf(0,1,350000,corners_));
-    cl_int icorner[1]={0};
-    errors.push_back(gpu_fast_->write_buf(0,2,1,icorner));
     gpu_fast_->make_kernel("compute_residual");
-    errors.push_back(gpu_fast_->write_buf(1,0,img));
-    errors.push_back(gpu_fast_->write_buf(1,1,img));
-    cl_float3 pose[1]={0.0};
-    errors.push_back(gpu_fast_->write_buf(1,2,3,pose));
-    errors.push_back(gpu_fast_->write_buf(1,3,3,pose));
-    cl_float3 f[300]={0};
-    errors.push_back(gpu_fast_->write_buf(1,4,300,f));
-    cl_float2 px[300]={0};
-    errors.push_back(gpu_fast_->write_buf(1,5,300,px));
-    cl_float  e[300]={0};
-    errors.push_back(gpu_fast_->write_buf(1,7,300,e));
-    cl_float H[9*300]={0};
-    errors.push_back(gpu_fast_->write_buf(1,8,9*300,H));
-    cl_float3 J[300];
-    errors.push_back(gpu_fast_->write_buf(1,9,300,J));
-    cl_float chi2[300]={0.01};
-    errors.push_back(gpu_fast_->write_buf(1,10,300,chi2));
-    for(auto e:errors){
-        if(e!=true){
-            std::cerr<<"Failed to write into GPU goodbye :) "<<'\n';
-            exit(0);
-        }
-    }
     initialize();
 #if VIO_DEBUG
     log_ =fopen((std::string(PROJECT_DIR)+"/frame_handler_log.txt").c_str(),"w+");
@@ -84,9 +53,6 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,
 
 void FrameHandlerMono::initialize()
 {
-  feature_detection::DetectorPtr feature_detector(
-      new feature_detection::FastDetector(
-          cam_->width(), cam_->height(), Config::gridSize(),gpu_fast_, Config::nPyrLevels()));
   ba_glob_ = new BA_Glob(map_);
   ba_glob_->startThread();
 }
@@ -98,7 +64,7 @@ FrameHandlerMono::~FrameHandlerMono()
 
 void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp,const ros::Time& time)
 {
-
+    ukfPtr_.setImuTime();
   if(!startFrameProcessingCommon(timestamp)){
       return;
   }
@@ -130,9 +96,9 @@ FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame()
   if(klt_homography_init_->addFirstFrame(new_frame_) == initialization::FAILURE){
       return RESULT_NO_KEYFRAME;
   }
-  new_frame_->setKeyframe();
-  map_.reset();
-  map_.addKeyframe(new_frame_);
+  //new_frame_->setKeyframe();
+  //map_.reset();
+  //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());
@@ -156,18 +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
-  //std::cerr<<"run BA\n";
-  ba_glob_->new_key_frame();
+  //ba_glob_->new_key_frame();
   ROS_INFO("VIO initialized :)");
   ROS_INFO("Running ...");
   return RESULT_IS_KEYFRAME;
@@ -179,38 +144,22 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   auto init_f= ukfPtr_.get_location();
   new_frame_->T_f_w_=init_f.second;
   new_frame_->Cov_ = init_f.first;
-  // sparse image align
-  SparseImgAlignGpu img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_);
-  size_t err=img_align.run(last_frame_, new_frame_, log_);
-  if(err==-1){
-#if VIO_DEBUG
-      fprintf(log_,"[%s] Image Alignment failed -1\n",vio::time_in_HH_MM_SS_MMM().c_str());
-#endif
-      new_frame_=last_frame_;
-      return RESULT_FAILURE;
-  }else if(err==0){
-#if VIO_DEBUG
-      fprintf(log_,"[%s] Image Alignment failed 0\n",vio::time_in_HH_MM_SS_MMM().c_str());
-#endif
-      new_frame_=last_frame_;
-      return RESULT_FAILURE;
-  }
-  reprojector_.reprojectMap2(new_frame_, last_frame_,overlap_kfs_, gpu_fast_, log_);
+  reprojector_.reprojectMap(new_frame_, last_frame_,overlap_kfs_, gpu_fast_, log_);
   int n_point=0;
   for(auto i:overlap_kfs_)n_point+=i.second;
 #if VIO_DEBUG
-    fprintf(log_,"[%s] After Reprojection Map nPoint:%d ,nCell:%d ,nMatches:%d ,distance between ekf and vo x:%f ,z=%f,angle between two frames:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
+    fprintf(log_,"[%s] After Reprojection Map nMatches:%d ,distance between ekf and vo x:%f ,z=%f,angle between two frames:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
             n_point,
-            reprojector_.n_trials_,reprojector_.n_matches_,
             new_frame_->T_f_w_.se2().translation().x()-init_f.second.se2().translation().x(),
             new_frame_->T_f_w_.se2().translation().y()-init_f.second.se2().translation().y(),
             fabs(new_frame_->T_f_w_.pitch()-init_f.second.pitch()));
 #endif
+  boost::unique_lock< boost::mutex > lock(ba_glob_->ba_mux_);
   size_t sfba_n_edges_final=0;
   double sfba_thresh, sfba_error_init, sfba_error_final;
   pose_optimizer::optimizeGaussNewton(
-            Config::poseOptimThresh(), 70, false,
-            new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final,log_);
+            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(),
             new_frame_->T_f_w_.se2().translation().x()-init_f.second.se2().translation().x(),
@@ -218,26 +167,34 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
             fabs(new_frame_->T_f_w_.pitch()-init_f.second.pitch()));
 #endif
     if((init_f.second.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>0.2 ||
-       fabs(new_frame_->T_f_w_.pitch()-init_f.second.pitch())>0.02 || sfba_n_edges_final<10){
+       fabs(new_frame_->T_f_w_.pitch()-init_f.second.pitch())>0.2 || sfba_n_edges_final<10){
         new_frame_=last_frame_;
         return RESULT_FAILURE;
     }
-  auto result=ukfPtr_.UpdateSvo(new_frame_->T_f_w_.se2().translation()(0),
+  auto result=ukfPtr_.UpdateVO(new_frame_->T_f_w_.se2().translation()(0),
                                 new_frame_->T_f_w_.se2().translation()(1),new_frame_->T_f_w_.pitch());
   new_frame_->T_f_w_ =result.second;
   new_frame_->Cov_ = result.first;
 #if VIO_DEBUG
     fprintf(log_,"[%s] Update EKF and 3D points the number of feature in the new frame: %d and number of obs: %d\n",vio::time_in_HH_MM_SS_MMM().c_str(),
             new_frame_->fts_.size(),new_frame_->nObs());
+#endif
+  double depth_mean=0.0, depth_min=0.0;
+    new_frame_->getSceneDepth(map_, depth_mean, depth_min);
+#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
   {
         return RESULT_NO_KEYFRAME;
   }
-  double depth_mean=0.0, depth_min=0.0;
-  frame_utils::getSceneDepth(new_frame_, depth_mean, depth_min);
+
   new_frame_->setKeyframe();
 #if VIO_DEBUG
     fprintf(log_,"[%s] Choose frame as a key frame Scene Depth mean:%f ,depth min:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
@@ -246,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
@@ -257,9 +218,14 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   }
   // add keyframe to map
   map_.addKeyframe(new_frame_);
-  std::cerr<<"BA run\n";
-  ba_glob_->new_key_frame();
-  return RESULT_IS_KEYFRAME;
+  if(map_.checkKeyFrames()){
+      ba_glob_->new_key_frame();
+/*      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;
 }
 
 
@@ -276,12 +242,17 @@ bool FrameHandlerMono::needNewKf()
     SE2_5 closest_kfs(0,0,0);
   for(auto&& it:overlap_kfs_)
   {
+      if(it.first->id_==last_frame_->id_)continue;
       if(it.second>com_obs){
           com_obs=it.second;
           closest_kfs=SE2_5(it.first->T_f_w_.se2());
       }
   }
-  if(fabs(closest_kfs.pitch()-new_frame_->T_f_w_.pitch()) > 0.43 || fabs((closest_kfs.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm())>0.15)return true;
+#if VIO_DEBUG
+    fprintf(log_,"[%s] need key frame pitch dis: %f translation dif:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
+            fabs(closest_kfs.pitch()-new_frame_->T_f_w_.pitch()),(closest_kfs.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm());
+#endif
+  if(fabs(closest_kfs.pitch()-new_frame_->T_f_w_.pitch()) > 0.1 || fabs((closest_kfs.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm())>0.1)return true;
   return false;
 }
 void FrameHandlerMono::UpdateIMU(double* value,const ros::Time& time){

vio/src/global_optimizer.cpp

@@ -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

vio/src/initialization.cpp

@@ -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.
 //
@@ -51,14 +51,20 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
 {
   trackKlt(frame_ref_, frame_cur, px_ref_, px_cur_, features_ref_, disparities_);
   if(disparities_.size() < 1){
-      ukf_->UpdateSvo(0.0,0.0,0.0);
+      ukf_->UpdateVO(0.0,0.0,0.0);
       return NO_KEYFRAME;
   }
+  if(disparities_.size() < 20){
+#if VIO_DEBUG
+      fprintf(log_,"[%s] VIO can not be initialized goodbye :)\n",vio::time_in_HH_MM_SS_MMM().c_str());
+#endif
+      assert(0);
+  }
   double min,max;
   double disparity = vk::getMean(disparities_,min,max);
     if(disparity < Config::initMinDisparity()){
         auto result=ukf_->get_location();
-        if(fabs(result.second.se2().translation().x())>0.2 || fabs(result.second.pitch())>0.0698132)ukf_->UpdateSvo(0.0,result.second.se2().translation().y(),0.0);
+        if(fabs(result.second.se2().translation().x())>0.2 || fabs(result.second.pitch())>0.0698132)ukf_->UpdateVO(0.0,result.second.se2().translation().y(),0.0);
 #if VIO_DEBUG
         fprintf(log_,"[%s] Init: px average disparity is:%f ,While minimum is: %f  KLT tracked : %d\n",vio::time_in_HH_MM_SS_MMM().c_str(),
                 disparity,
@@ -69,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
@@ -90,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);
@@ -105,6 +111,7 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
           frame_ref_->addFeature(f.item);
       }
   }
+  //debug(frame_ref_,frame_cur);
 #if VIO_DEBUG
     fprintf(log_,"[%s] Init finished: Homography RANSAC (inlier) is:%d ,While %d inliers minimum required.  px average disparity is:%f ,While minimum is: %f  KLT tracked: %d\n",
             vio::time_in_HH_MM_SS_MMM().c_str(),
@@ -130,9 +137,9 @@ void detectFeatures(
     opencl* gpu_fast)
 {
 
-  feature_detection::FastDetector detector(
+  std::unique_ptr<feature_detection::FastDetector> detector=std::make_unique<feature_detection::FastDetector>(
       frame->img().cols, frame->img().rows, Config::gridSize(), gpu_fast,Config::nPyrLevels());
-  detector.detect(frame, frame->img_pyr_, Config::triangMinCornerScore(), new_features);
+  detector->detect(frame, frame->img_pyr_, Config::triangMinCornerScore(), new_features);
 
   // now for all maximum corners, initialize a new seed
   px_vec.clear();

vio/src/map.cpp

@@ -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.
 //
@@ -39,6 +39,7 @@ void Map::reset()
 
 bool Map::safeDeleteFrame(FramePtr frame)
 {
+  //boost::unique_lock<boost::mutex> lock(point_mut_);
   bool found = false;
   size_t position;
   for(auto&& keyframe:_for(keyframes_)){
@@ -56,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;
 }
 
@@ -76,16 +77,26 @@ void Map::removePtFrameRef(FramePtr frame, std::shared_ptr<Feature> ftr)
 
 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);
-          ftr->point.reset();
+          if(ftr->frame!=nullptr){
+              if(ftr->frame->is_keyframe_){
+                  ftr->frame->removeKeyPoint(ftr);
+              }
+          }
+          if(ftr->point!=nullptr)ftr->point.reset();
       }
   }else{
-      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);
 }
@@ -101,41 +112,22 @@ void Map::addKeyframe(FramePtr new_keyframe)
   keyframes_.push_back(new_keyframe);
 }
 
-void Map::getCloseKeyframes(
-    const FramePtr& frame,
-    std::list< std::pair<FramePtr,double> >& close_kfs) const
-{
-  for(auto kf : keyframes_)
-  {
-    // check if kf has overlaping field of view with frame, use therefore KeyPoints
-    for(auto keypoint : kf->key_pts_)
-    {
-      if(keypoint == nullptr)
-          continue;
-      if(keypoint->point==NULL)continue;
-      if(frame->isVisible(keypoint->point->pos_))
-      {
-        close_kfs.push_back(
-            std::make_pair(
-                kf, (frame->T_f_w_.se2().translation()-kf->T_f_w_.se2().translation()).norm()));
-        break; // this keyframe has an overlapping field of view -> add to close_kfs
-      }
-    }
-  }
-}
 
-
-FramePtr Map::getFurthestKeyframe(const Vector2d& pos) const
+FramePtr Map::getFurthestKeyframe(const Vector2d& pos)
 {
   FramePtr furthest_kf;
   double maxdist = 0.0;
-  for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it)
+  for(auto it=keyframes_.begin(); it!=keyframes_.end(); )
   {
+      if((*it)->T_f_w_.empty()){
+          it=keyframes_.erase(it);
+      }
     double dist = ((*it)->pos()-pos).norm();
     if(dist > maxdist) {
       maxdist = dist;
       furthest_kf = *it;
     }
+      ++it;
   }
   return furthest_kf;
 }
@@ -147,6 +139,17 @@ void Map::emptyTrash()
   for(auto&& t:trash_points_)t.reset();
   trash_points_.clear();
 }
+bool Map::checkKeyFrames() {
+    for(auto it=keyframes_.begin();it!=keyframes_.end();){
+        if((*it)->T_f_w_.empty()){
+            for(auto&& fts:(*it)->fts_)removePtFrameRef(*it, fts);
+            it=keyframes_.erase(it);
+        }
+        ++it;
+    }
+    if(keyframes_.size()>0)return true;
+    return false;
+}
 
 
 namespace map_debug {

vio/src/matcher.cpp

@@ -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.
 //
@@ -42,12 +42,9 @@ void getWarpMatrixAffine(
 {
   // Compute affine warp matrix A_ref_cur
   const int halfpatch_size = 4;
-  const Vector3d xyz_ref(f_ref*depth_ref);
   Vector3d xyz_du_ref(cam_ref.cam2world(px_ref + Vector2d(halfpatch_size,0)*(1<<level_ref)));
   Vector3d xyz_dv_ref(cam_ref.cam2world(px_ref + Vector2d(0,halfpatch_size)*(1<<level_ref)));
-  xyz_du_ref *= xyz_ref[2]/xyz_du_ref[2];
-  xyz_dv_ref *= xyz_ref[2]/xyz_dv_ref[2];
-  const Vector2d px_cur(cam_cur.world2cam(T_cur_ref*(xyz_ref)));
+  const Vector2d px_cur(cam_cur.world2cam(T_cur_ref*(f_ref)));
   const Vector2d px_du(cam_cur.world2cam(T_cur_ref*(xyz_du_ref)));
   const Vector2d px_dv(cam_cur.world2cam(T_cur_ref*(xyz_dv_ref)));
   A_cur_ref.col(0) = (px_du - px_cur)/halfpatch_size;
@@ -81,11 +78,6 @@ bool warpAffine(
   if(patch==NULL || img_ref.empty())return false;
   const int patch_size = halfpatch_size*2 ;
   const Matrix2f A_ref_cur = A_cur_ref.inverse().cast<float>();
-  if(isnan(A_ref_cur(0,0)))
-  {
-    printf("Affine warp is NaN, probably camera has no translation\n"); // TODO
-    return false;
-  }
   // Perform the warp on a larger patch.
   uint8_t* patch_ptr = patch;
   const Vector2f px_ref_pyr = px_ref.cast<float>() / (1<<level_ref);
@@ -134,6 +126,42 @@ void Matcher::createPatchFromPatchWithBorder()
       ref_patch_ptr[x] = ref_patch_border_ptr[x];
   }
 }
+cv::Scalar getMSSIM( const cv::Mat& i1, const cv::Mat& i2){
+        const double C1 = 6.5025, C2 = 58.5225;
+        /**************************** INITS *********************************/
+        int d     = CV_32F;
+        cv::Mat I1, I2;
+        i1.convertTo(I1, d);           // cannot calculate on one byte large values
+        i2.convertTo(I2, d);
+        cv::Mat I2_2   = I2.mul(I2);        // I2^2
+        cv::Mat I1_2   = I1.mul(I1);        // I1^2
+        cv::Mat I1_I2  = I1.mul(I2);        // I1 * I2
+        /************************** END INITS *********************************/
+        cv::Mat mu1, mu2;   // PRELIMINARY COMPUTING
+        GaussianBlur(I1, mu1, cv::Size(11, 11), 1.5);
+        GaussianBlur(I2, mu2, cv::Size(11, 11), 1.5);
+        cv::Mat mu1_2   =   mu1.mul(mu1);
+        cv::Mat mu2_2   =   mu2.mul(mu2);
+        cv::Mat mu1_mu2 =   mu1.mul(mu2);
+        cv::Mat sigma1_2, sigma2_2, sigma12;
+        GaussianBlur(I1_2, sigma1_2, cv::Size(11, 11), 1.5);
+        sigma1_2 -= mu1_2;
+        GaussianBlur(I2_2, sigma2_2, cv::Size(11, 11), 1.5);
+        sigma2_2 -= mu2_2;
+        GaussianBlur(I1_I2, sigma12, cv::Size(11, 11), 1.5);
+        sigma12 -= mu1_mu2;
+        cv::Mat t1, t2, t3;
+        t1 = 2 * mu1_mu2 + C1;
+        t2 = 2 * sigma12 + C2;
+        t3 = t1.mul(t2);              // t3 = ((2*mu1_mu2 + C1).*(2*sigma12 + C2))
+        t1 = mu1_2 + mu2_2 + C1;
+        t2 = sigma1_2 + sigma2_2 + C2;
+        t1 = t1.mul(t2);               // t1 =((mu1_2 + mu2_2 + C1).*(sigma1_2 + sigma2_2 + C2))
+        cv::Mat ssim_map;
+        divide(t3, t1, ssim_map);      // ssim_map =  t3./t1;
+        cv::Scalar mssim = mean( ssim_map ); // mssim = average of ssim map
+        return mssim;
+}
 
 bool Matcher::findMatchDirect(
     const Point& pt,
@@ -166,30 +194,25 @@ bool Matcher::findMatchDirect(
       cur_frame.se3().inverse() * ref_ftr_->frame->se3(), ref_ftr_->level, A_cur_ref_);
 
   //search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1);
+  /// TODO paches will be mirrored while robot is rotating around it self
   if(!warp::warpAffine(A_cur_ref_, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px,
                    ref_ftr_->level, ref_ftr_->level, halfpatch_size_+1, patch_with_border_))return false;
   createPatchFromPatchWithBorder();
   // px_cur should be set
-  Vector2d px_scaled(px_cur);
 
   bool success = false;
-  if(ref_ftr_->type == Feature::EDGELET)
-  {
-    Vector2d dir_cur(A_cur_ref_*ref_ftr_->grad);
-    dir_cur.normalize();
-    success = feature_alignment::align1D(
-          cur_frame.img_pyr_[ref_ftr_->level], dir_cur.cast<float>(),
-          patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_);
-  }
+  if(!warp::warpAffine(A_cur_ref_.inverse(), cur_frame.img_pyr_[ref_ftr_->level], px_cur,
+                         ref_ftr_->level, ref_ftr_->level, halfpatch_size_+1, patch_with_border_cur_))return false;
+  cv::Mat Patch_ref = cv::Mat(patch_size_+2, patch_size_+2, CV_8UC1, patch_with_border_);
+  cv::Mat Patch_cur = cv::Mat(patch_size_+2, patch_size_+2, CV_8UC1, patch_with_border_cur_);
+  cv::Scalar res = getMSSIM(Patch_cur, Patch_ref);
+  if(res[0] >= 0.85)
+        success = true;
   else
-  {
-    success = feature_alignment::align2D(
-      cur_frame.img_pyr_[ref_ftr_->level], patch_with_border_, patch_,
-      options_.align_max_iter, px_scaled);
-  }
+        success = false;
 /*  debug(ref_ftr_->frame->img_pyr_[ref_ftr_->level],cur_frame.img_pyr_[ref_ftr_->level],ref_ftr_->px,
-        px_cur,px_scaled ,success,patch_,patch_with_border_);*/
-  px_cur = px_scaled;
+        px_cur,px_scaled ,success,patch_,patch_with_border_,(ref_ftr_->frame->se3().inverse()*pt.pos_).z());*/
+  //px_cur = px_scaled;
   return success;
 }
 

vio/src/math_utils.cpp

@@ -7,31 +7,41 @@
 
 #include <vio/math_utils.h>
 #include <ros/ros.h>
+#include <Eigen/SVD>
 
 namespace vk {
 
 using namespace Eigen;
 /*
- * R and T are the rotation and translation of the current frames with respect to the refrence frame
+ * R and T are the rotation and translation of the current frames with respect to the reference frame, T from ref to current
  * */
-
+///TODO we need to add the restriction of the movement here
 Vector3d
 triangulateFeatureNonLin(const Matrix3d& R,  const Vector3d& t,
                          const Vector3d& feature1, const Vector3d& feature2 )
 {
-  Vector3d f2 = R * feature2;
-  Vector2d b;
-  b[0] = t.dot(feature1);
-  b[1] = t.dot(f2);//cross(t,R*f2)
-  Matrix2d A;
-  A(0,0) = feature1.dot(feature1);
-  A(1,0) = feature1.dot(f2);
-  A(0,1) = -A(1,0);
-  A(1,1) = -f2.dot(f2);
-  Vector2d lambda = A.inverse() * b;
-  Vector3d xm = lambda[0] * feature1;
-  Vector3d xn = t + lambda[1] * f2;
-  return ( xm + xn )/2;
+        Vector3d m0_hat = R * feature2;//T*A=R*A+t
+        Vector3d m1_hat = feature1;
+        Eigen::Matrix<double,3,2> x;
+        x<<m0_hat,m1_hat;
+        Eigen::Matrix<double,2,3> svd_in = x.transpose() * (Eigen::Matrix3d::Identity() - (t/t.norm())*(t/t.norm()).transpose());
+        Eigen::JacobiSVD<MatrixXd> svd( svd_in, ComputeFullU | ComputeFullV);
+        Vector3d n_p_hat = svd.matrixV().col(1);
+        Vector3d m0_p = m0_hat - (m0_hat.dot(n_p_hat)) * n_p_hat;
+        Vector3d m1_p = m1_hat - (m1_hat.dot(n_p_hat)) * n_p_hat;
+        Vector3d R_F2_p = m0_p;
+        Vector3d f1_p = m1_p;
+        Vector3d z = f1_p.cross(R_F2_p);
+        Vector3d b0 = t.cross(f1_p);
+        Vector3d b1 = t.cross(R_F2_p);
+        double lamda0 = z.dot(b0)/pow(z.norm(), 2);
+        double lamda1 = z.dot(b1)/pow(z.norm(), 2);
+        if (lamda0 <= 0 || lamda1 <= 0)return Vector3d(0.0,0.0,0.0);
+        if (max(m0_hat.dot(R_F2_p), feature1.dot(f1_p)) > M_PI_2)return Vector3d(0.0,0.0,0.0);
+        if(R_F2_p.dot(f1_p) < 0.01)return Vector3d(0.0,0.0,0.0);
+        Vector3d xm =  lamda1 * f1_p;
+        Vector3d xn = t + lamda0 * R_F2_p;
+        return ( xm + xn )/2;
 }
 
 bool
@@ -73,7 +83,7 @@ computeInliers(const vector<Vector3d>& features2, // c2
                const Vector3d& t,                 // T_c1_to_c2
                const double reproj_thresh,
                double error_multiplier2,
-               vector<Vector3d>& xyz_vec,         // 3d point: optimal triangulation is to minimally correct the feature rays
+               vector<Vector3d>& xyz_vec,         // 3d point: with respect to the reference frame
                vector<int>& inliers,
                vector<int>& outliers)
 {
@@ -84,18 +94,16 @@ computeInliers(const vector<Vector3d>& features2, // c2
   //triangulate all features and compute reprojection errors and inliers
   for(size_t j=0; j<features1.size(); ++j)
   {
-    if(t.z()>0.0){
-        xyz_vec.push_back(triangulateFeatureNonLin(R, t, features1[j]/*refrence*/, features2[j]/*current*/ ));
+        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], R.transpose()*(xyz_vec.back()-t), error_multiplier2);
-        if(e1 > reproj_thresh || e2 > reproj_thresh)
+        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 ||xyz_vec.back().z()==0. ||xyz_vec.back().hasNaN())
             outliers.push_back(j);
         else
         {
             inliers.push_back(j);
             tot_error += e1+e2;
         }
-    }
   }
   return tot_error;
 }