debug GPU version

GPU_version/svo/CMakeLists.txt

@@ -5,8 +5,8 @@ CMAKE_MINIMUM_REQUIRED (VERSION 2.8.3)
 SET(CMAKE_BUILD_TYPE Release) # Release, RelWithDebInfo
 SET(CMAKE_VERBOSE_MAKEFILE OFF)
 SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/CMakeModules/")
-set(CMAKE_C_STANDARD 14)
-set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_C_STANDARD 11)
+set(CMAKE_CXX_STANDARD 11)
 
 # Set definitions
 ADD_DEFINITIONS(-DSVO_TRACE)
@@ -21,11 +21,6 @@ IF(DEFINED ENV{ARM_ARCHITECTURE})
 ELSE()
   SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mmmx -msse -msse -msse2 -msse3 -mssse3")
 ENDIF()
-IF(CMAKE_COMPILER_IS_GNUCC)
-  SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
-ELSE()
-  SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
-ENDIF()
 SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops")
 
 # Add plain cmake packages 
@@ -88,12 +83,17 @@ LIST(APPEND SOURCEFILES
         src/config.cpp
         src/sparse_img_align.cpp
         src/imu_integration.cpp
-        src/cl_class.cpp)
+        src/cl_class.cpp
+        src/atan_camera.cpp
+        src/math_utils.cpp
+        src/robust_cost.cpp
+        src/vision.cpp
+        src/homography.cpp)
 
 # Create svo library
 ADD_LIBRARY(${PROJECT_NAME} SHARED ${SOURCEFILES})
 TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${LINK_LIBS} )
-set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 14)
+set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 11)
 set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD_REQUIRED ON)
 target_compile_features(${PROJECT_NAME} PRIVATE cxx_range_for)
 ADD_DEFINITIONS(-DKERNEL_DIR=\"${PROJECT_SOURCE_DIR}/kernel\")

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_

GPU_version/svo/src/atan_camera.cpp

@@ -0,0 +1,82 @@
+/*
+ * atan_camera.cpp
+ *
+ *  Created on: Aug 21, 2012
+ *      Author: cforster
+ */
+
+
+#include <math.h>
+#include <gpu_svo/atan_camera.h>
+#include <gpu_svo/math_utils.h>
+
+namespace vk {
+
+ATANCamera::
+ATANCamera(double width, double height,
+           double fx, double fy,
+           double cx, double cy,
+           double s) :
+           AbstractCamera(width, height),
+           fx_(width*fx), fy_(height*fy),
+           fx_inv_(1.0/fx_), fy_inv_(1.0/fy_),
+           cx_(cx*width - 0.5), cy_(cy*height - 0.5),
+           s_(s), s_inv_(1.0/s_)
+{
+  if(s_ != 0.0)
+  {
+    tans_ = 2.0 * tan(s_ / 2.0);
+    tans_inv_ = 1.0 / tans_;
+    s_inv_ = 1.0 / s_;
+    distortion_ = true;
+  }
+  else
+  {
+    s_inv_ = 0.0;
+    tans_ = 0.0;
+    distortion_ = false;
+  }
+}
+
+ATANCamera::
+~ATANCamera()
+{}
+
+Vector3d ATANCamera::
+cam2world(const double& x, const double& y) const
+{
+  Vector2d dist_cam((x - cx_) * fx_inv_,
+                    (y - cy_) * fy_inv_);
+  double dist_r = dist_cam.norm();
+  double r = invrtrans(dist_r);
+  double d_factor;
+  if(dist_r > 0.01)
+    d_factor =  r / dist_r;
+  else
+    d_factor = 1.0;
+  return unproject2d(d_factor * dist_cam).normalized();
+}
+
+Vector3d ATANCamera::
+cam2world (const Vector2d& px) const
+{
+  return cam2world(px[0], px[1]);
+}
+
+Vector2d ATANCamera::
+world2cam(const Vector3d& xyz_c) const
+{
+  return world2cam(project2d(xyz_c));
+}
+
+Vector2d ATANCamera::
+world2cam(const Vector2d& uv) const
+{
+  double r = uv.norm();
+  double factor = rtrans_factor(r);
+  Vector2d dist_cam = factor * uv;
+  return Vector2d(cx_ + fx_ * dist_cam[0],
+                  cy_ + fy_ * dist_cam[1]);
+}
+
+} /* end vk */

GPU_version/svo/src/bundle_adjustment.cpp

@@ -14,7 +14,7 @@
 // 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/math_utils.h>
+#include <gpu_svo/math_utils.h>
 #include <boost/thread.hpp>
 #include <g2o/core/sparse_optimizer.h>
 #include <g2o/core/block_solver.h>

GPU_version/svo/src/config.cpp

@@ -14,15 +14,12 @@
 // You should have received a copy of the GNU General Public License
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
-#ifdef SVO_USE_ROS
-#include <vikit/params_helper.h>
-#endif
+#include <gpu_svo/params_helper.h>
 #include <gpu_svo/config.h>
 
 namespace svo {
 
 Config::Config() :
-#ifdef SVO_USE_ROS
     trace_name(vk::getParam<string>("svo/trace_name", "svo")),
     trace_dir(vk::getParam<string>("svo/trace_dir", "/tmp")),
     n_pyr_levels(vk::getParam<int>("svo/n_pyr_levels", 3)),
@@ -58,38 +55,11 @@ Config::Config() :
     abc(vk::getParam<double>("svo/ABC", 1e-1)),
     gbc(vk::getParam<double>("svo/GBC", 1e-1)),
     ebp(vk::getParam<double>("svo/EBP", 1e-2)),
-    nrp(vk::getParam<double>("svo/NRP", 1e-5))
-#else
-    trace_name("svo"),
-    trace_dir("/tmp"),
-    n_pyr_levels(3),
-    use_imu(false),
-    core_n_kfs(3),
-    map_scale(1.0),
-    grid_size(25),
-    init_min_disparity(50.0),
-    init_min_tracked(50),
-    init_min_inliers(40),
-    klt_max_level(4),
-    klt_min_level(2),
-    reproj_thresh(2.0),
-    poseoptim_thresh(2.0),
-    poseoptim_num_iter(10),
-    structureoptim_max_pts(20),
-    structureoptim_num_iter(5),
-    loba_thresh(2.0),
-    loba_robust_huber_width(1.0),
-    loba_num_iter(0),
-    kfselect_mindist(0.12),
-    triang_min_corner_score(20.0),
-    triang_half_patch_size(4),
-    subpix_n_iter(10),
-    max_n_kfs(0),
-    img_imu_delay(0.0),
-    max_fts(120),
-    quality_min_fts(50),
-    quality_max_drop_fts(40)
-#endif
+    nrp(vk::getParam<double>("svo/NRP", 1e-5)),
+    gravity(vk::getParam<double>("svo/gravity", 9.8)),
+    acc_x_off(vk::getParam<double>("svo/Acceleration_offset_x", 0.0)),
+    acc_y_off(vk::getParam<double>("svo/Acceleration_offset_y", 0.0)),
+    gyr_z_off(vk::getParam<double>("svo/Gyroscope_offset_z", 0.0))
 {}
 
 Config& Config::getInstance()

GPU_version/svo/src/depth_filter.cpp

@@ -15,9 +15,9 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 #include <algorithm>
-#include <vikit/math_utils.h>
-#include <vikit/abstract_camera.h>
-#include <vikit/vision.h>
+#include <gpu_svo/math_utils.h>
+#include <gpu_svo/abstract_camera.h>
+#include <gpu_svo/vision.h>
 #include <boost/bind.hpp>
 #include <boost/math/distributions/normal.hpp>
 #include <gpu_svo/global.h>
@@ -340,13 +340,8 @@ double DepthFilter::computeTau(
   Vector3d t(T_ref_cur.translation());
   Vector3d a = f*z-t;
   double t_norm = t.norm();
-  double a_norm = a.norm();
-  double alpha = acos(f.dot(t)/t_norm); // dot product
-  double beta = acos(a.dot(-t)/(t_norm*a_norm)); // dot product
-  double beta_plus = beta + px_error_angle;
-  double gamma_plus = PI-alpha-beta_plus; // triangle angles sum to PI
-  double z_plus = t_norm*sin(beta_plus)/sin(gamma_plus); // law of sines
-  return (z_plus - z); // tau
+  double beta_plus = acos(a.dot(-t)/(t_norm*a.norm())) + px_error_angle;
+  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

GPU_version/svo/src/feature_alignment.cpp

@@ -25,7 +25,7 @@
 namespace svo {
 namespace feature_alignment {
 
-#define SUBPIX_VERBOSE 0
+#define SUBPIX_VERBOSE 1
 
 bool align1D(
     const cv::Mat& cur_img,
@@ -258,7 +258,7 @@ bool align2D(
 #if SUBPIX_VERBOSE
     cout << "Iter " << iter << ":"
          << "\t u=" << u << ", v=" << v
-         << "\t update = " << update[0] << ", " << update[1]
+         << "\t update = " << update[0] << ", " << update[1]<< endl;
 //         << "\t new chi2 = " << new_chi2 << endl;
 #endif
 

GPU_version/svo/src/feature_detection.cpp

@@ -16,7 +16,7 @@
 
 #include <gpu_svo/feature_detection.h>
 #include <gpu_svo/feature.h>
-#include <vikit/vision.h>
+#include <gpu_svo/vision.h>
 
 namespace svo {
 namespace feature_detection {

GPU_version/svo/src/frame.cpp

@@ -19,9 +19,8 @@
 #include <gpu_svo/feature.h>
 #include <gpu_svo/point.h>
 #include <gpu_svo/config.h>
-#include <vikit/math_utils.h>
-#include <vikit/vision.h>
-#include <vikit/performance_monitor.h>
+#include <gpu_svo/math_utils.h>
+#include <gpu_svo/vision.h>
 
 namespace svo {
 

GPU_version/svo/src/frame_handler_base.cpp

@@ -14,7 +14,7 @@
 // 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/abstract_camera.h>
+#include <gpu_svo/abstract_camera.h>
 #include <stdlib.h>
 #include <Eigen/StdVector>
 #include <boost/bind.hpp>
@@ -30,48 +30,14 @@ namespace svo
 {
 
 // definition of global and static variables which were declared in the header
-#ifdef SVO_TRACE
-vk::PerformanceMonitor* g_permon = NULL;
-#endif
 
 FrameHandlerBase::FrameHandlerBase() :
   stage_(STAGE_PAUSED),
   set_reset_(false),
   set_start_(false),
-  acc_frame_timings_(10),
-  acc_num_obs_(10),
   num_obs_last_(0),
   tracking_quality_(TRACKING_INSUFFICIENT)
 {
-#ifdef SVO_TRACE
-  // Initialize Performance Monitor
-  g_permon = new vk::PerformanceMonitor();
-  g_permon->addTimer("pyramid_creation");
-  g_permon->addTimer("sparse_img_align");
-  g_permon->addTimer("reproject");
-  g_permon->addTimer("reproject_kfs");
-  g_permon->addTimer("reproject_candidates");
-  g_permon->addTimer("feature_align");
-  g_permon->addTimer("pose_optimizer");
-  g_permon->addTimer("point_optimizer");
-  g_permon->addTimer("local_ba");
-  g_permon->addTimer("tot_time");
-  g_permon->addLog("timestamp");
-  g_permon->addLog("img_align_n_tracked");
-  g_permon->addLog("repr_n_mps");
-  g_permon->addLog("repr_n_new_references");
-  g_permon->addLog("sfba_thresh");
-  g_permon->addLog("sfba_error_init");
-  g_permon->addLog("sfba_error_final");
-  g_permon->addLog("sfba_n_edges_final");
-  g_permon->addLog("loba_n_erredges_init");
-  g_permon->addLog("loba_n_erredges_fin");
-  g_permon->addLog("loba_err_init");
-  g_permon->addLog("loba_err_fin");
-  g_permon->addLog("n_candidates");
-  g_permon->addLog("dropout");
-  g_permon->init(Config::traceName(), Config::traceDir());
-#endif
 
   SVO_INFO_STREAM("SVO initialized");
 }
@@ -79,9 +45,6 @@ FrameHandlerBase::FrameHandlerBase() :
 FrameHandlerBase::~FrameHandlerBase()
 {
   SVO_INFO_STREAM("SVO destructor invoked");
-#ifdef SVO_TRACE
-  delete g_permon;
-#endif
 }
 
 bool FrameHandlerBase::startFrameProcessingCommon(const double timestamp)
@@ -95,10 +58,7 @@ bool FrameHandlerBase::startFrameProcessingCommon(const double timestamp)
   if(stage_ == STAGE_PAUSED)
     return false;
 
-  SVO_LOG(timestamp);
   SVO_DEBUG_STREAM("New Frame");
-  SVO_START_TIMER("tot_time");
-  timer_.start();
 
   // some cleanup from last iteration, can't do before because of visualization
   map_.emptyTrash();
@@ -110,24 +70,15 @@ int FrameHandlerBase::finishFrameProcessingCommon(
     const UpdateResult dropout,
     const size_t num_observations)
 {
-  SVO_DEBUG_STREAM("Frame: "<<update_id<<"\t fps-avg = "<< 1.0/acc_frame_timings_.getMean()<<"\t nObs = "<<acc_num_obs_.getMean());
-  SVO_LOG(dropout);
+//  SVO_DEBUG_STREAM("Frame: "<<update_id<<"\t fps-avg = "<< 1.0/acc_frame_timings_.getMean()<<"\t nObs = "<<acc_num_obs_.getMean());
+ // SVO_LOG(dropout);
 
   // save processing time to calculate fps
-  acc_frame_timings_.push_back(timer_.stop());
+//  acc_frame_timings_.push_back(timer_.stop());
   if(stage_ == STAGE_DEFAULT_FRAME)
-    acc_num_obs_.push_back(num_observations);
+ //   acc_num_obs_.push_back(num_observations);
   num_obs_last_ = num_observations;
-  SVO_STOP_TIMER("tot_time");
-
-#ifdef SVO_TRACE
-  g_permon->writeToFile();
-  {
-    boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_);
-    size_t n_candidates = map_.point_candidates_.candidates_.size();
-    SVO_LOG(n_candidates);
-  }
-#endif
+ // SVO_STOP_TIMER("tot_time");
 
   if(dropout == RESULT_FAILURE &&
       (stage_ == STAGE_DEFAULT_FRAME || stage_ == STAGE_RELOCALIZING ))
@@ -181,10 +132,8 @@ void FrameHandlerBase::optimizeStructure(
     int max_iter)
 {
   deque<Point*> pts;
-  for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
-  {
-    if((*it)->point != NULL)
-      pts.push_back((*it)->point);
+  for(auto&& it:frame->fts_){
+      if(it->point!=NULL)pts.push_back(it->point);
   }
   max_n_pts = min(max_n_pts, pts.size());
   nth_element(pts.begin(), pts.begin() + max_n_pts, pts.end(), ptLastOptimComparator);

GPU_version/svo/src/frame_handler_mono.cpp

@@ -22,8 +22,8 @@
 #include <gpu_svo/point.h>
 #include <gpu_svo/pose_optimizer.h>
 #include <gpu_svo/sparse_img_align.h>
-#include <vikit/performance_monitor.h>
 #include <gpu_svo/depth_filter.h>
+#include <gpu_svo/for_it.hpp>
 #ifdef USE_BUNDLE_ADJUSTMENT
 #include <gpu_svo/bundle_adjustment.h>
 #endif
@@ -47,7 +47,7 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Sophus::SE3& SE_init)
     gpu_fast_->write_buf(0,3,(1 << 18));
     klt_homography_init_=new initialization::KltHomographyInit(gpu_fast_);
   initialize();
-  imu_integPtr_=std::make_unique<Imu_Integration>(SE_init);
+  imu_integPtr_=std::make_shared<Imu_Integration>(SE_init);
 }
 
 void FrameHandlerMono::initialize()
@@ -111,21 +111,19 @@ FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame()
 
 FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
 {
+  new_frame_->T_f_w_ = last_frame_->T_f_w_;
   initialization::InitResult res = klt_homography_init_->addSecondFrame(new_frame_);
-  if(res == initialization::FAILURE)
-    return RESULT_FAILURE;
-  else if(res == initialization::NO_KEYFRAME)
+  if(res == initialization::NO_KEYFRAME)
     return RESULT_NO_KEYFRAME;
 
-  // two-frame bundle adjustment
-#ifdef USE_BUNDLE_ADJUSTMENT
-  ba::twoViewBA(new_frame_.get(), map_.lastKeyframe().get(), Config::lobaThresh(), &map_);
-#endif
-
   new_frame_->setKeyframe();
   double depth_mean, depth_min;
   frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min);
+
   depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min);
+  if(!imu_integPtr_->init(new_frame_)){
+      return RESULT_FAILURE;
+  }
 
   // add frame to map
   map_.addKeyframe(new_frame_);
@@ -135,37 +133,49 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
   return RESULT_IS_KEYFRAME;
 }
 
+
 FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
 {
-  // Set initial pose TODO use prior
-  new_frame_->T_f_w_ = last_frame_->T_f_w_;
-
+  SE3 tem=imu_integPtr_->preintegrate_predict();
+  Eigen::Quaternion<double> q(tem.rotation_matrix());
+  q.vec() *= 1/q.norm();
+  q.w() *= 1/q.norm();
+  q.x()=0.0;
+  q.z()=0.0;
+  tem.translation().y()=0.0;
+  new_frame_->T_f_w_ = SE3(q,tem.translation());
   // sparse image align
   SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),
-                           30, SparseImgAlign::GaussNewton, false, false);
+                           30, SparseImgAlign::LevenbergMarquardt, false, true);
   img_align.run(last_frame_, new_frame_);
   // map reprojection & feature alignment
   reprojector_.reprojectMap(new_frame_, overlap_kfs_);
-  std::cerr<<"Reprojection Map:\t nPoints = "<<reprojector_.n_trials_<<"\t \t nMatches = "<<reprojector_.n_matches_<<" \t Quality min: "<<Config::qualityMinFts()<<'\n';
-  if(reprojector_.n_matches_< Config::qualityMinFts())
-  {
-    new_frame_->T_f_w_ = last_frame_->T_f_w_; // reset to avoid crazy pose jumps
-    tracking_quality_ = TRACKING_INSUFFICIENT;
-    return RESULT_FAILURE;
+  std::cerr<<"Reprojection Map: "<<"\t nPoint:"<<overlap_kfs_.back().second<<"\t nCell = "<<reprojector_.n_trials_<<"\t \t nMatches = "<<reprojector_.n_matches_<<" \t Quality min: "<<Config::qualityMinFts()<<'\n';
+  size_t sfba_n_edges_final=0;
+  if(!imu_integPtr_->predict(new_frame_,last_frame_,sfba_n_edges_final,Config::poseOptimThresh())){
+        std::cerr<<"imu failed\n";
+        return RESULT_FAILURE;
   }
 
+  //usleep(5000);
+  double sfba_thresh, sfba_error_init, sfba_error_final;
+  pose_optimizer::optimizeGaussNewton(
+            Config::poseOptimThresh(), Config::poseOptimNumIter(), false,
+            new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final);
   // pose optimization subject to change for adding IMU
-  size_t sfba_n_edges_final;
-  if(!imu_integPtr_->predict(new_frame_,sfba_n_edges_final,Config::poseOptimThresh()))
+  /*
+  if(!imu_integPtr_->predict(new_frame_,last_frame_,sfba_n_edges_final,Config::poseOptimThresh())){
+      std::cerr<<"imu failed\n";
       return RESULT_FAILURE;
-
+  }*/
 
   std::cerr<<"Reprojected points after opmization: "<<sfba_n_edges_final<<'\n';
-  if(sfba_n_edges_final < Config::qualityMinFts())
-    return RESULT_FAILURE;
+  if(sfba_n_edges_final < Config::qualityMinFts() || reprojector_.n_matches_ < Config::qualityMinFts()){
+      return RESULT_FAILURE;
+  }
 
   // structure optimization
-  //SVO_START_TIMER("point_optimizer");
+//  SVO_START_TIMER("point_optimizer");
   optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
   //SVO_STOP_TIMER("point_optimizer");
 
@@ -174,7 +184,6 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   setTrackingQuality(sfba_n_edges_final);
   if(tracking_quality_ == TRACKING_INSUFFICIENT)
   {
-    new_frame_->T_f_w_ = last_frame_->T_f_w_; // reset to avoid crazy pose jumps
     return RESULT_FAILURE;
   }
   double depth_mean, depth_min;
@@ -187,28 +196,10 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   new_frame_->setKeyframe();
 
   // new keyframe selected
-  for(Features::iterator it=new_frame_->fts_.begin(); it!=new_frame_->fts_.end(); ++it)
-    if((*it)->point != NULL)
-      (*it)->point->addFrameRef(*it);
-  map_.point_candidates_.addCandidatePointToFrame(new_frame_);
-
-  // optional bundle adjustment
-#ifdef USE_BUNDLE_ADJUSTMENT
-  if(Config::lobaNumIter() > 0)
-  {
-    SVO_START_TIMER("local_ba");
-    setCoreKfs(Config::coreNKfs());
-    size_t loba_n_erredges_init, loba_n_erredges_fin;
-    double loba_err_init, loba_err_fin;
-    ba::localBA(new_frame_.get(), &core_kfs_, &map_,
-                loba_n_erredges_init, loba_n_erredges_fin,
-                loba_err_init, loba_err_fin);
-    SVO_STOP_TIMER("local_ba");
-    SVO_LOG4(loba_n_erredges_init, loba_n_erredges_fin, loba_err_init, loba_err_fin);
-    SVO_DEBUG_STREAM("Local BA:\t RemovedEdges {"<<loba_n_erredges_init<<", "<<loba_n_erredges_fin<<"} \t "
-                     "Error {"<<loba_err_init<<", "<<loba_err_fin<<"}");
+  for(auto&& it:new_frame_->fts_){
+      if(it->point != NULL)it->point->addFrameRef(it);
   }
-#endif
+  map_.point_candidates_.addCandidatePointToFrame(new_frame_);
 
   // init new depth-filters
   depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min);
@@ -294,12 +285,12 @@ void FrameHandlerMono::setFirstFrame(const FramePtr& first_frame)
 
 bool FrameHandlerMono::needNewKf(double scene_depth_mean)
 {
-  for(auto it=overlap_kfs_.begin(), ite=overlap_kfs_.end(); it!=ite; ++it)
+  for(auto&& it:overlap_kfs_)
   {
-    Vector3d relpos = new_frame_->w2f(it->first->pos());
+    Vector3d relpos = new_frame_->w2f(it.first->pos());
     if(fabs(relpos.x())/scene_depth_mean < Config::kfSelectMinDist() &&
-       fabs(relpos.y())/scene_depth_mean < Config::kfSelectMinDist()*0.8 &&
-       fabs(relpos.z())/scene_depth_mean < Config::kfSelectMinDist()*1.3)
+       fabs(relpos.y())/scene_depth_mean < Config::kfSelectMinDist() &&
+       fabs(relpos.z())/scene_depth_mean < Config::kfSelectMinDist())
       return false;
   }
   return true;

GPU_version/svo/src/homography.cpp

@@ -0,0 +1,285 @@
+//
+// Created by root on 5/24/21.
+//
+/*
+ * 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
+ */
+
+#include <gpu_svo/homography.h>
+#include <opencv2/opencv.hpp>
+#include <stdio.h>
+
+namespace vk {
+
+    Homography::
+    Homography(const vector<Vector2d, aligned_allocator<Vector2d> >& _fts1,
+               const vector<Vector2d, aligned_allocator<Vector2d> >& _fts2,
+               double _error_multiplier2,
+               double _thresh_in_px) :
+            thresh(_thresh_in_px),
+            error_multiplier2(_error_multiplier2),
+            fts_c1(_fts1),
+            fts_c2(_fts2)
+    {
+    }
+
+    void Homography::
+    calcFromPlaneParams(const Vector3d& n_c1, const Vector3d& xyz_c1)
+    {
+        double d = n_c1.dot(xyz_c1); // normal distance from plane to KF
+        H_c2_from_c1 = T_c2_from_c1.rotation_matrix() + (T_c2_from_c1.translation()*n_c1.transpose())/d;
+    }
+
+    void Homography::
+    calcFromMatches()
+    {
+        vector<cv::Point2f> src_pts(fts_c1.size()), dst_pts(fts_c1.size());
+        for(size_t i=0; i<fts_c1.size(); ++i)
+        {
+            src_pts[i] = cv::Point2f(fts_c1[i][0], fts_c1[i][1]);
+            dst_pts[i] = cv::Point2f(fts_c2[i][0], fts_c2[i][1]);
+        }
+
+        // TODO: replace this function to remove dependency from opencv!
+        cv::Mat cvH = cv::findHomography(src_pts, dst_pts, CV_RANSAC, 2./error_multiplier2);
+        H_c2_from_c1(0,0) = cvH.at<double>(0,0);
+        H_c2_from_c1(0,1) = cvH.at<double>(0,1);
+        H_c2_from_c1(0,2) = cvH.at<double>(0,2);
+        H_c2_from_c1(1,0) = cvH.at<double>(1,0);
+        H_c2_from_c1(1,1) = cvH.at<double>(1,1);
+        H_c2_from_c1(1,2) = cvH.at<double>(1,2);
+        H_c2_from_c1(2,0) = cvH.at<double>(2,0);
+        H_c2_from_c1(2,1) = cvH.at<double>(2,1);
+        H_c2_from_c1(2,2) = cvH.at<double>(2,2);
+    }
+
+    size_t Homography::
+    computeMatchesInliers()
+    {
+        inliers.clear(); inliers.resize(fts_c1.size());
+        size_t n_inliers = 0;
+        for(size_t i=0; i<fts_c1.size(); i++)
+        {
+            Vector2d projected = project2d(H_c2_from_c1 * unproject2d(fts_c1[i]));
+            Vector2d e = fts_c2[i] - projected;
+            double e_px = error_multiplier2 * e.norm();
+            inliers[i] = (e_px < thresh);
+            n_inliers += inliers[i];
+        }
+        return n_inliers;
+
+    }
+
+    bool Homography::
+    computeSE3fromMatches()
+    {
+        calcFromMatches();
+        bool res = decompose();
+        if(!res)
+            return false;
+        computeMatchesInliers();
+        findBestDecomposition();
+        Eigen::Quaternion<double> q(decompositions.front().T.rotation_matrix());
+        q.vec() *= 1/q.norm();
+        q.w() *= 1/q.norm();
+        q.x()=0.0;
+        q.z()=0.0;
+        T_c2_from_c1=Sophus::SE3(q.toRotationMatrix(),Eigen::Vector3d(decompositions.front().T.translation().x(),0.0,decompositions.front().T.translation().z()));
+        return true;
+    }
+
+    bool Homography::
+    decompose()
+    {
+        decompositions.clear();
+        JacobiSVD<MatrixXd> svd(H_c2_from_c1, ComputeThinU | ComputeThinV);
+
+        Vector3d singular_values = svd.singularValues();
+
+        double d1 = fabs(singular_values[0]); // The paper suggests the square of these (e.g. the evalues of AAT)
+        double d2 = fabs(singular_values[1]); // should be used, but this is wrong. c.f. Faugeras' book.
+        double d3 = fabs(singular_values[2]);
+
+        Matrix3d U = svd.matrixU();
+        Matrix3d V = svd.matrixV();                    // VT^T
+        double s = U.determinant() * V.determinant();
+
+        if(!(d1 != d2 && d2 != d3))
+        {
+            printf("FATAL Homography Initialization: This motion case is not implemented or is degenerate. Try again. ");
+            return false;
+        }
+
+        double x1_PM;
+        double x3_PM;
+
+        // All below deals with the case = 1 case.
+        // Case 1 implies (d1 != d3)
+        { // Eq. 12
+            x1_PM = sqrt((d1*d1 - d2*d2) / (d1*d1 - d3*d3));
+            x3_PM = sqrt((d2*d2 - d3*d3) / (d1*d1 - d3*d3));
+        };
+
+        double e1[4] = {1.0,-1.0, 1.0,-1.0};
+        double e3[4] = {1.0, 1.0,-1.0,-1.0};
+
+        Vector3d np;
+        HomographyDecomposition decomp;
+
+        // Case 1, d' > 0:
+        decomp.d = s * d2;
+        for(size_t signs=0; signs<4; signs++)
+        {
+            // Eq 13
+            decomp.R = Matrix3d::Identity();
+            double dSinTheta = (d1 - d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2;
+            double dCosTheta = (d1 * x3_PM * x3_PM + d3 * x1_PM * x1_PM) / d2;
+            /*
+             * correct the rotation matrix by Majid
+             */
+            decomp.R(0,0) = dCosTheta; //1
+            decomp.R(0,2) = -dSinTheta; //0
+            decomp.R(2,0) = dSinTheta;
+            decomp.R(2,2) = dCosTheta;
+
+            // Eq 14
+            decomp.t[0] = (d1 - d3) * x1_PM * e1[signs];
+            decomp.t[1] = 0.0;
+            decomp.t[2] = (d1 - d3) * -x3_PM * e3[signs];
+
+            np[0] = x1_PM * e1[signs];
+            np[1] = 0.0;
+            np[2] = x3_PM * e3[signs];
+            decomp.n = V * np;
+
+            decompositions.push_back(decomp);
+        }
+
+        // Case 1, d' < 0:
+        decomp.d = s * -d2;
+        for(size_t signs=0; signs<4; signs++)
+        {
+            // Eq 15
+            decomp.R = -1 * Matrix3d::Identity();
+            double dSinPhi = (d1 + d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2;
+            double dCosPhi = (d3 * x1_PM * x1_PM - d1 * x3_PM * x3_PM) / d2;
+            /*
+            * correct the rotation matrix by Majid
+           */
+            decomp.R(0,0) = dCosPhi;
+            decomp.R(0,2) = dSinPhi;
+            decomp.R(2,0) = dSinPhi;
+            decomp.R(2,2) = -dCosPhi;
+
+            // Eq 16
+            decomp.t[0] = (d1 + d3) * x1_PM * e1[signs];
+            decomp.t[1] = 0.0;
+            decomp.t[2] = (d1 + d3) * x3_PM * e3[signs];
+
+            np[0] = x1_PM * e1[signs];
+            np[1] = 0.0;
+            np[2] = x3_PM * e3[signs];
+            decomp.n = V * np;
+
+            decompositions.push_back(decomp);
+        }
+
+        // Save rotation and translation of the decomposition
+        for(unsigned int i=0; i<decompositions.size(); i++)
+        {
+            Matrix3d R = s * U * decompositions[i].R * V.transpose();
+            Vector3d t = U * decompositions[i].t;
+            decompositions[i].T = Sophus::SE3(R, t);
+        }
+        return true;
+    }
+
+    bool operator<(const HomographyDecomposition lhs, const HomographyDecomposition rhs)
+    {
+        return lhs.score < rhs.score;
+    }
+
+    void Homography::
+    findBestDecomposition()
+    {
+        assert(decompositions.size() == 8);
+        for(size_t i=0; i<decompositions.size(); i++)
+        {
+            HomographyDecomposition &decom = decompositions[i];
+            size_t nPositive = 0;
+            for(size_t m=0; m<fts_c1.size(); m++)
+            {
+                if(!inliers[m])
+                    continue;
+                const Vector2d& v2 = fts_c1[m];
+                double dVisibilityTest = (H_c2_from_c1(2,0) * v2[0] + H_c2_from_c1(2,1) * v2[1] + H_c2_from_c1(2,2)) / decom.d;
+                if(dVisibilityTest > 0.0)
+                    nPositive++;
+            }
+            decom.score = -nPositive;
+        }
+
+        sort(decompositions.begin(), decompositions.end());
+        decompositions.resize(4);
+
+        for(size_t i=0; i<decompositions.size(); i++)
+        {
+            HomographyDecomposition &decom = decompositions[i];
+            int nPositive = 0;
+            for(size_t m=0; m<fts_c1.size(); m++)
+            {
+                if(!inliers[m])
+                    continue;
+                Vector3d v3 = unproject2d(fts_c1[m]);
+                double dVisibilityTest = v3.dot(decom.n) / decom.d;
+                if(dVisibilityTest > 0.0)
+                    nPositive++;
+            };
+            decom.score = -nPositive;
+        }
+
+        sort(decompositions.begin(), decompositions.end());
+        decompositions.resize(2);
+
+        // According to Faugeras and Lustman, ambiguity exists if the two scores are equal
+        // but in practive, better to look at the ratio!
+        double dRatio = (double) decompositions[1].score / (double) decompositions[0].score;
+
+        if(dRatio < 0.9) // no ambiguity!
+            decompositions.erase(decompositions.begin() + 1);
+        else  // two-way ambiguity. Resolve by sampsonus score of all points.
+        {
+            double dErrorSquaredLimit  = thresh * thresh * 4;
+            double adSampsonusScores[2];
+            for(size_t i=0; i<2; i++)
+            {
+                Sophus::SE3 T = decompositions[i].T;
+                Matrix3d Essential = T.rotation_matrix() * sqew(T.translation());
+                double dSumError = 0;
+                for(size_t m=0; m < fts_c1.size(); m++ )
+                {
+                    double d = sampsonusError(fts_c1[m], Essential, fts_c2[m]);
+                    if(d > dErrorSquaredLimit)
+                        d = dErrorSquaredLimit;
+                    dSumError += d;
+                }
+                adSampsonusScores[i] = dSumError;
+            }
+
+            if(adSampsonusScores[0] <= adSampsonusScores[1])
+                decompositions.erase(decompositions.begin() + 1);
+            else
+                decompositions.erase(decompositions.begin());
+        }
+    }
+
+
+} /* end namespace vk */
+

GPU_version/svo/src/imu_integration.cpp

@@ -7,11 +7,14 @@
 #include <utility>
 #include "gpu_svo/feature.h"
 #include "gpu_svo/point.h"
-#include "gpu_svo/svoVisionFactor.h"
+#include "gpu_svo/svoVisionFactorSmall.h"
+//#include "gpu_svo/svoVisionFactor.h"
+//#include "gpu_svo/svoVisionFactorLandMark.h"
 #include <gtsam/nonlinear/Marginals.h>
 #include <gtsam/geometry/Pose3.h>
 #include <vikit/params_helper.h>
 #include <gpu_svo/config.h>
+#include <gpu_svo/for_it.hpp>
 
 Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
     graphPtr=std::make_shared<gtsam::NonlinearFactorGraph>();
@@ -19,7 +22,7 @@ Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
     // Example: pitch and roll of aircraft in an ENU Cartesian frame.
     // If pitch and roll are zero for an aerospace frame,
     // that means Z is pointing down, i.e., direction of Z = (0,0,-1)
-    gtsam::Vector4 g=SE_init.matrix()*gtsam::Vector4(0, 0, 9.8,1.0);
+    gtsam::Vector4 g=SE_init.matrix()*gtsam::Vector4(0, 0, svo::Config::Gravity(),1.0);
     parameterPtr = boost::shared_ptr<gtsam::PreintegrationCombinedParams>(new gtsam::PreintegrationCombinedParams(gtsam::Vector3(g.x(),g.y(),g.z())));
     parameterPtr->setBodyPSensor(gtsam::Pose3(SE_init.matrix()));// The pose of the sensor in the body frame
     parameterPtr->accelerometerCovariance=gtsam::I_3x3 * svo::Config::ACC_Noise(); // acc white noise in continuous
@@ -34,7 +37,8 @@ Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
     optimizerParamPtr=std::make_shared<gtsam::ISAM2Params>();
     optimizerParamPtr->relinearizeThreshold = 0.1;
     optimizerParamPtr->relinearizeSkip = 1;
-    //optimizerParamPtr->evaluateNonlinearError=true;
+    optimizerParamPtr->evaluateNonlinearError=true;
+    optimizerParamPtr->enableDetailedResults=true;
     assert(preintegratedPtr);
 }
 Imu_Integration::~Imu_Integration(){
@@ -46,9 +50,9 @@ bool Imu_Integration::update(double* imu){
     auto in=std::chrono::steady_clock::now();
     if(std::chrono::duration<double>(in-t_1).count()>20.0){
         t_1=in;
-        preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0],imu[1],imu[2]),Eigen::Vector3d(imu[3],imu[4],imu[5]),0.005);
+        preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0]-svo::Config::X_off(),imu[1]-svo::Config::Y_off(),imu[2]),Eigen::Vector3d(imu[3],imu[4],imu[5]-svo::Config::Teta_off()),0.005);
     }else{
-        preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0],imu[1],imu[2]),Eigen::Vector3d(imu[3],imu[4],imu[5]),std::chrono::duration<double>(in-t_1).count());
+        preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0]-svo::Config::X_off(),imu[1]-svo::Config::Y_off(),imu[2]),Eigen::Vector3d(imu[3],imu[4],imu[5]-svo::Config::Teta_off()),std::chrono::duration<double>(in-t_1).count());
         t_1=in;
         ++imu_n;
     }
@@ -73,10 +77,18 @@ bool Imu_Integration::reset(gtsam::ISAM2& optimizer,boost::shared_ptr<svo::Frame
     graphPtr->addPrior(P(imu_factor_id-1),statePtr->pose(),optimizer.marginalCovariance(P(imu_factor_id-1)));
     graphPtr->addPrior(V(imu_factor_id-1),statePtr->v(),optimizer.marginalCovariance(V(imu_factor_id-1)));
     graphPtr->addPrior(B(imu_factor_id-1),*imu_biasPtr,optimizer.marginalCovariance(B(imu_factor_id-1)));
+    for(auto&& f_n:_for(new_frame->fts_)){
+        if(f_n.item->point == NULL)continue;
+        if(!result.exists(L(f_n.index)))continue;
+        f_n.item->point->pos_=result.at<gtsam::Point3>(L(f_n.index));
+    //    valuesPtr->insert(L(f_n.index),f_n.item->point->pos_);
+//        graphPtr->emplace_shared<svo::VisionFactor>(f_n.item->f,f_n.item->level,ProjectNoisePtr,P(imu_factor_id),L(f_n.index));
+    }
     return true;
 }
 bool Imu_Integration::reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost::shared_ptr<svo::Frame>& new_frame){
     gtsam::Values result = optimizer.optimize();
+    optimizer.print();
     // Overwrite the beginning of the preintegration for the next step.
     statePtr=std::make_shared<gtsam::NavState>(result.at<gtsam::Pose3>(P(imu_factor_id)), result.at<gtsam::Vector3>(V(imu_factor_id)));
     imu_biasPtr=std::make_shared<gtsam::imuBias::ConstantBias>(result.at<gtsam::imuBias::ConstantBias>(B(imu_factor_id)));
@@ -84,6 +96,7 @@ bool Imu_Integration::reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost:
     ++imu_factor_id;
     syn=false;
     gtsam::Marginals marginals(*graphPtr,result);
+    marginals.print();
     new_frame->T_f_w_=Sophus::SE3(statePtr->R(),statePtr->t());
     new_frame->Cov_=marginals.marginalCovariance(P(imu_factor_id-1));
     graphPtr->resize(0);
@@ -94,50 +107,66 @@ bool Imu_Integration::reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost:
     graphPtr->addPrior(P(imu_factor_id-1),statePtr->pose(),marginals.marginalCovariance(P(imu_factor_id-1)));
     graphPtr->addPrior(V(imu_factor_id-1),statePtr->v(),marginals.marginalCovariance(V(imu_factor_id-1)));
     graphPtr->addPrior(B(imu_factor_id-1),*imu_biasPtr,marginals.marginalCovariance(B(imu_factor_id-1)));
+    /*
+    for(auto&& f_n:_for(new_frame->fts_)){
+        if(f_n.item->point == NULL)continue;
+        if(!result.exists(L(f_n.index)))continue;
+        f_n.item->point->pos_=result.at<gtsam::Point3>(L(f_n.index));
+        //valuesPtr->insert(L(f_n.index),f_n.item->point->pos_);
+        //graphPtr->emplace_shared<svo::VisionFactor>(f_n.item->f,f_n.item->level,ProjectNoisePtr,P(imu_factor_id),L(f_n.index));
+    }
+    //graphPtr->addPrior(L(0),new_frame->fts_.front()->point->pos_,gtsam::noiseModel::Isotropic::Sigma(3, 1e-4));
+     */
     return true;
 }
-bool Imu_Integration::predict(boost::shared_ptr<svo::Frame>& new_frame,std::size_t& num_obs,const double reproj_thresh){
-    usleep(5000);
-    syn=true;
-    if(imu_factor_id<1){
-        statePtr=std::make_shared<gtsam::NavState>(gtsam::Pose3(new_frame->T_f_w_.matrix()),
-                                                   gtsam::Vector3(1e-12,1e-12,1e-12));
-        valuesPtr->insert(P(0),statePtr->pose());
-        valuesPtr->insert(V(0),statePtr->v());
-        valuesPtr->insert(B(0),*imu_biasPtr);
-        graphPtr->addPrior(P(0),statePtr->pose(),gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9).finished()));
-        graphPtr->addPrior(V(0),statePtr->v(),gtsam::noiseModel::Isotropic::Sigma(3, 1e-4));
-        graphPtr->addPrior(B(0),*imu_biasPtr,gtsam::noiseModel::Isotropic::Sigma(6, 1e-2));
+Sophus::SE3 Imu_Integration::preintegrate_predict() {
+    if(imu_n<1){
+        syn= false;
+        usleep(10000);
+        syn=true;
     }else{
-        if(imu_n<1)return false;
-        auto preint_imu_combined =dynamic_cast<const gtsam::PreintegratedCombinedMeasurements&>(*preintegratedPtr);
-        //preint_imu_combined.predict(*statePtr,*imu_biasPtr).print();
-        //std::cerr<<new_frame->T_f_w_.matrix();
-        valuesPtr->insert(P(imu_factor_id), gtsam::Pose3(new_frame->T_f_w_.matrix()));
-        valuesPtr->insert(V(imu_factor_id), statePtr->v());
-        valuesPtr->insert(B(imu_factor_id), *imu_biasPtr);
-        gtsam::CombinedImuFactor imu_factor(P(imu_factor_id - 1), V(imu_factor_id - 1), P(imu_factor_id), V(imu_factor_id), B(imu_factor_id - 1), B(imu_factor_id),
-                                            preint_imu_combined);
-        graphPtr->emplace_shared<gtsam::CombinedImuFactor>(imu_factor);
-        imu_n=0;
+        syn=true;
     }
+    auto preint_imu_combined =dynamic_cast<const gtsam::PreintegratedCombinedMeasurements&>(*preintegratedPtr);
+    Integ_predict=preint_imu_combined.predict(*statePtr,*imu_biasPtr);
+    return Sophus::SE3(Integ_predict.R(),Integ_predict.t());
+}
+bool Imu_Integration::predict(boost::shared_ptr<svo::Frame>& new_frame,
+                              boost::shared_ptr<svo::Frame>& last_frame,
+                              std::size_t& num_obs,const double reproj_thresh){
+
+    auto preint_imu_combined =dynamic_cast<const gtsam::PreintegratedCombinedMeasurements&>(*preintegratedPtr);
+    valuesPtr->insert(P(imu_factor_id), gtsam::Pose3(new_frame->T_f_w_.matrix()));
+    valuesPtr->insert(V(imu_factor_id), Integ_predict.v());
+    valuesPtr->insert(B(imu_factor_id), *imu_biasPtr);
+    gtsam::CombinedImuFactor imu_factor(P(imu_factor_id - 1), V(imu_factor_id - 1), P(imu_factor_id), V(imu_factor_id), B(imu_factor_id - 1), B(imu_factor_id),
+                                            preint_imu_combined);
+    graphPtr->emplace_shared<gtsam::CombinedImuFactor>(imu_factor);
+    imu_n=0;
 
-    num_obs=0;
     for(auto&& f:new_frame->fts_){
-       if(f->point==NULL)continue;
+        if(f->point==NULL)continue;
         graphPtr->emplace_shared<svo::VisionFactor>(f->f, f->point->pos_,f->level, ProjectNoisePtr,P(imu_factor_id));
         ++num_obs;
     }
-    // Now optimize.
-    gtsam::LevenbergMarquardtOptimizer optimizer(*graphPtr, *valuesPtr);
 
+    std::cout<<"------------------------------------------------------------------------------\n";
+    std::cout<<"------------------------------------------------------------------------------\n";
+    std::cout<<"------------------------------------------------------------------------------\n";
+    graphPtr->print();
+    gtsam::LevenbergMarquardtParams params0;
+    //params0.verbosity=gtsam::LevenbergMarquardtParams::LINEAR;
+    params0.verbosityLM = gtsam::LevenbergMarquardtParams::SUMMARY;
+    params0.setlambdaFactor(10.0);
+    gtsam::LevenbergMarquardtOptimizer optimizer(*graphPtr, *valuesPtr,params0);
     reset(optimizer,new_frame);
+    std::cout<<"------------------------------------------------------------------------------\n";
     // Remove Measurements with too large reprojection error
+    /*
     double reproj_thresh_scaled = reproj_thresh / new_frame->cam_->errorMultiplier2();
     for(auto&& f:new_frame->fts_)
     {
-        if(f->point == NULL)
-            continue;
+        if(f->point == NULL)continue;
         Eigen::Vector2d e = (vk::project2d(f->f) - vk::project2d(new_frame->T_f_w_ * f->point->pos_))*1.0 / (1<<f->level);
         if(e.norm() > reproj_thresh_scaled)
         {
@@ -146,5 +175,26 @@ bool Imu_Integration::predict(boost::shared_ptr<svo::Frame>& new_frame,std::size
             --num_obs;
         }
     }
+     */
+    return true;
+}
+bool Imu_Integration::init(boost::shared_ptr<svo::Frame>& new_frame){
+    statePtr=std::make_shared<gtsam::NavState>(gtsam::Pose3(new_frame->T_f_w_.matrix()),
+                                               gtsam::Vector3(1e-9,1e-9,1e-9));
+    valuesPtr->insert(P(0),statePtr->pose());
+    valuesPtr->insert(V(0),statePtr->v());
+    valuesPtr->insert(B(0),*imu_biasPtr);
+    graphPtr->addPrior(P(0),statePtr->pose(),gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-5, 1e-15, 1e-5, 1e-15, 1e-8, 1e-15).finished()));
+    graphPtr->addPrior(V(0),statePtr->v(),gtsam::noiseModel::Isotropic::Sigma(3, 1e-4));
+    graphPtr->addPrior(B(0),*imu_biasPtr,gtsam::noiseModel::Isotropic::Sigma(6, 1e-2));
+    /*
+    for(auto&& f_n:_for(new_frame->fts_)){
+        if(f_n.item->point == NULL)continue;
+        valuesPtr->insert(L(f_n.index),f_n.item->point->pos_);
+        graphPtr->emplace_shared<svo::VisionFactor>(f_n.item->f,f_n.item->level,ProjectNoisePtr,P(imu_factor_id),L(f_n.index));
+    }
+    graphPtr->addPrior(L(0),new_frame->fts_.front()->point->pos_,gtsam::noiseModel::Isotropic::Sigma(3, 1e-4));
+     */
+    ++imu_factor_id;
     return true;
 }

GPU_version/svo/src/initialization.cpp

@@ -20,8 +20,8 @@
 #include <gpu_svo/feature.h>
 #include <gpu_svo/initialization.h>
 #include <gpu_svo/feature_detection.h>
-#include <vikit/math_utils.h>
-#include <vikit/homography.h>
+#include <gpu_svo/math_utils.h>
+#include <gpu_svo/homography.h>
 
 namespace svo {
 namespace initialization {
@@ -43,26 +43,24 @@ InitResult KltHomographyInit::addFirstFrame(FramePtr frame_ref)
 InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
 {
   trackKlt(frame_ref_, frame_cur, px_ref_, px_cur_, f_ref_, f_cur_, disparities_);
-  SVO_INFO_STREAM("Init: KLT tracked "<< disparities_.size() <<" features");
-
   if(disparities_.size() < Config::initMinTracked())
-    return FAILURE;
-
+        return NO_KEYFRAME;
   double disparity = vk::getMedian(disparities_);
-  SVO_INFO_STREAM("Init: KLT "<<disparity<<"px average disparity.");
-  if(disparity < Config::initMinDisparity())
-    return NO_KEYFRAME;
-
+  if(disparity < Config::initMinDisparity()){
+      SVO_INFO_STREAM("Init: px average disparity is: "<<disparity<<" While: "<<Config::initMinDisparity()<<" minimum required."
+      <<"  KLT tracked : "<<disparities_.size());
+      return NO_KEYFRAME;
+  }
   computeHomography(
       f_ref_, f_cur_,
       frame_ref_->cam_->errorMultiplier2(), Config::poseOptimThresh(),
       inliers_, xyz_in_cur_, T_cur_from_ref_);
-  SVO_INFO_STREAM("Init: Homography RANSAC "<<inliers_.size()<<" inliers.");
 
   if(inliers_.size() < Config::initMinInliers())
   {
-    SVO_WARN_STREAM("Init WARNING: "<<Config::initMinInliers()<<" inliers minimum required.");
-    return FAILURE;
+    SVO_INFO_STREAM("Init Homography RANSAC (inlier) is: "<<inliers_.size()<<" While: "<<Config::initMinInliers()<<" inliers minimum required.\n"
+       <<"Init: px average disparity is: "<<disparity<<" While: "<<Config::initMinDisparity()<<" minimum required.");
+      return NO_KEYFRAME;
   }
 
   // Rescale the map such that the mean scene depth is equal to the specified scale
@@ -72,29 +70,31 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
   double scene_depth_median = vk::getMedian(depth_vec);
   double scale = Config::mapScale()/scene_depth_median;
   frame_cur->T_f_w_ = T_cur_from_ref_ * frame_ref_->T_f_w_;
-  frame_cur->T_f_w_.translation() =
-      -frame_cur->T_f_w_.rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos()));
+  frame_cur->T_f_w_.translation() = -frame_cur->T_f_w_.rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos()));
 
   // For each inlier create 3D point and add feature in both frames
   SE3 T_world_cur = frame_cur->T_f_w_.inverse();
-  for(vector<int>::iterator it=inliers_.begin(); it!=inliers_.end(); ++it)
-  {
-    Vector2d px_cur(px_cur_[*it].x, px_cur_[*it].y);
-    Vector2d px_ref(px_ref_[*it].x, px_ref_[*it].y);
-    if(frame_ref_->cam_->isInFrame(px_cur.cast<int>(), 10) && frame_ref_->cam_->isInFrame(px_ref.cast<int>(), 10) && xyz_in_cur_[*it].z() > 0)
+    for(vector<int>::iterator it=inliers_.begin(); it!=inliers_.end(); ++it)
     {
-      Vector3d pos = T_world_cur * (xyz_in_cur_[*it]*scale);
-      Point* new_point = new Point(pos);
+        Vector2d px_cur(px_cur_[*it].x, px_cur_[*it].y);
+        Vector2d px_ref(px_ref_[*it].x, px_ref_[*it].y);
+        if(frame_ref_->cam_->isInFrame(px_cur.cast<int>(), 10) && frame_ref_->cam_->isInFrame(px_ref.cast<int>(), 10) && xyz_in_cur_[*it].z() > 0)
+        {
+            Vector3d pos = T_world_cur * (xyz_in_cur_[*it]*scale);
+            Point* new_point = new Point(pos);
 
-      Feature* ftr_cur(new Feature(frame_cur.get(), new_point, px_cur, f_cur_[*it], 0));
-      frame_cur->addFeature(ftr_cur);
-      new_point->addFrameRef(ftr_cur);
+            Feature* ftr_cur(new Feature(frame_cur.get(), new_point, px_cur, f_cur_[*it], 0));
+            frame_cur->addFeature(ftr_cur);
+            new_point->addFrameRef(ftr_cur);
 
-      Feature* ftr_ref(new Feature(frame_ref_.get(), new_point, px_ref, f_ref_[*it], 0));
-      frame_ref_->addFeature(ftr_ref);
-      new_point->addFrameRef(ftr_ref);
+            Feature* ftr_ref(new Feature(frame_ref_.get(), new_point, px_ref, f_ref_[*it], 0));
+            frame_ref_->addFeature(ftr_ref);
+            new_point->addFrameRef(ftr_ref);
+        }
     }
-  }
+    SVO_INFO_STREAM("Init Homography RANSAC (inlier) is: "<<inliers_.size()
+                    <<"\nKLT disparity is: "<<disparity<<" px average disparity."
+                    <<"\nThe number of feature: "<<frame_cur->fts_.size());
   return SUCCESS;
 }
 
@@ -118,11 +118,11 @@ void detectFeatures(
   // now for all maximum corners, initialize a new seed
   px_vec.clear(); px_vec.reserve(new_features.size());
   f_vec.clear(); f_vec.reserve(new_features.size());
-  std::for_each(new_features.begin(), new_features.end(), [&](Feature* ftr){
-    px_vec.push_back(cv::Point2f(ftr->px[0], ftr->px[1]));
-    f_vec.push_back(ftr->f);
-    delete ftr;
-  });
+  for(auto&& ftr:new_features){
+      px_vec.push_back(cv::Point2f(ftr->px[0], ftr->px[1]));
+      f_vec.push_back(ftr->f);
+      delete ftr;
+  }
 }
 
 void trackKlt(
@@ -134,8 +134,8 @@ void trackKlt(
     vector<Vector3d>& f_cur,
     vector<double>& disparities)
 {
-  const double klt_win_size = 30.0;
-  const int klt_max_iter = 30;
+  const double klt_win_size = 45.0;//30.0
+  const int klt_max_iter = 30;//30
   const double klt_eps = 0.001;
   vector<uchar> status;
   vector<float> error;

GPU_version/svo/src/matcher.cpp

@@ -15,10 +15,10 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 #include <cstdlib>
-#include <vikit/abstract_camera.h>
-#include <vikit/vision.h>
-#include <vikit/math_utils.h>
-#include <vikit/patch_score.h>
+#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>
@@ -139,7 +139,6 @@ bool Matcher::findMatchDirect(
 {
   if(!pt.getCloseViewObs(cur_frame.pos(), ref_ftr_))
     return false;
-
   if(!ref_ftr_->frame->cam_->isInFrame(
       ref_ftr_->px.cast<int>()/(1<<ref_ftr_->level), halfpatch_size_+2, ref_ftr_->level))
     return false;

GPU_version/svo/src/math_utils.cpp

@@ -0,0 +1,202 @@
+/*
+ * math_utils.cpp
+ *
+ *  Created on: Jul 20, 2012
+ *      Author: cforster
+ */
+
+#include <gpu_svo/math_utils.h>
+
+namespace vk {
+
+using namespace Eigen;
+
+Vector3d
+triangulateFeatureNonLin(const Matrix3d& R,  const Vector3d& t,
+                         const Vector3d& feature1, const Vector3d& feature2 )
+{
+  Vector3d f2 = R * feature2;
+  Vector2d b;
+  b[0] = t.dot(feature1);
+  b[1] = t.dot(f2);
+  Matrix2d A;
+  A(0,0) = feature1.dot(feature1);
+  A(1,0) = feature1.dot(f2);
+  A(0,1) = -A(1,0);
+  A(1,1) = -f2.dot(f2);
+  Vector2d lambda = A.inverse() * b;
+  Vector3d xm = lambda[0] * feature1;
+  Vector3d xn = t + lambda[1] * f2;
+  return ( xm + xn )/2;
+}
+
+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)
+{
+  // bearing vectors (f_r, f_c) do not need to be unit length
+  const Vector3d f_c_in_r(R_r_c*f_c);
+  const double a = f_c_in_r.dot(f_r) / t_r_c.dot(f_r);
+  const double b = f_c_in_r.dot(t_r_c);
+  const double denom = (a*b - f_c_in_r.dot(f_c_in_r));
+
+  if(abs(denom) < 0.000001)
+    return false;
+
+  depth_in_c = (b-a*t_r_c.dot(t_r_c)) / denom;
+  depth_in_r = (t_r_c + f_c_in_r*depth_in_c).norm();
+  return true;
+}
+
+double
+reprojError(const Vector3d& f1,
+            const Vector3d& f2,
+            double error_multiplier2)
+{
+  Vector2d e = project2d(f1) - project2d(f2);
+  return error_multiplier2 * e.norm();
+}
+
+double
+computeInliers(const vector<Vector3d>& features1, // c1
+               const vector<Vector3d>& features2, // c2
+               const Matrix3d& R,                 // R_c1_c2
+               const Vector3d& t,                 // c1_t
+               const double reproj_thresh,
+               double error_multiplier2,
+               vector<Vector3d>& xyz_vec,         // in frame c1
+               vector<int>& inliers,
+               vector<int>& outliers)
+{
+  inliers.clear(); inliers.reserve(features1.size());
+  outliers.clear(); outliers.reserve(features1.size());
+  xyz_vec.clear(); xyz_vec.reserve(features1.size());
+  double tot_error = 0;
+  //triangulate all features and compute reprojection errors and inliers
+  for(size_t j=0; j<features1.size(); ++j)
+  {
+    xyz_vec.push_back(triangulateFeatureNonLin(R, t, features1[j], features2[j] ));
+    double e1 = reprojError(features1[j], xyz_vec.back(), error_multiplier2);
+    double e2 = reprojError(features2[j], R.transpose()*(xyz_vec.back()-t), error_multiplier2);
+    if(e1 > reproj_thresh || e2 > reproj_thresh)
+      outliers.push_back(j);
+    else
+    {
+      inliers.push_back(j);
+      tot_error += e1+e2;
+    }
+  }
+  return tot_error;
+}
+
+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)
+{
+  inliers.clear(); inliers.reserve(xyz_vec.size());
+  outliers.clear(); outliers.reserve(xyz_vec.size());
+  for(size_t j = 0; j < xyz_vec.size(); j++ )
+  {
+    double e = reprojError(feature_sphere_vec[j],
+                           R.transpose() * ( xyz_vec[j] - t ),
+                           error_multiplier2);
+    if(e < reproj_thresh)
+      inliers.push_back(j);
+    else
+      outliers.push_back(j);
+  }
+}
+
+Vector3d
+dcm2rpy(const Matrix3d &R)
+{
+  Vector3d rpy;
+  rpy[1] = atan2( -R(2,0), sqrt( pow( R(0,0), 2 ) + pow( R(1,0), 2 ) ) );
+  if( fabs( rpy[1] - M_PI/2 ) < 0.00001 )
+  {
+    rpy[2] = 0;
+    rpy[0] = -atan2( R(0,1), R(1,1) );
+  }
+  else
+  {
+    if( fabs( rpy[1] + M_PI/2 ) < 0.00001 )
+    {
+      rpy[2] = 0;
+      rpy[0] = -atan2( R(0,1), R(1,1) );
+    }
+    else
+    {
+      rpy[2] = atan2( R(1,0)/cos(rpy[1]), R(0,0)/cos(rpy[1]) );
+      rpy[0] = atan2( R(2,1)/cos(rpy[1]), R(2,2)/cos(rpy[1]) );
+    }
+  }
+  return rpy;
+}
+
+Matrix3d
+rpy2dcm(const Vector3d &rpy)
+{
+  Matrix3d R1;
+  R1(0,0) = 1.0; R1(0,1) = 0.0; R1(0,2) = 0.0;
+  R1(1,0) = 0.0; R1(1,1) = cos(rpy[0]); R1(1,2) = -sin(rpy[0]);
+  R1(2,0) = 0.0; R1(2,1) = -R1(1,2); R1(2,2) = R1(1,1);
+
+  Matrix3d R2;
+  R2(0,0) = cos(rpy[1]); R2(0,1) = 0.0; R2(0,2) = sin(rpy[1]);
+  R2(1,0) = 0.0; R2(1,1) = 1.0; R2(1,2) = 0.0;
+  R2(2,0) = -R2(0,2); R2(2,1) = 0.0; R2(2,2) = R2(0,0);
+
+  Matrix3d R3;
+  R3(0,0) = cos(rpy[2]); R3(0,1) = -sin(rpy[2]); R3(0,2) = 0.0;
+  R3(1,0) = -R3(0,1); R3(1,1) = R3(0,0); R3(1,2) = 0.0;
+  R3(2,0) = 0.0; R3(2,1) = 0.0; R3(2,2) = 1.0;
+
+  return R3 * R2 * R1;
+}
+
+Quaterniond
+angax2quat(const Vector3d& n, const double& angle)
+{
+  // n must be normalized!
+  double s(sin(angle/2));
+  return Quaterniond( cos(angle/2), n[0]*s, n[1]*s, n[2]*s );
+}
+
+
+Matrix3d
+angax2dcm(const Vector3d& n, const double& angle)
+{
+  // n must be normalized
+  Matrix3d sqewn(sqew(n));
+  return Matrix3d(Matrix3d::Identity() + sqewn*sin(angle) + sqewn*sqewn*(1-cos(angle)));
+}
+
+double
+sampsonusError(const Vector2d &v2Dash, const Matrix3d& Essential, const Vector2d& v2)
+{
+  Vector3d v3Dash = unproject2d(v2Dash);
+  Vector3d v3 = unproject2d(v2);
+
+  double dError = v3Dash.transpose() * Essential * v3;
+
+  Vector3d fv3 = Essential * v3;
+  Vector3d fTv3Dash = Essential.transpose() * v3Dash;
+
+  Vector2d fv3Slice = fv3.head<2>();
+  Vector2d fTv3DashSlice = fTv3Dash.head<2>();
+
+  return (dError * dError / (fv3Slice.dot(fv3Slice) + fTv3DashSlice.dot(fTv3DashSlice)));
+}
+
+} // end namespace vk

GPU_version/svo/src/point.cpp

@@ -15,7 +15,7 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 #include <stdexcept>
-#include <vikit/math_utils.h>
+#include <gpu_svo/math_utils.h>
 #include <gpu_svo/point.h>
 #include <gpu_svo/frame.h>
 #include <gpu_svo/feature.h>

GPU_version/svo/src/pose_optimizer.cpp

@@ -19,8 +19,8 @@
 #include <gpu_svo/frame.h>
 #include <gpu_svo/feature.h>
 #include <gpu_svo/point.h>
-#include <vikit/robust_cost.h>
-#include <vikit/math_utils.h>
+#include <gpu_svo/robust_cost.h>
+#include <gpu_svo/math_utils.h>
 
 namespace svo {
 namespace pose_optimizer {

GPU_version/svo/src/reprojector.cpp

@@ -24,9 +24,9 @@
 #include <gpu_svo/config.h>
 #include <boost/bind.hpp>
 #include <boost/thread.hpp>
-#include <vikit/abstract_camera.h>
-#include <vikit/math_utils.h>
-#include <vikit/timer.h>
+#include <gpu_svo/abstract_camera.h>
+#include <gpu_svo/math_utils.h>
+#include <gpu_svo/timer.h>
 
 namespace svo {
 
@@ -68,10 +68,11 @@ void Reprojector::reprojectMap(
   resetGrid();
 
   // Identify those Keyframes which share a common field of view.
-  SVO_START_TIMER("reproject_kfs");
+//  SVO_START_TIMER("reproject_kfs");
   list< pair<FramePtr,double> > close_kfs;
   map_.getCloseKeyframes(frame, close_kfs);
 
+  std::cout<<"The number of closed frames: "<<std::to_string(close_kfs.size())<<'\n';
   // Sort KFs with overlap according to their closeness
   close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <
                  boost::bind(&std::pair<FramePtr, double>::second, _2));
@@ -98,14 +99,16 @@ void Reprojector::reprojectMap(
       if((*it_ftr)->point->last_projected_kf_id_ == frame->id_)
         continue;
       (*it_ftr)->point->last_projected_kf_id_ = frame->id_;
-      if(reprojectPoint(frame, (*it_ftr)->point))
-        overlap_kfs.back().second++;
+      if(reprojectPoint(frame, (*it_ftr)->point)){
+          overlap_kfs.back().second++;
+      }
+
     }
   }
-  SVO_STOP_TIMER("reproject_kfs");
+  //SVO_STOP_TIMER("reproject_kfs");
 
   // Now project all point candidates
-  SVO_START_TIMER("reproject_candidates");
+ // SVO_START_TIMER("reproject_candidates");
   {
     boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_);
     auto it=map_.point_candidates_.candidates_.begin();
@@ -124,11 +127,11 @@ void Reprojector::reprojectMap(
       ++it;
     }
   } // unlock the mutex when out of scope
-  SVO_STOP_TIMER("reproject_candidates");
+  //SVO_STOP_TIMER("reproject_candidates");
 
   // Now we go through each grid cell and select one point to match.
   // At the end, we should have at maximum one reprojected point per cell.
-  SVO_START_TIMER("feature_align");
+  //SVO_START_TIMER("feature_align");
   for(size_t i=0; i<grid_.cells.size(); ++i)
   {
     // we prefer good quality points over unkown quality (more likely to match)
@@ -138,7 +141,7 @@ void Reprojector::reprojectMap(
     if(n_matches_ > (size_t) Config::maxFts())
       break;
   }
-  SVO_STOP_TIMER("feature_align");
+// SVO_STOP_TIMER("feature_align");
 }
 
 bool Reprojector::pointQualityComparator(Candidate& lhs, Candidate& rhs)
@@ -161,7 +164,6 @@ bool Reprojector::reprojectCell(Cell& cell, FramePtr frame)
       it = cell.erase(it);
       continue;
     }
-
     bool found_match = true;
     if(options_.find_match_direct)
       found_match = matcher_.findMatchDirect(*it->pt, *frame, it->px);

GPU_version/svo/src/robust_cost.cpp

@@ -0,0 +1,162 @@
+/**
+* This file is part of dvo.
+*
+* Copyright 2012 Christian Kerl <christian.kerl@in.tum.de> (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/>.
+*/
+
+#include <numeric>
+#include <algorithm>
+#include <gpu_svo/robust_cost.h>
+#include <gpu_svo/math_utils.h>
+
+namespace vk {
+namespace robust_cost {
+
+const float TDistributionScaleEstimator::INITIAL_SIGMA = 5.0f;
+const float TDistributionScaleEstimator::DEFAULT_DOF = 5.0f;
+
+TDistributionScaleEstimator::
+TDistributionScaleEstimator(const float dof) :
+  dof_(dof),
+  initial_sigma_(INITIAL_SIGMA)
+{}
+
+float TDistributionScaleEstimator::
+compute(std::vector<float>& errors) const
+{
+  float initial_lamda = 1.0f / (initial_sigma_ * initial_sigma_);
+  int num = 0;
+  float lambda = initial_lamda;
+  int iterations = 0;
+  do
+  {
+    ++iterations;
+    initial_lamda = lambda;
+    num = 0;
+    lambda = 0.0f;
+
+    for(std::vector<float>::iterator it=errors.begin(); it!=errors.end(); ++it)
+    {
+      if(std::isfinite(*it))
+      {
+        ++num;
+        const float error2 = (*it)*(*it);
+        lambda += error2 * ( (dof_ + 1.0f) / (dof_ + initial_lamda * error2) );
+      }
+    }
+    lambda = float(num) / lambda;
+  } while(std::abs(lambda - initial_lamda) > 1e-3);
+
+  return std::sqrt(1.0f / lambda);
+}
+
+const float MADScaleEstimator::NORMALIZER = 1.48f; // 1 / 0.6745
+
+float MADScaleEstimator::
+compute(std::vector<float>& errors) const
+{
+  // error must be in absolute values!
+  return NORMALIZER * vk::getMedian(errors);
+}
+
+float NormalDistributionScaleEstimator::
+compute(std::vector<float>& errors) const
+{
+  const float mean = std::accumulate(errors.begin(), errors.end(), 0)/errors.size();
+  float var = 0.0;
+  std::for_each(errors.begin(), errors.end(), [&](const float d) {
+      var += (d - mean) * (d - mean);
+  });
+  return std::sqrt(var); // return standard deviation
+}
+
+const float TukeyWeightFunction::DEFAULT_B = 4.6851f;
+
+TukeyWeightFunction::TukeyWeightFunction(const float b)
+{
+  configure(b);
+}
+
+float TukeyWeightFunction::value(const float& x) const
+{
+  const float x_square = x * x;
+  if(x_square <= b_square)
+  {
+    const float tmp = 1.0f - x_square / b_square;
+    return tmp * tmp;
+  }
+  else
+  {
+    return 0.0f;
+  }
+}
+
+void TukeyWeightFunction::
+configure(const float& param)
+{
+  b_square = param * param;
+}
+
+const float TDistributionWeightFunction::DEFAULT_DOF = 5.0f;
+
+TDistributionWeightFunction::
+TDistributionWeightFunction(const float dof)
+{
+  configure(dof);
+}
+
+float TDistributionWeightFunction::
+value(const float & x) const
+{
+  return ((dof_ + 1.0f) / (dof_ + (x * x)));
+}
+
+void TDistributionWeightFunction::
+configure(const float& param)
+{
+  dof_ = param;
+  normalizer_ = dof_ / (dof_ + 1.0f);
+}
+
+const float HuberWeightFunction::DEFAULT_K = 1.345f;
+
+HuberWeightFunction::
+HuberWeightFunction(const float k)
+{
+  configure(k);
+}
+
+void HuberWeightFunction::
+configure(const float& param)
+{
+  k = param;
+}
+
+float HuberWeightFunction::
+value(const float& t) const
+{
+  const float t_abs = std::abs(t);
+  if(t_abs < k)
+    return 1.0f;
+  else
+    return k / t_abs;
+}
+
+} // namespace robust_cost
+} // namespace vk
+
+

GPU_version/svo/src/sparse_img_align.cpp

@@ -20,9 +20,9 @@
 #include <gpu_svo/feature.h>
 #include <gpu_svo/config.h>
 #include <gpu_svo/point.h>
-#include <vikit/abstract_camera.h>
-#include <vikit/vision.h>
-#include <vikit/math_utils.h>
+#include <gpu_svo/abstract_camera.h>
+#include <gpu_svo/vision.h>
+#include <gpu_svo/math_utils.h>
 
 namespace svo {
 
@@ -54,12 +54,12 @@ size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame)
   cur_frame_ = cur_frame;
   ref_patch_cache_ = cv::Mat(ref_frame_->fts_.size(), patch_area_, CV_32F);
   jacobian_cache_.resize(Eigen::NoChange, ref_patch_cache_.rows*patch_area_);
-  visible_fts_.resize(ref_patch_cache_.rows, false); // TODO: should it be reset at each level?
-
+  //visible_fts_.resize(ref_patch_cache_.rows, false); // TODO: should it be reset at each level?
   SE3 T_cur_from_ref(cur_frame_->T_f_w_ * ref_frame_->T_f_w_.inverse());
 
   for(level_=max_level_; level_>=min_level_; --level_)
   {
+    visible_fts_.resize(ref_patch_cache_.rows, false); // TODO: should it be reset at each level?
     mu_ = 0.1;
     jacobian_cache_.setZero();
     have_ref_patch_cache_ = false;
@@ -69,8 +69,6 @@ size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame)
   }
   cur_frame_->T_f_w_ = T_cur_from_ref * ref_frame_->T_f_w_;
 
-
-
   return n_meas_/patch_area_;
 }
 

GPU_version/svo/src/vision.cpp

@@ -0,0 +1,295 @@
+/*
+ * vision.cpp
+ *
+ *  Created on: May 14, 2013
+ *      Author: cforster
+ */
+
+#include <vikit/vision.h>
+
+#if __SSE2__
+# include <emmintrin.h>
+#elif __ARM_NEON__
+# include <arm_neon.h>
+#endif
+
+namespace vk {
+
+#ifdef __SSE2__
+void halfSampleSSE2(const unsigned char* in, unsigned char* out, int w, int h)
+{
+  const unsigned long long mask[2] = {0x00FF00FF00FF00FFull, 0x00FF00FF00FF00FFull};
+  const unsigned char* nextRow = in + w;
+  __m128i m = _mm_loadu_si128((const __m128i*)mask);
+  int sw = w >> 4;
+  int sh = h >> 1;
+  for (int i=0; i<sh; i++)
+  {
+    for (int j=0; j<sw; j++)
+    {
+      __m128i here = _mm_load_si128((const __m128i*)in);
+      __m128i next = _mm_load_si128((const __m128i*)nextRow);
+      here = _mm_avg_epu8(here,next);
+      next = _mm_and_si128(_mm_srli_si128(here,1), m);
+      here = _mm_and_si128(here,m);
+      here = _mm_avg_epu16(here, next);
+      _mm_storel_epi64((__m128i*)out, _mm_packus_epi16(here,here));
+      in += 16;
+      nextRow += 16;
+      out += 8;
+    }
+    in += w;
+    nextRow += w;
+  }
+}
+#endif 
+
+#ifdef __ARM_NEON__
+void halfSampleNEON( const cv::Mat& in, cv::Mat& out )
+{
+  for( int y = 0; y < in.rows; y += 2)
+  {
+    const uint8_t * in_top = in.data + y*in.cols;
+    const uint8_t * in_bottom = in.data + (y+1)*in.cols;
+    uint8_t * out_data = out.data + (y >> 1)*out.cols;
+    for( int x = in.cols; x > 0 ; x-=16, in_top += 16, in_bottom += 16, out_data += 8)
+    {
+      uint8x8x2_t top  = vld2_u8( (const uint8_t *)in_top );
+      uint8x8x2_t bottom = vld2_u8( (const uint8_t *)in_bottom );
+      uint16x8_t sum = vaddl_u8( top.val[0], top.val[1] );
+      sum = vaddw_u8( sum, bottom.val[0] );
+      sum = vaddw_u8( sum, bottom.val[1] );
+      uint8x8_t final_sum = vshrn_n_u16(sum, 2);
+      vst1_u8(out_data, final_sum);
+    }
+  }
+}
+#endif
+
+
+void
+halfSample(const cv::Mat& in, cv::Mat& out)
+{
+  assert( in.rows/2==out.rows && in.cols/2==out.cols);
+  assert( in.type()==CV_8U && out.type()==CV_8U);
+
+#ifdef __SSE2__
+  if(aligned_mem::is_aligned16(in.data) && aligned_mem::is_aligned16(out.data) && ((in.cols % 16) == 0))
+  {
+    halfSampleSSE2(in.data, out.data, in.cols, in.rows);
+    return;
+  }
+#endif 
+#ifdef __ARM_NEON__ 
+  if( (in.cols % 16) == 0 )
+  {
+    halfSampleNEON(in, out);
+    return;
+  }
+#endif
+
+  const int stride = in.step.p[0];
+  uint8_t* top = (uint8_t*) in.data;
+  uint8_t* bottom = top + stride;
+  uint8_t* end = top + stride*in.rows;
+  const int out_width = out.cols;
+  uint8_t* p = (uint8_t*) out.data;
+  while (bottom < end)
+  {
+    for (int j=0; j<out_width; j++)
+    {
+      *p = static_cast<uint8_t>( (uint16_t (top[0]) + top[1] + bottom[0] + bottom[1])/4 );
+      p++;
+      top += 2;
+      bottom += 2;
+    }
+    top += stride;
+    bottom += stride;
+  }
+}
+
+
+float
+shiTomasiScore(const cv::Mat& img, int u, int v)
+{
+  assert(img.type() == CV_8UC1);
+
+  float dXX = 0.0;
+  float dYY = 0.0;
+  float dXY = 0.0;
+  const int halfbox_size = 4;
+  const int box_size = 2*halfbox_size;
+  const int box_area = box_size*box_size;
+  const int x_min = u-halfbox_size;
+  const int x_max = u+halfbox_size;
+  const int y_min = v-halfbox_size;
+  const int y_max = v+halfbox_size;
+
+  if(x_min < 1 || x_max >= img.cols-1 || y_min < 1 || y_max >= img.rows-1)
+    return 0.0; // patch is too close to the boundary
+
+  const int stride = img.step.p[0];
+  for( int y=y_min; y<y_max; ++y )
+  {
+    const uint8_t* ptr_left   = img.data + stride*y + x_min - 1;
+    const uint8_t* ptr_right  = img.data + stride*y + x_min + 1;
+    const uint8_t* ptr_top    = img.data + stride*(y-1) + x_min;
+    const uint8_t* ptr_bottom = img.data + stride*(y+1) + x_min;
+    for(int x = 0; x < box_size; ++x, ++ptr_left, ++ptr_right, ++ptr_top, ++ptr_bottom)
+    {
+      float dx = *ptr_right - *ptr_left;
+      float dy = *ptr_bottom - *ptr_top;
+      dXX += dx*dx;
+      dYY += dy*dy;
+      dXY += dx*dy;
+    }
+  }
+
+  // Find and return smaller eigenvalue:
+  dXX = dXX / (2.0 * box_area);
+  dYY = dYY / (2.0 * box_area);
+  dXY = dXY / (2.0 * box_area);
+  return 0.5 * (dXX + dYY - sqrt( (dXX + dYY) * (dXX + dYY) - 4 * (dXX * dYY - dXY * dXY) ));
+}
+
+
+void
+calcSharrDeriv(const cv::Mat& src, cv::Mat& dst)
+{
+
+  using namespace cv;
+  typedef short deriv_type;
+
+  int rows = src.rows, cols = src.cols, cn = src.channels(), colsn = cols*cn, depth = src.depth();
+  CV_Assert(depth == CV_8U);
+  dst.create(rows, cols, CV_MAKETYPE(DataType<deriv_type>::depth, cn*2));
+
+  int x, y, delta = (int)alignSize((cols + 2)*cn, 16);
+  AutoBuffer<deriv_type> _tempBuf(delta*2 + 64);
+  deriv_type *trow0 = alignPtr(_tempBuf + cn, 16), *trow1 = alignPtr(trow0 + delta, 16);
+
+#ifdef __SSE2__
+  __m128i z = _mm_setzero_si128(), c3 = _mm_set1_epi16(3), c10 = _mm_set1_epi16(10);
+#endif
+
+  for( y = 0; y < rows; y++ )
+  {
+      const uchar* srow0 = src.ptr<uchar>(y > 0 ? y-1 : rows > 1 ? 1 : 0);
+      const uchar* srow1 = src.ptr<uchar>(y);
+      const uchar* srow2 = src.ptr<uchar>(y < rows-1 ? y+1 : rows > 1 ? rows-2 : 0);
+      deriv_type* drow = dst.ptr<deriv_type>(y);
+
+      // do vertical convolution
+      x = 0;
+#ifdef __SSE2__
+      for( ; x <= colsn - 8; x += 8 )
+      {
+          __m128i s0 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(srow0 + x)), z);
+          __m128i s1 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(srow1 + x)), z);
+          __m128i s2 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(srow2 + x)), z);
+          __m128i t0 = _mm_add_epi16(_mm_mullo_epi16(_mm_add_epi16(s0, s2), c3), _mm_mullo_epi16(s1, c10));
+          __m128i t1 = _mm_sub_epi16(s2, s0);
+          _mm_store_si128((__m128i*)(trow0 + x), t0);
+          _mm_store_si128((__m128i*)(trow1 + x), t1);
+      }
+#endif
+      for( ; x < colsn; x++ )
+      {
+          int t0 = (srow0[x] + srow2[x])*3 + srow1[x]*10;
+          int t1 = srow2[x] - srow0[x];
+          trow0[x] = (deriv_type)t0;
+          trow1[x] = (deriv_type)t1;
+      }
+
+      // make border
+      int x0 = (cols > 1 ? 1 : 0)*cn, x1 = (cols > 1 ? cols-2 : 0)*cn;
+      for( int k = 0; k < cn; k++ )
+      {
+          trow0[-cn + k] = trow0[x0 + k]; trow0[colsn + k] = trow0[x1 + k];
+          trow1[-cn + k] = trow1[x0 + k]; trow1[colsn + k] = trow1[x1 + k];
+      }
+
+      // do horizontal convolution, interleave the results and store them to dst
+      x = 0;
+#ifdef __SSE2__
+      for( ; x <= colsn - 8; x += 8 )
+      {
+          __m128i s0 = _mm_loadu_si128((const __m128i*)(trow0 + x - cn));
+          __m128i s1 = _mm_loadu_si128((const __m128i*)(trow0 + x + cn));
+          __m128i s2 = _mm_loadu_si128((const __m128i*)(trow1 + x - cn));
+          __m128i s3 = _mm_load_si128((const __m128i*)(trow1 + x));
+          __m128i s4 = _mm_loadu_si128((const __m128i*)(trow1 + x + cn));
+
+          __m128i t0 = _mm_sub_epi16(s1, s0);
+          __m128i t1 = _mm_add_epi16(_mm_mullo_epi16(_mm_add_epi16(s2, s4), c3), _mm_mullo_epi16(s3, c10));
+          __m128i t2 = _mm_unpacklo_epi16(t0, t1);
+          t0 = _mm_unpackhi_epi16(t0, t1);
+          // this can probably be replaced with aligned stores if we aligned dst properly.
+          _mm_storeu_si128((__m128i*)(drow + x*2), t2);
+          _mm_storeu_si128((__m128i*)(drow + x*2 + 8), t0);
+      }
+#endif
+      for( ; x < colsn; x++ )
+      {
+          deriv_type t0 = (deriv_type)(trow0[x+cn] - trow0[x-cn]);
+          deriv_type t1 = (deriv_type)((trow1[x+cn] + trow1[x-cn])*3 + trow1[x]*10);
+          drow[x*2] = t0; drow[x*2+1] = t1;
+      }
+  }
+
+//  vector<cv::Mat> vec_mat;
+//  cv::split(dst, vec_mat);
+//  cv::namedWindow("deriv");
+//  cv::imshow("deriv", vec_mat[0]);
+//  cv::namedWindow("derivy");
+//  cv::imshow("derivy", vec_mat[1]);
+//  cv::waitKey(0);
+}
+
+#ifdef __SSE2__
+void convertRawDepthImageSse_16u_to_32f(cv::Mat& depth_16u, cv::Mat& depth_32f, float scale)
+{
+  depth_32f.create(depth_16u.rows, depth_16u.cols, CV_32FC1);
+
+  const unsigned short* input_ptr = depth_16u.ptr<unsigned short>();
+  float* output_ptr = depth_32f.ptr<float>();
+
+  __m128 _scale = _mm_set1_ps(scale);
+  __m128 _zero  = _mm_setzero_ps();
+  __m128 _nan   = _mm_set1_ps(std::numeric_limits<float>::quiet_NaN());
+
+  for(int idx = 0; idx < depth_16u.size().area(); idx += 8, input_ptr += 8, output_ptr += 8)
+  {
+    __m128 _input, mask;
+    __m128i _inputi = _mm_load_si128((__m128i*) input_ptr);
+
+    // load low shorts and convert to float
+    _input = _mm_cvtepi32_ps(_mm_unpacklo_epi16(_inputi, _mm_setzero_si128()));
+
+    mask = _mm_cmpeq_ps(_input, _zero);
+
+    // zero to nan
+    _input = _mm_or_ps(_input, _mm_and_ps(mask, _nan));
+    // scale
+    _input = _mm_mul_ps(_input, _scale);
+    // save
+    _mm_store_ps(output_ptr + 0, _input);
+
+    // load high shorts and convert to float
+    _input = _mm_cvtepi32_ps(_mm_unpackhi_epi16(_inputi, _mm_setzero_si128()));
+
+    mask = _mm_cmpeq_ps(_input, _zero);
+
+    // zero to nan
+    _input = _mm_or_ps(_input, _mm_and_ps(mask, _nan));
+    // scale
+    _input = _mm_mul_ps(_input, _scale);
+    // save
+    _mm_store_ps(output_ptr + 4, _input);
+  }
+}
+#endif
+
+}
+
+

GPU_version/vio/include/vio_gpu/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 <gpu_svo/abstract_camera.h>
+#include <gpu_svo/atan_camera.h>
+#include <gpu_svo/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_gpu/output_helper.h

@@ -0,0 +1,97 @@
+/*
+ * 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>
+
+namespace vk {
+namespace output_helper {
+
+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));
+
+
+} // namespace output_helper
+} // namespace vk
+
+
+#endif /* VIKIT_OUTPUT_HELPER_H_ */

GPU_version/vio/include/vio_gpu/params_helper.h

@@ -0,0 +1,54 @@
+/*
+ * ros_params_helper.h
+ *
+ *  Created on: Feb 22, 2013
+ *      Author: cforster
+ *
+ * from libpointmatcher_ros
+ */
+
+#ifndef ROS_PARAMS_HELPER_H_
+#define ROS_PARAMS_HELPER_H_
+
+#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 // ROS_PARAMS_HELPER_H_

GPU_version/vio/launch/Px30.launch

@@ -9,10 +9,10 @@
     <!-- Camera calibration file -->
     <rosparam file="$(find vio_gpu)/param/px30.yaml" />
 
-    <!-- Initial camera orientation, make it point downwards -->
-    <param name="init_rx" value="0.0" />
-    <param name="init_ry" value="-0.785398" /> <!--  -tetha (-35) -->
-    <param name="init_rz" value="3.14" /> <!-- 180  -->
+    <!-- IMU 2 Camera Frame -->
+    <param name="init_rx" value="1.88496" /><!--  90 + 18 -->
+    <param name="init_ry" value="0.0" /> <!--  00 -->
+    <param name="init_rz" value="0.0" /> <!-- 0  -->
 
   </node>
 

GPU_version/vio/param/camera_atan.yaml

@@ -1,8 +0,0 @@
-cam_model: ATAN
-cam_width: 752
-cam_height: 480
-cam_fx: 0.509326
-cam_fy: 0.796651
-cam_cx: 0.45905
-cam_cy: 0.510056
-cam_d0: 0.9320

GPU_version/vio/param/camera_pinhole.yaml

@@ -1,11 +0,0 @@
-cam_model: Pinhole
-cam_width: 640
-cam_height: 480
-cam_fx: 789.860721
-cam_fy: 790.097440
-cam_cx: 305.323524
-cam_cy: 260.464072
-cam_d0: 0.027470
-cam_d1: -0.051518
-cam_d2: 0.004375
-cam_d3: -0.004028

GPU_version/vio/param/unity.yaml

@@ -1,8 +0,0 @@
-cam_model: ATAN
-cam_width: 1024
-cam_height: 720
-cam_fx: 1.98248
-cam_fy: 2.68212
-cam_cx: 0.502524
-cam_cy: 0.497744
-cam_d0: -0.000499999

GPU_version/vio/param/vo_accurate.yaml

@@ -1,4 +0,0 @@
-grid_size: 20
-max_n_kfs: 50
-loba_num_iter: 10
-max_fts: 200

GPU_version/vio/param/vo_fast.yaml

@@ -4,10 +4,19 @@ svo:
   loba_num_iter: 0         #Number of iterations in the local bundle adjustment.
   quality_min_fts: 10      #If the number of tracked features drops below this threshold. Tracking quality is bad.
   quality_max_drop_fts: 60 #If within one frame, this amount of features are dropped. Tracking quality is bad.
-  ACC_noise: 0.00001          #acc white noise in continuous
-  GYO_noise: 0.00001          #gyro white noise in continuous
+  init_min_tracked: 100
+  init_min_disparity: 25
+  structureoptim_max_pts: 50
+  kfselect_mindist: 0.12
+  poseoptim_thresh: 4.0
+  ACC_noise: 0.0001          #acc white noise in continuous
+  GYO_noise: 0.0001          #gyro white noise in continuous
   IUC: 0.1                #integration uncertainty continuous
   ABC: 0.1                #acc bias in continuous
   GBC: 0.1                #gyro bias in continuous
   EBP: 0.1                #error in the bias used for preintegration
   NRP: 0.0000001                #noise used for projection in preintegration
+  gravity: 9.65
+  Acceleration_offset_x: -0.05
+  Acceleration_offset_y: -0.04
+  Gyroscope_offset_z: 0.001

GPU_version/vio/src/output_helper.cpp

@@ -0,0 +1,410 @@
+/*
+ * output_helper.cpp
+ *
+ *  Created on: Jan 20, 2013
+ *      Author: chrigi
+ */
+
+#include <vio_gpu/output_helper.h>
+#include <visualization_msgs/Marker.h>
+
+namespace vk {
+namespace output_helper {
+
+void
+publishTfTransform(const Sophus::SE3& T, const ros::Time& stamp,
+                   const string& frame_id, const string& child_frame_id,
+                   tf::TransformBroadcaster& br)
+{
+  tf::Transform transform_msg;
+  Eigen::Quaterniond q(T.rotation_matrix());
+  transform_msg.setOrigin(tf::Vector3(T.translation().x(), T.translation().y(), T.translation().z()));
+  tf::Quaternion tf_q; tf_q.setX(q.x()); tf_q.setY(q.y()); tf_q.setZ(q.z()); tf_q.setW(q.w());
+  transform_msg.setRotation(tf_q);
+  br.sendTransform(tf::StampedTransform(transform_msg, stamp, frame_id, child_frame_id));
+}
+
+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)
+{
+  visualization_msgs::Marker msg;
+  msg.header.frame_id = "/world";
+  msg.header.stamp = timestamp;
+  msg.ns = ns;
+  msg.id = id;
+  msg.type = visualization_msgs::Marker::CUBE;
+  msg.action = action; // 0 = add/modify
+  msg.scale.x = marker_scale;
+  msg.scale.y = marker_scale;
+  msg.scale.z = marker_scale;
+  msg.color.a = 1.0;
+  msg.color.r = color[0];
+  msg.color.g = color[1];
+  msg.color.b = color[2];
+  msg.lifetime = lifetime;
+  msg.pose.position.x = pos[0];
+  msg.pose.position.y = pos[1];
+  msg.pose.position.z = pos[2];
+  pub.publish(msg);
+}
+
+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)
+{
+  visualization_msgs::Marker msg;
+  msg.header.frame_id = "/world";
+  msg.header.stamp = timestamp;
+  msg.ns = ns;
+  msg.id = id;
+  msg.type = visualization_msgs::Marker::LINE_STRIP;
+  msg.action = action; // 0 = add/modify
+  msg.scale.x = marker_scale;
+  msg.color.a = 1.0;
+  msg.color.r = color[0];
+  msg.color.g = color[1];
+  msg.color.b = color[2];
+  msg.points.resize(2);
+  msg.lifetime = lifetime;
+  msg.points[0].x = start[0];
+  msg.points[0].y = start[1];
+  msg.points[0].z = start[2];
+  msg.points[1].x = end[0];
+  msg.points[1].y = end[1];
+  msg.points[1].z = end[2];
+  pub.publish(msg);
+}
+
+
+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)
+{
+  visualization_msgs::Marker msg;
+  msg.header.frame_id = "/world";
+  msg.header.stamp = timestamp;
+  msg.ns = ns;
+  msg.id = id;
+  msg.type = visualization_msgs::Marker::ARROW;
+  msg.action = action; // 0 = add/modify
+  msg.scale.x = marker_scale;
+  msg.scale.y = marker_scale*0.35;
+  msg.scale.z = 0.0;
+  msg.color.a = 1.0;
+  msg.color.r = color[0];
+  msg.color.g = color[1];
+  msg.color.b = color[2];
+  msg.points.resize(2);
+  msg.points[0].x = pos[0];
+  msg.points[0].y = pos[1];
+  msg.points[0].z = pos[2];
+  msg.points[1].x = pos[0] + scale*dir[0];
+  msg.points[1].y = pos[1] + scale*dir[1];
+  msg.points[1].z = pos[2] + scale*dir[2];
+  pub.publish(msg);
+}
+
+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)
+{
+  /*
+   * Function by Markus Achtelik from libsfly_viz.
+   * Thank you.
+   */
+  const double sqrt2_2 = sqrt(2) / 2;
+
+  visualization_msgs::Marker marker;
+
+  // the marker will be displayed in frame_id
+  marker.header.frame_id = frame_id;
+  marker.header.stamp = timestamp;
+  marker.ns = ns;
+  marker.action = 0;
+  marker.id = id;
+
+  // make rotors
+  marker.type = visualization_msgs::Marker::CYLINDER;
+  marker.scale.x = 0.2*marker_scale;
+  marker.scale.y = 0.2*marker_scale;
+  marker.scale.z = 0.01*marker_scale;
+  marker.color.r = 0.4;
+  marker.color.g = 0.4;
+  marker.color.b = 0.4;
+  marker.color.a = 0.8;
+  marker.pose.position.z = 0;
+
+  // front left/right
+  marker.pose.position.x = 0.19*marker_scale;
+  marker.pose.position.y = 0.11*marker_scale;
+  marker.id--;
+  pub.publish(marker);
+
+  marker.pose.position.x = 0.19*marker_scale;
+  marker.pose.position.y = -0.11*marker_scale;
+  marker.id--;
+  pub.publish(marker);
+
+  // left/right
+  marker.pose.position.x = 0;
+  marker.pose.position.y = 0.22*marker_scale;
+  marker.id--;
+  pub.publish(marker);
+
+  marker.pose.position.x = 0;
+  marker.pose.position.y = -0.22*marker_scale;
+  marker.id--;
+  pub.publish(marker);
+
+  // back left/right
+  marker.pose.position.x = -0.19*marker_scale;
+  marker.pose.position.y = 0.11*marker_scale;
+  marker.id--;
+  pub.publish(marker);
+
+  marker.pose.position.x = -0.19*marker_scale;
+  marker.pose.position.y = -0.11*marker_scale;
+  marker.id--;
+  pub.publish(marker);
+
+  // make arms
+  marker.type = visualization_msgs::Marker::CUBE;
+  marker.scale.x = 0.44*marker_scale;
+  marker.scale.y = 0.02*marker_scale;
+  marker.scale.z = 0.01*marker_scale;
+  marker.color.r = color[0];
+  marker.color.g = color[1];
+  marker.color.b = color[2];
+  marker.color.a = 1;
+
+  marker.pose.position.x = 0;
+  marker.pose.position.y = 0;
+  marker.pose.position.z = -0.015*marker_scale;
+  marker.pose.orientation.x = 0;
+  marker.pose.orientation.y = 0;
+
+  marker.pose.orientation.w = sqrt2_2;
+  marker.pose.orientation.z = sqrt2_2;
+  marker.id--;
+  pub.publish(marker);
+
+  // 30 deg rotation  0.9659  0  0  0.2588
+  marker.pose.orientation.w = 0.9659;
+  marker.pose.orientation.z = 0.2588;
+  marker.id--;
+  pub.publish(marker);
+
+  marker.pose.orientation.w = 0.9659;
+  marker.pose.orientation.z = -0.2588;
+  marker.id--;
+  pub.publish(marker);
+}
+
+void
+publishCameraMarker(ros::Publisher pub,
+                    const string& frame_id,
+                    const string& ns,
+                    const ros::Time& timestamp,
+                    int id,
+                    double marker_scale,
+                    const Vector3d& color)
+{
+  /*
+   * draw a pyramid as the camera marker
+   */
+  const double sqrt2_2 = sqrt(2) / 2;
+
+  visualization_msgs::Marker marker;
+
+  // the marker will be displayed in frame_id
+  marker.header.frame_id = frame_id;
+  marker.header.stamp = timestamp;
+  marker.ns = ns;
+  marker.action = 0;
+  marker.id = id;
+
+  // make rectangles as frame
+  double r_w = 1.0;
+  double z_plane = (r_w / 2.0)*marker_scale;
+  marker.pose.position.x = 0;
+  marker.pose.position.y = (r_w / 4.0) *marker_scale;
+  marker.pose.position.z = z_plane;
+
+  marker.type = visualization_msgs::Marker::CUBE;
+  marker.scale.x = r_w*marker_scale;
+  marker.scale.y = 0.04*marker_scale;
+  marker.scale.z = 0.04*marker_scale;
+  marker.color.r = color[0];
+  marker.color.g = color[1];
+  marker.color.b = color[2];
+  marker.color.a = 1;
+
+  marker.pose.orientation.x = 0;
+  marker.pose.orientation.y = 0;
+  marker.pose.orientation.z = 0;
+  marker.pose.orientation.w = 1;
+  marker.id--;
+  pub.publish(marker);
+  marker.pose.position.y = -(r_w/ 4.0)*marker_scale;
+  marker.id--;
+  pub.publish(marker);
+
+  marker.scale.x = (r_w/2.0)*marker_scale;
+  marker.pose.position.x = (r_w / 2.0) *marker_scale;
+  marker.pose.position.y = 0;
+  marker.pose.orientation.w = sqrt2_2;
+  marker.pose.orientation.z = sqrt2_2;
+  marker.id--;
+  pub.publish(marker);
+  marker.pose.position.x = -(r_w / 2.0) *marker_scale;
+  marker.id--;
+  pub.publish(marker);
+
+  // make pyramid edges
+  marker.scale.x = (3.0*r_w/4.0)*marker_scale;
+  marker.pose.position.z = 0.5*z_plane;
+
+  marker.pose.position.x = (r_w / 4.0) *marker_scale;
+  marker.pose.position.y = (r_w / 8.0) *marker_scale;
+  //  0.08198092, -0.34727674,  0.21462883,  0.9091823
+  marker.pose.orientation.x = 0.08198092;
+  marker.pose.orientation.y = -0.34727674;
+  marker.pose.orientation.z = 0.21462883;
+  marker.pose.orientation.w = 0.9091823;
+  marker.id--;
+  pub.publish(marker);
+
+  marker.pose.position.x = -(r_w / 4.0) *marker_scale;
+  marker.pose.position.y = (r_w / 8.0) *marker_scale;
+  // -0.27395078, -0.22863284,  0.9091823 ,  0.21462883
+  marker.pose.orientation.x = 0.08198092;
+  marker.pose.orientation.y = 0.34727674;
+  marker.pose.orientation.z = -0.21462883;
+  marker.pose.orientation.w = 0.9091823;
+  marker.id--;
+  pub.publish(marker);
+
+  marker.pose.position.x = -(r_w / 4.0) *marker_scale;
+  marker.pose.position.y = -(r_w / 8.0) *marker_scale;
+  //  -0.08198092,  0.34727674,  0.21462883,  0.9091823
+  marker.pose.orientation.x = -0.08198092;
+  marker.pose.orientation.y = 0.34727674;
+  marker.pose.orientation.z = 0.21462883;
+  marker.pose.orientation.w = 0.9091823;
+  marker.id--;
+  pub.publish(marker);
+
+  marker.pose.position.x = (r_w / 4.0) *marker_scale;
+  marker.pose.position.y = -(r_w / 8.0) *marker_scale;
+  // -0.08198092, -0.34727674, -0.21462883,  0.9091823
+  marker.pose.orientation.x = -0.08198092;
+  marker.pose.orientation.y = -0.34727674;
+  marker.pose.orientation.z = -0.21462883;
+  marker.pose.orientation.w = 0.9091823;
+  marker.id--;
+  pub.publish(marker);
+}
+
+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)
+{
+  visualization_msgs::Marker marker;
+  marker.header.frame_id = "/world";
+  marker.header.stamp = timestamp;
+  marker.ns = ns;
+  marker.id = id++;
+  marker.type = visualization_msgs::Marker::ARROW;
+  marker.action = action; // 0 = add/modify
+  marker.points.reserve(2);
+  geometry_msgs::Point point;
+  point.x = static_cast<float>(pos.x());
+  point.y = static_cast<float>(pos.y());
+  point.z = static_cast<float>(pos.z());
+  marker.points.push_back(point);
+  point.x = static_cast<float>(pos.x() + marker_scale*rot(0, 2)); // Draw arrow in z-direction
+  point.y = static_cast<float>(pos.y() + marker_scale*rot(1, 2)); // Draw arrow in z-direction
+  point.z = static_cast<float>(pos.z() + marker_scale*rot(2, 2)); // Draw arrow in z-direction
+  marker.points.push_back(point);
+  marker.scale.x = 0.5*marker_scale;
+  marker.scale.y = 0.5*marker_scale;
+  marker.color.a = 1.0;
+  marker.color.r = 0.0;
+  marker.color.g = 0.0;
+  marker.color.b = 1.0;
+  marker.lifetime = lifetime;
+  pub.publish(marker);
+
+  marker.id = id++;
+  marker.points.clear();
+  point.x = static_cast<float>(pos.x());
+  point.y = static_cast<float>(pos.y());
+  point.z = static_cast<float>(pos.z());
+  marker.points.push_back(point);
+  point.x = static_cast<float>(pos.x() + marker_scale*rot(0, 0)); // Draw arrow in x-direction
+  point.y = static_cast<float>(pos.y() + marker_scale*rot(1, 0)); // Draw arrow in x-direction
+  point.z = static_cast<float>(pos.z() + marker_scale*rot(2, 0)); // Draw arrow in x-direction
+  marker.points.push_back(point);
+  marker.color.r = 1.0;
+  marker.color.g = 0.0;
+  marker.color.b = 0.0;
+  marker.lifetime = lifetime;
+  pub.publish(marker);
+
+  marker.id = id++;
+  marker.points.clear();
+  point.x = static_cast<float>(pos.x());
+  point.y = static_cast<float>(pos.y());
+  point.z = static_cast<float>(pos.z());
+  marker.points.push_back(point);
+  point.x = static_cast<float>(pos.x() + marker_scale*rot(0, 1)); // Draw arrow in y-direction
+  point.y = static_cast<float>(pos.y() + marker_scale*rot(1, 1)); // Draw arrow in y-direction
+  point.z = static_cast<float>(pos.z() + marker_scale*rot(2, 1)); // Draw arrow in y-direction
+  marker.points.push_back(point);
+  marker.color.r = 0.0;
+  marker.color.g = 1.0;
+  marker.color.b = 0.0;
+  marker.lifetime = lifetime;
+  pub.publish(marker);
+}
+
+} // namespace output_helper
+} // namespace vk
+
+

GPU_version/vio/src/visualizer.cpp

@@ -21,8 +21,8 @@
 #include <gpu_svo/feature.h>
 #include <vio_gpu/Info.h>
 #include <geometry_msgs/PoseWithCovarianceStamped.h>
-#include <vikit/timer.h>
-#include <vikit/output_helper.h>
+#include <gpu_svo/timer.h>
+#include <vio_gpu/output_helper.h>
 #include <deque>
 
 namespace svo {
@@ -57,16 +57,18 @@ void Visualizer::publishMinimal(
   {
       vio_gpu::Info msg_info;
     msg_info.header = header_msg;
-    msg_info.processing_time = slam.lastProcessingTime();
+    //msg_info.processing_time = slam.lastProcessingTime();
     msg_info.keyframes.reserve(slam.map().keyframes_.size());
     for(list<FramePtr>::const_iterator it=slam.map().keyframes_.begin(); it!=slam.map().keyframes_.end(); ++it)
       msg_info.keyframes.push_back((*it)->id_);
     msg_info.stage = static_cast<int>(slam.stage());
     msg_info.tracking_quality = static_cast<int>(slam.trackingQuality());
+    /*
     if(frame != NULL)
       msg_info.num_matches = slam.lastNumObservations();
     else
       msg_info.num_matches = 0;
+      */
     pub_info_.publish(msg_info);
   }
   if(pub_pose_.getNumSubscribers() > 0 && slam.stage() == FrameHandlerBase::STAGE_DEFAULT_FRAME)

GPU_version/vio/src/vo_node.cpp

@@ -17,15 +17,15 @@
 #include <string>
 #include <gpu_svo/frame_handler_mono.h>
 #include <vio_gpu/visualizer.h>
-#include <vikit/params_helper.h>
+#include <gpu_svo/params_helper.h>
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/Imu.h>
 #include <image_transport/image_transport.h>
 #include <boost/thread.hpp>
 #include <cv_bridge/cv_bridge.h>
 #include <Eigen/Core>
-#include <vikit/abstract_camera.h>
-#include <vikit/camera_loader.h>
+#include <gpu_svo/abstract_camera.h>
+#include <vio_gpu/camera_loader.h>
 #include <gpu_svo/frame.h>