debug GPU version

GPU_version/svo/include/gpu_svo/abstract_camera.h

@@ -0,0 +1,77 @@
+/*
+ * 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;
+
+  /// 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/svo/include/gpu_svo/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/svo/include/gpu_svo/atan_camera.h

@@ -0,0 +1,91 @@
+/*
+ * 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 <gpu_svo/abstract_camera.h>
+#include <gpu_svo/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?
+
+  //! 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_;
+  }
+};
+
+} // end namespace vk
+
+#endif /* ATAN_CAMERA_H_ */

GPU_version/svo/include/gpu_svo/config.h

@@ -134,6 +134,10 @@ public:
   static double& EBP() { return getInstance().ebp; }
     /// the noise used for projection
   static double& NRP() { return getInstance().nrp; }
+  static double& Gravity() { return getInstance().gravity; }
+    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; }
 
 
 private:
@@ -169,7 +173,7 @@ private:
   size_t max_fts;
   size_t quality_min_fts;
   int quality_max_drop_fts;
-  double nrp,ebp,gbc,abc,iuc,ACC_noise,GYO_noise;
+  double nrp,ebp,gbc,abc,iuc,ACC_noise,GYO_noise,gravity,acc_x_off,acc_y_off,gyr_z_off;
 };
 
 } // namespace svo

GPU_version/svo/include/gpu_svo/depth_filter.h

@@ -20,7 +20,7 @@
 #include <queue>
 #include <boost/thread.hpp>
 #include <boost/function.hpp>
-#include <vikit/performance_monitor.h>
+//#include <vikit/performance_monitor.h>
 #include <gpu_svo/global.h>
 #include <gpu_svo/feature_detection.h>
 #include <gpu_svo/matcher.h>
@@ -79,7 +79,7 @@ public:
       epi_search_1d(false),
       verbose(false),
       use_photometric_disparity_error(false),
-      max_n_kfs(3),
+      max_n_kfs(10),
       sigma_i_sq(5e-4),
       seed_convergence_sigma2_thresh(200.0)
     {}
@@ -150,7 +150,7 @@ protected:
   bool new_keyframe_set_;               //!< Do we have a new keyframe to process?.
   double new_keyframe_min_depth_;       //!< Minimum depth in the new keyframe. Used for range in new seeds.
   double new_keyframe_mean_depth_;      //!< Maximum depth in the new keyframe. Used for range in new seeds.
-  vk::PerformanceMonitor permon_;       //!< Separate performance monitor since the DepthFilter runs in a parallel thread.
+  //vk::PerformanceMonitor permon_;       //!< Separate performance monitor since the DepthFilter runs in a parallel thread.
   Matcher matcher_;
 
   /// Initialize new seeds from a frame.

GPU_version/svo/include/gpu_svo/for_it.hpp

@@ -0,0 +1,87 @@
+//
+// Created by root on 5/12/21.
+//
+
+#ifndef GPU_SVO_FOR_IT_H
+#define GPU_SVO_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 //GPU_SVO_FOR_IT_H

GPU_version/svo/include/gpu_svo/frame.h

@@ -18,8 +18,8 @@
 #define SVO_FRAME_H_
 
 #include <sophus/se3.h>
-#include <vikit/math_utils.h>
-#include <vikit/abstract_camera.h>
+#include <gpu_svo/math_utils.h>
+#include <gpu_svo/abstract_camera.h>
 #include <boost/noncopyable.hpp>
 #include <gpu_svo/global.h>
 
@@ -92,6 +92,8 @@ public:
 
   /// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c).
   inline Vector2d w2c(const Vector3d& xyz_w) const { return cam_->world2cam( T_f_w_ * xyz_w ); }
+  /// Transforms 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]); }
@@ -111,6 +113,7 @@ public:
   /// Return the pose of the frame in the (w)orld coordinate frame.
   inline Vector3d pos() const { return T_f_w_.inverse().translation(); }
 
+
   /// Frame jacobian for projection of 3D point in (f)rame coordinate to
   /// unit plane coordinates uv (focal length = 1).
   inline static void jacobian_xyz2uv(
@@ -129,13 +132,26 @@ public:
     J(0,4) = -(1.0 + x*J(0,2));   // -(1.0 + x^2/z^2)
     J(0,5) = y*z_inv;             // y/z
 
-    J(1,0) = 0.0;                 // 0
+    J(1,0) = 0.0;                // 0
     J(1,1) = -z_inv;              // -1/z
     J(1,2) = y*z_inv_2;           // y/z^2
     J(1,3) = 1.0 + y*J(1,2);      // 1.0 + y^2/z^2
     J(1,4) = -J(0,3);             // -x*y/z^2
     J(1,5) = -x*z_inv;            // x/z
   }
+  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) = 0.0;
+      J(0,2) = p.x()*A;
+
+      J(1,0) = 0.0;
+      J(1,1) = z_inv;
+      J(1,2) =A*p.y();
+  }
 };
 
 

GPU_version/svo/include/gpu_svo/frame_handler_base.h

@@ -18,8 +18,8 @@
 #define SVO_FRAME_HANDLER_BASE_H_
 
 #include <queue>
-#include <vikit/timer.h>
-#include <vikit/ringbuffer.h>
+//#include <vikit/timer.h>
+//#include <vikit/ringbuffer.h>
 #include <boost/noncopyable.hpp>
 #include <boost/function.hpp>
 #include <boost/thread.hpp>
@@ -29,7 +29,7 @@
 namespace vk
 {
 class AbstractCamera;
-class PerformanceMonitor;
+//class PerformanceMonitor;
 }
 
 namespace svo
@@ -80,19 +80,19 @@ public:
   TrackingQuality trackingQuality() const { return tracking_quality_; }
 
   /// Get the processing time of the previous iteration.
-  double lastProcessingTime() const { return timer_.getTime(); }
+  //double lastProcessingTime() const { return timer_.getTime(); }
 
   /// Get the number of feature observations of the last frame.
-  size_t lastNumObservations() const { return num_obs_last_; }
+ // size_t lastNumObservations() const { return num_obs_last_; }
 
 protected:
   Stage stage_;                 //!< Current stage of the algorithm.
   bool set_reset_;              //!< Flag that the user can set. Will reset the system before the next iteration.
   bool set_start_;              //!< Flag the user can set to start the system when the next image is received.
   Map map_;                     //!< Map of keyframes created by the slam system.
-  vk::Timer timer_;             //!< Stopwatch to measure time to process frame.
-  vk::RingBuffer<double> acc_frame_timings_;    //!< Total processing time of the last 10 frames, used to give some user feedback on the performance.
-  vk::RingBuffer<size_t> acc_num_obs_;          //!< Number of observed features of the last 10 frames, used to give some user feedback on the tracking performance.
+  //vk::Timer timer_;             //!< Stopwatch to measure time to process frame.
+  //vk::RingBuffer<double> acc_frame_timings_;    //!< Total processing time of the last 10 frames, used to give some user feedback on the performance.
+  //vk::RingBuffer<size_t> acc_num_obs_;          //!< Number of observed features of the last 10 frames, used to give some user feedback on the tracking performance.
   size_t num_obs_last_;                         //!< Number of observations in the previous frame.
   TrackingQuality tracking_quality_;            //!< An estimate of the tracking quality based on the number of tracked features.
 

GPU_version/svo/include/gpu_svo/frame_handler_mono.h

@@ -18,7 +18,7 @@
 #define SVO_FRAME_HANDLER_H_
 
 #include <set>
-#include <vikit/abstract_camera.h>
+#include <gpu_svo/abstract_camera.h>
 #include <gpu_svo/frame_handler_base.h>
 #include <gpu_svo/reprojector.h>
 #include <gpu_svo/initialization.h>
@@ -61,7 +61,7 @@ public:
       const SE3& T_kf_f,
       const cv::Mat& img,
       const double timestamp);
-  std::unique_ptr<Imu_Integration> imu_integPtr_;  //Integration of IMU
+  std::shared_ptr<Imu_Integration> imu_integPtr_;  //Integration of IMU
 
 protected:
   vk::AbstractCamera* cam_;                     //!< Camera model, can be ATAN, Pinhole or Ocam (see vikit).
@@ -73,6 +73,7 @@ protected:
   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_;
+  uint image_count_=0;
 
   /// Initialize the visual odometry algorithm.
   virtual void initialize();

GPU_version/svo/include/gpu_svo/global.h

@@ -27,7 +27,7 @@
 #include <Eigen/Core>
 #include <opencv2/opencv.hpp>
 #include <sophus/se3.h>
-#include <vikit/performance_monitor.h>
+//#include <vikit/performance_monitor.h>
 #include <boost/shared_ptr.hpp>
 #include<Eigen/StdVector>
 #ifndef RPG_SVO_VIKIT_IS_VECTOR_SPECIALIZED //Guard for rpg_vikit
@@ -78,6 +78,7 @@ namespace svo
   const double PI = 3.14159265;
 
 #ifdef SVO_TRACE
+  /*
   extern vk::PerformanceMonitor* g_permon;
   #define SVO_LOG(value) g_permon->log(std::string((#value)),(value))
   #define SVO_LOG2(value1, value2) SVO_LOG(value1); SVO_LOG(value2)
@@ -85,6 +86,7 @@ namespace svo
   #define SVO_LOG4(value1, value2, value3, value4) SVO_LOG2(value1, value2); SVO_LOG2(value3, value4)
   #define SVO_START_TIMER(name) g_permon->startTimer((name))
   #define SVO_STOP_TIMER(name) g_permon->stopTimer((name))
+   */
 #else
   #define SVO_LOG(v)
   #define SVO_LOG2(v1, v2)

GPU_version/svo/include/gpu_svo/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 <gpu_svo/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/svo/include/gpu_svo/imu_integration.h

@@ -18,83 +18,7 @@
 using gtsam::symbol_shorthand::B;  // Bias  (ax,ay,az,gx,gy,gz)
 using gtsam::symbol_shorthand::V;  // Vel   (xdot,ydot,zdot)
 using gtsam::symbol_shorthand::P;  // Pose3 (x,y,z,r,p,y)
-using gtsam::symbol_shorthand::C;  // Camera pose
-
-//https://stackoverflow.com/questions/24881799/get-index-in-c11-foreach-loop
-// Wrapper class
-template <typename T>
-class enumerate_impl
-{
-public:
-    // The return value of the operator* of the iterator, this
-    // is what you will get inside of the for loop
-    struct item
-    {
-        size_t index;
-        typename T::value_type & item;
-    };
-    typedef item value_type;
-
-    // Custom iterator with minimal interface
-    struct iterator
-    {
-        iterator(typename T::iterator _it, size_t counter=0) :
-                it(_it), counter(counter)
-        {}
-
-        iterator operator++()
-        {
-            return iterator(++it, ++counter);
-        }
-
-        bool operator!=(iterator other)
-        {
-            return it != other.it;
-        }
-
-        typename T::iterator::value_type item()
-        {
-            return *it;
-        }
-
-        value_type operator*()
-        {
-            return value_type{counter, *it};
-        }
-
-        size_t index()
-        {
-            return counter;
-        }
-
-    private:
-        typename T::iterator it;
-        size_t counter;
-    };
-
-    enumerate_impl(T & t) : container(t) {}
-
-    iterator begin()
-    {
-        return iterator(container.begin());
-    }
-
-    iterator end()
-    {
-        return iterator(container.end());
-    }
-
-private:
-    T & container;
-};
-
-// A templated free function allows you to create the wrapper class
-// conveniently
-template <typename T>
-enumerate_impl<T> enumerate(T & t)
-{
-    return enumerate_impl<T>(t);
-}
+using gtsam::symbol_shorthand::L;  // Landmark
 
 class Imu_Integration{
 public:
@@ -103,7 +27,10 @@ public:
     bool reset(gtsam::ISAM2& optimizer,boost::shared_ptr<svo::Frame>& new_frame);
     bool reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost::shared_ptr<svo::Frame>& new_frame);
     bool update(double* imu= nullptr);
-    bool predict(boost::shared_ptr<svo::Frame>&,std::size_t&,const double reproj_thresh);
+    bool predict(boost::shared_ptr<svo::Frame>&,
+                 boost::shared_ptr<svo::Frame>& last_frame ,std::size_t&,const double reproj_thresh);
+    bool init(boost::shared_ptr<svo::Frame>&);
+    Sophus::SE3 preintegrate_predict();
 private:
     std::shared_ptr<gtsam::NavState> statePtr;
     std::shared_ptr<gtsam::imuBias::ConstantBias> imu_biasPtr;
@@ -116,6 +43,7 @@ private:
     std::uint32_t imu_factor_id=0;
     uint imu_n=0;
     std::shared_ptr<gtsam::ISAM2Params> optimizerParamPtr;
+    gtsam::NavState Integ_predict;
     bool syn=false;
 };
 #endif //SVO_IMU_INTEGRATION_H

GPU_version/svo/include/gpu_svo/initialization.h

@@ -19,6 +19,7 @@
 
 #include <gpu_svo/global.h>
 #include <gpu_svo/cl_class.h>
+#include <gpu_svo/imu_integration.h>
 
 namespace svo {
 

GPU_version/svo/include/gpu_svo/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/svo/include/gpu_svo/nlls_solver.h

@@ -0,0 +1,169 @@
+/*
+ * 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 <gpu_svo/math_utils.h>
+#include <gpu_svo/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;
+
+  /// 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
+  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(ModelType& model);
+
+  /// 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 "gpu_svo/nlls_solver_impl.hpp"
+
+#endif /* LM_SOLVER_H_ */

GPU_version/svo/include/gpu_svo/nlls_solver_impl.hpp

@@ -0,0 +1,323 @@
+/*
+ * 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(model);
+  else if(method_ == LevenbergMarquardt)
+    optimizeLevenbergMarquardt(model);
+}
+
+template <int D, typename T>
+void vk::NLLSSolver<D, T>::optimizeGaussNewton(ModelType& model)
+{
+  // Compute weight scale
+  if(use_weights_)
+    computeResiduals(model, false, true);
+
+  // Save the old model to rollback in case of unsuccessful update
+  ModelType old_model(model);
+
+  // perform iterative estimation
+  for (iter_ = 0; iter_<n_iter_; ++iter_)
+  {
+    rho_ = 0;
+    startIteration();
+
+    H_.setZero();
+    Jres_.setZero();
+
+    // compute initial error
+    n_meas_ = 0;
+    double new_chi2 = computeResiduals(model, true, false);
+
+    // add prior
+    if(have_prior_)
+      applyPrior(model);
+
+    // 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;
+      std::cout << "H = " << H_ << std::endl;
+      std::cout << "Jres = " << Jres_ << 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."
+                  << std::endl;
+      }
+      model = old_model; // rollback
+      break;
+    }
+
+    // update the model
+    ModelType new_model;
+    update(model, new_model);
+    old_model = model;
+    model = new_model;
+
+    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_)
+                << std::endl;
+    }
+
+    finishIteration();
+
+    // 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();
+
+    // 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, false, 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_;
+        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_
+               << endl;
+        }
+      }
+      else
+      {
+        // update increased the error -> fail
+        mu_ *= nu_;
+        nu_ *= 2.;
+        ++n_trials_;
+        if (n_trials_ >= n_trials_max_)
+          stop_ = true;
+
+        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_
+               << endl;
+        }
+      }
+
+      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/svo/include/gpu_svo/params_helper.h

@@ -0,0 +1,57 @@
+//
+// Created by root on 5/24/21.
+//
+
+#ifndef GPU_SVO_PARAMS_HELPER_H
+#define GPU_SVO_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 //GPU_SVO_PARAMS_HELPER_H

GPU_version/svo/include/gpu_svo/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/svo/include/gpu_svo/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/svo/include/gpu_svo/sparse_img_align.h

@@ -17,8 +17,7 @@
 #ifndef SVO_SPARSE_IMG_ALIGN_H_
 #define SVO_SPARSE_IMG_ALIGN_H_
 
-#include <vikit/nlls_solver.h>
-#include <vikit/performance_monitor.h>
+#include <gpu_svo/nlls_solver.h>
 #include <gpu_svo/global.h>
 
 namespace vk {
@@ -32,8 +31,8 @@ class Feature;
 /// Optimize the pose of the frame by minimizing the photometric error of feature patches.
 class SparseImgAlign : public vk::NLLSSolver<6, SE3>
 {
-  static const int patch_halfsize_ = 2;
-  static const int patch_size_ = 2*patch_halfsize_;
+  static const int patch_halfsize_ = 6;
+  static const int patch_size_ = 6*patch_halfsize_;
   static const int patch_area_ = patch_size_*patch_size_;
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW

GPU_version/svo/include/gpu_svo/svoVisionFactor.h

@@ -16,13 +16,11 @@
 
 namespace svo {
 
-    class VisionFactor: public gtsam::NoiseModelFactor1<gtsam::Pose3> {
+    class VisionFactor: public gtsam::NoiseModelFactor2<gtsam::Pose3,gtsam::Point3> {
 
     private:
         // measurement information
         gtsam::Point3 feature;   ///<Unit-bearing vector of the feature.
-        gtsam::Point3 landmark;   ///<Unit-bearing vector of the feature.
-        boost::optional<gtsam::Point3> body_P_sensor_; ///< TODO The pose of the sensor in the body frame
         int level;            ///< Image pyramid level where feature was extracted.
 
     public:
@@ -39,22 +37,27 @@ namespace svo {
         * @param body_P_sensor is the transform from body to sensor frame (default identity)
         * @param level Image pyramid level where feature was extracted.
         */
-        VisionFactor(const gtsam::Point3& feature, const gtsam::Point3& landmark,const int level, const gtsam::SharedNoiseModel& Hp,
-                      gtsam::Key poseKey,boost::optional<gtsam::Point3> body_P_sensor = boost::none) :
-                gtsam::NoiseModelFactor1<gtsam::Pose3>(Hp, poseKey), feature(feature), landmark(landmark),body_P_sensor_(body_P_sensor),level(level){}
+        VisionFactor(const gtsam::Point3& feature,const int level,
+                     const gtsam::SharedNoiseModel& H,
+                      gtsam::Key poseKey,gtsam::Key landKey ) :
+                gtsam::NoiseModelFactor2<gtsam::Pose3,gtsam::Point3>(H, poseKey,landKey), feature(feature),level(level){}
         virtual ~VisionFactor(){}
 
         /// error function
         /// @param p    the Estimated pose in Pose3
         /// @param H    the optional Jacobian matrix, which use boost optional and has default null pointer
-        gtsam::Vector evaluateError(const gtsam::Pose3& p, boost::optional<gtsam::Matrix&> Hp = boost::none) const {
+        gtsam::Vector evaluateError(const gtsam::Pose3& p,const gtsam::Point3& l,
+                                    boost::optional<gtsam::Matrix&> H1 = boost::none,boost::optional<gtsam::Matrix&> H2 = boost::none) const {
             // note that use boost optional like a pointer
             // only calculate jacobian matrix when non-null pointer exists
-            gtsam::Matrix26 J;
-            gtsam::Point3 point(p*landmark);
-            svo::Frame::jacobian_xyz2uv(point, J);
-            Vector2d e = (vk::project2d(feature) - vk::project2d(point))*1.0 / (1<<level);
-            if(Hp)(*Hp) = (gtsam::Matrix(2,6)<< J * 1.0 / (1<<level)).finished();
+            gtsam::Matrix26 J1;
+            gtsam::Matrix23 J2;
+            gtsam::Point3 point(p*l);
+            svo::Frame::jacobian_xyz2uv(point, J1);
+            svo::Frame::jacobian_l2uv(point, J2);
+            Vector2d e=(vk::project2d(feature) - vk::project2d(point))*1.0 / (1<<level);
+            if(H1)(*H1) = (gtsam::Matrix(2,6)<< J1*1.0 / (1<<level)).finished();
+            if(H2)(*H2) = (gtsam::Matrix(2,3)<< J2*1.0 / (1<<level)).finished();
             return e;
         }
         // The second is a 'clone' function that allows the factor to be copied. Under most

GPU_version/svo/include/gpu_svo/svoVisionFactorLandMark.h

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

GPU_version/svo/include/gpu_svo/svoVisionFactorSmall.h

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

GPU_version/svo/include/gpu_svo/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/svo/include/gpu_svo/vision.h

@@ -0,0 +1,77 @@
+/*
+ * vision.h
+ *
+ *  Created on: May 14, 2013
+ *      Author: cforster
+ */
+
+#ifndef VIKIT_VISION_H_
+#define VIKIT_VISION_H_
+
+#include <gpu_svo/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)
+{
+  assert(mat.type()==CV_8U);
+  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;
+  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_