corrent situation

GPU_version/vio/CMakeLists.txt

@@ -68,6 +68,7 @@ LIST(APPEND LINK_LIBS
   ${G2O_SOLVER_PCG}
   ${G2O_TYPES_SBA}
   ${G2O_TYPES_SLAM3D}
+  ${G2O_TYPES_ICP}
   cholmod cxsparse
 )
 

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

GPU_version/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.

GPU_version/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;
 

GPU_version/vio/include/vio/cl_class.h

@@ -13,6 +13,7 @@
 #include <CL/opencl.h>
 #include <opencv2/opencv.hpp>
 #include <vio/abstract_camera.h>
+#include <vio/for_it.hpp>
 
 class kernel{
 public:
@@ -22,96 +23,86 @@ public:
         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);
+        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>
-    int32_t write(size_t id/*buffer ID*/,T* buf,cl::CommandQueue* queue,cl::Context* context,size_t buf_size){
+    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::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);
+        _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);
-        return error;
+        cl_int err=_kernel->setArg(id,*_buffers.back().first);
+        assert(err == CL_SUCCESS);
 
     };
-    int32_t write(size_t id/*buffer ID*/,cv::Mat& buf,cl::Context* context){
+    void load(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,
+        _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));
-        cl_int err=_kernel->setArg(id,_images.back().first);
-        assert(err == CL_SUCCESS);
         assert(error == CL_SUCCESS);
-        return error;
+        cl_int err=_kernel->setArg(id,*_images.back().first);
+        assert(err == CL_SUCCESS);
     };
-
     template<typename T>
-    int32_t reload(size_t id,T* buf,cl::CommandQueue* queue){
+    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.first,CL_NON_BLOCKING,CL_MAP_WRITE,0,sizeof(T) * i.first.second,NULL,&event,&error);
+                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) * i.first.second);
-                assert(queue->enqueueUnmapMemObject(i.first.first,Map_buf,NULL,&event)==CL_SUCCESS);
+                memcpy(Map_buf,buf,sizeof(T) * size);
+                assert(queue->enqueueUnmapMemObject(*i.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;
+    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;
+            }
         }
     };
-    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;
+    void release(){
+        _buffers.clear();
+        _images.clear();
     }
-    cl::Image2D read_img(size_t id){
-        for(auto i:_images)if(i.second==id)return i.first;
+    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::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;
+    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 *)malloc(0x100000);
+        src_str=(char *)calloc(0x100000, sizeof(char));
         FILE *fp;
         fp = fopen(path.c_str(), "r");
         size = fread(src_str, 1, 0x100000, fp);
@@ -129,35 +120,24 @@ public:
     ~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;
+    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);
     }
-    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;
+    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>
-    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;
+    void load(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,T&& value){
+        _kernels.at(id1)._kernel->setArg(id2,value);
     };
     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;
-    }
+    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;
-        if(queue->flush()==CL_OUT_OF_HOST_MEMORY){
-            std::cerr<<"GPU out of memory goodbye:)\n";
-            exit(0);
-        }
+        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){
@@ -167,10 +147,7 @@ public:
         };
         assert(err==CL_SUCCESS);
         event.wait();
-        if(queue->finish()==CL_OUT_OF_HOST_MEMORY){
-            std::cerr<<"GPU out of memory goodbye:)\n";
-            exit(0);
-        }
+        assert(queue->finish()==CL_SUCCESS);
         return 1;
 
     }
@@ -179,14 +156,19 @@ public:
         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);
+        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);
-        if(queue->enqueueUnmapMemObject(_kernels.at(id1).read(id2).first,Map_buf,NULL,&event)!=CL_SUCCESS)std::cerr<<"Read buffer on GPU failed"<<'\n';
+        assert(queue->enqueueUnmapMemObject(*_kernels.at(id1).read(id2),Map_buf,NULL,&event)==CL_SUCCESS);
         event.wait();
     }
-    void clear_buf();
+    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;

GPU_version/vio/include/vio/config.h

@@ -124,21 +124,10 @@ public:
 
   /// 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,7 +163,7 @@ 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

GPU_version/vio/include/vio/feature_detection.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)

GPU_version/vio/include/vio/frame.h

@@ -93,7 +93,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]); }

GPU_version/vio/include/vio/frame_handler_mono.h

@@ -24,7 +24,6 @@
 #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 {
@@ -34,7 +33,6 @@ class FrameHandlerMono : public FrameHandlerBase
 {
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-  
   FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,3,1>& init);
   virtual ~FrameHandlerMono();
 

GPU_version/vio/include/vio/global.h

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

GPU_version/vio/include/vio/global_optimizer.h

@@ -31,10 +31,9 @@
 #include <g2o/core/robust_kernel_impl.h>
 #include <g2o/core/optimization_algorithm_levenberg.h>
 #include <g2o/solvers/cholmod/linear_solver_cholmod.h>
-#include <g2o/solvers/csparse/linear_solver_csparse.h>
-#include <g2o/solvers/dense/linear_solver_dense.h>
 #include <g2o/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>
 
@@ -61,16 +60,14 @@ 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_;
@@ -86,14 +83,6 @@ protected:
             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,

GPU_version/vio/include/vio/initialization.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_hom;
         }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_ekf;
         }
 
     }
+    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.

GPU_version/vio/include/vio/map.h

@@ -22,6 +22,8 @@
 #include <boost/thread.hpp>
 #include <vio/global.h>
 #include <vio/point.h>
+#include <vio/frame.h>
+#include <vio/feature.h>
 
 namespace vio {
 
@@ -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(); }

GPU_version/vio/include/vio/matcher.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);

GPU_version/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';

GPU_version/vio/include/vio/point.h

@@ -46,17 +46,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.
   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 +64,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 +78,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,

GPU_version/vio/include/vio/pose_optimizer.h

@@ -18,6 +18,7 @@
 #define SVO_POSE_OPTIMIZER_H_
 
 #include <vio/global.h>
+#include <vio/map.h>
 
 namespace vio {
 
@@ -35,14 +36,13 @@ 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

GPU_version/vio/include/vio/reprojector.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,
@@ -97,10 +96,8 @@ 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

GPU_version/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_;

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

GPU_version/vio/param/vo_fast.yaml

@@ -1,16 +1,18 @@
-svo:
-  grid_size: 30            #Feature grid size of a cell in [px].
+vio:
+  grid_size: 8            #Feature grid size of a cell in [px].
   max_n_kfs: 20            #Limit the number of keyframes in the map. This makes nslam essentially. a Visual Odometry. Set to 0 if unlimited number of keyframes are allowed.  Minimum number of keyframes is 3.
-  loba_num_iter: 0         #Number of iterations in the local bundle adjustment.
+  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: 100
+  init_min_tracked: 120
   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
+  init_min_inliers: 40
+  ACC_ekf: 0.2          #acc white noise in continuous in EKF
+  GYO_ekf: 0.5         #gyro white noise in continuous in EKF
+  VO_ekf_translation: 1.0              #VO noise  in EKF
+  VO_ekf_orientation: 1.5              #VO noise  in EKF
+  CMD_ekf_translation: 0.1  #control vector noise in EKF
+  CMD_ekf_orientation: 0.01  #control vector noise in EKF

GPU_version/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;
-}
 

GPU_version/vio/src/config.cpp

@@ -20,47 +20,41 @@
 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))
+    trace_name(vk::getParam<string>("vio/trace_name", "svo")),
+    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", 5)),
+    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()

GPU_version/vio/src/feature_alignment.cpp

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

GPU_version/vio/src/feature_detection.cpp

@@ -70,62 +70,62 @@ 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);
   }
-  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

GPU_version/vio/src/frame.cpp

@@ -143,8 +143,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
@@ -172,26 +172,20 @@ void createImgPyramid(const cv::Mat& img_level_0, int n_levels, ImgPyr& pyr)
 bool getSceneDepth(const FramePtr frame, double& depth_mean, double& depth_min)
 {
   vector<double> depth_vec;
-  //depth_vec.reserve(frame.fts_.size());
-  depth_min = std::numeric_limits<double>::max();
   for(auto it=frame->fts_.begin(), ite=frame->fts_.end(); it!=ite; ++it)
   {
     if((*it)->point != NULL)
     {
-        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(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;
 }
 

GPU_version/vio/src/frame_handler_mono.cpp

@@ -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;
   }
@@ -166,7 +132,6 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
     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();
   ROS_INFO("VIO initialized :)");
   ROS_INFO("Running ...");
@@ -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_);
+            Config::poseOptimThresh(),
+            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,11 +167,11 @@ 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;
@@ -230,14 +179,18 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
     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;
+  frame_utils::getSceneDepth(new_frame_, depth_mean, depth_min);
+  if(depth_mean>10.0)return RESULT_NO_KEYFRAME;
+
   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(),
@@ -257,7 +210,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   }
   // add keyframe to map
   map_.addKeyframe(new_frame_);
-  std::cerr<<"BA run\n";
+  map_.checkKeyFrames();
   ba_glob_->new_key_frame();
   return RESULT_IS_KEYFRAME;
 }
@@ -276,12 +229,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){

GPU_version/vio/src/global_optimizer.cpp

@@ -24,6 +24,7 @@
 #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>
@@ -73,7 +74,7 @@ namespace vio {
             while(map_.keyframes_.empty() || new_keyframe_ == false)
                 cond_.wait(lk);
 #if VIO_DEBUG
-            fprintf(log_,"[%s] BA loop \n",
+            fprintf(log_,"[%s] BA loop run \n",
                     vio::time_in_HH_MM_SS_MMM().c_str());
 #endif
             new_keyframe_=false;
@@ -96,9 +97,9 @@ namespace vio {
             // init g2o
             list<EdgeContainerSE3> edges;
             list< pair<FramePtr,std::shared_ptr<Feature>> > incorrect_edges;
+            ba_mux_.lock();
             // Go through all Keyframes
             size_t v_id = 0;
-            point_mut_.lock();
             for(list<FramePtr>::iterator it_kf = map_.keyframes_.begin();
                 it_kf != map_.keyframes_.end(); ++it_kf)
             {
@@ -108,15 +109,15 @@ namespace vio {
                 optimizer.addVertex(v_kf);
                 for(auto&& it_ftr:(*it_kf)->fts_)
                 {
+                    if(it_ftr->point==NULL)continue;
                     // for each keyframe add edges to all observed mapoints
                     std::shared_ptr<Point> mp = it_ftr->point;
-                    if(mp == NULL)
-                        continue;
+                    if(mp->pos_.hasNaN())continue;
                     g2o::VertexSBAPointXYZ* v_mp = mp->v_pt_;
                     if(v_mp == NULL)
                     {
                         // mappoint-vertex doesn't exist yet. create a new one:
-                        v_mp = createG2oPoint(mp->pos_, v_id++, false);
+                        v_mp = createG2oPoint(mp->pos_, v_id++, true);
                         mp->v_pt_ = v_mp;
                         optimizer.addVertex(v_mp);
                     }
@@ -128,6 +129,7 @@ namespace vio {
                         incorrect_edges.push_back(pair<FramePtr,std::shared_ptr<Feature>>(*it_kf, it_ftr));
                     else
                     {
+                        g2o::Edge_XYZ_VSC e1;
                         g2o::EdgeProjectXYZ2UV* e = createG2oEdgeSE3(v_kf, v_mp, vk::project2d(it_ftr->f),
                                                                          true,
                                                                          Config::poseOptimThresh()/(*it_kf)->cam_->errorMultiplier2()*Config::lobaRobustHuberWidth());
@@ -140,18 +142,35 @@ namespace vio {
                     }
                 }
             }
-
+            if(optimizer.vertices().size()<1)continue;
             // Optimization
-            double init_error=0.0, final_error=0.0;
-            if(Config::lobaNumIter() > 0)
-                runSparseBAOptimizer( Config::lobaNumIter(), init_error, final_error);
+            optimizer.initializeOptimization();
+            optimizer.computeActiveErrors();
 
+/*            g2o::StructureOnlySolver<3> structure_only_ba;
+            g2o::OptimizableGraph::VertexContainer points;
+            for (g2o::OptimizableGraph::VertexIDMap::const_iterator it = optimizer.vertices().begin(); it != optimizer.vertices().end(); ++it) {
+                g2o::OptimizableGraph::Vertex* v = static_cast<g2o::OptimizableGraph::Vertex*>(it->second);
+                if (v->dimension() == 3)
+                    points.push_back(v);
+            }
+
+            structure_only_ba.calc(points, 10);*/
+#if VIO_DEBUG
+            fprintf(log_,"[%s] init error: %f \n",
+                    vio::time_in_HH_MM_SS_MMM().c_str(),optimizer.activeChi2());
+#endif
+            if(optimizer.optimize(vio::Config::lobaNumIter())<1)continue;
+#if VIO_DEBUG
+            fprintf(log_,"[%s] end error: %f \n",
+                    vio::time_in_HH_MM_SS_MMM().c_str(),optimizer.activeChi2());
+#endif
             // Update Keyframe and MapPoint Positions
             for(list<FramePtr>::iterator it_kf = map_.keyframes_.begin();
-                it_kf != map_.keyframes_.end(); ++it_kf)
+                it_kf != 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)->T_f_w_ = SE2_5(SE3((*it_kf)->v_kf_->estimate().rotation(),
+                                        (*it_kf)->v_kf_->estimate().translation()));
                 (*it_kf)->v_kf_ = NULL;
                 for(Features::iterator it_ftr=(*it_kf)->fts_.begin(); it_ftr!=(*it_kf)->fts_.end(); ++it_ftr)
                 {
@@ -177,47 +196,24 @@ namespace vio {
                     map_.removePtFrameRef(it->frame, it->feature);
                 }
             }
-            point_mut_.unlock();
+            ba_mux_.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)
    {
+/*        if(frame->id_==0 || frame->id_==1){
+            g2o::VertexSE3Expmap* v= new g2o::VertexSE3Expmap();
+            v->setId(id);
+            v->setToOrigin();
+            v->setFixed(true);
+            v->setEstimate(g2o::SE3Quat(frame->se3().unit_quaternion(), frame->se3().translation()));
+        }*/
        g2o::VertexSE3Expmap* v= new g2o::VertexSE3Expmap();
        v->setId(id);
-       v->setFixed(fixed);
-       v->setEstimate(g2o::SE3Quat(frame->T_f_w_.se3().unit_quaternion(), frame->T_f_w_.se3().translation()));
+       v->setFixed(false);
+       v->setEstimate(g2o::SE3Quat(frame->se3().unit_quaternion(), frame->se3().translation()));
        return v;
    }
 
@@ -228,13 +224,14 @@ namespace vio {
    {
        g2o::VertexSBAPointXYZ* v =new g2o::VertexSBAPointXYZ();
        v->setId(id);
-       v->setFixed(fixed);
+       //v->setFixed(fixed);
+       v->setMarginalized(true);
        v->setEstimate(pos);
        return v;
 
    }
 
-   g2o::EdgeProjectXYZ2UV* BA_Glob::createG2oEdgeSE3( g2o::VertexSE3Expmap* v_frame,
+    g2o::EdgeProjectXYZ2UV* BA_Glob::createG2oEdgeSE3( g2o::VertexSE3Expmap* v_frame,
                      g2o::VertexSBAPointXYZ* v_point,
                      const Vector2d& f_up,
                      bool robust_kernel,
@@ -244,8 +241,7 @@ namespace vio {
        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->setMeasurement(f_up);
        e->information() = Eigen::Matrix2d::Identity();
        g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
        rk->setDelta(huber_width);

GPU_version/vio/src/initialization.cpp

@@ -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,
@@ -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();

GPU_version/vio/src/map.cpp

@@ -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_)){
@@ -76,14 +77,15 @@ 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->obs_.size()>1){
       for(auto&& ftr:pt->obs_){
           if(ftr->frame)ftr->frame->removeKeyPoint(ftr);
-          ftr->point.reset();
+          if(ftr->point!=nullptr)ftr->point.reset();
       }
   }else{
-      pt->obs_.back()->point.reset();
+      if(!pt->obs_.empty())if(pt->obs_.back()->point!=nullptr)pt->obs_.back()->point.reset();
   }
   pt->obs_.clear();
   // Delete mappoint
@@ -101,41 +103,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 +130,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 {

GPU_version/vio/src/matcher.cpp

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

GPU_version/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)
             outliers.push_back(j);
         else
         {
             inliers.push_back(j);
             tot_error += e1+e2;
         }
-    }
   }
   return tot_error;
 }

GPU_version/vio/src/point.cpp

@@ -27,28 +27,22 @@ int Point::point_counter_ = 0;
 Point::Point(const Vector3d& pos) :
   id_(point_counter_++),
   pos_(pos),
-  normal_set_(false),
   n_obs_(0),
   v_pt_(NULL),
-  last_published_ts_(0),
   last_frame_overlap_id_(-10),
   type_(TYPE_UNKNOWN),
   n_failed_reproj_(0),
-  n_succeeded_reproj_(0),
   last_structure_optim_(0)
 {}
 
 Point::Point(const Vector3d& pos, std::shared_ptr<Feature> ftr) :
   id_(point_counter_++),
   pos_(pos),
-  normal_set_(false),
   n_obs_(1),
   v_pt_(NULL),
-  last_published_ts_(0),
   last_frame_overlap_id_(-10),
   type_(TYPE_UNKNOWN),
   n_failed_reproj_(0),
-  n_succeeded_reproj_(0),
   last_structure_optim_(0)
 {
   obs_.push_front(ftr);
@@ -59,7 +53,6 @@ Point::~Point()
 
 void Point::addFrameRef(std::shared_ptr<Feature> ftr)
 {
-    boost::unique_lock<boost::mutex> lock(point_mut_);
   obs_.push_front(ftr);
   ++n_obs_;
 }
@@ -91,17 +84,6 @@ bool Point::deleteFrameRef(FramePtr frame)
   return false;
 }
 
-void Point::initNormal()
-{
-  assert(!obs_.empty());
-  const std::shared_ptr<Feature> ftr = obs_.back();
-  assert(ftr->frame != NULL);
-  normal_ = ftr->frame->se3().rotation_matrix().transpose()*(-ftr->f);
-  double x=pow(20/(pos_-Vector3d(ftr->frame->pos()(0),0.0,ftr->frame->pos()(1))).norm(),2);
-  normal_information_ = DiagonalMatrix<double,3,3>(x, 1.0, 1.0);
-  normal_set_ = true;
-}
-
 bool Point::getCloseViewObs(const Vector2d& framepos, std::shared_ptr<Feature>& ftr,int id) const
 {
     boost::unique_lock<boost::mutex> lock(point_mut_);
@@ -143,7 +125,6 @@ void Point::optimize(const size_t n_iter)
   double chi2 = 0.0;
   Matrix3d A;
   Vector3d b;
-
   for(size_t i=0; i<n_iter; i++)
   {
     A.setZero();
@@ -155,7 +136,6 @@ void Point::optimize(const size_t n_iter)
     {
       Matrix23d J;
       const Vector3d p_in_f((*it)->frame->se3()*pos_);
-      //Point::jacobian_xyz2uv(p_in_f, (*it)->frame->se3().rotation_matrix(), J);
       Point::jacobian_xyz2uv_(p_in_f, (*it)->frame->se3().rotation_matrix(), J, (*it)->frame->cam_->params(), (*it)->frame->T_f_w_);
       const Vector2d e(vk::project2d((*it)->f) - vk::project2d(p_in_f));
       new_chi2 += e.squaredNorm();
@@ -183,17 +163,13 @@ void Point::optimize(const size_t n_iter)
     if(vk::norm_max(dp) <= EPS)
       break;
   }
+  for(auto it=obs_.begin(); it!=obs_.end(); ++it) {
+            Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d((*it)->frame->se3() * pos_));
+            e /= (1<<(*it)->level);
+            if (e.norm() > 3.0/ (*it)->frame->cam_->errorMultiplier2())
+                n_failed_reproj_++;
+  }
 
-/*    for(auto it=obs_.begin(); it!=obs_.end(); ++it) {
-            Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d((*it)->frame->se3() * (*it)->point->pos_));
-            //e /= (1 << (*it)->level);
-//            std::cerr<<"error - point.h : "<<e.norm()<<std::endl;
-            if (e.norm() > 1.0) //{
-                // we don't need to delete a reference in the point since it was not created yet
-                (*it)->point->n_failed_reproj_++;
-//            } else
-//                it++;
-    }*/
 
 }
 

GPU_version/vio/src/pose_optimizer.cpp

@@ -27,14 +27,13 @@ namespace vio {
 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)
 {
   // init
@@ -54,12 +53,11 @@ void optimizeGaussNewton(
     //Reprojection error
     Vector2d e = vk::project2d((*it)->f)
                - vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));
-    e *= 1.0 / (1<<(*it)->level);
-    //std::cerr<<"pose optimizer - error : "<<e.norm()<<std::endl;
-    if(e.norm() > 0.01){
+    if(std::isnan(e.norm())){
+        map.safeDeletePoint((*it)->point);
         it = frame->fts_.erase(it);
-        continue;
     }
+    e *= 1.0 / (1<<(*it)->level);
     chi2_vec_init.push_back(e.norm()); // just for debug
     errors.push_back(e.norm());
     it++;
@@ -69,9 +67,6 @@ void optimizeGaussNewton(
     return;
   vk::robust_cost::MADScaleEstimator scale_estimator;
   estimated_scale = scale_estimator.compute(errors);
-//#if VIO_DEBUG
-//    fprintf(log,"[%s] Init estimate scale:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),estimated_scale);
-//#endif
 
   num_obs = errors.size();
   double scale = estimated_scale;
@@ -122,37 +117,27 @@ void optimizeGaussNewton(
     if(vk::norm_max(dT) <= EPS)
       break;
   }
-
+  //point_mut_.lock();
   size_t n_deleted_refs = 0;
-  //for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); /*++it*/)
   for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); /*++it*/)
   {
-    //std::cerr<<"123 - pose optimizer - frame->fts_.size() : "<<frame->fts_.size()<<std::endl;
-    //std::cerr<<"124 - pose optimizer - (*it)->point : "<<(*it)->point<<std::endl;
     if((*it)->point == NULL) {
         it++;
-        //std::cerr<<"126 - pose optimizer - (*it)->point == NULL : "<<std::endl;
         continue;
     }
-    //std::cerr<<"129 - pose optimizer "<<(*it)->f.x()<<" "<<(*it)->f.y()<<" "<<(*it)->f.z()<<"\t"<<(*it)->point->pos_.x()<<" "<<(*it)->point->pos_.y()<<" "<<(*it)->point->pos_.z()<<std::endl;
     Vector2d e = vk::project2d((*it)->f) - vk::project2d(Vector3d(frame->se3()*(*it)->point->pos_));
-    //std::cerr<<"131 - pose optimizer"<<std::endl;
     e /= (1<<(*it)->level);
-    //std::cerr<<"133 - pose optimizer"<<std::endl;
     chi2_vec_final.push_back(e.norm());
-    //std::cerr<<"135 - pose optimizer"<<std::endl;
-    if(e.norm() >  2.0 / frame->cam_->errorMultiplier2())
+    if(e.norm() >  1.0 / frame->cam_->errorMultiplier2())
     {
-      // we don't need to delete a reference in the point since it was not created yet
-      (*it)->point.reset();
-      //std::cerr<<"befor erase 140 pose optimizer"<<std::endl;
+      map.safeDeletePoint((*it)->point);
       it = frame->fts_.erase(it);
-      //std::cerr<<"after erase 142 pose optimizer"<<std::endl;
       ++n_deleted_refs;
     }
     else
         it++;
   }
+  //point_mut_.unlock();
   num_obs -= n_deleted_refs;
 #if VIO_DEBUG
     error_init=0.0;

GPU_version/vio/src/reprojector.cpp

@@ -29,7 +29,7 @@
 #include <vio/timer.h>
 #include <fstream>
 #include <vio/feature_detection.h>
-
+#include <vio/sparse_img_align_gpu.h>
 #include <opencv2/xfeatures2d.hpp>
 #include <opencv2/features2d.hpp>
 #include <vio/for_it.hpp>
@@ -65,7 +65,7 @@ namespace vio {
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 // TODO - second reimplementation
-    void Reprojector::reprojectMap2(
+    void Reprojector::reprojectMap(
             FramePtr frame,
             FramePtr last_frame,
             std::vector<std::pair<FramePtr, std::size_t> > &overlap_kfs,
@@ -75,43 +75,27 @@ namespace vio {
         resetGrid();
         Features keypoints;
         cv::Ptr<cv::BFMatcher> matcher = cv::BFMatcher::create(cv::NORM_HAMMING2,false);
-        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(), keypoints);
+        detector->detect(frame, frame->img_pyr_, Config::triangMinCornerScore(), keypoints);
         std::vector<cv::KeyPoint> keypoints_cur;
         list<std::shared_ptr<Feature>>::iterator it_cur=keypoints.begin();
         cv::Mat cur_des=cv::Mat(keypoints.size(),64,CV_8UC1);
-//        std::cerr<<"84"<<std::endl;
-        //cv::Mat maskup = cv::Mat(keypoints.size(),64,CV_8UC1);
-        //cv::Mat maskdown = cv::Mat(keypoints.size(),64,CV_8UC1);
         cv::Mat maskup = cv::Mat_<uchar>(1, keypoints.size());
         cv::Mat maskdown = cv::Mat_<uchar>(1, keypoints.size());
-//        std::cerr<<"88"<<std::endl;
-////        uint8_t one[64] = {255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
-////                           255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
-////                           255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,
-////                           255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255};
-////        uint8_t zero[64] = {0};
-//        std::cerr<<"94"<<std::endl;
-
         for (int i=0;i<keypoints.size() && it_cur !=keypoints.end();++i) {
             keypoints_cur.push_back(cv::KeyPoint((*it_cur)->px.x(), (*it_cur)->px.y(), 7.f, (*it_cur)->score));
             memcpy(cur_des.data+(i*64),(*it_cur)->descriptor,sizeof(uint8_t)*64);
             if ((*it_cur)->px.y() < frame->img().rows/2) {
                 maskup.at<uchar>(0, i) = 1;
                 maskdown.at<uchar>(0, i) = 0;
-                //memcpy(maskup.data + (i * 64), one, sizeof(uint8_t) * 64);
-                //memcpy(maskdown.data + (i * 64), zero, sizeof(uint8_t) * 64);
             }
             else{
                 maskup.at<uchar>(0, i) = 0;
                 maskdown.at<uchar>(0, i) = 1;
-                //memcpy(maskup.data + (i * 64), zero, sizeof(uint8_t) * 64);
-                //memcpy(maskdown.data + (i * 64), one, sizeof(uint8_t) * 64);
             }
             ++it_cur;
         }
-//        std::cerr<<"114"<<std::endl;
         list<pair<FramePtr, double> > close_kfs;
         map_.getCloseKeyframes(frame, close_kfs);
         if (!last_frame->fts_.empty())
@@ -119,8 +103,13 @@ namespace vio {
                                                                     last_frame->T_f_w_.se2().translation()).norm()));
         close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <
                        boost::bind(&std::pair<FramePtr, double>::second, _2));
+        std::unique_ptr<SparseImgAlignGpu> img_align=std::make_unique<SparseImgAlignGpu>(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_);
+        for (auto &&it_frame:_for(close_kfs)) {
+            img_align->run(it_frame.item.first, frame, log_);
+        }
         overlap_kfs.reserve(options_.max_n_kfs);
         std::vector<int> added_keypoints;
+        int points_count=0;
         for (auto &&it_frame:_for(close_kfs)) {
             if (it_frame.index > options_.max_n_kfs)continue;
             overlap_kfs.push_back(pair<FramePtr, size_t>(it_frame.item.first, 0));
@@ -129,34 +118,34 @@ namespace vio {
                 std::vector<cv::KeyPoint> keypoints_kfs;
                 std::vector<std::vector<cv::DMatch>>  matches;
                 keypoints_kfs.push_back(cv::KeyPoint((*it_ref)->px.x(), (*it_ref)->px.y(), 7.f,(*it_ref)->score));
-
                 cv::Mat ref_des=cv::Mat(1,64,CV_8UC1,(*it_ref)->descriptor);
-
                 if ((*it_ref)->px.y() < frame->img().rows/2) {
                     matcher->knnMatch(ref_des, cur_des, matches, 1, cv::InputArray(maskup));
                 }
                 else{
                     matcher->knnMatch(ref_des, cur_des, matches, 1, cv::InputArray(maskdown));
                 }
-                //matcher->knnMatch(ref_des, cur_des, matches, 1);
-
                 for(auto&& match:matches.back()){
                     it_cur=keypoints.begin();
                     std::advance(it_cur,match.trainIdx);
                     if(!(*it_cur))continue;
+                    Vector2d px((int) (*it_cur)->px.x(),
+                                (int) (*it_cur)->px.y());
+                    const int k = static_cast<int>((*it_cur)->px.y() / grid_.cell_size) *
+                                  grid_.grid_n_cols
+                                  + static_cast<int>((*it_cur)->px.x() / grid_.cell_size);
+                    if(grid_.cells.at(k)->size()> Config::gridSize()-1)continue;
                     if ((*it_ref)->point == NULL){
-                        const int k = static_cast<int>((*it_cur)->px.y() / grid_.cell_size) *
-                                      grid_.grid_n_cols
-                                      + static_cast<int>((*it_cur)->px.x() / grid_.cell_size);
-                        if(k>grid_.cells.size()-1)continue;
-                        assert(grid_.cells.at(k) != nullptr);
-                        Vector2d px((int) (*it_cur)->px.x(),
-                                    (int) (*it_cur)->px.y());
                         SE3 T_ref_cur=it_frame.item.first->se3().inverse()*frame->se3();
+                        // pose with respect to reference frame
                         Vector3d pos=vk::triangulateFeatureNonLin(T_ref_cur.rotation_matrix(),T_ref_cur.translation(),
-                                                                        frame->c2f(px),(*it_ref)->f);
-                        if(pos.z()<0.0)continue;
+                                                                  (*it_ref)->f,frame->c2f(px));
+                        if(pos.norm()==0.0)continue;
+                        // point in world frame
                         std::shared_ptr<Point> new_point=std::make_shared<Point>(it_frame.item.first->se3()*pos,(*it_ref));
+
+                        if(!matcher_.findMatchDirect(*new_point, *frame, px))continue;
+
                         frame->addFeature(std::make_shared<Feature>(frame,
                                                                     new_point,
                                                                     px,(*it_cur)->level,(*it_cur)->score,(*it_cur)->descriptor));
@@ -165,86 +154,26 @@ namespace vio {
                         frame->fts_.back()->point->last_frame_overlap_id_=it_frame.item.first->id_;
                         grid_.cells.at(k)->push_back(Candidate(frame->fts_.back()->point, px));
                         overlap_kfs.back().second++;
+                        ++points_count;
                     }else{
+                        if(!matcher_.findMatchDirect(*(*it_ref)->point, *frame, px))continue;
+
                         (*it_ref)->point->last_frame_overlap_id_ = frame->id_;
-                        const int k = static_cast<int>((*it_cur)->px.y()/ grid_.cell_size) *
-                                      grid_.grid_n_cols
-                                      + static_cast<int>((*it_cur)->px.x() / grid_.cell_size);
-                        if(k>grid_.cells.size()-1)continue;
-                        assert(grid_.cells.at(k) != nullptr);
-                        Vector2d px((int) (*it_cur)->px.x(),
-                                    (int) (*it_cur)->px.y());
                         frame->addFeature(std::make_shared<Feature>(frame,
                                                                     (*it_ref)->point,
                                                                     px,(*it_cur)->level,(*it_cur)->score,(*it_cur)->descriptor));
                         added_keypoints.push_back(match.trainIdx);
-                        grid_.cells.at(k)->push_back(Candidate((*it_ref)->point, px));
+                        grid_.cells.at(k)->push_back(Candidate(frame->fts_.back()->point, px));
                         overlap_kfs.back().second++;
+                        ++points_count;
                     }
                 }
-/*                cv::Mat imgMatch,key_point_ref,key_point_cur;
-                cv::drawMatches( it_frame.item.first->img_pyr_[0], keypoints_kfs, frame->img_pyr_[0], keypoints_cur,matches, imgMatch);
-                cv::imshow("img",imgMatch);
-                cv::waitKey();*/
                 ++it_ref;
             }
+            if(points_count>300)return;
         }
-        //exit(0);
-        {
-            for (auto&& cell : grid_.cells) {
-                if (reprojectCell(*cell, frame, log_))
-                    ++n_matches_;
-                if (n_matches_ > (size_t) Config::maxFts())
-                    break;
-            }
-        }
-    }
-
-////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-    bool Reprojector::pointQualityComparator(Candidate &lhs, Candidate &rhs) {
-        if (lhs.pt->type_ > rhs.pt->type_)
-            return true;
-        return false;
-    }
-
-    bool Reprojector::reprojectCell(Cell &cell, FramePtr frame, FILE* log_) {
-        if(cell.size()<1){
-            return false;
-        }
-        cell.sort(boost::bind(&Reprojector::pointQualityComparator, _1, _2));
-        Cell::iterator it=cell.begin();
-        while(it!=cell.end())
-        {
-            ++n_trials_;
-            if( it->pt ==NULL){
-                it = cell.erase(it);
-                continue;
-            }
-            if(it->pt->type_ == Point::TYPE_DELETED)
-            {
-                it = cell.erase(it);
-                continue;
-            }
-            bool res = matcher_.findMatchDirect(*it->pt, *frame, it->px);
-            if(!res)
-            {
-                it->pt->n_failed_reproj_++;
-                if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_failed_reproj_ > 15)it->pt.reset();
-                it = cell.erase(it);
-                continue;
-            }
-
-            it->pt->n_succeeded_reproj_++;
-            if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_succeeded_reproj_ > 10)
-                it->pt->type_ = Point::TYPE_GOOD;
-
-            cell.erase(it);
-
-            // Maximum one point per cell.
-            return true;
-        }
-
-        return false;
+/*        std::cerr<<"here\n";
+        exit(0);*/
     }
 
 }

GPU_version/vio/src/sparse_img_align_gpu.cpp

@@ -15,6 +15,7 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 #include <vio/sparse_img_align_gpu.h>
+#include <vio/for_it.hpp>
 
 namespace vio {
 
@@ -35,49 +36,69 @@ SparseImgAlignGpu::SparseImgAlignGpu(
 size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame, FILE* log)
 {
   reset();
-  errors.clear();
   cl_float3 ref_pos[1]={(float)ref_frame->pos()(0),(float)ref_frame->pos()(1),(float)ref_frame->T_f_w_.pitch()};
   cl_float3 cur_pos[1]={(float)cur_frame->pos()(0),(float)cur_frame->pos()(1),(float)cur_frame->T_f_w_.pitch()};
-  errors.push_back(residual_->reload_buf(1,2,cur_pos));
-  errors.push_back(residual_->reload_buf(1,3,ref_pos));
-  cl_float3 features[ref_frame->fts_.size()];
-  cl_float2 featue_px[ref_frame->fts_.size()];
+  cl_float3* features=(cl_float3*)calloc(ref_frame->fts_.size(), sizeof(cl_float3));
+  cl_float2* featue_px=(cl_float2*)calloc(ref_frame->fts_.size(), sizeof(cl_float2));
   feature_counter_ = 0; // is used to compute the index of the cached jacobian
-  for(auto it=ref_frame->fts_.begin(); it!=ref_frame->fts_.end();++it){
-      if(*it == nullptr)continue;
-      if((*it)->point == nullptr)continue;
-        Vector3d xyz_ref=(*it)->f*((*it)->point->pos_ - Eigen::Vector3d(ref_pos[0].x,0.0,ref_pos[0].y)).norm();
+  for(auto it=ref_frame->fts_.begin();it!=ref_frame->fts_.end();++it){
+        if((*it)->point == nullptr)continue;
+        Vector3d xyz_ref=(*it)->f*(ref_frame->se3().inverse()*(*it)->point->pos_).norm();
         features[feature_counter_].x=xyz_ref(0);
         features[feature_counter_].y=xyz_ref(1);
         features[feature_counter_].z=xyz_ref(2);
         featue_px[feature_counter_].x=(*it)->px(0);
         featue_px[feature_counter_].y=(*it)->px(1);
-      if(feature_counter_>299)break;
         ++feature_counter_;
   }
   if(!feature_counter_) // more than 10
   {
+      free(featue_px);
+      free(features);
       return 0;
   }
-  errors.push_back(residual_->reload_buf(1,4,features));
-  errors.push_back(residual_->reload_buf(1,5,featue_px));
+  residual_->load(1,2,1,cur_pos);
+  residual_->load(1,3,1,ref_pos);
+  residual_->load(1,4,feature_counter_,features);
+  residual_->load(1,5,feature_counter_,featue_px);
+  cl_float* error_=(cl_float*)calloc(feature_counter_, sizeof(cl_float));
+  cl_float* H=(cl_float*)calloc(9*feature_counter_, sizeof(cl_float));
+  cl_float3* J=(cl_float3*)calloc(feature_counter_, sizeof(cl_float3));
+  cl_float* chi2_=(cl_float*)calloc(feature_counter_, sizeof(cl_float));
+  residual_->load(1,7,feature_counter_,error_);
+  residual_->load(1,8,9*feature_counter_,H);
+  residual_->load(1,9,feature_counter_,J);
+  residual_->load(1,10,feature_counter_,chi2_);
   SE2 T_cur(cur_frame->T_f_w_.se2());///TODO temporary, we can remove it
   for(level_=max_level_; level_>=min_level_; --level_)
   {
       cv::Mat cur_img = cur_frame->img_pyr_.at(level_);
       cv::Mat ref_img = ref_frame->img_pyr_.at(level_);
-      errors.push_back(residual_->reload_buf(1,0,cur_img));
-      errors.push_back(residual_->reload_buf(1,1,ref_img));
-      errors.push_back(residual_->write_buf(1,6,level_));
+      residual_->load(1,0,cur_img);
+      residual_->load(1,1,ref_img);
+      residual_->load(1,6,level_);
       mu_ = 1.0;
-      for(auto&& e:errors)if(!e)return -1;
-      errors.clear();
       optimize(T_cur);
+      residual_->release(1,0);
+      residual_->release(1,1);
   }
-  for(auto&& e:errors)if(!e)return -1;
   cl_float3 pos[1]={0};
   residual_->read(1,2,1,pos);
-  if(isnan(pos[0].x) || isnan(pos[0].y) || isnan(pos[0].z))return 0;
+  residual_->release(1,2);
+  residual_->release(1,3);
+  residual_->release(1,4);
+  residual_->release(1,5);
+  residual_->release(1,7);
+  residual_->release(1,8);
+  residual_->release(1,9);
+  residual_->release(1,10);
+  free(error_);
+  free(H);
+  free(J);
+  free(chi2_);
+  free(featue_px);
+  free(features);
+  if(isnan(pos[0].x) || isnan(pos[0].y) || isnan(pos[0].z) || fabs(pos[0].z-cur_pos[0].z)>M_PI_2)return 1;
   cur_frame->T_f_w_ = SE2_5(pos[0].x,pos[0].y,pos[0].z);
   return 1;
 }
@@ -86,17 +107,19 @@ double SparseImgAlignGpu::computeResiduals(
     bool linearize_system,
     bool compute_weight_scale)
 {
-    cl_float H[9*300]={0};
-    cl_float3 J[300]={0.0};
-    cl_float chi2_[300]={0.0};
+    cl_float* error_=(cl_float*)calloc(feature_counter_, sizeof(cl_float));
+    cl_float* H=(cl_float*)calloc(9*feature_counter_, sizeof(cl_float));
+    cl_float3* J=(cl_float3*)calloc(feature_counter_, sizeof(cl_float3));
+    cl_float* chi2_=(cl_float*)calloc(feature_counter_, sizeof(cl_float));
     cl_float scale=(float)scale_;
-    errors.push_back(residual_->write_buf(1,11,scale));
-    errors.push_back(residual_->reload_buf(1,10,chi2_));
-    errors.push_back(residual_->reload_buf(1,8,H));
-    errors.push_back(residual_->reload_buf(1,9,J));
+    residual_->reload(1,7,feature_counter_,error_);
+    residual_->reload(1,8,9*feature_counter_,H);
+    residual_->reload(1,9,feature_counter_,J);
+    residual_->reload(1,10,feature_counter_,chi2_);
+    residual_->load(1,11,scale);
     residual_->run(1,feature_counter_);
-    cl_float error[300]={0.0};
-    cl_float chi[300]={0.0};
+    cl_float* error=(cl_float*)calloc(feature_counter_, sizeof(cl_float));
+    cl_float* chi=(cl_float*)calloc(feature_counter_, sizeof(cl_float));
     residual_->read(1,10,feature_counter_,chi);
     residual_->read(1,7,feature_counter_,error);
     for(int i = 1; i < feature_counter_; ++i){
@@ -104,13 +127,20 @@ double SparseImgAlignGpu::computeResiduals(
         chi[0] += chi[i];
     }
     scale_ = error[0]*(1.48f / feature_counter_);
-    return chi[0]/(feature_counter_*8);
+    double out=chi[0]/(feature_counter_*8);
+    free(error);
+    free(error_);
+    free(chi);
+    free(H);
+    free(J);
+    free(chi2_);
+    return out;
 }
 
 bool SparseImgAlignGpu::solve()
 {
-    cl_float H[300*9]={0.0};
-    cl_float3 J[300]={0.0};
+    cl_float* H=(cl_float*)calloc(9*feature_counter_, sizeof(cl_float));
+    cl_float3* J=(cl_float3*)calloc(feature_counter_, sizeof(cl_float3));
     residual_->read(1,8,feature_counter_*9,H);
     residual_->read(1,9,feature_counter_,J);
     for(int i = 1; i < feature_counter_; ++i){
@@ -133,6 +163,8 @@ bool SparseImgAlignGpu::solve()
     x_ = Eigen::Matrix<double,3,3>(Hd).ldlt().solve(Eigen::Vector3d((double)J[0].x,(double)J[0].y,(double)J[0].z));
     double norm=x_.norm();
     if(norm<=0 ||norm > 1.0)x_=Eigen::Vector3d(0.1,0.1,0.1);
+    free(H);
+    free(J);
     return true;
 }
 void SparseImgAlignGpu::update()
@@ -143,7 +175,7 @@ void SparseImgAlignGpu::update()
     pos[0].x=(float)update.translation()(0);
     pos[0].y=(float)update.translation()(1);
     pos[0].z=(float)atan2(update.so2().unit_complex().imag(),update.so2().unit_complex().real());
-    errors.push_back(residual_->reload_buf(1,2,pos));
+    residual_->reload(1,2,1,pos);
 }
 
 void SparseImgAlignGpu::startIteration()

GPU_version/vio/src/visualizer.cpp

@@ -55,7 +55,7 @@ void Visualizer::publishMinimal(
     //Rviz frame z up, x  right, y forward -> right hands (yaw counts from y)
     //Camera frame z front, x right, y down -> right hands (pitch counts from z)
     //IMU frame y front, x right, z up -> left hands (theta counts from y)
-    Quaterniond q(AngleAxisd(-1.0*(odom.second.pitch()-M_PI_2), Vector3d::UnitZ()));
+    Quaterniond q(AngleAxisd((odom.second.pitch()+M_PI_2), Vector3d::UnitZ()));
     geometry_msgs::PoseWithCovarianceStampedPtr msg_pose_with_cov(new geometry_msgs::PoseWithCovarianceStamped);
       msg_pose_with_cov->header = header_msg;
       msg_pose_with_cov->pose.pose.position.x = odom.second.se2().translation()(0);

GPU_version/vio/src/vo_node.cpp

@@ -116,13 +116,7 @@ VioNode::~VioNode()
 
 void VioNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
 {
-/*    ++cam_syn_;
-    if(vo_->ukfPtr_.cam_syn_[cam_syn_ % 3]){
-        vo_->ukfPtr_.cam_syn_[cam_syn_ % 3]=false;
-    }else{
-        return;
-    }
-    if(cam_syn_>50)cam_syn_=0;*/
+
     if(!start_)return;
       try {
           cv::Mat img=cv_bridge::toCvShare(msg, "mono8")->image;
@@ -150,9 +144,12 @@ void VioNode::imgCb(const sensor_msgs::ImageConstPtr& msg)
 void VioNode::imuCb(const sensor_msgs::ImuPtr &imu) {
     if(!start_)return;
     double imu_in[3];
-    imu_in[0] = imu->linear_acceleration.x;
-    imu_in[1] = imu->linear_acceleration.y;
-    imu_in[2] = imu->angular_velocity.z;
+    imu_in[0] = 0.2*imu_[0]+0.8*imu->linear_acceleration.x;
+    imu_in[1] = 0.2*imu_[1]+0.8*imu->linear_acceleration.y;
+    imu_in[2] = 0.2*imu_[2]+0.8*imu->angular_velocity.z;
+    if(fabs(imu_in[2])>0.4)return;
+    if(fabs(imu_in[1])>1.0)return;
+    if(fabs(imu_in[0])>1.0)return;
     memcpy(imu_, imu_in, static_cast<std::size_t>(3*sizeof(double)));
     vo_->UpdateIMU(imu_in,imu->header.stamp);
     imu_time_=imu->header.stamp;