add GPU svo under development

GPU_version/svo/include/svo/bundle_adjustment.h

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

GPU_version/svo/include/svo/config.h

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

GPU_version/svo/include/svo/depth_filter.h

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

GPU_version/svo/include/svo/feature.h

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

GPU_version/svo/include/svo/feature_alignment.h

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

GPU_version/svo/include/svo/feature_detection.h

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

GPU_version/svo/include/svo/frame.h

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

GPU_version/svo/include/svo/frame_handler_base.h

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

GPU_version/svo/include/svo/frame_handler_mono.h

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

GPU_version/svo/include/svo/global.h

@@ -0,0 +1,101 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef SVO_GLOBAL_H_
+#define SVO_GLOBAL_H_
+
+#include <list>
+#include <vector>
+#include <string>
+#include <stdint.h>
+#include <stdio.h>
+#include <math.h>
+
+#include <Eigen/Core>
+#include <opencv2/opencv.hpp>
+#include <sophus/se3.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
+#define RPG_SVO_VIKIT_IS_VECTOR_SPECIALIZED
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector3d)
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2d)
+#endif
+
+#ifdef SVO_USE_ROS
+  #include <ros/console.h>
+  #define SVO_DEBUG_STREAM(x) ROS_DEBUG_STREAM(x)
+  #define SVO_INFO_STREAM(x) ROS_INFO_STREAM(x)
+  #define SVO_WARN_STREAM(x) ROS_WARN_STREAM(x)
+  #define SVO_WARN_STREAM_THROTTLE(rate, x) ROS_WARN_STREAM_THROTTLE(rate, x)
+  #define SVO_ERROR_STREAM(x) ROS_ERROR_STREAM(x)
+#else
+  #define SVO_INFO_STREAM(x) std::cerr<<"\033[0;0m[INFO] "<<x<<"\033[0;0m"<<std::endl;
+  #ifdef SVO_DEBUG_OUTPUT
+    #define SVO_DEBUG_STREAM(x) SVO_INFO_STREAM(x)
+  #else
+    #define SVO_DEBUG_STREAM(x)
+  #endif
+  #define SVO_WARN_STREAM(x) std::cerr<<"\033[0;33m[WARN] "<<x<<"\033[0;0m"<<std::endl;
+  #define SVO_ERROR_STREAM(x) std::cerr<<"\033[1;31m[ERROR] "<<x<<"\033[0;0m"<<std::endl;
+  #include <chrono> // Adapted from rosconsole. Copyright (c) 2008, Willow Garage, Inc.
+  #define SVO_WARN_STREAM_THROTTLE(rate, x) \
+    do { \
+      static double __log_stream_throttle__last_hit__ = 0.0; \
+      std::chrono::time_point<std::chrono::system_clock> __log_stream_throttle__now__ = \
+      std::chrono::system_clock::now(); \
+      if (__log_stream_throttle__last_hit__ + rate <= \
+          std::chrono::duration_cast<std::chrono::seconds>( \
+          __log_stream_throttle__now__.time_since_epoch()).count()) { \
+        __log_stream_throttle__last_hit__ = \
+        std::chrono::duration_cast<std::chrono::seconds>( \
+        __log_stream_throttle__now__.time_since_epoch()).count(); \
+        SVO_WARN_STREAM(x); \
+      } \
+    } while(0)
+#endif
+
+namespace svo
+{
+  using namespace Eigen;
+  using namespace Sophus;
+
+  const double EPS = 0.0000000001;
+  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)
+  #define SVO_LOG3(value1, value2, value3) SVO_LOG2(value1, value2); SVO_LOG(value3)
+  #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)
+  #define SVO_LOG3(v1, v2, v3)
+  #define SVO_LOG4(v1, v2, v3, v4)
+  #define SVO_START_TIMER(name)
+  #define SVO_STOP_TIMER(name)
+#endif
+
+  class Frame;
+  typedef boost::shared_ptr<Frame> FramePtr;
+} // namespace svo
+
+#endif // SVO_GLOBAL_H_

GPU_version/svo/include/svo/imu_integration.h

@@ -0,0 +1,121 @@
+//
+// Created by gufe on 4/9/21.
+//
+
+#ifndef SVO_IMU_INTEGRATION_H
+#define SVO_IMU_INTEGRATION_H
+#include <gtsam/navigation/CombinedImuFactor.h>
+#include <gtsam/navigation/ImuFactor.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/navigation/ImuBias.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/slam/ProjectionFactor.h>
+#include <gtsam/nonlinear/ISAM2.h>
+#include <sophus/se3.h>
+#include <svo/frame.h>
+
+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);
+}
+
+class Imu_Integration{
+public:
+    Imu_Integration(Sophus::SE3& SE_init);
+    ~Imu_Integration();
+    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);
+private:
+    std::shared_ptr<gtsam::NavState> statePtr;
+    std::shared_ptr<gtsam::imuBias::ConstantBias> imu_biasPtr;
+    std::shared_ptr<gtsam::Values> valuesPtr;
+    std::shared_ptr<gtsam::NonlinearFactorGraph> graphPtr;
+    boost::shared_ptr<gtsam::PreintegratedCombinedMeasurements::Params> parameterPtr;
+    std::shared_ptr<gtsam::PreintegratedCombinedMeasurements> preintegratedPtr;
+    boost::shared_ptr<gtsam::noiseModel::Isotropic> ProjectNoisePtr;
+    std::chrono::steady_clock::time_point t_1;
+    std::uint32_t imu_factor_id=0;
+    uint imu_n=0;
+    std::shared_ptr<gtsam::ISAM2Params> optimizerParamPtr;
+    bool syn=false;
+};
+#endif //SVO_IMU_INTEGRATION_H

GPU_version/svo/include/svo/initialization.h

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

GPU_version/svo/include/svo/map.h

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

GPU_version/svo/include/svo/matcher.h

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

GPU_version/svo/include/svo/point.h

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

GPU_version/svo/include/svo/pose_optimizer.h

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

GPU_version/svo/include/svo/reprojector.h

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

GPU_version/svo/include/svo/sparse_img_align.h

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

GPU_version/svo/include/svo/svoVisionFactor.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 <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