add kernel image alingment

GPU_version/vio/gtsam/imu_integration.cpp

@@ -5,14 +5,14 @@
 #include "imu_integration.h"
 #include <memory>
 #include <utility>
-#include "gpu_svo/feature.h"
-#include "gpu_svo/point.h"
-#include "gpu_svo/svoVisionFactor.h"
+#include "vio/feature.h"
+#include "vio/point.h"
+#include "vio/svoVisionFactor.h"
 #include <gtsam/nonlinear/Marginals.h>
 #include <gtsam/geometry/Pose3.h>
-#include <gpu_svo/params_helper.h>
-#include <gpu_svo/config.h>
-#include <gpu_svo/for_it.hpp>
+#include <vio/params_helper.h>
+#include <vio/config.h>
+#include <vio/for_it.hpp>
 #define debug 0
 Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
     graphPtr=std::make_shared<gtsam::NonlinearFactorGraph>();

GPU_version/vio/gtsam/imu_integration.h

@@ -13,7 +13,7 @@
 #include <gtsam/slam/ProjectionFactor.h>
 #include <gtsam/nonlinear/ISAM2.h>
 #include <sophus/se3.h>
-#include <gpu_svo/frame.h>
+#include <vio/frame.h>
 
 using gtsam::symbol_shorthand::B;  // Bias  (ax,ay,az,gx,gy,gz)
 using gtsam::symbol_shorthand::V;  // Vel   (xdot,ydot,zdot)

GPU_version/vio/gtsam/svoVisionFactor.h

@@ -10,11 +10,11 @@
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/Vector.h>
 #include <gtsam/geometry/Pose3.h>
-#include <gpu_svo/robust_cost.h>
-#include <gpu_svo/math_utils.h>
-#include <gpu_svo/frame.h>
+#include <vio/robust_cost.h>
+#include <vio/math_utils.h>
+#include <vio/frame.h>
 
-namespace svo {
+namespace vio {
 
     class VisionFactor: public gtsam::NoiseModelFactor1<gtsam::Pose3> {
 

GPU_version/vio/gtsam/svoVisionFactorLandMark.h

@@ -12,9 +12,9 @@
 #include <gtsam/geometry/Pose3.h>
 #include <vikit/robust_cost.h>
 #include <vikit/math_utils.h>
-#include <gpu_svo/frame.h>
+#include <vio/frame.h>
 
-namespace svo {
+namespace vio {
 
     class VisionFactorLandMark: public gtsam::NoiseModelFactor3<gtsam::Pose3,gtsam::Pose3,gtsam::Point3> {
 

GPU_version/vio/gtsam/svoVisionFactorSmall.h

@@ -12,9 +12,9 @@
 #include <gtsam/geometry/Pose3.h>
 #include <vikit/robust_cost.h>
 #include <vikit/math_utils.h>
-#include <gpu_svo/frame.h>
+#include <vio/frame.h>
 
-namespace svo {
+namespace vio {
 
     class VisionFactor: public gtsam::NoiseModelFactor1<gtsam::Pose3> {
 

GPU_version/vio/include/vio/abstract_camera.h

@@ -0,0 +1,78 @@
+/*
+ * abstract_camera.h
+ *
+ *  Created on: Jul 23, 2012
+ *      Author: cforster
+ */
+
+#ifndef ABSTRACT_CAMERA_H_
+#define ABSTRACT_CAMERA_H_
+
+#include <Eigen/Core>
+
+namespace vk
+{
+
+using namespace std;
+using namespace Eigen;
+
+class AbstractCamera
+{
+protected:
+
+  int width_;   // TODO cannot be const because of omni-camera model
+  int height_;
+
+public:
+
+  AbstractCamera() {}; // need this constructor for omni camera
+  AbstractCamera(int width, int height) : width_(width), height_(height) {};
+
+  virtual ~AbstractCamera() {};
+
+  /// Project from pixels to world coordiantes. Returns a bearing vector of unit length.
+  virtual Vector3d
+  cam2world(const double& x, const double& y) const = 0;
+  virtual double* params() const =0;
+
+  /// Project from pixels to world coordiantes. Returns a bearing vector of unit length.
+  virtual Vector3d
+  cam2world(const Vector2d& px) const = 0;
+
+  virtual Vector2d
+  world2cam(const Vector3d& xyz_c) const = 0;
+
+  /// projects unit plane coordinates to camera coordinates
+  virtual Vector2d
+  world2cam(const Vector2d& uv) const = 0;
+
+  virtual double
+  errorMultiplier2() const = 0;
+
+  virtual double
+  errorMultiplier() const = 0;
+
+  inline int width() const { return width_; }
+
+  inline int height() const { return height_; }
+
+  inline bool isInFrame(const Vector2i & obs, int boundary=0) const
+  {
+    if(obs[0]>=boundary && obs[0]<width()-boundary
+        && obs[1]>=boundary && obs[1]<height()-boundary)
+      return true;
+    return false;
+  }
+
+  inline bool isInFrame(const Vector2i &obs, int boundary, int level) const
+  {
+    if(obs[0] >= boundary && obs[0] < width()/(1<<level)-boundary
+        && obs[1] >= boundary && obs[1] <height()/(1<<level)-boundary)
+      return true;
+    return false;
+  }
+};
+
+} // end namespace CSfM
+
+#endif /* ABSTRACT_CAMERA_H_ */

GPU_version/vio/include/vio/aligned_mem.h

@@ -0,0 +1,127 @@
+/*
+ * aligned_mem.h
+ *
+ *  Created on: May 14, 2013
+ *      Author: cforster
+ *
+ *  Code from libcvd: cvd/internal/aligned_mem.h
+ *  https://github.com/edrosten/libcvd
+ *  Licence: LGPL 2.1
+ */
+
+#ifndef VIKIT_ALIGNED_MEM_H_
+#define VIKIT_ALIGNED_MEM_H_
+
+#include <string>       // memset
+#include <string.h>     // memset
+#include <cassert>
+#include <cstdlib>
+
+namespace vk {
+namespace aligned_mem {
+
+  /// Check if the pointer is aligned to the specified byte granularity
+  inline bool
+  is_aligned8(const void* ptr)
+  {
+    return ((reinterpret_cast<size_t>(ptr)) & 0x7) == 0;
+  }
+
+  inline bool
+  is_aligned16(const void* ptr)
+  {
+    return ((reinterpret_cast<size_t>(ptr)) & 0xF) == 0;
+  }
+
+  template <class T, int N=20> struct placement_delete
+  {
+    enum { Size = (1<<N) };
+
+    struct Array {
+      T data[Size];
+    };
+
+    static inline void destruct(T* buf)
+    {
+      (*(Array*)buf).~Array();
+    }
+
+    static inline void free(T* buf, size_t M)
+    {
+      if (M >= Size) {
+        placement_delete<T,N>::free(buf+Size,M-Size);
+        placement_delete<T,N>::destruct(buf);
+      } else {
+        placement_delete<T,N-1>::free(buf, M);
+      }
+    }
+  };
+
+  template <class T> struct placement_delete<T,-1>
+  {
+    static inline void free(T*, size_t ) {}
+  };
+
+  inline void * aligned_alloc(size_t count, size_t alignment){
+    void * mem = NULL;
+    assert(posix_memalign(&mem, alignment, count) == 0);
+    return mem;
+  }
+
+  inline void aligned_free(void * memory) {
+    free(memory);
+  }
+
+  template <class T>
+  inline T * aligned_alloc(size_t count, size_t alignment){
+    void * data = aligned_alloc(sizeof(T)* count, alignment);
+    return new (data) T[count];
+  }
+
+  template <class T>
+  inline void aligned_free(T * memory, size_t count){
+    placement_delete<T>::free(memory, count);
+    aligned_free(memory);
+  }
+
+  template<class T> inline void memfill(T* data, int n, const T val)
+  {
+    T* de = data + n;
+    for(;data < de; data++)
+      *data=val;
+  }
+
+  template<> inline void memfill(unsigned char* data, int n, const unsigned char val)
+  {
+    memset(data, val, n);
+  }
+
+  template<> inline void memfill(signed char* data, int n, const signed char val)
+  {
+    memset(data, val, n);
+  }
+
+  template<> inline void memfill(char* data, int n, const char val)
+  {
+    memset(data, val, n);
+  }
+
+  template <class T, int N>
+  struct AlignedMem {
+    T* mem;
+    size_t count;
+    AlignedMem(size_t c) : count(c) {
+      mem = aligned_alloc<T>(count, N);
+    }
+    ~AlignedMem() {
+      aligned_free<T>(mem, count);
+    }
+    T* data() { return mem; }
+    const T* data() const { return mem; }
+  };
+
+} // namespace aligned_mem
+} // namespace vikit
+
+
+#endif // VIKIT_ALIGNED_MEM_H_

GPU_version/vio/include/vio/atan_camera.h

@@ -0,0 +1,100 @@
+/*
+ * atan_camera.h
+ *
+ *  Created on: Aug 21, 2012
+ *      Author: cforster
+ *
+ *  This class implements the FOV distortion model of Deverneay and Faugeras,
+ *  Straight lines have to be straight, 2001.
+ *
+ *  The code is an implementation of the ATAN class in PTAM by Georg Klein using Eigen.
+ */
+
+#ifndef ATAN_CAMERA_H_
+#define ATAN_CAMERA_H_
+
+#include <stdlib.h>
+#include <string>
+#include <Eigen/Eigen>
+#include <vio/abstract_camera.h>
+#include <vio/math_utils.h>
+
+namespace vk {
+
+using namespace std;
+using namespace Eigen;
+
+class ATANCamera : public AbstractCamera {
+
+private:
+  double fx_, fy_;                      //!< focal length
+  double fx_inv_, fy_inv_;              //!< inverse focal length
+  double cx_, cy_;                      //!< projection center
+  double s_, s_inv_;                    //!< distortion model coeff
+  double tans_;                         //!< distortion model coeff
+  double tans_inv_;                     //!< distortion model coeff
+  bool distortion_;                     //!< use distortion model?
+  double* param_;
+
+  //! Radial distortion transformation factor: returns ration of distorted / undistorted radius.
+  inline double rtrans_factor(double r) const
+  {
+    if(r < 0.001 || s_ == 0.0)
+      return 1.0;
+    else
+      return (s_inv_* atan(r * tans_) / r);
+  };
+
+  //! Inverse radial distortion: returns un-distorted radius from distorted.
+  inline double invrtrans(double r) const
+  {
+    if(s_ == 0.0)
+      return r;
+    return (tan(r * s_) * tans_inv_);
+  };
+
+public:
+
+  ATANCamera(double width, double height, double fx, double fy, double dx, double dy, double s);
+
+  ~ATANCamera();
+
+  virtual Vector3d
+  cam2world(const double& x, const double& y) const;
+
+  virtual Vector3d
+  cam2world(const Vector2d& px) const;
+
+  virtual Vector2d
+  world2cam(const Vector3d& xyz_c) const;
+
+  virtual Vector2d
+  world2cam(const Vector2d& uv) const;
+
+  const Vector2d focal_length() const
+  {
+    return Vector2d(fx_, fy_);
+  }
+
+  virtual double errorMultiplier2() const
+  {
+    return fx_;
+  }
+
+  virtual double errorMultiplier() const
+  {
+    return 4*fx_*fy_;
+  }
+  virtual double* params() const{
+      param_[0]=fx_;
+      param_[1]=fy_;
+      param_[2]=cx_;
+      param_[3]=cy_;
+      param_[4]=s_;
+     return  param_;
+  }
+};
+
+} // end namespace vk
+
+#endif /* ATAN_CAMERA_H_ */

GPU_version/vio/include/vio/bundle_adjustment.h

@@ -0,0 +1,109 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_BUNDLE_ADJUSTMENT_H_
+#define SVO_BUNDLE_ADJUSTMENT_H_
+
+#include <vio/global.h>
+
+namespace g2o {
+class EdgeProjectXYZ2UV;
+class SparseOptimizer;
+class VertexSE3Expmap;
+class VertexSBAPointXYZ;
+}
+
+namespace vio {
+
+typedef g2o::EdgeProjectXYZ2UV g2oEdgeSE3;
+typedef g2o::VertexSE3Expmap g2oFrameSE3;
+typedef g2o::VertexSBAPointXYZ g2oPoint;
+
+class Frame;
+class Point;
+class Feature;
+class Map;
+
+/// Local, global and 2-view bundle adjustment with g2o
+namespace ba {
+
+/// Temporary container to hold the g2o edge with reference to frame and point.
+struct EdgeContainerSE3
+{
+  g2oEdgeSE3*     edge;
+  Frame*          frame;
+  Feature*        feature;
+  bool            is_deleted;
+  EdgeContainerSE3(g2oEdgeSE3* e, Frame* frame, Feature* feature) :
+    edge(e), frame(frame), feature(feature), is_deleted(false)
+  {}
+};
+
+/// Optimize two camera frames and their observed 3D points.
+/// Is used after initialization.
+void twoViewBA(Frame* frame1, Frame* frame2, double reproj_thresh, Map* map);
+
+/// Local bundle adjustment.
+/// Optimizes core_kfs and their observed map points while keeping the
+/// neighbourhood fixed.
+void localBA(
+    Frame* center_kf,
+    set<FramePtr>* core_kfs,
+    Map* map,
+    size_t& n_incorrect_edges_1,
+    size_t& n_incorrect_edges_2,
+    double& init_error,
+    double& final_error);
+
+/// Global bundle adjustment.
+/// Optimizes the whole map. Is currently not used in SVO.
+void globalBA(Map* map);
+
+/// Initialize g2o with solver type, optimization strategy and camera model.
+void setupG2o(g2o::SparseOptimizer * optimizer);
+
+/// Run the optimization on the provided graph.
+void runSparseBAOptimizer(
+    g2o::SparseOptimizer* optimizer,
+    unsigned int num_iter,
+    double& init_error,
+    double& final_error);
+
+/// Create a g2o vertice from a keyframe object.
+g2oFrameSE3* createG2oFrameSE3(
+    Frame* kf,
+    size_t id,
+    bool fixed);
+
+/// Creates a g2o vertice from a mappoint object.
+g2oPoint* createG2oPoint(
+    Vector3d pos,
+    size_t id,
+    bool fixed);
+
+/// Creates a g2o edge between a g2o keyframe and mappoint vertice with the provided measurement.
+g2oEdgeSE3* createG2oEdgeSE3(
+    g2oFrameSE3* v_kf,
+    g2oPoint* v_mp,
+    const Vector2d& f_up,
+    bool robust_kernel,
+    double huber_width,
+    double weight = 1);
+
+} // namespace ba
+} // namespace vio
+
+#endif // SVO_BUNDLE_ADJUSTMENT_H_

GPU_version/vio/include/vio/camera_loader.h

@@ -0,0 +1,46 @@
+/*
+ * camera_loader.h
+ *
+ *  Created on: Feb 11, 2014
+ *      Author: cforster
+ */
+
+#ifndef VIKIT_CAMERA_LOADER_H_
+#define VIKIT_CAMERA_LOADER_H_
+
+#include <string>
+#include <vio/abstract_camera.h>
+#include <vio/atan_camera.h>
+#include <vio/params_helper.h>
+
+namespace vk {
+namespace camera_loader {
+
+/// Load from ROS Namespace
+bool loadFromRosNs(const std::string& ns, vk::AbstractCamera*& cam)
+{
+  bool res = true;
+  std::string cam_model(getParam<std::string>(ns+"/cam_model"));
+  if(cam_model == "ATAN")
+  {
+    cam = new vk::ATANCamera(
+        getParam<int>(ns+"/cam_width"),
+        getParam<int>(ns+"/cam_height"),
+        getParam<double>(ns+"/cam_fx"),
+        getParam<double>(ns+"/cam_fy"),
+        getParam<double>(ns+"/cam_cx"),
+        getParam<double>(ns+"/cam_cy"),
+        getParam<double>(ns+"/cam_d0"));
+  }
+  else
+  {
+    cam = NULL;
+    res = false;
+  }
+  return res;
+}
+
+} // namespace camera_loader
+} // namespace vk
+
+#endif // VIKIT_CAMERA_LOADER_H_

GPU_version/vio/include/vio/cl_class.h

@@ -0,0 +1,191 @@
+//
+// Created by root on 4/27/21.
+//
+
+#ifndef VIO_OPENCL_CL_CLASS_H
+#define VIO_OPENCL_CL_CLASS_H
+#include <vio/cl.hpp>
+#include <iostream>
+#include <vector>
+#include <algorithm>
+#include <fstream>
+#include <exception>
+#include <CL/opencl.h>
+#include <opencv2/opencv.hpp>
+
+class kernel{
+public:
+    kernel(cl::Program* program,std::string name){_kernel = new cl::Kernel(*program,name.c_str());};
+    ~kernel(){
+        _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){
+        _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, sizeof(T) * buf_size),
+                                                                                                           buf_size),id));
+        cl_int err=queue->enqueueWriteBuffer(_buffers.back().first.first,CL_TRUE, 0, sizeof(T) * buf_size, buf);
+        if(err!=0){
+            std::cerr<<"Error C:Kernel, F:write, L:enqueueWriteBuffer\n"<<err<<'\n';
+            return err;
+        }
+        err=_kernel->setArg(id,_buffers.back().first.first);
+        if(err!=0){
+            std::cerr<<"Error C:Kernel, F:write, L3:setArg\n"<<err<<'\n';
+            return err;
+        }
+        return CL_SUCCESS;
+
+    };
+    int32_t write(size_t id/*buffer ID*/,cv::Mat& buf,cl::Context* context){
+        _images.push_back(std::pair<cl::Image2D,size_t>(cl::Image2D(*context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR,
+                                                                                cl::ImageFormat(CL_R, CL_UNSIGNED_INT8),
+                                                                                buf.size().width,
+                                                                                buf.size().height,
+                                                                                0,
+                                                                                reinterpret_cast<uchar*>(buf.data)),id));
+
+        cl_int err=_kernel->setArg(id,_images.back().first);
+        if(err!=0){
+            std::cerr<<"Error C:Kernel, F:write image, L:setArg\n"<<err<<'\n';
+            return err;
+        }
+        return CL_SUCCESS;
+    };
+
+    template<typename T>
+    int32_t reload(size_t id,T* buf,cl::CommandQueue* queue){
+        try{
+            if(id>_buffers.size())return CL_MAP_FAILURE;
+            for(auto i:_buffers)if(i.second==id)
+                queue->enqueueWriteBuffer(i.first.first,CL_TRUE, 0, sizeof(T) * i.first.second, buf);
+            return CL_SUCCESS;
+        }catch (std::exception& err){
+            std::cerr<<"Reload buffer on GPU failed\n"<<err.what()<<'\n';
+            return CL_MAP_FAILURE;
+        }
+    };
+    int32_t reload(size_t id,cv::Mat& buf,cl::Context* context){
+        try{
+            for(auto i:_images)if(i.second==id){
+                    i.first=cl::Image2D(*context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR,
+                                        cl::ImageFormat(CL_R, CL_UNSIGNED_INT8),
+                                        buf.size().width,
+                                        buf.size().height,
+                                        0,
+                                        reinterpret_cast<uchar*>(buf.data));
+            }
+
+            return CL_SUCCESS;
+        }catch (std::exception& err){
+            std::cerr<<"Reload buffer on GPU failed\n"<<err.what()<<'\n';
+            return CL_MAP_FAILURE;
+        }
+    };
+    cl::Kernel* _kernel = NULL;
+    std::pair<cl::Buffer,size_t> read(size_t id){
+        for(auto i:_buffers)if(i.second==id)return i.first;
+    }
+    cl::Image2D read_img(size_t id){
+        for(auto i:_images)if(i.second==id)return i.first;
+    }
+private:
+    std::vector<std::pair<std::pair<cl::Buffer,size_t>,size_t>> _buffers;
+    std::vector<std::pair<cl::Image2D,size_t>> _images;
+
+};
+class read_cl{
+public:
+    read_cl(std::string path){
+        src_str=(char *)malloc(0x100000);
+        FILE *fp;
+        fp = fopen(path.c_str(), "r");
+        size = fread(src_str, 1, 0x100000, fp);
+        fclose(fp);
+    }
+    void print(){
+        std::cerr<<src_str<<"\n";
+    }
+    char *src_str = nullptr;
+    size_t size;
+};
+class opencl{
+public:
+    opencl();
+    ~opencl();
+    int32_t make_kernel(std::string name){_kernels.push_back(kernel(program,name));};
+    template<typename T>
+    int32_t write_buf(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/, int size,T* buf) {
+        try{
+            _kernels.at(id1).write(id2,buf,queue,context,size);
+            return CL_SUCCESS;
+        }catch (std::exception& err){
+            std::cerr<<"Write buffer on GPU failed\n"<<err.what()<<'\n';
+            return CL_MAP_FAILURE;
+        }
+    }
+    int32_t write_buf(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,cv::Mat& buf) {
+        try{
+            _kernels.at(id1).write(id2,buf,context);
+            return CL_SUCCESS;
+        }catch (std::exception& err){
+            std::cerr<<"Write image on GPU failed\n"<<err.what()<<'\n';
+            return CL_MAP_FAILURE;
+        }
+    }
+    template<typename T>
+    int32_t write_buf(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,T&& value){
+        _kernels.at(id1)._kernel->setArg(id2,value);
+    };
+    template<typename T>
+    int32_t reload_buf(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,T* buf) {
+        try{
+            _kernels.at(id1).reload(id2,buf,queue);
+            return CL_SUCCESS;
+        }catch (std::exception& err){
+            std::cerr<<"Reload buffer on GPU failed\n"<<err.what()<<'\n';
+            return CL_MAP_FAILURE;
+        }
+    }
+    int32_t reload_buf(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,cv::Mat& buf) {
+        try{
+            _kernels.at(id1).reload(id2,buf,context);
+            return CL_SUCCESS;
+        }catch (std::exception& err){
+            std::cerr<<"Write buffer on GPU failed\n"<<err.what()<<'\n';
+            return CL_MAP_FAILURE;
+        }
+    }
+    int32_t 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;
+            if(z>1 && y>1){
+                err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x,y,z)/*Global*/, cl::NullRange/*local*/);
+            }else if(z<2 && y>1){
+                err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x,y)/*Global*/, cl::NullRange/*local*/);
+            }else{
+                err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x)/*Global*/, cl::NullRange/*local*/);
+            };
+            queue->finish();
+            if(err!=0){
+                std::cerr<<"kernel: "<<_kernels.at(id1)._kernel->getInfo<CL_KERNEL_FUNCTION_NAME>();
+                std::cerr<<" ,Args: "<<_kernels.at(id1)._kernel->getInfo<CL_KERNEL_NUM_ARGS>()<<" ,Run out: "<<err<<'\n';
+                for(int i=0;i<_kernels.at(id1)._kernel->getInfo<CL_KERNEL_NUM_ARGS>();++i)
+                std::cerr<<"argument type "<<i<<":"<<_kernels.at(id1)._kernel->getArgInfo<CL_KERNEL_ARG_TYPE_NAME>(i)<<'\n';
+            }
+        return err;
+
+    }
+    template<typename T>
+    void read(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/, T* out){
+        queue->enqueueReadBuffer(_kernels.at(id1).read(id2).first, CL_TRUE, 0, sizeof(T) * _kernels.at(id1).read(id2).second, out);
+    }
+    void clear_buf();
+private:
+    std::vector<kernel> _kernels;
+    cl::Context* context = NULL;
+    cl::Device device;
+    cl::Program* program = NULL;
+    cl::CommandQueue* queue=NULL;
+};
+
+#endif //VIO_OPENCL_CL_CLASS_H

GPU_version/vio/include/vio/config.h

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

GPU_version/vio/include/vio/depth_filter.h

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

GPU_version/vio/include/vio/feature.h

@@ -0,0 +1,75 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_FEATURE_H_
+#define SVO_FEATURE_H_
+
+#include <vio/frame.h>
+
+namespace vio {
+
+/// A salient image region that is tracked across frames.
+struct Feature
+{
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  enum FeatureType {
+    CORNER,
+    EDGELET
+  };
+
+  FeatureType type;     //!< Type can be corner or edgelet.
+  Frame* frame;         //!< Pointer to frame in which the feature was detected.
+  Vector2d px;          //!< Coordinates in pixels on pyramid level 0.
+  Vector3d f;           //!< Unit-bearing vector of the feature.
+  int level;            //!< Image pyramid level where feature was extracted.
+  Point* point;         //!< Pointer to 3D point which corresponds to the feature.
+  Vector2d grad;        //!< Dominant gradient direction for edglets, normalized.
+
+  Feature(Frame* _frame, const Vector2d& _px, int _level) :
+    type(CORNER),
+    frame(_frame),
+    px(_px),
+    f(frame->cam_->cam2world(px)),
+    level(_level),
+    point(NULL),
+    grad(1.0,0.0)
+  {}
+
+  Feature(Frame* _frame, const Vector2d& _px, const Vector3d& _f, int _level) :
+    type(CORNER),
+    frame(_frame),
+    px(_px),
+    f(_f),
+    level(_level),
+    point(NULL),
+    grad(1.0,0.0)
+  {}
+
+  Feature(Frame* _frame, Point* _point, const Vector2d& _px, const Vector3d& _f, int _level) :
+    type(CORNER),
+    frame(_frame),
+    px(_px),
+    f(_f),
+    level(_level),
+    point(_point),
+    grad(1.0,0.0)
+  {}
+};
+
+} // namespace vio
+
+#endif // SVO_FEATURE_H_

GPU_version/vio/include/vio/feature_alignment.h

@@ -0,0 +1,63 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_FEATURE_ALIGNMENT_H_
+#define SVO_FEATURE_ALIGNMENT_H_
+
+#include <vio/global.h>
+
+namespace vio {
+
+/// Subpixel refinement of a reference feature patch with the current image.
+/// Implements the inverse-compositional approach (see "Lucas-Kanade 20 Years on"
+/// paper by Baker.
+namespace feature_alignment {
+
+bool align1D(
+    const cv::Mat& cur_img,
+    const Vector2f& dir,                  // direction in which the patch is allowed to move
+    uint8_t* ref_patch_with_border,
+    uint8_t* ref_patch,
+    const int n_iter,
+    Vector2d& cur_px_estimate,
+    double& h_inv);
+
+bool align2D(
+    const cv::Mat& cur_img,
+    uint8_t* ref_patch_with_border,
+    uint8_t* ref_patch,
+    const int n_iter,
+    Vector2d& cur_px_estimate,
+    bool no_simd = false);
+
+bool align2D_SSE2(
+    const cv::Mat& cur_img,
+    uint8_t* ref_patch_with_border,
+    uint8_t* ref_patch,
+    const int n_iter,
+    Vector2d& cur_px_estimate);
+
+bool align2D_NEON(
+    const cv::Mat& cur_img,
+    uint8_t* ref_patch_with_border,
+    uint8_t* ref_patch,
+    const int n_iter,
+    Vector2d& cur_px_estimate);
+
+} // namespace feature_alignment
+} // namespace vio
+
+#endif // SVO_FEATURE_ALIGNMENT_H_

GPU_version/vio/include/vio/feature_detection.h

@@ -0,0 +1,111 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_FEATURE_DETECTION_H_
+#define SVO_FEATURE_DETECTION_H_
+
+#include <vio/global.h>
+#include <vio/frame.h>
+#include <vio/cl_class.h>
+
+namespace vio {
+
+/// Implementation of various feature detectors.
+namespace feature_detection {
+
+/// Temporary container used for corner detection. Features are initialized from these.
+struct Corner
+{
+  int x;        //!< x-coordinate of corner in the image.
+  int y;        //!< y-coordinate of corner in the image.
+  int level;    //!< pyramid level of the corner.
+  float score;  //!< shi-tomasi score of the corner.
+  float angle;  //!< for gradient-features: dominant gradient angle.
+  Corner(int x, int y, float score, int level, float angle) :
+    x(x), y(y), level(level), score(score), angle(angle)
+  {}
+};
+typedef vector<Corner> Corners;
+
+/// All detectors should derive from this abstract class.
+class AbstractDetector
+{
+public:
+  AbstractDetector(
+      const int img_width,
+      const int img_height,
+      const int cell_size,
+      const int n_pyr_levels);
+
+  virtual ~AbstractDetector() {
+  };
+
+  virtual void detect(
+      Frame* frame,
+      const ImgPyr& img_pyr,
+      const double detection_threshold,
+      Features& fts) = 0;
+
+  /// Flag the grid cell as occupied
+  void setGridOccpuancy(const Vector2d& px);
+
+  /// Set grid cells of existing features as occupied
+  void setExistingFeatures(const Features& fts);
+
+protected:
+
+  static const int border_ = 8; //!< no feature should be within 8px of border.
+  const int cell_size_;
+  const int n_pyr_levels_;
+  const int grid_n_cols_;
+  const int grid_n_rows_;
+  vector<bool> grid_occupancy_;
+
+  void resetGrid();
+
+  inline int getCellIndex(int x, int y, int level)
+  {
+    const int scale = (1<<level);
+    return (scale*y)/cell_size_*grid_n_cols_ + (scale*x)/cell_size_;
+  }
+};
+typedef boost::shared_ptr<AbstractDetector> DetectorPtr;
+
+/// FAST detector by Edward Rosten.
+class FastDetector : public AbstractDetector
+{
+public:
+  FastDetector(
+      const int img_width,
+      const int img_height,
+      const int cell_size,
+      opencl* gpu_fast_,
+      const int n_pyr_levels);
+
+  virtual ~FastDetector() {}
+
+  virtual void detect(
+      Frame* frame,
+      const ImgPyr& img_pyr,
+      const double detection_threshold,
+      Features& fts);
+  opencl* gpu_fast_;
+};
+
+} // namespace feature_detection
+} // namespace vio
+
+#endif // SVO_FEATURE_DETECTION_H_

GPU_version/vio/include/vio/for_it.hpp

@@ -0,0 +1,87 @@
+//
+// Created by root on 5/12/21.
+//
+
+#ifndef VIO_FOR_IT_H
+#define VIO_FOR_IT_H
+#include <iostream>
+#include <vector>
+#include <tuple>
+
+//https://stackoverflow.com/questions/24881799/get-index-in-c11-foreach-loop
+// Wrapper class
+template <typename T>
+class for_impl
+{
+public:
+    // The return value of the operator* of the iterator, this
+    // is what you will get inside of the for loop
+    struct item
+    {
+        size_t index;
+        typename T::value_type & item;
+    };
+    typedef item value_type;
+
+    // Custom iterator with minimal interface
+    struct iterator
+    {
+        iterator(typename T::iterator _it, size_t counter=0) :
+                it(_it), counter(counter)
+        {}
+
+        iterator operator++()
+        {
+            return iterator(++it, ++counter);
+        }
+
+        bool operator!=(iterator other)
+        {
+            return it != other.it;
+        }
+
+        typename T::iterator::value_type item()
+        {
+            return *it;
+        }
+
+        value_type operator*()
+        {
+            return value_type{counter, *it};
+        }
+
+        size_t index()
+        {
+            return counter;
+        }
+
+    private:
+        typename T::iterator it;
+        size_t counter;
+    };
+
+    for_impl(T & t) : container(t) {}
+
+    iterator begin()
+    {
+        return iterator(container.begin());
+    }
+
+    iterator end()
+    {
+        return iterator(container.end());
+    }
+
+private:
+    T & container;
+};
+
+
+// A templated free function allows you to create the wrapper class
+// conveniently
+template <typename T>
+for_impl<T> _for(T & t)
+{
+    return for_impl<T>(t);
+}
+#endif //VIO_FOR_IT_H

GPU_version/vio/include/vio/frame.h

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

GPU_version/vio/include/vio/frame_handler_base.h

@@ -0,0 +1,118 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_FRAME_HANDLER_BASE_H_
+#define SVO_FRAME_HANDLER_BASE_H_
+
+#include <queue>
+#include <boost/noncopyable.hpp>
+#include <boost/function.hpp>
+#include <boost/thread.hpp>
+#include <vio/global.h>
+#include <vio/map.h>
+
+namespace vk
+{
+class AbstractCamera;
+//class PerformanceMonitor;
+}
+
+namespace vio
+{
+class Point;
+class Matcher;
+class DepthFilter;
+
+/// Base class for various VO pipelines. Manages the map and the state machine.
+class FrameHandlerBase : boost::noncopyable
+{
+public:
+  enum Stage {
+    STAGE_PAUSED,
+    STAGE_FIRST_FRAME,
+    STAGE_SECOND_FRAME,
+    STAGE_DEFAULT_FRAME,
+    STAGE_RELOCALIZING
+  };
+  enum TrackingQuality {
+    TRACKING_INSUFFICIENT,
+    TRACKING_BAD,
+    TRACKING_GOOD
+  };
+  enum UpdateResult {
+    RESULT_NO_KEYFRAME,
+    RESULT_IS_KEYFRAME,
+    RESULT_FAILURE
+  };
+
+  FrameHandlerBase();
+
+  virtual ~FrameHandlerBase();
+
+  /// Get the current map.
+  const Map& map() const { return map_; }
+
+  /// Will reset the map as soon as the current frame is finished processing.
+  void reset() { set_reset_ = true; }
+
+  /// Start processing.
+  void start() { set_start_ = true; }
+
+  /// Get the current stage of the algorithm.
+  Stage stage() const { return stage_; }
+
+  /// Get tracking quality.
+  TrackingQuality trackingQuality() const { return tracking_quality_; }
+
+  /// Get the processing time of the previous iteration.
+  //double lastProcessingTime() const { return timer_.getTime(); }
+
+  /// Get the number of feature observations of the last frame.
+ // size_t lastNumObservations() const { return num_obs_last_; }
+
+protected:
+  Stage stage_;                 //!< Current stage of the algorithm.
+  bool set_reset_;              //!< Flag that the user can set. Will reset the system before the next iteration.
+  bool set_start_;              //!< Flag the user can set to start the system when the next image is received.
+  Map map_;                     //!< Map of keyframes created by the slam system.
+  size_t num_obs_last_;                         //!< Number of observations in the previous frame.
+  TrackingQuality tracking_quality_;            //!< An estimate of the tracking quality based on the number of tracked features.
+
+  /// Before a frame is processed, this function is called.
+  bool startFrameProcessingCommon(const double timestamp);
+
+  /// When a frame is finished processing, this function is called.
+  int finishFrameProcessingCommon(
+      const size_t update_id,
+      const UpdateResult dropout,
+      const size_t num_observations);
+
+  /// Reset the map and frame handler to start from scratch.
+  void resetCommon();
+
+  /// Reset the frame handler. Implement in derived class.
+  virtual void resetAll() { resetCommon(); }
+
+  /// Set the tracking quality based on the number of tracked features.
+  virtual void setTrackingQuality(const size_t num_observations);
+
+  /// Optimize some of the observed 3D points.
+  virtual void optimizeStructure(FramePtr frame, size_t max_n_pts, int max_iter);
+};
+
+} // namespace nslam
+
+#endif // SVO_FRAME_HANDLER_BASE_H_

GPU_version/vio/include/vio/frame_handler_mono.h

@@ -0,0 +1,112 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_FRAME_HANDLER_H_
+#define SVO_FRAME_HANDLER_H_
+
+#include <set>
+#include <vio/abstract_camera.h>
+#include <vio/frame_handler_base.h>
+#include <vio/reprojector.h>
+#include <vio/initialization.h>
+#include <vio/ukf.h>
+#include <vio/cl_class.h>
+#include <vio/sparse_img_align_gpu.h>
+
+namespace vio {
+
+/// Monocular Visual Odometry Pipeline as described in the SVO paper.
+class FrameHandlerMono : public FrameHandlerBase
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  
+  FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,3,1>& init);
+  virtual ~FrameHandlerMono();
+
+  /// Provide an image.
+  void addImage(const cv::Mat& img, double timestamp,const ros::Time& time);
+
+  /// Set the first frame (used for synthetic datasets in benchmark node)
+  void setFirstFrame(const FramePtr& first_frame);
+
+  /// Get the last frame that has been processed.
+  FramePtr lastFrame() { return last_frame_; }
+
+  /// Get the set of spatially closest keyframes of the last frame.
+  const set<FramePtr>& coreKeyframes() { return core_kfs_; }
+
+  /// Return the feature track to visualize the KLT tracking during initialization.
+  const vector<cv::Point2f>& initFeatureTrackRefPx() const { return klt_homography_init_->px_ref_; }
+  const vector<cv::Point2f>& initFeatureTrackCurPx() const { return klt_homography_init_->px_cur_; }
+
+  /// Access the depth filter.
+  DepthFilter* depthFilter() const{ return depth_filter_; }
+
+  /// An external place recognition module may know where to relocalize.
+  bool relocalizeFrameAtPose(
+      const int keyframe_id,
+      const SE3& T_kf_f,
+      const cv::Mat& img,
+      const double timestamp);
+  //std::shared_ptr<Imu_Integration> imu_integPtr_;  //Integration of IMU
+  void UpdateIMU(double* value,const ros::Time& time);
+  void UpdateCmd(double* value,const ros::Time& time);
+  UKF ukfPtr_;
+
+protected:
+  vk::AbstractCamera* cam_;                     //!< Camera model, can be ATAN, Pinhole or Ocam (see vikit).
+  Reprojector reprojector_;                     //!< Projects points from other keyframes into the current frame
+  FramePtr new_frame_;                          //!< Current frame.
+  FramePtr last_frame_;                         //!< Last frame, not necessarily a keyframe.
+  set<FramePtr> core_kfs_;                      //!< Keyframes in the closer neighbourhood.
+  vector< pair<FramePtr,size_t> > overlap_kfs_; //!< All keyframes with overlapping field of view. the paired number specifies how many common mappoints are observed TODO: why vector!?
+  initialization::KltHomographyInit* klt_homography_init_; //!< Used to estimate pose of the first two keyframes by estimating a homography.
+  DepthFilter* depth_filter_;                   //!< Depth estimation algorithm runs in a parallel thread and is used to initialize new 3D points.
+  opencl* gpu_fast_;
+  ros::Time time_;
+  SparseImgAlignGpu* imageAlign_;
+
+
+  /// Initialize the visual odometry algorithm.
+  virtual void initialize();
+
+  /// Processes the first frame and sets it as a keyframe.
+  virtual UpdateResult processFirstFrame();
+
+  /// Processes all frames after the first frame until a keyframe is selected.
+  virtual UpdateResult processSecondFrame();
+
+  /// Processes all frames after the first two keyframes.
+  virtual UpdateResult processFrame();
+
+  /// Try relocalizing the frame at relative position to provided keyframe.
+  virtual UpdateResult relocalizeFrame(
+      const SE3& T_cur_ref,
+      FramePtr ref_keyframe);
+
+  /// Reset the frame handler. Implement in derived class.
+  virtual void resetAll();
+
+  /// Keyframe selection criterion.
+  virtual bool needNewKf(double scene_depth_mean);
+
+  void setCoreKfs(size_t n_closest);
+};
+
+} // namespace vio
+
+#endif // SVO_FRAME_HANDLER_H_

GPU_version/vio/include/vio/global.h

@@ -0,0 +1,120 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_GLOBAL_H_
+#define SVO_GLOBAL_H_
+
+#include <list>
+#include <vector>
+#include <string>
+#include <stdint.h>
+#include <stdio.h>
+#include <math.h>
+
+#include <Eigen/Core>
+#include <opencv2/opencv.hpp>
+#include <sophus/se2.h>
+#include <sophus/se3.h>
+#include <boost/shared_ptr.hpp>
+#include<Eigen/StdVector>
+#ifndef RPG_SVO_VIKIT_IS_VECTOR_SPECIALIZED //Guard for rpg_vikit
+#define RPG_SVO_VIKIT_IS_VECTOR_SPECIALIZED
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector3d)
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2d)
+#endif
+#include <ros/console.h>
+#define SVO_DEBUG_STREAM(x) ROS_DEBUG_STREAM(x)
+#define SVO_INFO_STREAM(x) ROS_INFO_STREAM(x)
+#define SVO_WARN_STREAM(x) ROS_WARN_STREAM(x)
+#define SVO_WARN_STREAM_THROTTLE(rate, x) ROS_WARN_STREAM_THROTTLE(rate, x)
+#define SVO_ERROR_STREAM(x) ROS_ERROR_STREAM(x)
+
+
+namespace vio
+{
+  using namespace Eigen;
+  using namespace Sophus;
+
+  const double EPS = 0.0000000001;
+  const double PI = 3.14159265;
+  class SE2_5: public SE2{
+  public:
+        SE2_5(SE2&& se2):T2_(se2){
+        }
+        SE2_5(SE2& se2):T2_(se2){
+        }
+        SE2_5(SE3&& se3){
+            Quaterniond q=se3.unit_quaternion().normalized();
+            double pitch=asin(2.0*(q.w()*q.y()-q.z()*q.x()));
+            if (std::abs(pitch) >= 1)
+                pitch = std::copysign(M_PI / 2.0, pitch); // use 90 degrees if out of range
+            else
+                pitch = std::asin(pitch);
+            Eigen::Matrix<double, 2,2> R;
+            R<<cos(pitch),sin(pitch),
+               -sin(pitch),cos(pitch);
+            T2_ = SE2(R,Vector2d(se3.translation().x(),se3.translation().z()));
+        }
+        SE2_5(SE3& se3){
+            Quaterniond q=se3.unit_quaternion().normalized();
+            double pitch=asin(2.0*(q.w()*q.y()-q.z()*q.x()));
+            if (std::abs(pitch) >= 1)
+                pitch = std::copysign(M_PI / 2.0, pitch); // use 90 degrees if out of range
+            else
+                pitch = std::asin(pitch);
+             Eigen::Matrix<double, 2,2> R;
+             R<<cos(pitch),sin(pitch),
+                  -sin(pitch),cos(pitch);
+            T2_=SE2(R,Vector2d(se3.translation().x(),se3.translation().z()));
+        }
+        SE2_5(double x,double z,double pitch){
+            Eigen::Matrix<double, 2,2> R;
+            R<<cos(pitch),sin(pitch),
+               -sin(pitch),cos(pitch);
+            T2_=SE2(R,Vector2d(x,z));
+        };
+        SE2 se2() const{
+            return T2_;
+        }
+        SE2 inverse() const{
+            double yaw=atan(T2_.so2().unit_complex().imag()/T2_.so2().unit_complex().real());
+            if(yaw<0.0)
+                yaw=M_PI-yaw;
+            else
+                yaw=yaw-M_PI;
+            return SE2(yaw,-1.0*T2_.translation());
+        }
+        double pitch()const{
+            return atan(T2_.rotation_matrix()(0,1)/T2_.rotation_matrix()(0,0));
+        }
+        SE3 se3() const{
+            Eigen::Matrix<double,3,3> R;
+            R<<T2_.rotation_matrix()(0,0),0.0,T2_.rotation_matrix()(0,1),
+                     0.0,1.0,0.0,
+                    T2_.rotation_matrix()(1,0),0.0,T2_.rotation_matrix()(1,1);
+              return SE3(R,Vector3d(T2_.translation()(0),1e-19,T2_.translation()(1)));
+        }
+        ~SE2_5(){}
+
+  private:
+      SE2 T2_;
+  };
+
+  class Frame;
+  typedef boost::shared_ptr<Frame> FramePtr;
+} // namespace vio
+
+#endif // SVO_GLOBAL_H_

GPU_version/vio/include/vio/homography.h

@@ -0,0 +1,85 @@
+/*
+ * homography.cpp
+ * Adaptation of PTAM-GPL HomographyInit class.
+ * https://github.com/Oxford-PTAM/PTAM-GPL
+ * Licence: GPLv3
+ * Copyright 2008 Isis Innovation Limited
+ *
+ *  Created on: Sep 2, 2012
+ *      by: cforster
+ *
+ * This class implements the homography decomposition of Faugeras and Lustman's
+ * 1988 tech report. Code converted to Eigen from PTAM.
+ *
+ */
+
+#ifndef HOMOGRAPHY_H_
+#define HOMOGRAPHY_H_
+
+#include <Eigen/Core>
+#include <Eigen/StdVector>
+#include <Eigen/SVD>
+#include <vio/math_utils.h>
+
+namespace vk {
+
+using namespace Eigen;
+using namespace std;
+
+struct HomographyDecomposition
+{
+  Vector3d t;
+  Matrix3d R;
+  double   d;
+  Vector3d n;
+
+  // Resolved  Composition
+  Sophus::SE3 T; //!< second from first
+  int score;
+};
+
+class Homography
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  Homography            (const vector<Vector2d, aligned_allocator<Vector2d> >& _fts1,
+                         const vector<Vector2d, aligned_allocator<Vector2d> >& _fts2,
+                         double _error_multiplier2,
+                         double _thresh_in_px);
+
+  void
+  calcFromPlaneParams   (const Vector3d & normal,
+                         const Vector3d & point_on_plane);
+
+  void
+  calcFromMatches       ();
+
+  size_t
+  computeMatchesInliers ();
+
+  bool
+  computeSE3fromMatches ();
+
+  bool
+  decompose             ();
+
+  void
+  findBestDecomposition ();
+
+  double thresh;
+  double error_multiplier2;
+  const vector<Vector2d, aligned_allocator<Vector2d> >& fts_c1; //!< Features on first image on unit plane
+  const vector<Vector2d, aligned_allocator<Vector2d> >& fts_c2; //!< Features on second image on unit plane
+  vector<bool> inliers;
+  SE3 T_c2_from_c1;             //!< Relative translation and rotation of two images
+  Matrix3d H_c2_from_c1;                   //!< Homography
+  vector<HomographyDecomposition> decompositions;
+};
+
+
+
+
+} /* end namespace vk */
+
+#endif /* HOMOGRAPHY_H_ */

GPU_version/vio/include/vio/initialization.h

@@ -0,0 +1,87 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_INITIALIZATION_H
+#define SVO_INITIALIZATION_H
+
+#include <vio/global.h>
+#include <vio/cl_class.h>
+//#include "../../gtsam/imu_integration.h"
+
+namespace vio {
+
+class FrameHandlerMono;
+
+/// Bootstrapping the map from the first two views.
+namespace initialization {
+
+enum InitResult { FAILURE, NO_KEYFRAME, SUCCESS };
+
+/// Tracks features using Lucas-Kanade tracker and then estimates a homography.
+class KltHomographyInit {
+  friend class vio::FrameHandlerMono;
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  FramePtr frame_ref_;
+  KltHomographyInit(opencl* gpu_fast_):gpu_fast_(gpu_fast_),T_cur_from_ref_(0.0,0.0,0.0) {};
+  ~KltHomographyInit() {};
+  InitResult addFirstFrame(FramePtr frame_ref);
+  InitResult addSecondFrame(FramePtr frame_ref);
+  void reset();
+
+protected:
+  vector<cv::Point2f> px_ref_;      //!< keypoints to be tracked in reference frame.
+  vector<cv::Point2f> px_cur_;      //!< tracked keypoints in current frame.
+  vector<Vector3d> f_ref_;          //!< bearing vectors corresponding to the keypoints in the reference image.
+  vector<Vector3d> f_cur_;          //!< bearing vectors corresponding to the keypoints in the current image.
+  vector<double> disparities_;      //!< disparity between first and second frame.
+  vector<int> inliers_;             //!< inliers after the geometric check (e.g., Homography).
+  vector<Vector3d> xyz_in_cur_;     //!< 3D points computed during the geometric check.
+  SE2_5 T_cur_from_ref_;              //!< computed transformation between the first two frames.
+  opencl* gpu_fast_;
+};
+
+/// Detect Fast corners in the image.
+void detectFeatures(
+    FramePtr frame,
+    vector<cv::Point2f>& px_vec,
+    vector<Vector3d>& f_vec,
+    opencl* gpu_fast);
+
+/// Compute optical flow (Lucas Kanade) for selected keypoints.
+void trackKlt(
+    FramePtr frame_ref,
+    FramePtr frame_cur,
+    vector<cv::Point2f>& px_ref,
+    vector<cv::Point2f>& px_cur,
+    vector<Vector3d>& f_ref,
+    vector<Vector3d>& f_cur,
+    vector<double>& disparities);
+
+void computeHomography(
+    const vector<Vector3d>& f_ref,
+    const vector<Vector3d>& f_cur,
+    double focal_length,
+    double reprojection_threshold,
+    vector<int>& inliers,
+    vector<Vector3d>& xyz_in_cur,
+    SE2_5& T_cur_from_ref);
+
+} // namespace initialization
+} // namespace vio
+
+#endif // SVO_INITIALIZATION_H

GPU_version/vio/include/vio/map.h

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

GPU_version/vio/include/vio/matcher.h

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

GPU_version/vio/include/vio/math_utils.h

@@ -0,0 +1,171 @@
+/*
+ * math_utils.h
+ *
+ *  Created on: Jul 20, 2012
+ *      Author: cforster
+ */
+
+#ifndef MATH_UTILS_H_
+#define MATH_UTILS_H_
+
+
+#include <Eigen/Core>
+#include <Eigen/StdVector>
+#include <sophus/se3.h>
+
+namespace vk
+{
+
+using namespace Eigen;
+using namespace std;
+using namespace Sophus;
+
+Vector3d triangulateFeatureNonLin(
+    const Matrix3d& R,
+    const Vector3d& t,
+    const Vector3d& feature1,
+    const Vector3d& feature2);
+
+/// Assumes the bearing vectors f_c and f_r are on the epipolar plane, i.e.
+/// perfect triangulation without noise!
+bool depthFromTriangulationExact(
+    const Matrix3d& R_r_c,
+    const Vector3d& t_r_c,
+    const Vector3d& f_r,
+    const Vector3d& f_c,
+    double& depth_in_r,
+    double& depth_in_c);
+
+double reprojError(
+    const Vector3d& f1,
+    const Vector3d& f2,
+    double error_multiplier2);
+
+double computeInliers(
+    const vector<Vector3d>& features1,
+    const vector<Vector3d>& features2,
+    const Matrix3d& R,
+    const Vector3d& t,
+    const double reproj_thresh,
+    double error_multiplier2,
+    vector<Vector3d>& xyz_vec,
+    vector<int>& inliers,
+    vector<int>& outliers);
+
+void computeInliersOneView(
+    const vector<Vector3d> & feature_sphere_vec,
+    const vector<Vector3d> & xyz_vec,
+    const Matrix3d &R,
+    const Vector3d &t,
+    const double reproj_thresh,
+    const double error_multiplier2,
+    vector<int>& inliers,
+    vector<int>& outliers);
+
+//! Direct Cosine Matrix to Roll Pitch Yaw
+Vector3d dcm2rpy(const Matrix3d &R);
+
+//! Roll Pitch Yaw to Direct Cosine Matrix
+Matrix3d rpy2dcm(const Vector3d &rpy);
+
+//! Angle Axis parametrization to Quaternion
+Quaterniond angax2quat(const Vector3d& n, const double& angle);
+
+//! Angle Axis parametrization to Matrix representation
+Matrix3d angax2dcm(const Vector3d& n, const double& angle);
+
+double sampsonusError(
+    const Vector2d &v2Dash,
+    const Matrix3d& m3Essential,
+    const Vector2d& v2);
+
+inline Matrix3d sqew(const Vector3d& v)
+{
+  Matrix3d v_sqew;
+  v_sqew << 0, -v[2], v[1],
+            v[2], 0, -v[0],
+            -v[1], v[0], 0;
+  return v_sqew;
+}
+
+inline double norm_max(const Eigen::VectorXd & v)
+{
+  double max = -1;
+  for (int i=0; i<v.size(); i++)
+  {
+    double abs = fabs(v[i]);
+    if(abs>max){
+      max = abs;
+    }
+  }
+  return max;
+}
+
+inline Vector2d project2d(const Vector3d& v)
+{
+  return v.head<2>()/v[2];
+}
+
+inline Vector3d unproject2d(const Vector2d& v)
+{
+  return Vector3d(v[0], v[1], 1.0);
+}
+
+inline Vector3d project3d(const Vector4d& v)
+{
+  return v.head<3>()/v[3];
+}
+
+inline Vector4d unproject3d(const Vector3d& v)
+{
+  return Vector4d(v[0], v[1], v[2], 1.0);
+}
+
+template<class T>
+T getMedian(vector<T>& data_vec)
+{
+  assert(!data_vec.empty());
+  typename vector<T>::iterator it = data_vec.begin()+floor(data_vec.size()/2);
+  nth_element(data_vec.begin(), it, data_vec.end());
+  return *it;
+}
+
+inline double pyrFromZero_d(double x_0, int level)
+{
+  return x_0/(1<<level); // = 1 / 2^level
+}
+
+inline Vector2d pyrFromZero_2d(const Vector2d& uv_0, int level)
+{
+  return Vector2d(pyrFromZero_d(uv_0[0], level),
+                  pyrFromZero_d(uv_0[1], level));
+}
+
+inline void
+frameJac_xyz2uv(const Vector3d & xyz,
+                 const double & focal_length,
+                 Matrix<double,2,6> & frame_jac)
+{
+  const double x = xyz[0];
+  const double y = xyz[1];
+  const double z = xyz[2];
+  const double z_2 = z*z;
+
+  frame_jac(0,0) = -1./z *focal_length;
+  frame_jac(0,1) = 0;
+  frame_jac(0,2) = x/z_2 *focal_length;
+  frame_jac(0,3) =  x*y/z_2 * focal_length;
+  frame_jac(0,4) = -(1+(x*x/z_2)) *focal_length;
+  frame_jac(0,5) = y/z *focal_length;
+
+  frame_jac(1,0) = 0;
+  frame_jac(1,1) = -1./z *focal_length;
+  frame_jac(1,2) = y/z_2 *focal_length;
+  frame_jac(1,3) = (1+y*y/z_2) *focal_length;
+  frame_jac(1,4) = -x*y/z_2 *focal_length;
+  frame_jac(1,5) = -x/z *focal_length;
+}
+
+} // end namespace vk
+
+#endif /* MATH_UTILS_H_ */

GPU_version/vio/include/vio/nlls_solver.h

@@ -0,0 +1,174 @@
+/*
+ * Abstract Nonlinear Least-Squares Solver Class
+ *
+ * nlls_solver.h
+ *
+ *  Created on: Nov 5, 2012
+ *      Author: cforster
+ */
+
+#ifndef LM_SOLVER_H_
+#define LM_SOLVER_H_
+
+#include <stdint.h>
+#include <stdio.h>
+#include <iostream>
+#include <math.h>
+#include <Eigen/Core>
+#include <Eigen/StdVector>
+#include <vio/math_utils.h>
+#include <vio/robust_cost.h>
+
+namespace vk {
+
+using namespace std;
+using namespace Eigen;
+
+/**
+ * \brief Abstract Class for solving nonlinear least-squares (NLLS) problems.
+ *
+ * The function implements two algorithms: Levenberg Marquardt and Gauss Newton
+ *
+ * Example implementations of this function can be found in the rpl_examples
+ * package: img_align_2d.cpp, img_align_3d.cpp
+ *
+ * Template Parameters:
+ * D  : dimension of the residual
+ * T  : type of the model, e.g. SE2, SE3
+ */
+
+template <int D, typename T>
+class NLLSSolver {
+
+public:
+  typedef T ModelType;
+  enum Method{GaussNewton, LevenbergMarquardt};
+  enum ScaleEstimatorType{UnitScale, TDistScale, MADScale, NormalScale};
+  enum WeightFunctionType{UnitWeight, TDistWeight, TukeyWeight, HuberWeight};
+
+protected:
+  Matrix<double, D, D>  H_;       //!< Hessian approximation
+  Matrix<double, D, 1>  Jres_;    //!< Jacobian x Residual
+  Matrix<double, D, 1>  x_;       //!< update step
+  bool                  have_prior_;
+  ModelType prior_;
+  Matrix<double, D, D>  I_prior_; //!< Prior information matrix (inverse covariance)
+  double                chi2_;
+  double                rho_;
+  Method                method_;
+
+  /// If the flag linearize_system is set, the function must also compute the
+  /// Jacobian and set the member variables H_, Jres_
+  virtual double
+  computeResiduals      (const ModelType& model,
+                         bool linearize_system,
+                         bool compute_weight_scale) = 0;
+  virtual double
+  computeResiduals      (bool linearize_system,
+                           bool compute_weight_scale) = 0;
+
+  /// Solve the linear system H*x = Jres. This function must set the update
+  /// step in the member variable x_. Must return true if the system could be
+  /// solved and false if it was singular.
+  virtual int
+  solve                 () = 0;
+
+  virtual void
+  update                (const ModelType& old_model, ModelType& new_model) = 0;
+  virtual void
+  update                () = 0;
+
+  virtual void
+  applyPrior            (const ModelType& current_model) { }
+
+  virtual void
+  startIteration        () { }
+
+  virtual void
+  finishIteration       () { }
+
+  virtual void
+  finishTrial           () { }
+
+public:
+
+  /// Damping parameter. If mu > 0, coefficient matrix is positive definite, this
+  /// ensures that x is a descent direction. If mu is large, x is a short step in
+  /// the steepest direction. This is good if the current iterate is far from the
+  /// solution. If mu is small, LM approximates gauss newton iteration and we
+  /// have (almost) quadratic convergence in the final stages.
+  double                mu_init_, mu_;
+  double                nu_init_, nu_;          //!< Increase factor of mu after fail
+  size_t                n_iter_init_, n_iter_;  //!< Number of Iterations
+  size_t                n_trials_;              //!< Number of trials
+  size_t                n_trials_max_;          //!< Max number of trials
+  size_t                n_meas_;                //!< Number of measurements
+  bool                  stop_;                  //!< Stop flag
+  bool                  verbose_;               //!< Output Statistics
+  double                eps_;                   //!< Stop if update norm is smaller than eps
+  size_t                iter_;                  //!< Current Iteration
+
+  // robust least squares
+  bool                  use_weights_;
+  float                 scale_;
+  robust_cost::ScaleEstimatorPtr scale_estimator_;
+  robust_cost::WeightFunctionPtr weight_function_;
+
+  NLLSSolver() :
+    have_prior_(false),
+    method_(LevenbergMarquardt),
+    mu_init_(0.01f),
+    mu_(mu_init_),
+    nu_init_(2.0),
+    nu_(nu_init_),
+    n_iter_init_(15),
+    n_iter_(n_iter_init_),
+    n_trials_(0),
+    n_trials_max_(5),
+    n_meas_(0),
+    stop_(false),
+    verbose_(true),
+    eps_(0.0000000001),
+    iter_(0),
+    use_weights_(false),
+    scale_(0.0),
+    scale_estimator_(NULL),
+    weight_function_(NULL)
+  { }
+
+  virtual ~NLLSSolver() {}
+
+  /// Calls the GaussNewton or LevenbergMarquardt optimization strategy
+  void optimize(ModelType& model);
+
+  /// Gauss Newton optimization strategy
+  void optimizeGaussNewton();
+
+  /// Levenberg Marquardt optimization strategy
+  void optimizeLevenbergMarquardt(ModelType& model);
+
+  /// Specify the robust cost that should be used and the appropriate scale estimator
+  void setRobustCostFunction(
+      ScaleEstimatorType scale_estimator,
+      WeightFunctionType weight_function);
+
+  /// Add prior to optimization.
+  void setPrior(
+      const ModelType&  prior,
+      const Matrix<double, D, D>&  Information);
+
+  /// Reset all parameters to restart the optimization
+  void reset();
+
+  /// Get the squared error
+  const double& getChi2() const;
+
+  /// The Information matrix is equal to the inverse covariance matrix.
+  const Matrix<double, D, D>& getInformationMatrix() const;
+};
+
+} // end namespace vk
+
+#include "vio/nlls_solver_impl.hpp"
+
+#endif /* LM_SOLVER_H_ */

GPU_version/vio/include/vio/nlls_solver_impl.hpp

@@ -0,0 +1,316 @@
+/*
+ * Abstract Nonlinear Least-Squares Solver Class
+ *
+ * nlls_solver.h
+ *
+ *  Created on: Nov 5, 2012
+ *      Author: cforster
+ */
+
+#ifndef LM_SOLVER_IMPL_HPP_
+#define LM_SOLVER_IMPL_HPP_
+
+#include <stdexcept>
+
+template <int D, typename T>
+void vk::NLLSSolver<D, T>::optimize(ModelType& model)
+{
+  if(method_ == GaussNewton)
+    optimizeGaussNewton();
+  else if(method_ == LevenbergMarquardt)
+    optimizeLevenbergMarquardt(model);
+}
+
+template <int D, typename T>
+void vk::NLLSSolver<D, T>::optimizeGaussNewton()
+{
+  // Compute weight scale
+  if(use_weights_)
+    computeResiduals(false, true);
+  // perform iterative estimation
+  for (iter_ = 0; iter_<n_iter_; ++iter_)
+  {
+    rho_ = 0;
+    n_meas_ = 0;
+    double new_chi2 = computeResiduals(true, false);
+    // solve the linear system
+    if(!solve())
+    {
+      // matrix was singular and could not be computed
+      std::cout << "Matrix is close to singular! Stop Optimizing." << std::endl;
+      stop_ = true;
+    }
+    // check if error increased since last optimization
+    if((iter_ > 0 && new_chi2 > chi2_) || stop_)
+    {
+      if(verbose_)
+      {
+        std::cout << "It. " << iter_
+                  << "\t Failure"
+                  << "\t new_chi2 = " << new_chi2
+                  << "\t Error increased. Stop optimizing.\n";
+      }
+      break;
+    }
+    // update the model
+    update();
+    chi2_ = new_chi2;
+    if(verbose_)
+    {
+      std::cout << "It. " << iter_
+                << "\t Success"
+                << "\t new_chi2 = " << new_chi2
+                << "\t n_meas = " << n_meas_
+                << "\t x_norm = " << vk::norm_max(x_)<<'\n';
+    }
+    // stop when converged, i.e. update step too small
+    if(vk::norm_max(x_)<=eps_)
+      break;
+  }
+}
+
+template <int D, typename T>
+void vk::NLLSSolver<D, T>::optimizeLevenbergMarquardt(ModelType& model)
+{
+  // Compute weight scale
+  if(use_weights_)
+    computeResiduals(model, false, true);
+
+  // compute the initial error
+  chi2_ = computeResiduals(model, true, false);
+
+  if(verbose_)
+    cout << "init chi2 = " << chi2_
+         << "\t n_meas = " << n_meas_
+         << endl;
+
+  // TODO: compute initial lambda
+  // Hartley and Zisserman: "A typical init value of lambda is 10^-3 times the
+  // average of the diagonal elements of J'J"
+
+  // Compute Initial Lambda
+  if(mu_ < 0)
+  {
+    double H_max_diag = 0;
+    double tau = 1e-4;
+    for(size_t j=0; j<D; ++j)
+      H_max_diag = max(H_max_diag, fabs(H_(j,j)));
+    mu_ = tau*H_max_diag;
+  }
+
+  // perform iterative estimation
+  for (iter_ = 0; iter_<n_iter_; ++iter_)
+  {
+    rho_ = 0;
+    startIteration();
+    bool sign=false;
+    // try to compute and update, if it fails, try with increased mu
+    n_trials_ = 0;
+    do
+    {
+      // init variables
+      ModelType new_model;
+      double new_chi2 = -1;
+      H_.setZero();
+      //H_ = mu_ * Matrix<double,D,D>::Identity(D,D);
+      Jres_.setZero();
+
+      // compute initial error
+      n_meas_ = 0;
+      computeResiduals(model, true, false);
+
+      // add damping term:
+      H_ += (H_.diagonal()*mu_).asDiagonal();
+
+      // add prior
+      if(have_prior_)
+        applyPrior(model);
+
+      // solve the linear system
+      if(solve())
+      {
+        // update the model
+        update(model, new_model);
+
+        // compute error with new model and compare to old error
+        n_meas_ = 0;
+        new_chi2 = computeResiduals(new_model, true, false);
+        rho_ = chi2_-new_chi2;
+      }
+      else
+      {
+        // matrix was singular and could not be computed
+        cout << "Matrix is close to singular!" << endl;
+        cout << "H = " << H_ << endl;
+        cout << "Jres = " << Jres_ << endl;
+        rho_ = -1;
+      }
+
+      if(rho_>0)
+      {
+        // update decrased the error -> success
+        model = new_model;
+        chi2_ = new_chi2;
+        stop_ = vk::norm_max(x_)<=eps_ ? true : false;
+        mu_ *= max(1./3., min(1.-pow(2*rho_-1,3), 2./3.));
+        nu_ = 2.;
+        if(verbose_)
+        {
+          cout << "It. " << iter_
+               << "\t Trial " << n_trials_
+               << "\t Success"
+               << "\t n_meas = " << n_meas_
+               << "\t new_chi2 = " << new_chi2
+               << "\t mu = " << mu_
+               << "\t nu = " << nu_
+               << "\t x_norm = " << vk::norm_max(x_)
+               << endl;
+        }
+      }
+      else
+      {
+        if(sign){
+            if(verbose_)
+            {
+                cout << "It. " << iter_
+                     << "\t Trial " << n_trials_
+                     << "\t Failure"
+                     << "\t n_meas = " << n_meas_
+                     << "\t new_chi2 = " << new_chi2
+                     << "\t mu = " << mu_
+                     << "\t nu = " << nu_
+                     << "\t x_norm = " << vk::norm_max(x_)
+                     << endl;
+            }
+            mu_ /= nu_;
+            nu_ /= 2.;
+            ++n_trials_;
+            if (n_trials_ >= n_trials_max_)
+                stop_ = true;
+            sign=false;
+        }else{
+            if(verbose_)
+            {
+                cout << "It. " << iter_
+                     << "\t Trial " << n_trials_
+                     << "\t Failure"
+                     << "\t n_meas = " << n_meas_
+                     << "\t new_chi2 = " << new_chi2
+                     << "\t mu = " << mu_
+                     << "\t nu = " << nu_
+                     << "\t x_norm = " << vk::norm_max(x_)
+                     << endl;
+            }
+            mu_ *= nu_;
+            nu_ *= 2.;
+            ++n_trials_;
+            if (n_trials_ >= n_trials_max_)
+                stop_ = true;
+            sign=true;
+        }
+
+      }
+
+      finishTrial();
+
+    } while(!(rho_>0 || stop_));
+    if (stop_)
+      break;
+
+    finishIteration();
+  }
+}
+
+
+template <int D, typename T>
+void vk::NLLSSolver<D, T>::setRobustCostFunction(
+    ScaleEstimatorType scale_estimator,
+    WeightFunctionType weight_function)
+{
+  switch(scale_estimator)
+  {
+    case TDistScale:
+      if(verbose_)
+        printf("Using TDistribution Scale Estimator\n");
+      scale_estimator_.reset(new robust_cost::TDistributionScaleEstimator());
+      use_weights_=true;
+      break;
+    case MADScale:
+      if(verbose_)
+        printf("Using MAD Scale Estimator\n");
+      scale_estimator_.reset(new robust_cost::MADScaleEstimator());
+      use_weights_=true;
+    break;
+    case NormalScale:
+      if(verbose_)
+        printf("Using Normal Scale Estimator\n");
+      scale_estimator_.reset(new robust_cost::NormalDistributionScaleEstimator());
+      use_weights_=true;
+      break;
+    default:
+      if(verbose_)
+        printf("Using Unit Scale Estimator\n");
+      scale_estimator_.reset(new robust_cost::UnitScaleEstimator());
+      use_weights_=false;
+  }
+
+  switch(weight_function)
+  {
+    case TDistWeight:
+      if(verbose_)
+        printf("Using TDistribution Weight Function\n");
+      weight_function_.reset(new robust_cost::TDistributionWeightFunction());
+      break;
+    case TukeyWeight:
+      if(verbose_)
+        printf("Using Tukey Weight Function\n");
+      weight_function_.reset(new robust_cost::TukeyWeightFunction());
+      break;
+    case HuberWeight:
+      if(verbose_)
+        printf("Using Huber Weight Function\n");
+      weight_function_.reset(new robust_cost::HuberWeightFunction());
+      break;
+    default:
+      if(verbose_)
+        printf("Using Unit Weight Function\n");
+      weight_function_.reset(new robust_cost::UnitWeightFunction());
+  }
+}
+
+template <int D, typename T>
+void vk::NLLSSolver<D, T>::setPrior(
+    const T&  prior,
+    const Matrix<double, D, D>&  Information)
+{
+  have_prior_ = true;
+  prior_ = prior;
+  I_prior_ = Information;
+}
+
+template <int D, typename T>
+void vk::NLLSSolver<D, T>::reset()
+{
+  have_prior_ = false;
+  chi2_ = 1e10;
+  mu_ = mu_init_;
+  nu_ = nu_init_;
+  n_meas_ = 0;
+  n_iter_ = n_iter_init_;
+  iter_ = 0;
+  stop_ = false;
+}
+
+template <int D, typename T>
+inline const double& vk::NLLSSolver<D, T>::getChi2() const
+{
+  return chi2_;
+}
+
+template <int D, typename T>
+inline const vk::Matrix<double, D, D>& vk::NLLSSolver<D, T>::getInformationMatrix() const
+{
+  return H_;
+}
+
+#endif /* LM_SOLVER_IMPL_HPP_ */

GPU_version/vio/include/vio/output_helper.h

@@ -0,0 +1,90 @@
+/*
+ * output_helper.h
+ *
+ *  Created on: Jan 20, 2013
+ *      Author: cforster
+ */
+
+#ifndef VIKIT_OUTPUT_HELPER_H_
+#define VIKIT_OUTPUT_HELPER_H_
+
+#include <string>
+#include <ros/ros.h>
+#include <Eigen/Core>
+#include <sophus/se3.h>
+#include <tf/transform_broadcaster.h>
+
+using namespace std;
+using namespace Eigen;
+
+void
+publishTfTransform      (const Sophus::SE3& T, const ros::Time& stamp,
+                         const string& frame_id, const string& child_frame_id,
+                         tf::TransformBroadcaster& br);
+
+void
+publishPointMarker      (ros::Publisher pub,
+                         const Vector3d& pos,
+                         const string& ns,
+                         const ros::Time& timestamp,
+                         int id,
+                         int action,
+                         double marker_scale,
+                         const Vector3d& color,
+                         ros::Duration lifetime = ros::Duration(0.0));
+
+void
+publishLineMarker       (ros::Publisher pub,
+                         const Vector3d& start,
+                         const Vector3d& end,
+                         const string& ns,
+                         const ros::Time& timestamp,
+                         int id,
+                         int action,
+                         double marker_scale,
+                         const Vector3d& color,
+                         ros::Duration lifetime = ros::Duration(0.0));
+
+void
+publishArrowMarker      (ros::Publisher pub,
+                         const Vector3d& pos,
+                         const Vector3d& dir,
+                         double scale,
+                         const string& ns,
+                         const ros::Time& timestamp,
+                         int id,
+                         int action,
+                         double marker_scale,
+                         const Vector3d& color);
+
+void
+publishHexacopterMarker (ros::Publisher pub,
+                         const string& frame_id,
+                         const string& ns,
+                         const ros::Time& timestamp,
+                         int id,
+                         int action,
+                         double marker_scale,
+                         const Vector3d& color);
+
+void
+publishCameraMarker(ros::Publisher pub,
+                    const string& frame_id,
+                    const string& ns,
+                    const ros::Time& timestamp,
+                    int id,
+                    double marker_scale,
+                    const Vector3d& color);
+void
+publishFrameMarker     (ros::Publisher pub,
+                        const Matrix3d& rot,
+                        const Vector3d& pos,
+                        const string& ns,
+                        const ros::Time& timestamp,
+                        int id,
+                        int action,
+                        double marker_scale,
+                        ros::Duration lifetime = ros::Duration(0.0));
+
+
+#endif /* VIKIT_OUTPUT_HELPER_H_ */

GPU_version/vio/include/vio/params_helper.h

@@ -0,0 +1,57 @@
+//
+// Created by root on 5/24/21.
+//
+
+#ifndef VIO_PARAMS_HELPER_H
+#define VIO_PARAMS_HELPER_H
+/*
+ * ros_params_helper.h
+ *
+ *  Created on: Feb 22, 2013
+ *      Author: cforster
+ *
+ * from libpointmatcher_ros
+ */
+
+#include <string>
+#include <ros/ros.h>
+
+namespace vk {
+
+    inline
+    bool hasParam(const std::string& name)
+    {
+        return ros::param::has(name);
+    }
+
+    template<typename T>
+    T getParam(const std::string& name, const T& defaultValue)
+    {
+        T v;
+        if(ros::param::get(name, v))
+        {
+            ROS_INFO_STREAM("Found parameter: " << name << ", value: " << v);
+            return v;
+        }
+        else
+            ROS_WARN_STREAM("Cannot find value for parameter: " << name << ", assigning default: " << defaultValue);
+        return defaultValue;
+    }
+
+    template<typename T>
+    T getParam(const std::string& name)
+    {
+        T v;
+        if(ros::param::get(name, v))
+        {
+            ROS_INFO_STREAM("Found parameter: " << name << ", value: " << v);
+            return v;
+        }
+        else
+            ROS_ERROR_STREAM("Cannot find value for parameter: " << name);
+        return T();
+    }
+
+} // namespace vk
+
+#endif //VIO_PARAMS_HELPER_H

GPU_version/vio/include/vio/patch_score.h

@@ -0,0 +1,225 @@
+/*
+ * patch_score.h
+ *
+ *  Created on: Dec 5, 2013
+ *      Author: cforster
+ */
+
+#ifndef VIKIT_PATCH_SCORE_H_
+#define VIKIT_PATCH_SCORE_H_
+
+#include <stdint.h>
+
+#if __SSE2__
+#include <tmmintrin.h>
+#endif
+
+namespace vk {
+namespace patch_score {
+
+
+#if __SSE2__
+// Horizontal sum of uint16s stored in an XMM register
+inline int SumXMM_16(__m128i &target)
+{
+  unsigned short int sums_store[8];
+  _mm_storeu_si128((__m128i*)sums_store, target);
+  return sums_store[0] + sums_store[1] + sums_store[2] + sums_store[3] +
+    sums_store[4] + sums_store[5] + sums_store[6] + sums_store[7];
+}
+// Horizontal sum of uint32s stored in an XMM register
+inline int SumXMM_32(__m128i &target)
+{
+  unsigned int sums_store[4];
+  _mm_storeu_si128((__m128i*)sums_store, target);
+  return sums_store[0] + sums_store[1] + sums_store[2] + sums_store[3];
+}
+#endif
+
+/// Zero Mean Sum of Squared Differences Cost
+template<int HALF_PATCH_SIZE>
+class ZMSSD {
+public:
+
+  static const int patch_size_ = 2*HALF_PATCH_SIZE;
+  static const int patch_area_ = patch_size_*patch_size_;
+  static const int threshold_  = 2000*patch_area_;
+  uint8_t* ref_patch_;
+  int sumA_, sumAA_;
+
+  ZMSSD(uint8_t* ref_patch) :
+    ref_patch_(ref_patch)
+  {
+    uint32_t sumA_uint=0, sumAA_uint=0;
+    for(int r = 0; r < patch_area_; r++)
+    {
+      uint8_t n = ref_patch_[r];
+      sumA_uint += n;
+      sumAA_uint += n*n;
+    }
+    sumA_ = sumA_uint;
+    sumAA_ = sumAA_uint;
+  }
+
+  static int threshold() { return threshold_; }
+
+  int computeScore(uint8_t* cur_patch) const
+  {
+    uint32_t sumB_uint = 0;
+    uint32_t sumBB_uint = 0;
+    uint32_t sumAB_uint = 0;
+    for(int r = 0; r < patch_area_; r++)
+    {
+      const uint8_t cur_pixel = cur_patch[r];
+      sumB_uint  += cur_pixel;
+      sumBB_uint += cur_pixel*cur_pixel;
+      sumAB_uint += cur_pixel * ref_patch_[r];
+    }
+    const int sumB = sumB_uint;
+    const int sumBB = sumBB_uint;
+    const int sumAB = sumAB_uint;
+    return sumAA_ - 2*sumAB + sumBB - (sumA_*sumA_ - 2*sumA_*sumB + sumB*sumB)/patch_area_;
+  }
+
+  int computeScore(uint8_t* cur_patch, int stride) const
+  {
+    int sumB, sumBB, sumAB;
+#if __SSE2__
+    if(patch_size_ == 8)
+    {
+      // From PTAM-GPL, Copyright 2008 Isis Innovation Limited
+      __m128i xImageAsEightBytes;
+      __m128i xImageAsWords;
+      __m128i xTemplateAsEightBytes;
+      __m128i xTemplateAsWords;
+      __m128i xZero;
+      __m128i xImageSums;   // These sums are 8xuint16
+      __m128i xImageSqSums; // These sums are 4xint32
+      __m128i xCrossSums;   // These sums are 4xint32
+      __m128i xProduct;
+
+      xImageSums = _mm_setzero_si128();
+      xImageSqSums = _mm_setzero_si128();
+      xCrossSums = _mm_setzero_si128();
+      xZero = _mm_setzero_si128();
+
+      uint8_t* imagepointer = cur_patch;
+      uint8_t* templatepointer = ref_patch_;
+      long unsigned int cur_stride = stride;
+
+      xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer);
+      imagepointer += cur_stride;
+      xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero);
+      xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums);
+      xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords);
+      xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums);
+      xTemplateAsEightBytes=_mm_load_si128((__m128i*) templatepointer);
+      templatepointer += 16;
+      xTemplateAsWords = _mm_unpacklo_epi8(xTemplateAsEightBytes,xZero);
+      xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords);
+      xCrossSums = _mm_add_epi32(xProduct, xCrossSums);
+      xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer);
+      imagepointer += cur_stride;
+      xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero);
+      xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums);
+      xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords);
+      xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums);
+      xTemplateAsWords = _mm_unpackhi_epi8(xTemplateAsEightBytes,xZero);
+      xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords);
+      xCrossSums = _mm_add_epi32(xProduct, xCrossSums);
+
+      xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer);
+      imagepointer += cur_stride;
+      xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero);
+      xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums);
+      xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords);
+      xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums);
+      xTemplateAsEightBytes=_mm_load_si128((__m128i*) templatepointer);
+      templatepointer += 16;
+      xTemplateAsWords = _mm_unpacklo_epi8(xTemplateAsEightBytes,xZero);
+      xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords);
+      xCrossSums = _mm_add_epi32(xProduct, xCrossSums);
+      xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer);
+      imagepointer += cur_stride;
+      xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero);
+      xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums);
+      xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords);
+      xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums);
+      xTemplateAsWords = _mm_unpackhi_epi8(xTemplateAsEightBytes,xZero);
+      xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords);
+      xCrossSums = _mm_add_epi32(xProduct, xCrossSums);
+
+      xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer);
+      imagepointer += cur_stride;
+      xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero);
+      xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums);
+      xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords);
+      xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums);
+      xTemplateAsEightBytes=_mm_load_si128((__m128i*) templatepointer);
+      templatepointer += 16;
+      xTemplateAsWords = _mm_unpacklo_epi8(xTemplateAsEightBytes,xZero);
+      xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords);
+      xCrossSums = _mm_add_epi32(xProduct, xCrossSums);
+      xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer);
+      imagepointer += cur_stride;
+      xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero);
+      xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums);
+      xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords);
+      xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums);
+      xTemplateAsWords = _mm_unpackhi_epi8(xTemplateAsEightBytes,xZero);
+      xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords);
+      xCrossSums = _mm_add_epi32(xProduct, xCrossSums);
+
+      xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer);
+      imagepointer += cur_stride;
+      xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero);
+      xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums);
+      xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords);
+      xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums);
+      xTemplateAsEightBytes=_mm_load_si128((__m128i*) templatepointer);
+      templatepointer += 16;
+      xTemplateAsWords = _mm_unpacklo_epi8(xTemplateAsEightBytes,xZero);
+      xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords);
+      xCrossSums = _mm_add_epi32(xProduct, xCrossSums);
+      xImageAsEightBytes=_mm_loadl_epi64((__m128i*) imagepointer);
+      xImageAsWords = _mm_unpacklo_epi8(xImageAsEightBytes,xZero);
+      xImageSums = _mm_adds_epu16(xImageAsWords,xImageSums);
+      xProduct = _mm_madd_epi16(xImageAsWords, xImageAsWords);
+      xImageSqSums = _mm_add_epi32(xProduct, xImageSqSums);
+      xTemplateAsWords = _mm_unpackhi_epi8(xTemplateAsEightBytes,xZero);
+      xProduct = _mm_madd_epi16(xImageAsWords, xTemplateAsWords);
+      xCrossSums = _mm_add_epi32(xProduct, xCrossSums);
+
+      sumB = SumXMM_16(xImageSums);
+      sumAB = SumXMM_32(xCrossSums);
+      sumBB = SumXMM_32(xImageSqSums);
+    }
+    else
+#endif
+    {
+      uint32_t sumB_uint = 0;
+      uint32_t sumBB_uint = 0;
+      uint32_t sumAB_uint = 0;
+      for(int y=0, r=0; y < patch_size_; ++y)
+      {
+        uint8_t* cur_patch_ptr = cur_patch + y*stride;
+        for(int x=0; x < patch_size_; ++x, ++r)
+        {
+          const uint8_t cur_px = cur_patch_ptr[x];
+          sumB_uint  += cur_px;
+          sumBB_uint += cur_px * cur_px;
+          sumAB_uint += cur_px * ref_patch_[r];
+        }
+      }
+      sumB = sumB_uint;
+      sumBB = sumBB_uint;
+      sumAB = sumAB_uint;
+    }
+    return sumAA_ - 2*sumAB + sumBB - (sumA_*sumA_ - 2*sumA_*sumB + sumB*sumB)/patch_area_;
+  }
+};
+
+} // namespace patch_score
+} // namespace vk
+
+#endif // VIKIT_PATCH_SCORE_H_

GPU_version/vio/include/vio/point.h

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

GPU_version/vio/include/vio/pose_optimizer.h

@@ -0,0 +1,50 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_POSE_OPTIMIZER_H_
+#define SVO_POSE_OPTIMIZER_H_
+
+#include <vio/global.h>
+
+namespace vio {
+
+using namespace Eigen;
+using namespace Sophus;
+using namespace std;
+
+typedef Matrix<double,6,6> Matrix6d;
+typedef Matrix<double,2,6> Matrix26d;
+typedef Matrix<double,6,1> Vector6d;
+
+class Point;
+
+/// Motion-only bundle adjustment. Minimize the reprojection error of a single frame.
+namespace pose_optimizer {
+
+void optimizeGaussNewton(
+    const double reproj_thresh,
+    const size_t n_iter,
+    const bool verbose,
+    FramePtr& frame,
+    double& estimated_scale,
+    double& error_init,
+    double& error_final,
+    size_t& num_obs);
+
+} // namespace pose_optimizer
+} // namespace vio
+
+#endif // SVO_POSE_OPTIMIZER_H_

GPU_version/vio/include/vio/reprojector.h

@@ -0,0 +1,101 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_REPROJECTION_H_
+#define SVO_REPROJECTION_H_
+
+#include <vio/global.h>
+#include <vio/matcher.h>
+
+namespace vk {
+class AbstractCamera;
+}
+
+namespace vio {
+
+class Map;
+class Point;
+
+/// Project points from the map into the image and find the corresponding
+/// feature (corner). We don't search a match for every point but only for one
+/// point per cell. Thereby, we achieve a homogeneously distributed set of
+/// matched features and at the same time we can save processing time by not
+/// projecting all points.
+class Reprojector
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /// Reprojector config parameters
+  struct Options {
+    size_t max_n_kfs;   //!< max number of keyframes to reproject from
+    bool find_match_direct;
+    Options()
+    : max_n_kfs(10),
+      find_match_direct(true)
+    {}
+  } options_;
+
+  size_t n_matches_;
+  size_t n_trials_;
+
+  Reprojector(vk::AbstractCamera* cam, Map& map);
+
+  ~Reprojector();
+
+  /// Project points from the map into the image. First finds keyframes with
+  /// overlapping field of view and projects only those map-points.
+  void reprojectMap(
+      FramePtr frame,
+      std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs);
+
+private:
+
+  /// A candidate is a point that projects into the image plane and for which we
+  /// will search a maching feature in the image.
+  struct Candidate {
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    Point* pt;       //!< 3D point.
+    Vector2d px;     //!< projected 2D pixel location.
+    Candidate(Point* pt, Vector2d& px) : pt(pt), px(px) {}
+  };
+  typedef std::list<Candidate > Cell;
+  typedef std::vector<Cell*> CandidateGrid;
+
+  /// The grid stores a set of candidate matches. For every grid cell we try to find one match.
+  struct Grid
+  {
+    CandidateGrid cells;
+    vector<int> cell_order;
+    int cell_size;
+    int grid_n_cols;
+    int grid_n_rows;
+  };
+
+  Grid grid_;
+  Matcher matcher_;
+  Map& map_;
+
+  static bool pointQualityComparator(Candidate& lhs, Candidate& rhs);
+  void initializeGrid(vk::AbstractCamera* cam);
+  void resetGrid();
+  bool reprojectCell(Cell& cell, FramePtr frame);
+  bool reprojectPoint(FramePtr frame, Point* point);
+};
+
+} // namespace vio
+
+#endif // SVO_REPROJECTION_H_

GPU_version/vio/include/vio/robust_cost.h

@@ -0,0 +1,156 @@
+/**
+* This file is part of dvo.
+*
+* Copyright 2012 Christian Kerl <[email protected]> (Technical University of Munich)
+* For more information see <http://vision.in.tum.de/data/software/dvo>.
+*
+* dvo is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* dvo is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with dvo. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef VIKIT_ROBUST_COST_H_
+#define VIKIT_ROBUST_COST_H_
+
+#include <vector>
+#include <stdlib.h>
+#include <memory>
+namespace vk {
+namespace robust_cost {
+
+// interface for scale estimators
+class ScaleEstimator
+{
+public:
+  virtual ~ScaleEstimator() {};
+  virtual float compute(std::vector<float>& errors) const = 0;
+};
+typedef std::shared_ptr<ScaleEstimator> ScaleEstimatorPtr;
+
+class UnitScaleEstimator : public ScaleEstimator
+{
+public:
+  UnitScaleEstimator() {}
+  virtual ~UnitScaleEstimator() {}
+  virtual float compute(std::vector<float>& errors) const { return 1.0f; };
+};
+
+// estimates scale by fitting a t-distribution to the data with the given degrees of freedom
+class TDistributionScaleEstimator : public ScaleEstimator
+{
+public:
+  TDistributionScaleEstimator(const float dof = DEFAULT_DOF);
+  virtual ~TDistributionScaleEstimator() {};
+  virtual float compute(std::vector<float>& errors) const;
+
+  static const float DEFAULT_DOF;
+  static const float INITIAL_SIGMA;
+protected:
+  float dof_;
+  float initial_sigma_;
+};
+
+// estimates scale by computing the median absolute deviation
+class MADScaleEstimator : public ScaleEstimator
+{
+public:
+  MADScaleEstimator() {};
+  virtual ~MADScaleEstimator() {};
+  virtual float compute(std::vector<float>& errors) const;
+
+private:
+  static const float NORMALIZER;;
+};
+
+// estimates scale by computing the standard deviation
+class NormalDistributionScaleEstimator : public ScaleEstimator
+{
+public:
+  NormalDistributionScaleEstimator() {};
+  virtual ~NormalDistributionScaleEstimator() {};
+  virtual float compute(std::vector<float>& errors) const;
+private:
+};
+
+/**
+ * Interface for weight functions. A weight function is the first derivative of a symmetric robust function p(sqrt(t)).
+ * The errors are assumed to be normalized to unit variance.
+ *
+ * See:
+ *   "Lucas-Kanade 20 Years On: A Unifying Framework: Part 2" - Page 23, Equation (54)
+ */
+class WeightFunction
+{
+public:
+  virtual ~WeightFunction() {};
+  virtual float value(const float& x) const = 0;
+  virtual void configure(const float& param) {};
+};
+typedef std::shared_ptr<WeightFunction> WeightFunctionPtr;
+
+class UnitWeightFunction : public WeightFunction
+{
+public:
+  UnitWeightFunction() {};
+  virtual ~UnitWeightFunction() {};
+  virtual float value(const float& x) const { return 1.0f; };
+};
+
+/**
+ * Tukey's hard re-descending function.
+ *
+ * See:
+ *   http://en.wikipedia.org/wiki/Redescending_M-estimator
+ */
+class TukeyWeightFunction : public WeightFunction
+{
+public:
+  TukeyWeightFunction(const float b = DEFAULT_B);
+  virtual ~TukeyWeightFunction() {};
+  virtual float value(const float& x) const;
+  virtual void configure(const float& param);
+
+  static const float DEFAULT_B;
+private:
+  float b_square;
+};
+
+class TDistributionWeightFunction : public WeightFunction
+{
+public:
+  TDistributionWeightFunction(const float dof = DEFAULT_DOF);
+  virtual ~TDistributionWeightFunction() {};
+  virtual float value(const float& x) const;
+  virtual void configure(const float& param);
+
+  static const float DEFAULT_DOF;
+private:
+  float dof_;
+  float normalizer_;
+};
+
+class HuberWeightFunction : public WeightFunction
+{
+public:
+  HuberWeightFunction(const float k = DEFAULT_K);
+  virtual ~HuberWeightFunction() {};
+  virtual float value(const float& x) const;
+  virtual void configure(const float& param);
+
+  static const float DEFAULT_K;
+private:
+  float k;
+};
+
+} // namespace robust_cost
+} // namespace vk
+#endif // VIKIT_ROBUST_COST_H_

GPU_version/vio/include/vio/sparse_img_align.h

@@ -0,0 +1,85 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_SPARSE_IMG_ALIGN_H_
+#define SVO_SPARSE_IMG_ALIGN_H_
+
+#include <vio/nlls_solver.h>
+#include <vio/global.h>
+
+
+namespace vk {
+class AbstractCamera;
+}
+
+namespace vio {
+
+class Feature;
+
+/// Optimize the pose of the frame by minimizing the photometric error of feature patches.
+class SparseImgAlign : public vk::NLLSSolver<3, SE2>
+{
+  static const int patch_halfsize_ = 12;
+  static const int patch_size_ = 12*patch_halfsize_;
+  static const int patch_area_ = patch_size_*patch_size_;
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  cv::Mat resimg_;
+
+  SparseImgAlign(
+      int n_levels,
+      int min_level,
+      int n_iter,
+      Method method,
+      bool display,
+      bool verbose);
+
+  size_t run(
+      FramePtr ref_frame,
+      FramePtr cur_frame);
+
+  /// Return fisher information matrix, i.e. the Hessian of the log-likelihood
+  /// at the converged state.
+  Matrix<double, 3, 3> getFisherInformation();
+
+protected:
+  FramePtr ref_frame_;            //!< reference frame, has depth for gradient pixels.
+  FramePtr cur_frame_;            //!< only the image is known!
+  int level_;                     //!< current pyramid level on which the optimization runs.
+  bool display_;                  //!< display residual image.
+  int max_level_;                 //!< coarsest pyramid level for the alignment.
+  int min_level_;                 //!< finest pyramid level for the alignment.
+
+  // cache:
+  Matrix<double, 3, Dynamic, ColMajor> jacobian_cache_;
+  bool have_ref_patch_cache_;
+  cv::Mat ref_patch_cache_;
+  std::vector<bool> visible_fts_;
+
+  void precomputeReferencePatches();
+  virtual double computeResiduals(const SE2& model, bool linearize_system, bool compute_weight_scale = false);
+  virtual double computeResiduals(bool linearize_system, bool compute_weight_scale = false){};
+  virtual int solve();
+  virtual void update (const ModelType& old_model, ModelType& new_model);
+  virtual void update(){};
+  virtual void startIteration();
+  virtual void finishIteration();
+};
+
+} // namespace vio
+
+#endif // SVO_SPARSE_IMG_ALIGN_H_

GPU_version/vio/include/vio/sparse_img_align_gpu.h

@@ -0,0 +1,82 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef VIO_SPARSE_IMG_ALIGN_GPU_H_
+#define VIO_SPARSE_IMG_ALIGN_GPU_H_
+
+#include <vio/nlls_solver.h>
+#include <vio/global.h>
+#include <vio/cl_class.h>
+#include <vio/frame.h>
+#include <vio/feature.h>
+#include <vio/config.h>
+#include <vio/point.h>
+#include <vio/abstract_camera.h>
+#include <vio/vision.h>
+#include <vio/math_utils.h>
+#include <algorithm>
+
+
+namespace vio {
+
+class Feature;
+
+/// Optimize the pose of the frame by minimizing the photometric error of feature patches.
+class SparseImgAlignGpu : public vk::NLLSSolver<3, SE2>
+{
+  static const int patch_halfsize_ = 12;
+  static const int patch_size_ = 12*patch_halfsize_;
+  static const int patch_area_ = patch_size_*patch_size_;
+public:
+
+  SparseImgAlignGpu(
+      int n_levels,
+      int min_level,
+      int n_iter,
+      Method method,
+      bool verbose,
+      opencl* residual);
+
+  size_t run(
+      FramePtr ref_frame,
+      FramePtr cur_frame);
+
+
+protected:
+  int level_;                     //!< current pyramid level on which the optimization runs.
+  int max_level_;                 //!< coarsest pyramid level for the alignment.
+  int min_level_;                 //!< finest pyramid level for the alignment.
+  size_t feature_counter_=0;
+  opencl* residual_= nullptr;
+
+  // cache:
+  std::vector<bool> visible_fts_;
+  virtual double computeResiduals(const ModelType& model,
+                           bool linearize_system,
+                           bool compute_weight_scale) {
+        return 0.0;
+    };
+  virtual double computeResiduals(bool linearize_system, bool compute_weight_scale = false);
+  virtual int solve();
+  virtual void update();
+  virtual void update(const ModelType& old_model, ModelType& new_model) { };
+  virtual void startIteration();
+  virtual void finishIteration();
+};
+
+} // namespace vio
+
+#endif // VIO_SPARSE_IMG_ALIGN_GPU_H_

GPU_version/vio/include/vio/timer.h

@@ -0,0 +1,81 @@
+#ifndef TIMER_H
+#define TIMER_H
+
+#include <sys/time.h>
+#include <stdio.h>
+#include <unistd.h>
+
+namespace vk
+{
+
+class Timer
+{
+private:
+  timeval start_time_;
+  double time_;
+  double accumulated_;
+public:
+
+  /// The constructor directly starts the timer.
+  Timer() :
+    time_(0.0),
+    accumulated_(0.0)
+  {
+    start();
+  }
+
+  ~Timer()
+  {}
+
+  inline void start()
+  {
+    accumulated_ = 0.0;
+    gettimeofday(&start_time_, NULL);
+  }
+
+  inline void resume()
+  {
+    gettimeofday(&start_time_, NULL);
+  }
+
+  inline double stop()
+  {
+    timeval end_time;
+    gettimeofday(&end_time, NULL);
+    long seconds  = end_time.tv_sec  - start_time_.tv_sec;
+    long useconds = end_time.tv_usec - start_time_.tv_usec;
+    time_ = ((seconds) + useconds*0.000001) + accumulated_;
+    accumulated_ = time_;
+    return time_;
+  }
+
+  inline double getTime() const
+  {
+    return time_;
+  }
+
+  inline void reset()
+  {
+    time_ = 0.0;
+    accumulated_ = 0.0;
+  }
+
+  static double getCurrentTime()
+  {
+    timeval time_now;
+    gettimeofday(&time_now, NULL);
+    return time_now.tv_sec + time_now.tv_usec*0.000001;
+  }
+
+  static double getCurrentSecond()
+  {
+    timeval time_now;
+    gettimeofday(&time_now, NULL);
+    return time_now.tv_sec;
+  }
+
+};
+
+} // end namespace vk
+
+#endif

GPU_version/vio/include/vio/ukf.h

@@ -0,0 +1,155 @@
+//
+// Created by root on 6/8/21.
+//
+#ifndef VIO_UKF_H
+#define VIO_UKF_H
+
+#include <iostream>
+#include <fstream>
+#include "vio/config.h"
+#include <Eigen/Dense>
+class Base{
+public:
+    Base(const Eigen::Matrix<double, 3, 1> &init) {
+        state_=init;
+        cov_.setIdentity();
+        cov_*=0.0001;
+    };
+    virtual ~Base(){
+    };
+    void predict(double dx,double dz,double dpitch,double time){
+        if(t_==0.0){
+            t_=time;
+            return;
+        }
+        double dt=time-t_;
+        t_=time;
+        state_h_(2)=state_(2)+dt*dpitch;
+        state_h_(1)=state_(1)+dz*cos(state_(2))-dx*sin(state_(2));
+        state_h_(0)=state_(0)+dx*cos(state_(2))+dz*sin(state_(2));
+        Eigen::Matrix<double,3,3> R;
+        R<<vio::Config::Cmd_Cov(),0,0,
+           0,vio::Config::Cmd_Cov(),0,
+           0,0,vio::Config::Cmd_Cov();
+        Eigen::Matrix<double,3,3> G;
+        G<<1.0,0,-dx*sin(state_(2))+dz*cos(state_(2)),
+           0,1.0,-dz*sin(state_(2))-dx*cos(state_(2)),
+           0,0,1.0;
+        cov_h_=G*cov_*G.transpose()+R;
+        predict_up=true;
+    }
+    void correct(double ddx, double ddz, double dpitch, double time){
+        Eigen::Matrix<double,3,3> H;
+        Eigen::Matrix<double,3,3> Q;
+        Eigen::Matrix<double,3,1> E;
+        Eigen::Matrix<double,3,3> k;
+        double dt=time-t_;
+        t_=time;
+        H<<1.0,0,0.5*pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2))),
+           0,1.0,-0.5*pow(dt,2)*(ddx*cos(state_(2))+ddz*sin(state_(2))),
+           0,0,1.0;
+        Q<<vio::Config::ACC_Noise(),0,0,
+           0,vio::Config::ACC_Noise(),0,
+           0,0,vio::Config::GYO_Noise();
+        E(0)=state_(0)+0.5*pow(dt,2)*(ddx*cos(state_(2))+ddz*sin(state_(2)))-state_h_(0);
+        E(1)=state_(1)+0.5*pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2)))-state_h_(1);
+        E(2)=state_(2)+dt*dpitch-state_h_(2);
+        k=cov_h_*H.transpose()*(H*cov_h_*H.transpose()+Q).inverse();
+        state_=state_h_+k*E;
+        cov_=(Eigen::MatrixXd::Identity(3,3)-k*H)*cov_h_;
+        state_h_(2)=state_(2)+dt*dpitch;
+        state_h_(1)=state_(1)+0.5*pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2)));
+        state_h_(0)=state_(0)+0.5*pow(dt,2)*(ddx*cos(state_(2))+ddz*sin(state_(2)));
+        Eigen::Matrix<double,3,3> R;
+        R<<vio::Config::ACC_Noise(),0,0,
+                0,vio::Config::ACC_Noise(),0,
+                0,0,vio::Config::GYO_Noise();
+        Eigen::Matrix<double,3,3> G;
+        G<<1.0,0,0.5*pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2))),
+                0,1.0,-0.5*pow(dt,2)*(ddx*cos(state_(2))+ddz*sin(state_(2))),
+                0,0,1.0;
+        cov_h_=G*cov_*G.transpose()+R;
+        yaw_t_1_=state_(2);
+
+    }
+    void correct(double x, double z,double pitch,size_t match,double time){
+        Eigen::Matrix<double,3,3> H;
+        Eigen::Matrix<double,3,3> Q;
+        Eigen::Matrix<double,3,1> E;
+        Eigen::Matrix<double,3,3> k;
+        t_=time;
+        ++match;
+        H<<1.0,0,0,
+           0,1.0,0,
+           0,0,1.0;
+        Q<<vio::Config::Svo_Ekf(),0,0,
+           0,vio::Config::Svo_Ekf(),0,
+           0,0,vio::Config::Svo_Ekf();
+        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(3,3)-k*H)*cov_h_;
+        yaw_t_1_=state_(2);
+        predict_up=false;
+    }
+    Eigen::Matrix<double,3,3> cov_;
+    Eigen::Matrix<double, 3, 1> state_;
+    bool predict_up=false;
+private:
+    double t_=0.0;
+    Eigen::Matrix<double,3,3> cov_h_;
+    Eigen::Matrix<double, 3, 1> state_h_;//state{x,z,pitch}
+    double yaw_t_1_=0.0;
+
+};
+
+class UKF {
+public:
+    UKF(const Eigen::Matrix<double, 3, 1>& init) : filter_(new Base(init)) {
+    };
+    virtual ~UKF() {
+        delete filter_;
+    };
+    void UpdateIMU(double x,double z,double pitch,const ros::Time& time) {
+        while(lock)usleep(5);
+        lock=true;
+        if(abs(x)<1.0)x=pow(x,3);//picked up from your code
+        if(abs(z)<1.0)z=pow(z,3);//picked up from your code
+        if(filter_->predict_up)filter_->correct(x,z,pitch,1e-9*time.toNSec());
+        lock=false;
+    };
+    void UpdateCmd(double x,double z,double pitch,const ros::Time& time) {
+        while(lock)usleep(5);
+        lock=true;
+        filter_->predict(x,z,pitch,1e-9*time.toNSec());
+        lock=false;
+    };
+    std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> UpdateSvo(double x,double z,double pitch,size_t match,ros::Time& time) {
+        while(lock)usleep(5);
+        lock=true;
+        if(pitch<0.0)
+            pitch=M_PI-pitch;
+        else
+            pitch=pitch-M_PI;
+        if(filter_->predict_up)filter_->correct(-x,-z,pitch,match,1e-9*time.toNSec());
+        lock=false;
+        double pitch_f;
+        if(filter_->state_(0)<0.0)
+            pitch_f=M_PI-filter_->state_(2);
+        else
+            pitch_f=filter_->state_(2)-M_PI;
+        return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(0),-filter_->state_(1),pitch_f));
+    };
+    std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> get_location(){
+        double pitch_f;
+        if(filter_->state_(0)<0.0)
+            pitch_f=M_PI-filter_->state_(2);
+        else
+            pitch_f=filter_->state_(2)-M_PI;
+        return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(0),-filter_->state_(1),pitch_f));
+    }
+private:
+     Base* filter_= nullptr;
+     bool lock=false;
+};
+#endif //VIO_UKF_H

GPU_version/vio/include/vio/vision.h

@@ -0,0 +1,80 @@
+/*
+ * vision.h
+ *
+ *  Created on: May 14, 2013
+ *      Author: cforster
+ */
+
+#ifndef VIKIT_VISION_H_
+#define VIKIT_VISION_H_
+
+#include <vio/aligned_mem.h>
+#include <Eigen/Core>
+#include <opencv2/opencv.hpp>
+
+namespace vk
+{
+
+//! Return value between 0 and 1
+//! WARNING This function does not check whether the x/y is within the border
+inline float
+interpolateMat_32f(const cv::Mat& mat, float u, float v)
+{
+  assert(mat.type()==CV_32F);
+  float x = floor(u);
+  float y = floor(v);
+  float subpix_x = u-x;
+  float subpix_y = v-y;
+  float wx0 = 1.0-subpix_x;
+  float wx1 =  subpix_x;
+  float wy0 = 1.0-subpix_y;
+  float wy1 =  subpix_y;
+
+  float val00 = mat.at<float>(y,x);
+  float val10 = mat.at<float>(y,x+1);
+  float val01 = mat.at<float>(y+1,x);
+  float val11 = mat.at<float>(y+1,x+1);
+  return (wx0*wy0)*val00 + (wx1*wy0)*val10 + (wx0*wy1)*val01 + (wx1*wy1)*val11;
+}
+
+//! Return value between 0 and 255
+//! WARNING This function does not check whether the x/y is within the border
+inline float
+interpolateMat_8u(const cv::Mat& mat, float u, float v)
+{
+  if(mat.empty())return 0.0;
+  if(mat.type()!=CV_8U)return 0.0;
+  int x = floor(u);
+  int y = floor(v);
+  float subpix_x = u-x;
+  float subpix_y = v-y;
+
+  float w00 = (1.0f-subpix_x)*(1.0f-subpix_y);
+  float w01 = (1.0f-subpix_x)*subpix_y;
+  float w10 = subpix_x*(1.0f-subpix_y);
+  float w11 = 1.0f - w00 - w01 - w10;
+
+  const int stride = mat.step.p[0];
+  unsigned char* ptr = mat.data + y*stride + x;
+  if(isnan(w00) || isnan(w01 )||isnan( w10) || isnan(w11) || isinf(w00) || isinf(w01)|| isinf(w10)|| isinf(w11))return 0.0;
+  if(ptr[0] == NULL || ptr[1]== NULL || ptr[stride]==NULL ||ptr[stride+1]==NULL)return 0.0;
+  return w00*ptr[0] + w01*ptr[stride] + w10*ptr[1] + w11*ptr[stride+1];
+}
+
+void halfSample(const cv::Mat& in, cv::Mat& out);
+
+float shiTomasiScore(const cv::Mat& img, int u, int v);
+
+void calcSharrDeriv(const cv::Mat& src, cv::Mat& dst);
+
+#ifdef __SSE2__
+
+/// Used to convert a Kinect depthmap
+/// Code by Christian Kerl DVO, GPL Licence
+void convertRawDepthImageSse_16u_to_32f(cv::Mat& depth_16u, cv::Mat& depth_32f, float scale);
+
+#endif
+
+} // namespace vk
+
+#endif // VIKIT_VISION_H_

GPU_version/vio/include/vio/visualizer.h

@@ -0,0 +1,62 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_VISUALIZER_H_
+#define SVO_VISUALIZER_H_
+
+#include <queue>
+#include <ros/ros.h>
+#include <vio/global.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+#include <tf/transform_broadcaster.h>
+#include <image_transport/image_transport.h>
+#include <boost/thread.hpp>
+#include <boost/shared_ptr.hpp>
+#include <vio/ukf.h>
+
+namespace vio {
+
+class Frame;
+class Point;
+class Map;
+class FrameHandlerMono;
+
+typedef boost::shared_ptr<Frame> FramePtr;
+
+/// This class bundles all functions to publish visualisation messages.
+class Visualizer
+{
+public:
+  ros::NodeHandle pnh_;
+  size_t trace_id_;
+  ros::Publisher pub_frames_;
+  ros::Publisher pub_points_;
+  ros::Publisher pub_pose_with_cov_;
+
+  Visualizer();
+
+  ~Visualizer() {};
+
+  void publishMinimal(
+      UKF& ukf,
+      const double timestamp);
+
+};
+
+} // end namespace vio
+
+#endif /* SVO_VISUALIZER_H_ */

GPU_version/vio/kernel/compute-residual.cl

@@ -0,0 +1,50 @@
+// Copyright (C) 2021  Majid Geravand
+// Copyright (C) 2021  Gfuse
+
+// Enable OpenCL 32-bit integer atomic functions.
+#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable
+
+double2 world2cam(double f_x,double f_y,double c_x,double c_y,double s_, double x, double y,double z)
+{
+    double2 uv=(x/z,y/z);
+    double r = sqrt(pow(uv[0],2)+pow(uv[1],2));
+    double factor=1.0;
+    if(s_ !=0 || r > 0.001)factor=(atan(r * 2.0 * tan(0.5 * s_)) / (r*s_));
+    return double3(cx_ + fx_ * factor * uv[0],cy_ + fy_ * factor * uv[1]);
+}
+double3 xyz_cur(double cur_x,double cur_z,double cur_pitch,double ref_x,double ref_z,double ref_pitch,double3 ref_feature){
+    if(ref_pitch<0.0)
+        ref_pitch=3.141592653589793238462643383279502884197169399375-ref_pitch;
+    else
+        ref_pitch=ref_pitch-3.1415926535897932384626433832795028841971693993;
+    double cc_ss=cos(cur_pitch)*cos(ref_pitch)-sin(cur_pitch)*sin(ref_pitch);
+    double sc_cs=cos(cur_pitch)*sin(ref_pitch)+cos(ref_pitch)*sin(cur_pitch);
+    return  double3(cc_ss*ref_feature.x()+sc_cs*ref_feature.z()+cur_x-ref_x*cos(cur_pitch)-ref_z*sin(cur_pitch),
+                    ref_feature.y(),
+                    -1.0*sc_cs*ref_feature.x())+cc_ss*ref_feature.z()+cur_z+ref_x*sin(cur_pitch)-ref_z*cos(cur_pitch));
+}
+
+__kernel void compute-residual(
+    __read_only  image2d_t   image_cur, // current frame
+    __read_only  image2d_t   image_ref, // reference frame
+    __global     double      * cur_pose,//[reference frame pose{x,z,pitch}]
+    __global     double      * ref_pose,//[current frame pose{x,z,pitch}]
+    __global     double3     * ref_feature, // feature on the reference frame, when we applied the distance calculation: xyz_ref((*it)->f*((*it)->point->pos_ - Eigen::Vector3d(ref_pos[0],0.0,ref_pos[1])).norm());
+    __global     int         * level,
+    __global     double      * camera_model, // camera model [f_x,f_y,c_x,c_y,s_]
+                 float       * errors,
+                 double      * Hessian,
+                 double      * Jacobian,
+                 double      * chi
+) {
+    // Prepare a suitable OpenCL image sampler.
+    sampler_t const sampler = CLK_ADDRESS_CLAMP | CLK_FILTER_NEAREST;
+
+    // Use global work feature.
+    int  const f   = get_global_id(0);
+    ref_feature[f].x;
+    ref_feature[f].y;
+    ref_feature[f].z;
+    PATCH_SIZE;
+}
+

GPU_version/vio/src/atan_camera.cpp

@@ -7,8 +7,8 @@
 
 
 #include <math.h>
-#include <gpu_svo/atan_camera.h>
-#include <gpu_svo/math_utils.h>
+#include <vio/atan_camera.h>
+#include <vio/math_utils.h>
 
 namespace vk {
 

GPU_version/vio/src/cl_class.cpp

@@ -1,7 +1,7 @@
 //
 // Created by root on 4/27/21.
 //
-#include "gpu_svo/cl_class.h"
+#include "vio/cl_class.h"
 opencl::opencl() {
     std::vector<cl::Platform> all_platforms;
     cl::Platform::get(&all_platforms);
@@ -33,7 +33,7 @@ opencl::opencl() {
     read_cl fast(std::string(KERNEL_DIR)+"/fast-gray.cl");
     sources.push_back({ fast.src_str, fast.size });
     program=new cl::Program(*context, sources);
-    if(program->build({ device },"-DFAST_THRESH=40") !=0)
+    if(program->build({ device },"-DFAST_THRESH=40 -DPATCH_SIZE=4") !=0)
         std::cout << " Error building: " << program->getBuildInfo<CL_PROGRAM_BUILD_STATUS>(device)<<'\n'
         <<program->getBuildInfo<CL_PROGRAM_BUILD_LOG>(device) << '\n';
     queue=new cl::CommandQueue(*context,device,0,NULL);

GPU_version/vio/src/config.cpp

@@ -14,10 +14,10 @@
 // You should have received a copy of the GNU General Public License
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
-#include <gpu_svo/params_helper.h>
-#include <gpu_svo/config.h>
+#include <vio/params_helper.h>
+#include <vio/config.h>
 
-namespace svo {
+namespace vio {
 
 Config::Config() :
     trace_name(vk::getParam<string>("svo/trace_name", "svo")),
@@ -69,5 +69,5 @@ Config& Config::getInstance()
   return instance;
 }
 
-} // namespace svo
+} // namespace vio
 

GPU_version/vio/src/depth_filter.cpp

@@ -15,21 +15,21 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 #include <algorithm>
-#include <gpu_svo/math_utils.h>
-#include <gpu_svo/abstract_camera.h>
-#include <gpu_svo/vision.h>
+#include <vio/math_utils.h>
+#include <vio/abstract_camera.h>
+#include <vio/vision.h>
 #include <boost/bind.hpp>
 #include <boost/math/distributions/normal.hpp>
-#include <gpu_svo/global.h>
-#include <gpu_svo/depth_filter.h>
-#include <gpu_svo/frame.h>
-#include <gpu_svo/point.h>
-#include <gpu_svo/feature.h>
-#include <gpu_svo/matcher.h>
-#include <gpu_svo/config.h>
-#include <gpu_svo/feature_detection.h>
+#include <vio/global.h>
+#include <vio/depth_filter.h>
+#include <vio/frame.h>
+#include <vio/point.h>
+#include <vio/feature.h>
+#include <vio/matcher.h>
+#include <vio/config.h>
+#include <vio/feature_detection.h>
 
-namespace svo {
+namespace vio {
 
 int Seed::batch_counter = 0;
 int Seed::seed_counter = 0;
@@ -354,4 +354,4 @@ double DepthFilter::computeTau(
   return (t_norm*sin(beta_plus)/sin(PI-acos(f.dot(t)/t_norm)-beta_plus) - z); // ( triangle angles sum to PI and law of sines ) tau
 }
 
-} // namespace svo
+} // namespace vio

GPU_version/vio/src/feature_alignment.cpp

@@ -20,9 +20,9 @@
 #ifdef __ARM_NEON__
 #include <arm_neon.h>
 #endif
-#include <gpu_svo/feature_alignment.h>
+#include <vio/feature_alignment.h>
 
-namespace svo {
+namespace vio {
 namespace feature_alignment {
 
 #define SUBPIX_VERBOSE 0
@@ -613,4 +613,4 @@ bool align2D_NEON (
 }
 
 } // namespace feature_alignment
-} // namespace svo
+} // namespace vio

GPU_version/vio/src/feature_detection.cpp

@@ -14,11 +14,11 @@
 // You should have received a copy of the GNU General Public License
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
-#include <gpu_svo/feature_detection.h>
-#include <gpu_svo/feature.h>
-#include <gpu_svo/vision.h>
+#include <vio/feature_detection.h>
+#include <vio/feature.h>
+#include <vio/vision.h>
 
-namespace svo {
+namespace vio {
 namespace feature_detection {
 
 AbstractDetector::AbstractDetector(
@@ -110,5 +110,5 @@ void FastDetector::detect(
 }
 
 } // namespace feature_detection
-} // namespace svo
+} // namespace vio
 

GPU_version/vio/src/frame.cpp

@@ -15,14 +15,14 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 #include <stdexcept>
-#include <gpu_svo/frame.h>
-#include <gpu_svo/feature.h>
-#include <gpu_svo/point.h>
-#include <gpu_svo/config.h>
-#include <gpu_svo/math_utils.h>
-#include <gpu_svo/vision.h>
+#include <vio/frame.h>
+#include <vio/feature.h>
+#include <vio/point.h>
+#include <vio/config.h>
+#include <vio/math_utils.h>
+#include <vio/vision.h>
 
-namespace svo {
+namespace vio {
 
 int Frame::frame_counter_ = 0;
 
@@ -186,4 +186,4 @@ bool getSceneDepth(const Frame& frame, double& depth_mean, double& depth_min)
 }
 
 } // namespace frame_utils
-} // namespace svo
+} // namespace vio

GPU_version/vio/src/frame_handler_base.cpp

@@ -14,19 +14,19 @@
 // You should have received a copy of the GNU General Public License
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
-#include <gpu_svo/abstract_camera.h>
+#include <vio/abstract_camera.h>
 #include <stdlib.h>
 #include <Eigen/StdVector>
 #include <boost/bind.hpp>
 #include <fstream>
-#include <gpu_svo/frame_handler_base.h>
-#include <gpu_svo/config.h>
-#include <gpu_svo/feature.h>
-#include <gpu_svo/matcher.h>
-#include <gpu_svo/map.h>
-#include <gpu_svo/point.h>
+#include <vio/frame_handler_base.h>
+#include <vio/config.h>
+#include <vio/feature.h>
+#include <vio/matcher.h>
+#include <vio/map.h>
+#include <vio/point.h>
 
-namespace svo
+namespace vio
 {
 
 // definition of global and static variables which were declared in the header
@@ -137,4 +137,4 @@ void FrameHandlerBase::optimizeStructure(
 }
 
 
-} // namespace svo
+} // namespace vio

GPU_version/vio/src/frame_handler_mono.cpp

@@ -14,18 +14,18 @@
 // You should have received a copy of the GNU General Public License
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
-#include <gpu_svo/config.h>
-#include <gpu_svo/frame_handler_mono.h>
-#include <gpu_svo/map.h>
-#include <gpu_svo/frame.h>
-#include <gpu_svo/feature.h>
-#include <gpu_svo/point.h>
-#include <gpu_svo/pose_optimizer.h>
-#include <gpu_svo/sparse_img_align.h>
-#include <gpu_svo/depth_filter.h>
-#include <gpu_svo/for_it.hpp>
+#include <vio/config.h>
+#include <vio/frame_handler_mono.h>
+#include <vio/map.h>
+#include <vio/frame.h>
+#include <vio/feature.h>
+#include <vio/point.h>
+#include <vio/pose_optimizer.h>
+#include <vio/sparse_img_align.h>
+#include <vio/depth_filter.h>
+#include <vio/for_it.hpp>
 
-namespace svo {
+namespace vio {
 
 FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,3,1>& init) :
   FrameHandlerBase(),
@@ -39,11 +39,33 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,
     gpu_fast_->make_kernel("fast_gray");
     cv::Mat img=cv::Mat(cv::Size(700, 500),CV_8UC1);
     gpu_fast_->write_buf(0,0,img);
-    cl_int2 corners_[350000];
+    cl_int2 corners_[350000]={0};
     gpu_fast_->write_buf(0,1,350000,corners_);
     int icorner[1]={0};
     gpu_fast_->write_buf(0,2,1,icorner);
     gpu_fast_->write_buf(0,3,(1 << 18));
+    gpu_fast_->make_kernel("compute-residual");
+    gpu_fast_->write_buf(1,0,img);
+    gpu_fast_->write_buf(1,1,img);
+    double pose[3]={0};
+    gpu_fast_->write_buf(1,2,3,pose);
+    gpu_fast_->write_buf(1,3,3,pose);
+    cl_double3 f[300]={0};
+    gpu_fast_->write_buf(1,4,300,f);
+    int level=0;
+    gpu_fast_->write_buf(1,5,level);
+    double c[5]={0};
+    gpu_fast_->write_buf(1,6,5,c);
+    float  e[300]={0};
+    gpu_fast_->write_buf(1,7,300,e);
+    float H[6]={0};
+    gpu_fast_->write_buf(1,8,6,H);
+    float J[3]={0};
+    gpu_fast_->write_buf(1,9,3,J);
+    double chi2=0.0;
+    gpu_fast_->write_buf(1,10,chi2);
+    imageAlign_=new SparseImgAlignGpu(Config::kltMaxLevel(), Config::kltMinLevel(),
+                                      30, SparseImgAlign::GaussNewton, false,gpu_fast_);
     klt_homography_init_=new initialization::KltHomographyInit(gpu_fast_);
     initialize();
 }
@@ -143,6 +165,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),
                            30, SparseImgAlign::LevenbergMarquardt, false, false);
   if(img_align.run(last_frame_, new_frame_)==0)return  RESULT_FAILURE;
+  if(imageAlign_->run(last_frame_, new_frame_)==0)return  RESULT_FAILURE;
   reprojector_.reprojectMap(new_frame_, overlap_kfs_);
   size_t sfba_n_edges_final=0;
   double sfba_thresh, sfba_error_init, sfba_error_final;
@@ -299,4 +322,4 @@ void FrameHandlerMono::UpdateCmd(double* value,const ros::Time& time){
     ukfPtr_.UpdateCmd(value[0],value[1],value[2],time);
 }
 
-} // namespace svo
+} // namespace vio

GPU_version/vio/src/homography.cpp

@@ -12,7 +12,7 @@
  *      by: cforster
  */
 
-#include <gpu_svo/homography.h>
+#include <vio/homography.h>
 #include <opencv2/opencv.hpp>
 #include <stdio.h>
 

GPU_version/vio/src/initialization.cpp

@@ -14,16 +14,16 @@
 // You should have received a copy of the GNU General Public License
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
-#include <gpu_svo/config.h>
-#include <gpu_svo/frame.h>
-#include <gpu_svo/point.h>
-#include <gpu_svo/feature.h>
-#include <gpu_svo/initialization.h>
-#include <gpu_svo/feature_detection.h>
-#include <gpu_svo/math_utils.h>
-#include <gpu_svo/homography.h>
+#include <vio/config.h>
+#include <vio/frame.h>
+#include <vio/point.h>
+#include <vio/feature.h>
+#include <vio/initialization.h>
+#include <vio/feature_detection.h>
+#include <vio/math_utils.h>
+#include <vio/homography.h>
 
-namespace svo {
+namespace vio {
 namespace initialization {
 
 InitResult KltHomographyInit::addFirstFrame(FramePtr frame_ref)
@@ -197,4 +197,4 @@ void computeHomography(
 
 
 } // namespace initialization
-} // namespace svo
+} // namespace vio

GPU_version/vio/src/map.cpp

@@ -15,13 +15,13 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 #include <set>
-#include <gpu_svo/map.h>
-#include <gpu_svo/point.h>
-#include <gpu_svo/frame.h>
-#include <gpu_svo/feature.h>
+#include <vio/map.h>
+#include <vio/point.h>
+#include <vio/frame.h>
+#include <vio/feature.h>
 #include <boost/bind.hpp>
 
-namespace svo {
+namespace vio {
 
 Map::Map() {}
 
@@ -376,4 +376,4 @@ void mapStatistics(Map* map)
 }
 
 } // namespace map_debug
-} // namespace svo
+} // namespace vio

GPU_version/vio/src/matcher.cpp

@@ -15,18 +15,18 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 #include <cstdlib>
-#include <gpu_svo/abstract_camera.h>
-#include <gpu_svo/vision.h>
-#include <gpu_svo/math_utils.h>
-#include <gpu_svo/patch_score.h>
-#include <gpu_svo/matcher.h>
-#include <gpu_svo/frame.h>
-#include <gpu_svo/feature.h>
-#include <gpu_svo/point.h>
-#include <gpu_svo/config.h>
-#include <gpu_svo/feature_alignment.h>
+#include <vio/abstract_camera.h>
+#include <vio/vision.h>
+#include <vio/math_utils.h>
+#include <vio/patch_score.h>
+#include <vio/matcher.h>
+#include <vio/frame.h>
+#include <vio/feature.h>
+#include <vio/point.h>
+#include <vio/config.h>
+#include <vio/feature_alignment.h>
 
-namespace svo {
+namespace vio {
 
 namespace warp {
 
@@ -324,4 +324,4 @@ bool Matcher::findEpipolarMatchDirect(
   return false;
 }
 
-} // namespace svo
+} // namespace vio

GPU_version/vio/src/math_utils.cpp

@@ -5,7 +5,7 @@
  *      Author: cforster
  */
 
-#include <gpu_svo/math_utils.h>
+#include <vio/math_utils.h>
 
 namespace vk {
 

GPU_version/vio/src/output_helper.cpp

@@ -5,7 +5,7 @@
  *      Author: chrigi
  */
 
-#include <gpu_svo/output_helper.h>
+#include <vio/output_helper.h>
 #include <visualization_msgs/Marker.h>
 
 

GPU_version/vio/src/point.cpp

@@ -15,12 +15,12 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 #include <stdexcept>
-#include <gpu_svo/math_utils.h>
-#include <gpu_svo/point.h>
-#include <gpu_svo/frame.h>
-#include <gpu_svo/feature.h>
+#include <vio/math_utils.h>
+#include <vio/point.h>
+#include <vio/frame.h>
+#include <vio/feature.h>
  
-namespace svo {
+namespace vio {
 
 int Point::point_counter_ = 0;
 
@@ -175,4 +175,4 @@ void Point::optimize(const size_t n_iter)
 #endif
 }
 
-} // namespace svo
+} // namespace vio

GPU_version/vio/src/pose_optimizer.cpp

@@ -15,15 +15,15 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 #include <stdexcept>
-#include <gpu_svo/pose_optimizer.h>
-#include <gpu_svo/frame.h>
-#include <gpu_svo/feature.h>
-#include <gpu_svo/point.h>
-#include <gpu_svo/robust_cost.h>
-#include <gpu_svo/math_utils.h>
+#include <vio/pose_optimizer.h>
+#include <vio/frame.h>
+#include <vio/feature.h>
+#include <vio/point.h>
+#include <vio/robust_cost.h>
+#include <vio/math_utils.h>
 #include <sophus/se2.h>
 
-namespace svo {
+namespace vio {
 namespace pose_optimizer {
 
 void optimizeGaussNewton(
@@ -175,4 +175,4 @@ void optimizeGaussNewton(
 }
 
 } // namespace pose_optimizer
-} // namespace svo
+} // namespace vio

GPU_version/vio/src/reprojector.cpp

@@ -16,19 +16,19 @@
 
 #include <algorithm>
 #include <stdexcept>
-#include <gpu_svo/reprojector.h>
-#include <gpu_svo/frame.h>
-#include <gpu_svo/point.h>
-#include <gpu_svo/feature.h>
-#include <gpu_svo/map.h>
-#include <gpu_svo/config.h>
+#include <vio/reprojector.h>
+#include <vio/frame.h>
+#include <vio/point.h>
+#include <vio/feature.h>
+#include <vio/map.h>
+#include <vio/config.h>
 #include <boost/bind.hpp>
 #include <boost/thread.hpp>
-#include <gpu_svo/abstract_camera.h>
-#include <gpu_svo/math_utils.h>
-#include <gpu_svo/timer.h>
+#include <vio/abstract_camera.h>
+#include <vio/math_utils.h>
+#include <vio/timer.h>
 
-namespace svo {
+namespace vio {
 
 Reprojector::Reprojector(vk::AbstractCamera* cam, Map& map) :
     map_(map)
@@ -216,4 +216,4 @@ bool Reprojector::reprojectPoint(FramePtr frame, Point* point)
   return false;
 }
 
-} // namespace svo
+} // namespace vio

GPU_version/vio/src/robust_cost.cpp

@@ -20,8 +20,8 @@
 
 #include <numeric>
 #include <algorithm>
-#include <gpu_svo/robust_cost.h>
-#include <gpu_svo/math_utils.h>
+#include <vio/robust_cost.h>
+#include <vio/math_utils.h>
 
 namespace vk {
 namespace robust_cost {

GPU_version/vio/src/sparse_img_align.cpp

@@ -15,16 +15,16 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 #include <algorithm>
-#include <gpu_svo/sparse_img_align.h>
-#include <gpu_svo/frame.h>
-#include <gpu_svo/feature.h>
-#include <gpu_svo/config.h>
-#include <gpu_svo/point.h>
-#include <gpu_svo/abstract_camera.h>
-#include <gpu_svo/vision.h>
-#include <gpu_svo/math_utils.h>
+#include <vio/sparse_img_align.h>
+#include <vio/frame.h>
+#include <vio/feature.h>
+#include <vio/config.h>
+#include <vio/point.h>
+#include <vio/abstract_camera.h>
+#include <vio/vision.h>
+#include <vio/math_utils.h>
 
-namespace svo {
+namespace vio {
 
 SparseImgAlign::SparseImgAlign(
     int max_level, int min_level, int n_iter,
@@ -99,8 +99,8 @@ void SparseImgAlign::precomputeReferencePatches()
     *visiblity_it = true;
 
     // cannot just take the 3d points coordinate because of the reprojection errors in the reference image!!!
-    const double depth(((*it)->point->pos_ - ref_pos).norm());
-    const Vector3d xyz_ref((*it)->f*depth);
+   // const double depth(((*it)->point->pos_ - ref_pos).norm());
+    const Vector3d xyz_ref((*it)->f*((*it)->point->pos_ - ref_pos).norm());
 
     // evaluate projection jacobian
     Matrix<double,2,3> frame_jac;
@@ -171,8 +171,8 @@ double SparseImgAlign::computeResiduals(
       continue;
 
     // compute pixel location in cur img
-    const double depth = ((*it)->point->pos_ - ref_pos).norm();
-    const Vector3d xyz_ref((*it)->f*depth);
+    //const double depth = ((*it)->point->pos_ - ref_pos).norm();
+    const Vector3d xyz_ref((*it)->f*((*it)->point->pos_ - ref_pos).norm());
     const Vector3d xyz_cur((T_cur_from_ref.rotation_matrix()(0,0)*xyz_ref.x())+(T_cur_from_ref.rotation_matrix()(0,1)*xyz_ref.z())+T_cur_from_ref.translation()(0),
                            xyz_ref.y(),
                            -1.0*(T_cur_from_ref.rotation_matrix()(1,0)*xyz_ref.x())+(T_cur_from_ref.rotation_matrix()(1,1)*xyz_ref.z())+T_cur_from_ref.translation()(1));
@@ -224,8 +224,6 @@ double SparseImgAlign::computeResiduals(
           const Vector3d J(jacobian_cache_.col(feature_counter*patch_area_ + pixel_counter));
           H_.noalias() += J*J.transpose()*weight;
           Jres_.noalias() -= J*res*weight;
-          if(display_)
-            resimg_.at<float>((int) v_cur+y-patch_halfsize_, (int) u_cur+x-patch_halfsize_) = res/255.0;
         }
       }
     }
@@ -265,5 +263,5 @@ void SparseImgAlign::finishIteration()
   }
 }
 
-} // namespace svo
+} // namespace vio
 

GPU_version/vio/src/sparse_img_align_gpu.cpp

@@ -0,0 +1,128 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <vio/sparse_img_align_gpu.h>
+
+namespace vio {
+
+SparseImgAlignGpu::SparseImgAlignGpu(
+    int max_level, int min_level, int n_iter,
+    Method method, bool verbose,opencl* residual) :
+        max_level_(max_level),
+        min_level_(min_level),
+        residual_(residual)
+{
+  n_iter_ = n_iter;
+  n_iter_init_ = n_iter_;
+  method_ = method;
+  verbose_ = verbose;
+  eps_ = 0.0001;
+}
+
+size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame)
+{
+  reset();
+
+  if(ref_frame->fts_.empty())
+  {
+    return 0;
+  }
+  double* camera=ref_frame->cam_->params();
+  double ref_pos[3]={ref_frame->pos()(0),ref_frame->pos()(1),ref_frame->T_f_w_.pitch()};
+  double cur_pos[3]={cur_frame->pos()(0),cur_frame->pos()(1),cur_frame->T_f_w_.pitch()};
+  residual_->reload_buf(1,2,cur_pos);
+  residual_->reload_buf(1,3,ref_pos);
+  residual_->reload_buf(1,6,camera);
+  cl_double3* features;
+  feature_counter_ = 0; // is used to compute the index of the cached jacobian
+  std::vector<bool>::iterator visiblity_it = visible_fts_.begin();
+  for(auto it=ref_frame->fts_.begin(); it!=ref_frame->fts_.end();
+        ++it, ++visiblity_it){
+        if(!*visiblity_it)
+            continue;
+        const Vector3d xyz_ref((*it)->f*((*it)->point->pos_ - Eigen::Vector3d(ref_pos[0],0.0,ref_pos[1])).norm());
+        features[feature_counter_].x=xyz_ref(0);
+        features[feature_counter_].y=xyz_ref(1);
+        features[feature_counter_].z=xyz_ref(2);
+        ++feature_counter_;
+  }
+  residual_->reload_buf(1,4,features);
+  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_);
+      residual_->reload_buf(1,0,cur_img);
+      residual_->reload_buf(1,1,ref_img);
+      residual_->reload_buf(1,5,&level_);
+    mu_ = 1.0;
+    optimize(T_cur);
+  }
+  cur_frame->T_f_w_ = SE2_5(T_cur);
+
+  return n_meas_/patch_area_;
+}
+
+double SparseImgAlignGpu::computeResiduals(
+    bool linearize_system,
+    bool compute_weight_scale)
+{
+  double chi2=0.0;
+  residual_->reload_buf(1,10,&chi2);
+  /// TODO GPU compute based on the feature
+  residual_->run(1,feature_counter_);
+  if(compute_weight_scale && iter_ == 0)  {
+      float error[feature_counter_];
+      residual_->read(1,7,error);
+      std::vector<float> errors(error,error+feature_counter_);
+      scale_ = scale_estimator_->compute(errors);
+  }
+  residual_->read(1,10,&chi2);
+  return chi2;
+}
+
+int SparseImgAlignGpu::solve()
+{
+    double* H;
+    double* J;
+    residual_->read(1,8,H);
+    residual_->read(1,9,J);
+    x_ = Eigen::Matrix<double,3,3>(H).ldlt().solve(Eigen::Matrix<double,3,1>(J));
+    if((bool) std::isnan((double) x_[0]))
+      return 0;
+    return 1;
+}
+void SparseImgAlignGpu::update()
+{
+/// TODO the update situation may have a smarter solution
+  double* pos;
+  residual_->read(1,2,pos);
+  SE2 update =  SE2(pos[2],Eigen::Vector2d(pos[0],pos[1])) * SE2::exp(-0.25*x_);
+  pos[0]=update.translation()(0);
+  pos[1]=update.translation()(1);
+  pos[2]=atan(update.so2().unit_complex().imag()/update.so2().unit_complex().real());
+  residual_->reload_buf(1,2,pos);
+}
+
+void SparseImgAlignGpu::startIteration()
+{}
+
+void SparseImgAlignGpu::finishIteration()
+{
+}
+
+} // namespace vio
+

GPU_version/vio/src/vision.cpp

@@ -5,7 +5,7 @@
  *      Author: cforster
  */
 
-#include <gpu_svo/vision.h>
+#include <vio/vision.h>
 
 #if __SSE2__
 # include <emmintrin.h>

GPU_version/vio/src/visualizer.cpp

@@ -14,19 +14,19 @@
 // You should have received a copy of the GNU General Public License
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
-#include <gpu_svo/visualizer.h>
-#include <gpu_svo/frame_handler_mono.h>
-#include <gpu_svo/point.h>
-#include <gpu_svo/map.h>
-#include <gpu_svo/feature.h>
+#include <vio/visualizer.h>
+#include <vio/frame_handler_mono.h>
+#include <vio/point.h>
+#include <vio/map.h>
+#include <vio/feature.h>
 #include <geometry_msgs/PoseWithCovarianceStamped.h>
 #include <geometry_msgs/PoseStamped.h>
-#include <gpu_svo/timer.h>
-#include <gpu_svo/output_helper.h>
+#include <vio/timer.h>
+#include <vio/output_helper.h>
 #include <deque>
-#include <gpu_svo/ukf.h>
+#include <vio/ukf.h>
 
-namespace svo {
+namespace vio {
 
 Visualizer::
 Visualizer() :
@@ -77,4 +77,4 @@ void Visualizer::publishMinimal(
   }
 
 }
-} // end namespace gpu_svo
+} // end namespace vio

GPU_version/vio/src/vo_node.cpp

@@ -17,8 +17,8 @@
 #include <ros/ros.h>
 #include <string>
 #include <chrono>
-#include <gpu_svo/frame_handler_mono.h>
-#include <gpu_svo/params_helper.h>
+#include <vio/frame_handler_mono.h>
+#include <vio/params_helper.h>
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/Imu.h>
 #include <geometry_msgs/Twist.h>
@@ -26,21 +26,21 @@
 #include <boost/thread.hpp>
 #include <cv_bridge/cv_bridge.h>
 #include <Eigen/Core>
-#include <gpu_svo/abstract_camera.h>
-#include <gpu_svo/camera_loader.h>
-#include <gpu_svo/frame.h>
+#include <vio/abstract_camera.h>
+#include <vio/camera_loader.h>
+#include <vio/frame.h>
 #include <vio/getOdom.h>
 #include <vio/start.h>
 #include <vio/stop.h>
-#include <gpu_svo/depth_filter.h>
+#include <vio/depth_filter.h>
 #if VIO_DEBUG
-#include <gpu_svo/visualizer.h>
+#include <vio/visualizer.h>
 #endif
 
 class VioNode
 {
 public:
-      svo::FrameHandlerMono* vo_;
+      vio::FrameHandlerMono* vo_;
       vk::AbstractCamera* cam_;
       VioNode();
       ~VioNode();
@@ -54,7 +54,7 @@ public:
     bool start(vio::start::Request& req, vio::start::Response& res){
         if(req.on==1){
             start_=true;
-            if(vo_->stage()!=svo::FrameHandlerBase::STAGE_DEFAULT_FRAME)
+            if(vo_->stage()!=vio::FrameHandlerBase::STAGE_DEFAULT_FRAME)
                 vo_->start();
             else{
                 vo_->depthFilter()->startThread();
@@ -106,7 +106,7 @@ VioNode::VioNode() :
         throw std::runtime_error("Camera model not correctly specified.");
     Eigen::Matrix<double,3,1> init;
     init<<1e-19,1e-19,1e-19;
-    vo_ = new svo::FrameHandlerMono(cam_,init);
+    vo_ = new vio::FrameHandlerMono(cam_,init);
     usleep(500);
 }
 

GPU_version/vio/test/test_pipeline.cpp

@@ -30,7 +30,7 @@
 #include <iostream>
 #include "test_utils.h"
 
-namespace svo {
+namespace vio {
 
 class BenchmarkNode
 {
@@ -84,7 +84,7 @@ void BenchmarkNode::runFromFolder()
   }
 }
 
-} // namespace svo
+} // namespace vio
 
 int main(int argc, char** argv)
 {

GPU_version/vio/test/test_pose_optimizer.cpp

@@ -29,7 +29,7 @@
 
 namespace {
 
-using namespace svo;
+using namespace vio;
 using namespace Eigen;
 
 /// Pose Optimizer Test-Fixture

GPU_version/vio/test/test_sparse_img_align.cpp

@@ -14,18 +14,17 @@
 // You should have received a copy of the GNU General Public License
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
-#include <vikit/pinhole_camera.h>
-#include <vikit/math_utils.h>
-#include <vikit/file_reader.h>
-#include <vikit/timer.h>
-#include <vikit/blender_utils.h>
+#include <vio/pinhole_camera.h>
+#include <vio/math_utils.h>
+#include <vio/file_reader.h>
+#include <vio/timer.h>
 #include <iostream>
-#include <svo/feature_detection.h>
-#include <svo/sparse_img_align.h>
-#include <svo/frame.h>
-#include <svo/point.h>
-#include <svo/feature.h>
-#include <svo/config.h>
+#include <vio/feature_detection.h>
+#include <vio/sparse_img_align.h>
+#include <vio/frame.h>
+#include <vio/point.h>
+#include <vio/feature.h>
+#include <vio/config.h>
 #include "test_utils.h"
 
 namespace {

GPU_version/vio/test/test_utils.h

@@ -24,7 +24,7 @@
 # include <vikit/params_helper.h>
 #endif
 
-namespace svo {
+namespace vio {
 namespace test_utils {
 
 std::string getDatasetDir()
@@ -51,7 +51,7 @@ std::string getTraceDir()
 }
 
 } // namespace test_utils
-} // namespace svo
+} // namespace vio
 
 
 #endif // TEST_UTILS_H_