init GPU

CPU_version/svo/src/bundle_adjustment.cpp

@@ -0,0 +1,435 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <vikit/math_utils.h>
+#include <boost/thread.hpp>
+#include <g2o/core/sparse_optimizer.h>
+#include <g2o/core/block_solver.h>
+#include <g2o/core/solver.h>
+#include <g2o/core/robust_kernel_impl.h>
+#include <g2o/core/optimization_algorithm_levenberg.h>
+#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
+#include <g2o/solvers/csparse/linear_solver_csparse.h>
+#include <g2o/solvers/dense/linear_solver_dense.h>
+#include <g2o/types/sba/types_six_dof_expmap.h>
+#include <g2o/solvers/structure_only/structure_only_solver.h>
+#include <svo/bundle_adjustment.h>
+#include <svo/frame.h>
+#include <svo/feature.h>
+#include <svo/point.h>
+#include <svo/config.h>
+#include <svo/map.h>
+
+#define SCHUR_TRICK 1
+
+namespace svo {
+namespace ba {
+
+void twoViewBA(
+    Frame* frame1,
+    Frame* frame2,
+    double reproj_thresh,
+    Map* map)
+{
+  // scale reprojection threshold in pixels to unit plane
+  reproj_thresh /= frame1->cam_->errorMultiplier2();
+
+  // init g2o
+  g2o::SparseOptimizer optimizer;
+  setupG2o(&optimizer);
+
+  list<EdgeContainerSE3> edges;
+  size_t v_id = 0;
+
+  // New Keyframe Vertex 1: This Keyframe is set to fixed!
+  g2oFrameSE3* v_frame1 = createG2oFrameSE3(frame1, v_id++, true);
+  optimizer.addVertex(v_frame1);
+
+  // New Keyframe Vertex 2
+  g2oFrameSE3* v_frame2 = createG2oFrameSE3(frame2, v_id++, false);
+  optimizer.addVertex(v_frame2);
+
+  // Create Point Vertices
+  for(Features::iterator it_ftr=frame1->fts_.begin(); it_ftr!=frame1->fts_.end(); ++it_ftr)
+  {
+    Point* pt = (*it_ftr)->point;
+    if(pt == NULL)
+      continue;
+    g2oPoint* v_pt = createG2oPoint(pt->pos_, v_id++, false);
+    optimizer.addVertex(v_pt);
+    pt->v_pt_ = v_pt;
+    g2oEdgeSE3* e = createG2oEdgeSE3(v_frame1, v_pt, vk::project2d((*it_ftr)->f), true, reproj_thresh*Config::lobaRobustHuberWidth());
+    optimizer.addEdge(e);
+    edges.push_back(EdgeContainerSE3(e, frame1, *it_ftr)); // TODO feature now links to frame, so we can simplify edge container!
+
+    // find at which index the second frame observes the point
+    Feature* ftr_frame2 = pt->findFrameRef(frame2);
+    e = createG2oEdgeSE3(v_frame2, v_pt, vk::project2d(ftr_frame2->f), true, reproj_thresh*Config::lobaRobustHuberWidth());
+    optimizer.addEdge(e);
+    edges.push_back(EdgeContainerSE3(e, frame2, ftr_frame2));
+  }
+
+  // Optimization
+  double init_error, final_error;
+  runSparseBAOptimizer(&optimizer, Config::lobaNumIter(), init_error, final_error);
+  printf("2-View BA: Error before/after = %f / %f\n", init_error, final_error);
+
+  // Update Keyframe Positions
+  frame1->T_f_w_.rotation_matrix() = v_frame1->estimate().rotation().toRotationMatrix();
+  frame1->T_f_w_.translation() = v_frame1->estimate().translation();
+  frame2->T_f_w_.rotation_matrix() = v_frame2->estimate().rotation().toRotationMatrix();
+  frame2->T_f_w_.translation() = v_frame2->estimate().translation();
+
+  // Update Mappoint Positions
+  for(Features::iterator it=frame1->fts_.begin(); it!=frame1->fts_.end(); ++it)
+  {
+    if((*it)->point == NULL)
+     continue;
+    (*it)->point->pos_ = (*it)->point->v_pt_->estimate();
+    (*it)->point->v_pt_ = NULL;
+  }
+
+  // Find Mappoints with too large reprojection error
+  const double reproj_thresh_squared = reproj_thresh*reproj_thresh;
+  size_t n_incorrect_edges = 0;
+  for(list<EdgeContainerSE3>::iterator it_e = edges.begin(); it_e != edges.end(); ++it_e)
+    if(it_e->edge->chi2() > reproj_thresh_squared)
+    {
+      if(it_e->feature->point != NULL)
+      {
+        map->safeDeletePoint(it_e->feature->point);
+        it_e->feature->point = NULL;
+      }
+      ++n_incorrect_edges;
+    }
+
+  printf("2-View BA: Wrong edges =  %zu\n", n_incorrect_edges);
+}
+
+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)
+{
+
+  // init g2o
+  g2o::SparseOptimizer optimizer;
+  setupG2o(&optimizer);
+
+  list<EdgeContainerSE3> edges;
+  set<Point*> mps;
+  list<Frame*> neib_kfs;
+  size_t v_id = 0;
+  size_t n_mps = 0;
+  size_t n_fix_kfs = 0;
+  size_t n_var_kfs = 1;
+  size_t n_edges = 0;
+  n_incorrect_edges_1 = 0;
+  n_incorrect_edges_2 = 0;
+
+  // Add all core keyframes
+  for(set<FramePtr>::iterator it_kf = core_kfs->begin(); it_kf != core_kfs->end(); ++it_kf)
+  {
+    g2oFrameSE3* v_kf = createG2oFrameSE3(it_kf->get(), v_id++, false);
+    (*it_kf)->v_kf_ = v_kf;
+    ++n_var_kfs;
+    assert(optimizer.addVertex(v_kf));
+
+    // all points that the core keyframes observe are also optimized:
+    for(Features::iterator it_pt=(*it_kf)->fts_.begin(); it_pt!=(*it_kf)->fts_.end(); ++it_pt)
+      if((*it_pt)->point != NULL)
+        mps.insert((*it_pt)->point);
+  }
+
+  // Now go throug all the points and add a measurement. Add a fixed neighbour
+  // Keyframe if it is not in the set of core kfs
+  double reproj_thresh_2 = Config::lobaThresh() / center_kf->cam_->errorMultiplier2();
+  double reproj_thresh_1 = Config::poseOptimThresh() / center_kf->cam_->errorMultiplier2();
+  double reproj_thresh_1_squared = reproj_thresh_1*reproj_thresh_1;
+  for(set<Point*>::iterator it_pt = mps.begin(); it_pt!=mps.end(); ++it_pt)
+  {
+    // Create point vertex
+    g2oPoint* v_pt = createG2oPoint((*it_pt)->pos_, v_id++, false);
+    (*it_pt)->v_pt_ = v_pt;
+    assert(optimizer.addVertex(v_pt));
+    ++n_mps;
+
+    // Add edges
+    list<Feature*>::iterator it_obs=(*it_pt)->obs_.begin();
+    while(it_obs!=(*it_pt)->obs_.end())
+    {
+      Vector2d error = vk::project2d((*it_obs)->f) - vk::project2d((*it_obs)->frame->w2f((*it_pt)->pos_));
+
+      if((*it_obs)->frame->v_kf_ == NULL)
+      {
+        // frame does not have a vertex yet -> it belongs to the neib kfs and
+        // is fixed. create one:
+        g2oFrameSE3* v_kf = createG2oFrameSE3((*it_obs)->frame, v_id++, true);
+        (*it_obs)->frame->v_kf_ = v_kf;
+        ++n_fix_kfs;
+        assert(optimizer.addVertex(v_kf));
+        neib_kfs.push_back((*it_obs)->frame);
+      }
+
+      // create edge
+      g2oEdgeSE3* e = createG2oEdgeSE3((*it_obs)->frame->v_kf_, v_pt,
+                                       vk::project2d((*it_obs)->f),
+                                       true,
+                                       reproj_thresh_2*Config::lobaRobustHuberWidth(),
+                                       1.0 / (1<<(*it_obs)->level));
+      assert(optimizer.addEdge(e));
+      edges.push_back(EdgeContainerSE3(e, (*it_obs)->frame, *it_obs));
+      ++n_edges;
+      ++it_obs;
+    }
+  }
+
+  // structure only
+  g2o::StructureOnlySolver<3> structure_only_ba;
+  g2o::OptimizableGraph::VertexContainer points;
+  for (g2o::OptimizableGraph::VertexIDMap::const_iterator it = optimizer.vertices().begin(); it != optimizer.vertices().end(); ++it)
+  {
+    g2o::OptimizableGraph::Vertex* v = static_cast<g2o::OptimizableGraph::Vertex*>(it->second);
+      if (v->dimension() == 3 && v->edges().size() >= 2)
+        points.push_back(v);
+  }
+  structure_only_ba.calc(points, 10);
+
+  // Optimization
+  if(Config::lobaNumIter() > 0)
+    runSparseBAOptimizer(&optimizer, Config::lobaNumIter(), init_error, final_error);
+
+  // Update Keyframes
+  for(set<FramePtr>::iterator it = core_kfs->begin(); it != core_kfs->end(); ++it)
+  {
+    (*it)->T_f_w_ = SE3( (*it)->v_kf_->estimate().rotation(),
+                         (*it)->v_kf_->estimate().translation());
+    (*it)->v_kf_ = NULL;
+  }
+
+  for(list<Frame*>::iterator it = neib_kfs.begin(); it != neib_kfs.end(); ++it)
+    (*it)->v_kf_ = NULL;
+
+  // Update Mappoints
+  for(set<Point*>::iterator it = mps.begin(); it != mps.end(); ++it)
+  {
+    (*it)->pos_ = (*it)->v_pt_->estimate();
+    (*it)->v_pt_ = NULL;
+  }
+
+  // Remove Measurements with too large reprojection error
+  double reproj_thresh_2_squared = reproj_thresh_2*reproj_thresh_2;
+  for(list<EdgeContainerSE3>::iterator it = edges.begin(); it != edges.end(); ++it)
+  {
+    if(it->edge->chi2() > reproj_thresh_2_squared) //*(1<<it->feature_->level))
+    {
+      map->removePtFrameRef(it->frame, it->feature);
+      ++n_incorrect_edges_2;
+    }
+  }
+
+  // TODO: delete points and edges!
+  init_error = sqrt(init_error)*center_kf->cam_->errorMultiplier2();
+  final_error = sqrt(final_error)*center_kf->cam_->errorMultiplier2();
+}
+
+void globalBA(Map* map)
+{
+  // init g2o
+  g2o::SparseOptimizer optimizer;
+  setupG2o(&optimizer);
+
+  list<EdgeContainerSE3> edges;
+  list< pair<FramePtr,Feature*> > incorrect_edges;
+
+  // Go through all Keyframes
+  size_t v_id = 0;
+  double reproj_thresh_2 = Config::lobaThresh() / map->lastKeyframe()->cam_->errorMultiplier2();
+  double reproj_thresh_1_squared = Config::poseOptimThresh() / map->lastKeyframe()->cam_->errorMultiplier2();
+  reproj_thresh_1_squared *= reproj_thresh_1_squared;
+  for(list<FramePtr>::iterator it_kf = map->keyframes_.begin();
+      it_kf != map->keyframes_.end(); ++it_kf)
+  {
+    // New Keyframe Vertex
+    g2oFrameSE3* v_kf = createG2oFrameSE3(it_kf->get(), v_id++, false);
+    (*it_kf)->v_kf_ = v_kf;
+    optimizer.addVertex(v_kf);
+    for(Features::iterator it_ftr=(*it_kf)->fts_.begin(); it_ftr!=(*it_kf)->fts_.end(); ++it_ftr)
+    {
+      // for each keyframe add edges to all observed mapoints
+      Point* mp = (*it_ftr)->point;
+      if(mp == NULL)
+        continue;
+      g2oPoint* v_mp = mp->v_pt_;
+      if(v_mp == NULL)
+      {
+        // mappoint-vertex doesn't exist yet. create a new one:
+        v_mp = createG2oPoint(mp->pos_, v_id++, false);
+        mp->v_pt_ = v_mp;
+        optimizer.addVertex(v_mp);
+      }
+
+      // Due to merging of mappoints it is possible that references in kfs suddenly
+      // have a very large reprojection error which may result in distorted results.
+      Vector2d error = vk::project2d((*it_ftr)->f) - vk::project2d((*it_kf)->w2f(mp->pos_));
+      if(error.squaredNorm() > reproj_thresh_1_squared)
+        incorrect_edges.push_back(pair<FramePtr,Feature*>(*it_kf, *it_ftr));
+      else
+      {
+        g2oEdgeSE3* e = createG2oEdgeSE3(v_kf, v_mp, vk::project2d((*it_ftr)->f),
+                                         true,
+                                         reproj_thresh_2*Config::lobaRobustHuberWidth());
+
+        edges.push_back(EdgeContainerSE3(e, it_kf->get(), *it_ftr));
+        optimizer.addEdge(e);
+      }
+    }
+  }
+
+  // Optimization
+  double init_error=0.0, final_error=0.0;
+  if(Config::lobaNumIter() > 0)
+    runSparseBAOptimizer(&optimizer, Config::lobaNumIter(), init_error, final_error);
+
+  // Update Keyframe and MapPoint Positions
+  for(list<FramePtr>::iterator it_kf = map->keyframes_.begin();
+        it_kf != map->keyframes_.end(); ++it_kf)
+  {
+    (*it_kf)->T_f_w_ = SE3( (*it_kf)->v_kf_->estimate().rotation(),
+                            (*it_kf)->v_kf_->estimate().translation());
+    (*it_kf)->v_kf_ = NULL;
+    for(Features::iterator it_ftr=(*it_kf)->fts_.begin(); it_ftr!=(*it_kf)->fts_.end(); ++it_ftr)
+    {
+      Point* mp = (*it_ftr)->point;
+      if(mp == NULL)
+        continue;
+      if(mp->v_pt_ == NULL)
+        continue;       // mp was updated before
+      mp->pos_ = mp->v_pt_->estimate();
+      mp->v_pt_ = NULL;
+    }
+  }
+
+  // Remove Measurements with too large reprojection error
+  for(list< pair<FramePtr,Feature*> >::iterator it=incorrect_edges.begin();
+      it!=incorrect_edges.end(); ++it)
+    map->removePtFrameRef(it->first.get(), it->second);
+
+  double reproj_thresh_2_squared = reproj_thresh_2*reproj_thresh_2;
+  for(list<EdgeContainerSE3>::iterator it = edges.begin(); it != edges.end(); ++it)
+  {
+    if(it->edge->chi2() > reproj_thresh_2_squared)
+    {
+      map->removePtFrameRef(it->frame, it->feature);
+    }
+  }
+}
+
+void setupG2o(g2o::SparseOptimizer * optimizer)
+{
+  // TODO: What's happening with all this HEAP stuff? Memory Leak?
+  optimizer->setVerbose(false);
+
+#if SCHUR_TRICK
+  // solver
+  g2o::BlockSolver_6_3::LinearSolverType* linearSolver;
+  linearSolver = new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>();
+  //linearSolver = new g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType>();
+
+  g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
+  g2o::OptimizationAlgorithmLevenberg * solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
+#else
+  g2o::BlockSolverX::LinearSolverType * linearSolver;
+  linearSolver = new g2o::LinearSolverCholmod<g2o::BlockSolverX::PoseMatrixType>();
+  //linearSolver = new g2o::LinearSolverCSparse<g2o::BlockSolverX::PoseMatrixType>();
+  g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver);
+  g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
+#endif
+
+  solver->setMaxTrialsAfterFailure(5);
+  optimizer->setAlgorithm(solver);
+
+  // setup camera
+  g2o::CameraParameters * cam_params = new g2o::CameraParameters(1.0, Vector2d(0.,0.), 0.);
+  cam_params->setId(0);
+  if (!optimizer->addParameter(cam_params)) {
+    assert(false);
+  }
+}
+
+void
+runSparseBAOptimizer(g2o::SparseOptimizer* optimizer,
+                     unsigned int num_iter,
+                     double& init_error, double& final_error)
+{
+  optimizer->initializeOptimization();
+  optimizer->computeActiveErrors();
+  init_error = optimizer->activeChi2();
+  optimizer->optimize(num_iter);
+  final_error = optimizer->activeChi2();
+}
+
+g2oFrameSE3*
+createG2oFrameSE3(Frame* frame, size_t id, bool fixed)
+{
+  g2oFrameSE3* v = new g2oFrameSE3();
+  v->setId(id);
+  v->setFixed(fixed);
+  v->setEstimate(g2o::SE3Quat(frame->T_f_w_.unit_quaternion(), frame->T_f_w_.translation()));
+  return v;
+}
+
+g2oPoint*
+createG2oPoint(Vector3d pos,
+               size_t id,
+               bool fixed)
+{
+  g2oPoint* v = new g2oPoint();
+  v->setId(id);
+#if SCHUR_TRICK
+  v->setMarginalized(true);
+#endif
+  v->setFixed(fixed);
+  v->setEstimate(pos);
+  return v;
+}
+
+g2oEdgeSE3*
+createG2oEdgeSE3( g2oFrameSE3* v_frame,
+                  g2oPoint* v_point,
+                  const Vector2d& f_up,
+                  bool robust_kernel,
+                  double huber_width,
+                  double weight)
+{
+  g2oEdgeSE3* e = new g2oEdgeSE3();
+  e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_point));
+  e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_frame));
+  e->setMeasurement(f_up);
+  e->information() = weight * Eigen::Matrix2d::Identity(2,2);
+  g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber();      // TODO: memory leak
+  rk->setDelta(huber_width);
+  e->setRobustKernel(rk);
+  e->setParameterId(0, 0); //old: e->setId(v_point->id());
+  return e;
+}
+
+} // namespace ba
+} // namespace svo

CPU_version/svo/src/config.cpp

@@ -0,0 +1,102 @@
+// 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/>.
+
+#ifdef SVO_USE_ROS
+#include <vikit/params_helper.h>
+#endif
+#include <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)),
+    use_imu(vk::getParam<bool>("svo/use_imu", false)),
+    core_n_kfs(vk::getParam<int>("svo/core_n_kfs", 3)),
+    map_scale(vk::getParam<double>("svo/map_scale", 1.0)),
+    grid_size(vk::getParam<int>("svo/grid_size", 30)),
+    init_min_disparity(vk::getParam<double>("svo/init_min_disparity", 50.0)),
+    init_min_tracked(vk::getParam<int>("svo/init_min_tracked", 50)),
+    init_min_inliers(vk::getParam<int>("svo/init_min_inliers", 40)),
+    klt_max_level(vk::getParam<int>("svo/klt_max_level", 4)),
+    klt_min_level(vk::getParam<int>("svo/klt_min_level", 2)),
+    reproj_thresh(vk::getParam<double>("svo/reproj_thresh", 2.0)),
+    poseoptim_thresh(vk::getParam<double>("svo/poseoptim_thresh", 2.0)),
+    poseoptim_num_iter(vk::getParam<int>("svo/poseoptim_num_iter", 10)),
+    structureoptim_max_pts(vk::getParam<int>("svo/structureoptim_max_pts", 20)),
+    structureoptim_num_iter(vk::getParam<int>("svo/structureoptim_num_iter", 5)),
+    loba_thresh(vk::getParam<double>("svo/loba_thresh", 2.0)),
+    loba_robust_huber_width(vk::getParam<double>("svo/loba_robust_huber_width", 1.0)),
+    loba_num_iter(vk::getParam<int>("svo/loba_num_iter", 0)),
+    kfselect_mindist(vk::getParam<double>("svo/kfselect_mindist", 0.12)),
+    triang_min_corner_score(vk::getParam<double>("svo/triang_min_corner_score", 20.0)),
+    triang_half_patch_size(vk::getParam<int>("svo/triang_half_patch_size", 4)),
+    subpix_n_iter(vk::getParam<int>("svo/subpix_n_iter", 10)),
+    max_n_kfs(vk::getParam<int>("svo/max_n_kfs", 10)),
+    img_imu_delay(vk::getParam<double>("svo/img_imu_delay", 0.0)),
+    max_fts(vk::getParam<int>("svo/max_fts", 120)),
+    quality_min_fts(vk::getParam<int>("svo/quality_min_fts", 50)),
+    quality_max_drop_fts(vk::getParam<int>("svo/quality_max_drop_fts", 40)),
+    ACC_noise(vk::getParam<double>("svo/ACC_noise", 1e-4)),
+    GYO_noise(vk::getParam<double>("svo/GYO_noise", 1e-4)),
+    iuc(vk::getParam<double>("svo/IUC", 1e-1)),
+    abc(vk::getParam<double>("svo/ABC", 1e-1)),
+    gbc(vk::getParam<double>("svo/GBC", 1e-1)),
+    ebp(vk::getParam<double>("svo/EBP", 1e-2)),
+    nrp(vk::getParam<double>("svo/NRP", 1e-5))
+#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
+{}
+
+Config& Config::getInstance()
+{
+  static Config instance; // Instantiated on first use and guaranteed to be destroyed
+  return instance;
+}
+
+} // namespace svo
+

CPU_version/svo/src/depth_filter.cpp

@@ -0,0 +1,352 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <algorithm>
+#include <vikit/math_utils.h>
+#include <vikit/abstract_camera.h>
+#include <vikit/vision.h>
+#include <boost/bind.hpp>
+#include <boost/math/distributions/normal.hpp>
+#include <svo/global.h>
+#include <svo/depth_filter.h>
+#include <svo/frame.h>
+#include <svo/point.h>
+#include <svo/feature.h>
+#include <svo/matcher.h>
+#include <svo/config.h>
+#include <svo/feature_detection.h>
+
+namespace svo {
+
+int Seed::batch_counter = 0;
+int Seed::seed_counter = 0;
+
+Seed::Seed(Feature* ftr, float depth_mean, float depth_min) :
+    batch_id(batch_counter),
+    id(seed_counter++),
+    ftr(ftr),
+    a(10),
+    b(10),
+    mu(1.0/depth_mean),
+    z_range(1.0/depth_min),
+    sigma2(z_range*z_range/36)
+{}
+
+DepthFilter::DepthFilter(feature_detection::DetectorPtr feature_detector, callback_t seed_converged_cb) :
+    feature_detector_(feature_detector),
+    seed_converged_cb_(seed_converged_cb),
+    seeds_updating_halt_(false),
+    thread_(NULL),
+    new_keyframe_set_(false),
+    new_keyframe_min_depth_(0.0),
+    new_keyframe_mean_depth_(0.0)
+{}
+
+DepthFilter::~DepthFilter()
+{
+  stopThread();
+  SVO_INFO_STREAM("DepthFilter destructed.");
+}
+
+void DepthFilter::startThread()
+{
+  thread_ = new boost::thread(&DepthFilter::updateSeedsLoop, this);
+}
+
+void DepthFilter::stopThread()
+{
+  SVO_INFO_STREAM("DepthFilter stop thread invoked.");
+  if(thread_ != NULL)
+  {
+    SVO_INFO_STREAM("DepthFilter interrupt and join thread... ");
+    seeds_updating_halt_ = true;
+    thread_->interrupt();
+    thread_->join();
+    thread_ = NULL;
+  }
+}
+
+void DepthFilter::addFrame(FramePtr frame)
+{
+  if(thread_ != NULL)
+  {
+    {
+      lock_t lock(frame_queue_mut_);
+      if(frame_queue_.size() > 2)
+        frame_queue_.pop();
+      frame_queue_.push(frame);
+    }
+    seeds_updating_halt_ = false;
+    frame_queue_cond_.notify_one();
+  }
+  else
+    updateSeeds(frame);
+}
+
+void DepthFilter::addKeyframe(FramePtr frame, double depth_mean, double depth_min)
+{
+  new_keyframe_min_depth_ = depth_min;
+  new_keyframe_mean_depth_ = depth_mean;
+  if(thread_ != NULL)
+  {
+    new_keyframe_ = frame;
+    new_keyframe_set_ = true;
+    seeds_updating_halt_ = true;
+    frame_queue_cond_.notify_one();
+  }
+  else
+    initializeSeeds(frame);
+}
+
+void DepthFilter::initializeSeeds(FramePtr frame)
+{
+  Features new_features;
+  feature_detector_->setExistingFeatures(frame->fts_);
+  feature_detector_->detect(frame.get(), frame->img_pyr_,
+                            Config::triangMinCornerScore(), new_features);
+
+  // initialize a seed for every new feature
+  seeds_updating_halt_ = true;
+  lock_t lock(seeds_mut_); // by locking the updateSeeds function stops
+  ++Seed::batch_counter;
+  std::for_each(new_features.begin(), new_features.end(), [&](Feature* ftr){
+    seeds_.push_back(Seed(ftr, new_keyframe_mean_depth_, new_keyframe_min_depth_));
+  });
+
+  if(options_.verbose)
+    SVO_INFO_STREAM("DepthFilter: Initialized "<<new_features.size()<<" new seeds");
+  seeds_updating_halt_ = false;
+}
+
+void DepthFilter::removeKeyframe(FramePtr frame)
+{
+  seeds_updating_halt_ = true;
+  lock_t lock(seeds_mut_);
+  std::list<Seed>::iterator it=seeds_.begin();
+  size_t n_removed = 0;
+  while(it!=seeds_.end())
+  {
+    if(it->ftr->frame == frame.get())
+    {
+      it = seeds_.erase(it);
+      ++n_removed;
+    }
+    else
+      ++it;
+  }
+  seeds_updating_halt_ = false;
+}
+
+void DepthFilter::reset()
+{
+  seeds_updating_halt_ = true;
+  {
+    lock_t lock(seeds_mut_);
+    seeds_.clear();
+  }
+  lock_t lock();
+  while(!frame_queue_.empty())
+    frame_queue_.pop();
+  seeds_updating_halt_ = false;
+
+  if(options_.verbose)
+    SVO_INFO_STREAM("DepthFilter: RESET.");
+}
+
+void DepthFilter::updateSeedsLoop()
+{
+  while(!boost::this_thread::interruption_requested())
+  {
+    FramePtr frame;
+    {
+      lock_t lock(frame_queue_mut_);
+      while(frame_queue_.empty() && new_keyframe_set_ == false)
+        frame_queue_cond_.wait(lock);
+      if(new_keyframe_set_)
+      {
+        new_keyframe_set_ = false;
+        seeds_updating_halt_ = false;
+        clearFrameQueue();
+        frame = new_keyframe_;
+      }
+      else
+      {
+        frame = frame_queue_.front();
+        frame_queue_.pop();
+      }
+    }
+    updateSeeds(frame);
+    if(frame->isKeyframe())
+      initializeSeeds(frame);
+  }
+}
+
+void DepthFilter::updateSeeds(FramePtr frame)
+{
+  // update only a limited number of seeds, because we don't have time to do it
+  // for all the seeds in every frame!
+  size_t n_updates=0, n_failed_matches=0, n_seeds = seeds_.size();
+  lock_t lock(seeds_mut_);
+  std::list<Seed>::iterator it=seeds_.begin();
+
+  const double focal_length = frame->cam_->errorMultiplier2();
+  double px_noise = 1.0;
+  double px_error_angle = atan(px_noise/(2.0*focal_length))*2.0; // law of chord (sehnensatz)
+
+  while( it!=seeds_.end())
+  {
+    // set this value true when seeds updating should be interrupted
+    if(seeds_updating_halt_)
+      return;
+
+    // check if seed is not already too old
+    if((Seed::batch_counter - it->batch_id) > options_.max_n_kfs) {
+      it = seeds_.erase(it);
+      continue;
+    }
+
+    // check if point is visible in the current image
+    SE3 T_ref_cur = it->ftr->frame->T_f_w_ * frame->T_f_w_.inverse();
+    const Vector3d xyz_f(T_ref_cur.inverse()*(1.0/it->mu * it->ftr->f) );
+    if(xyz_f.z() < 0.0)  {
+      ++it; // behind the camera
+      continue;
+    }
+    if(!frame->cam_->isInFrame(frame->f2c(xyz_f).cast<int>())) {
+      ++it; // point does not project in image
+      continue;
+    }
+
+    // we are using inverse depth coordinates
+    float z_inv_min = it->mu + sqrt(it->sigma2);
+    float z_inv_max = max(it->mu - sqrt(it->sigma2), 0.00000001f);
+    double z;
+    if(!matcher_.findEpipolarMatchDirect(
+        *it->ftr->frame, *frame, *it->ftr, 1.0/it->mu, 1.0/z_inv_min, 1.0/z_inv_max, z))
+    {
+      it->b++; // increase outlier probability when no match was found
+      ++it;
+      ++n_failed_matches;
+      continue;
+    }
+
+    // compute tau
+    double tau = computeTau(T_ref_cur, it->ftr->f, z, px_error_angle);
+    double tau_inverse = 0.5 * (1.0/max(0.0000001, z-tau) - 1.0/(z+tau));
+
+    // update the estimate
+    updateSeed(1./z, tau_inverse*tau_inverse, &*it);
+    ++n_updates;
+
+    if(frame->isKeyframe())
+    {
+      // The feature detector should not initialize new seeds close to this location
+      feature_detector_->setGridOccpuancy(matcher_.px_cur_);
+    }
+
+    // if the seed has converged, we initialize a new candidate point and remove the seed
+    if(sqrt(it->sigma2) < it->z_range/options_.seed_convergence_sigma2_thresh)
+    {
+      assert(it->ftr->point == NULL); // TODO this should not happen anymore
+      Vector3d xyz_world(it->ftr->frame->T_f_w_.inverse() * (it->ftr->f * (1.0/it->mu)));
+      Point* point = new Point(xyz_world, it->ftr);
+      it->ftr->point = point;
+      /* FIXME it is not threadsafe to add a feature to the frame here.
+      if(frame->isKeyframe())
+      {
+        Feature* ftr = new Feature(frame.get(), matcher_.px_cur_, matcher_.search_level_);
+        ftr->point = point;
+        point->addFrameRef(ftr);
+        frame->addFeature(ftr);
+        it->ftr->frame->addFeature(it->ftr);
+      }
+      else
+      */
+      {
+        seed_converged_cb_(point, it->sigma2); // put in candidate list
+      }
+      it = seeds_.erase(it);
+    }
+    else if(isnan(z_inv_min))
+    {
+      SVO_WARN_STREAM("z_min is NaN");
+      it = seeds_.erase(it);
+    }
+    else
+      ++it;
+  }
+}
+
+void DepthFilter::clearFrameQueue()
+{
+  while(!frame_queue_.empty())
+    frame_queue_.pop();
+}
+
+void DepthFilter::getSeedsCopy(const FramePtr& frame, std::list<Seed>& seeds)
+{
+  lock_t lock(seeds_mut_);
+  for(std::list<Seed>::iterator it=seeds_.begin(); it!=seeds_.end(); ++it)
+  {
+    if (it->ftr->frame == frame.get())
+      seeds.push_back(*it);
+  }
+}
+
+void DepthFilter::updateSeed(const float x, const float tau2, Seed* seed)
+{
+  float norm_scale = sqrt(seed->sigma2 + tau2);
+  if(std::isnan(norm_scale))
+    return;
+  boost::math::normal_distribution<float> nd(seed->mu, norm_scale);
+  float s2 = 1./(1./seed->sigma2 + 1./tau2);
+  float m = s2*(seed->mu/seed->sigma2 + x/tau2);
+  float C1 = seed->a/(seed->a+seed->b) * boost::math::pdf(nd, x);
+  float C2 = seed->b/(seed->a+seed->b) * 1./seed->z_range;
+  float normalization_constant = C1 + C2;
+  C1 /= normalization_constant;
+  C2 /= normalization_constant;
+  float f = C1*(seed->a+1.)/(seed->a+seed->b+1.) + C2*seed->a/(seed->a+seed->b+1.);
+  float e = C1*(seed->a+1.)*(seed->a+2.)/((seed->a+seed->b+1.)*(seed->a+seed->b+2.))
+          + C2*seed->a*(seed->a+1.0f)/((seed->a+seed->b+1.0f)*(seed->a+seed->b+2.0f));
+
+  // update parameters
+  float mu_new = C1*m+C2*seed->mu;
+  seed->sigma2 = C1*(s2 + m*m) + C2*(seed->sigma2 + seed->mu*seed->mu) - mu_new*mu_new;
+  seed->mu = mu_new;
+  seed->a = (e-f)/(f-e/f);
+  seed->b = seed->a*(1.0f-f)/f;
+}
+
+double DepthFilter::computeTau(
+      const SE3& T_ref_cur,
+      const Vector3d& f,
+      const double z,
+      const double px_error_angle)
+{
+  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
+}
+
+} // namespace svo

CPU_version/svo/src/feature_alignment.cpp

@@ -0,0 +1,601 @@
+// 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/>.
+
+#ifdef __SSE2__
+#include <emmintrin.h>
+#endif
+#ifdef __ARM_NEON__
+#include <arm_neon.h>
+#endif
+#include <svo/feature_alignment.h>
+
+namespace svo {
+namespace feature_alignment {
+
+#define SUBPIX_VERBOSE 0
+
+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)
+{
+  const int halfpatch_size_ = 4;
+  const int patch_size = 8;
+  const int patch_area = 64;
+  bool converged=false;
+
+  // compute derivative of template and prepare inverse compositional
+  float __attribute__((__aligned__(16))) ref_patch_dv[patch_area];
+  Matrix2f H; H.setZero();
+
+  // compute gradient and hessian
+  const int ref_step = patch_size+2;
+  float* it_dv = ref_patch_dv;
+  for(int y=0; y<patch_size; ++y)
+  {
+    uint8_t* it = ref_patch_with_border + (y+1)*ref_step + 1;
+    for(int x=0; x<patch_size; ++x, ++it, ++it_dv)
+    {
+      Vector2f J;
+      J[0] = 0.5*(dir[0]*(it[1] - it[-1]) + dir[1]*(it[ref_step] - it[-ref_step]));
+      J[1] = 1;
+      *it_dv = J[0];
+      H += J*J.transpose();
+    }
+  }
+  h_inv = 1.0/H(0,0)*patch_size*patch_size;
+  Matrix2f Hinv = H.inverse();
+  float mean_diff = 0;
+
+  // Compute pixel location in new image:
+  float u = cur_px_estimate.x();
+  float v = cur_px_estimate.y();
+
+  // termination condition
+  const float min_update_squared = 0.03*0.03;
+  const int cur_step = cur_img.step.p[0];
+  float chi2 = 0;
+  Vector2f update; update.setZero();
+  for(int iter = 0; iter<n_iter; ++iter)
+  {
+    int u_r = floor(u);
+    int v_r = floor(v);
+    if(u_r < halfpatch_size_ || v_r < halfpatch_size_ || u_r >= cur_img.cols-halfpatch_size_ || v_r >= cur_img.rows-halfpatch_size_)
+      break;
+
+    if(isnan(u) || isnan(v)) // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check
+      return false;
+
+    // compute interpolation weights
+    float subpix_x = u-u_r;
+    float subpix_y = v-v_r;
+    float wTL = (1.0-subpix_x)*(1.0-subpix_y);
+    float wTR = subpix_x * (1.0-subpix_y);
+    float wBL = (1.0-subpix_x)*subpix_y;
+    float wBR = subpix_x * subpix_y;
+
+    // loop through search_patch, interpolate
+    uint8_t* it_ref = ref_patch;
+    float* it_ref_dv = ref_patch_dv;
+    float new_chi2 = 0.0;
+    Vector2f Jres; Jres.setZero();
+    for(int y=0; y<patch_size; ++y)
+    {
+      uint8_t* it = (uint8_t*) cur_img.data + (v_r+y-halfpatch_size_)*cur_step + u_r-halfpatch_size_;
+      for(int x=0; x<patch_size; ++x, ++it, ++it_ref, ++it_ref_dv)
+      {
+        float search_pixel = wTL*it[0] + wTR*it[1] + wBL*it[cur_step] + wBR*it[cur_step+1];
+        float res = search_pixel - *it_ref + mean_diff;
+        Jres[0] -= res*(*it_ref_dv);
+        Jres[1] -= res;
+        new_chi2 += res*res;
+      }
+    }
+
+    if(iter > 0 && new_chi2 > chi2)
+    {
+#if SUBPIX_VERBOSE
+      cout << "error increased." << endl;
+#endif
+      u -= update[0];
+      v -= update[1];
+      break;
+    }
+
+    chi2 = new_chi2;
+    update = Hinv * Jres;
+    u += update[0]*dir[0];
+    v += update[0]*dir[1];
+    mean_diff += update[1];
+
+#if SUBPIX_VERBOSE
+    cout << "Iter " << iter << ":"
+         << "\t u=" << u << ", v=" << v
+         << "\t update = " << update[0] << ", " << update[1]
+         << "\t new chi2 = " << new_chi2 << endl;
+#endif
+
+    if(update[0]*update[0]+update[1]*update[1] < min_update_squared)
+    {
+#if SUBPIX_VERBOSE
+      cout << "converged." << endl;
+#endif
+      converged=true;
+      break;
+    }
+  }
+
+  cur_px_estimate << u, v;
+  return converged;
+}
+
+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)
+{
+#ifdef __ARM_NEON__
+  if(!no_simd)
+    return align2D_NEON(cur_img, ref_patch_with_border, ref_patch, n_iter, cur_px_estimate);
+#endif
+
+  const int halfpatch_size_ = 4;
+  const int patch_size_ = 8;
+  const int patch_area_ = 64;
+  bool converged=false;
+
+  // compute derivative of template and prepare inverse compositional
+  float __attribute__((__aligned__(16))) ref_patch_dx[patch_area_];
+  float __attribute__((__aligned__(16))) ref_patch_dy[patch_area_];
+  Matrix3f H; H.setZero();
+
+  // compute gradient and hessian
+  const int ref_step = patch_size_+2;
+  float* it_dx = ref_patch_dx;
+  float* it_dy = ref_patch_dy;
+  for(int y=0; y<patch_size_; ++y)
+  {
+    uint8_t* it = ref_patch_with_border + (y+1)*ref_step + 1;
+    for(int x=0; x<patch_size_; ++x, ++it, ++it_dx, ++it_dy)
+    {
+      Vector3f J;
+      J[0] = 0.5 * (it[1] - it[-1]);
+      J[1] = 0.5 * (it[ref_step] - it[-ref_step]);
+      J[2] = 1;
+      *it_dx = J[0];
+      *it_dy = J[1];
+      H += J*J.transpose();
+    }
+  }
+  Matrix3f Hinv = H.inverse();
+  float mean_diff = 0;
+
+  // Compute pixel location in new image:
+  float u = cur_px_estimate.x();
+  float v = cur_px_estimate.y();
+
+  // termination condition
+  const float min_update_squared = 0.03*0.03;
+  const int cur_step = cur_img.step.p[0];
+//  float chi2 = 0;
+  Vector3f update; update.setZero();
+  for(int iter = 0; iter<n_iter; ++iter)
+  {
+    int u_r = floor(u);
+    int v_r = floor(v);
+    if(u_r < halfpatch_size_ || v_r < halfpatch_size_ || u_r >= cur_img.cols-halfpatch_size_ || v_r >= cur_img.rows-halfpatch_size_)
+      break;
+
+    if(isnan(u) || isnan(v)) // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check
+      return false;
+
+    // compute interpolation weights
+    float subpix_x = u-u_r;
+    float subpix_y = v-v_r;
+    float wTL = (1.0-subpix_x)*(1.0-subpix_y);
+    float wTR = subpix_x * (1.0-subpix_y);
+    float wBL = (1.0-subpix_x)*subpix_y;
+    float wBR = subpix_x * subpix_y;
+
+    // loop through search_patch, interpolate
+    uint8_t* it_ref = ref_patch;
+    float* it_ref_dx = ref_patch_dx;
+    float* it_ref_dy = ref_patch_dy;
+//    float new_chi2 = 0.0;
+    Vector3f Jres; Jres.setZero();
+    for(int y=0; y<patch_size_; ++y)
+    {
+      uint8_t* it = (uint8_t*) cur_img.data + (v_r+y-halfpatch_size_)*cur_step + u_r-halfpatch_size_;
+      for(int x=0; x<patch_size_; ++x, ++it, ++it_ref, ++it_ref_dx, ++it_ref_dy)
+      {
+        float search_pixel = wTL*it[0] + wTR*it[1] + wBL*it[cur_step] + wBR*it[cur_step+1];
+        float res = search_pixel - *it_ref + mean_diff;
+        Jres[0] -= res*(*it_ref_dx);
+        Jres[1] -= res*(*it_ref_dy);
+        Jres[2] -= res;
+//        new_chi2 += res*res;
+      }
+    }
+
+
+/*
+    if(iter > 0 && new_chi2 > chi2)
+    {
+#if SUBPIX_VERBOSE
+      cout << "error increased." << endl;
+#endif
+      u -= update[0];
+      v -= update[1];
+      break;
+    }
+    chi2 = new_chi2;
+*/
+    update = Hinv * Jres;
+    u += update[0];
+    v += update[1];
+    mean_diff += update[2];
+
+#if SUBPIX_VERBOSE
+    cout << "Iter " << iter << ":"
+         << "\t u=" << u << ", v=" << v
+         << "\t update = " << update[0] << ", " << update[1]
+//         << "\t new chi2 = " << new_chi2 << endl;
+#endif
+
+    if(update[0]*update[0]+update[1]*update[1] < min_update_squared)
+    {
+#if SUBPIX_VERBOSE
+      cout << "converged." << endl;
+#endif
+      converged=true;
+      break;
+    }
+  }
+
+  cur_px_estimate << u, v;
+  return converged;
+}
+
+#define  DESCALE(x,n)     (((x) + (1 << ((n)-1))) >> (n)) // rounds to closest integer and descales
+
+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)
+{
+  // TODO: This function should not be used as the alignment is not robust to illumination changes!
+  const int halfpatch_size = 4;
+  const int patch_size = 8;
+  const int patch_area = 64;
+  bool converged=false;
+  const int W_BITS = 14;
+
+  // compute derivative of template and prepare inverse compositional
+  int16_t __attribute__((__aligned__(16))) ref_patch_dx[patch_area];
+  int16_t __attribute__((__aligned__(16))) ref_patch_dy[patch_area];
+
+  // compute gradient and hessian
+  const int ref_step = patch_size+2;
+  int16_t* it_dx = ref_patch_dx;
+  int16_t* it_dy = ref_patch_dy;
+  float A11=0, A12=0, A22=0;
+  for(int y=0; y<patch_size; ++y)
+  {
+    uint8_t* it = ref_patch_with_border + (y+1)*ref_step + 1;
+    for(int x=0; x<patch_size; ++x, ++it, ++it_dx, ++it_dy)
+    {
+      int16_t dx = static_cast<int16_t>(it[1]) - it[-1];
+      int16_t dy = static_cast<int16_t>(it[ref_step]) - it[-ref_step];
+      *it_dx = dx;
+      *it_dy = dy;  // we are missing a factor 1/2
+      A11 += static_cast<float>(dx*dx); // we are missing a factor 1/4
+      A12 += static_cast<float>(dx*dy);
+      A22 += static_cast<float>(dy*dy);
+    }
+  }
+
+  // Compute pixel location in new image:
+  float u = cur_px_estimate.x();
+  float v = cur_px_estimate.y();
+
+  // termination condition
+  const float min_update_squared = 0.03*0.03;
+  const int cur_step = cur_img.step.p[0];
+  const float Dinv = 1.0f/(A11*A22 - A12*A12); // we are missing an extra factor 16
+  float chi2 = 0;
+  float update_u = 0, update_v = 0;
+
+  for(int iter = 0; iter<n_iter; ++iter)
+  {
+    int u_r = floor(u);
+    int v_r = floor(v);
+    if(u_r < halfpatch_size || v_r < halfpatch_size || u_r >= cur_img.cols-halfpatch_size || v_r >= cur_img.rows-halfpatch_size)
+      break;
+
+    if(isnan(u) || isnan(v)) // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check
+      return false;
+
+    float subpix_x = u-u_r;
+    float subpix_y = v-v_r;
+    float b1=0, b2=0;
+    float new_chi2 = 0.0;
+
+#ifdef __SSE2__
+    // compute bilinear interpolation weights
+    int wTL = static_cast<int>((1.0f-subpix_x)*(1.0f-subpix_y)*(1 << W_BITS));
+    int wTR = static_cast<int>(subpix_x * (1.0f-subpix_y)*(1 << W_BITS));
+    int wBL = static_cast<int>((1.0f-subpix_x)*subpix_y*(1 << W_BITS));
+    int wBR = (1 << W_BITS) - wTL - wTR - wBL;
+
+    __m128i qw0 = _mm_set1_epi32(wTL + (wTR << 16)); // Sets the 4 signed 32-bit integer values to [wTL, wTR].
+    __m128i qw1 = _mm_set1_epi32(wBL + (wBR << 16));
+    __m128i z = _mm_setzero_si128();
+    __m128 qb0 = _mm_setzero_ps(); // 4 floats
+    __m128 qb1 = _mm_setzero_ps(); // 4 floats
+    __m128i qdelta = _mm_set1_epi32(1 << (W_BITS-1));
+    for(int y=0; y<patch_size; ++y)
+    {
+      const uint8_t* it  = (const uint8_t*) cur_img.data + (v_r+y-halfpatch_size)*cur_step + u_r-halfpatch_size;
+
+      // Iptr is aligned!
+      //__m128i diff0 = _mm_load_si128((const __m128i*)(ref_patch + y*8));
+      __m128i diff = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(ref_patch + y*8)), z);
+
+      // load the lower 64 bits and unpack [8u 0 8u 0..]
+      __m128i v00 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(it)), z);
+      __m128i v01 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(it + 1)), z);
+      __m128i v10 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(it + cur_step)), z);
+      __m128i v11 = _mm_unpacklo_epi8(_mm_loadl_epi64((const __m128i*)(it + cur_step + 1)), z);
+
+      // interpolate top row and bottom row
+      // _mm_unpacklo_epi16: Interleaves the lower 4 signed or unsigned 16-bit integers in a with the lower 4 signed or unsigned 16-bit integers in b.
+      // _mm_madd_epi16: Multiplies the 8 signed 16-bit integers from a by the 8 signed 16-bit integers from b.
+      __m128i t0 = _mm_add_epi32(_mm_madd_epi16(_mm_unpacklo_epi16(v00, v01), qw0),
+                                 _mm_madd_epi16(_mm_unpacklo_epi16(v10, v11), qw1));
+      __m128i t1 = _mm_add_epi32(_mm_madd_epi16(_mm_unpackhi_epi16(v00, v01), qw0),
+                                 _mm_madd_epi16(_mm_unpackhi_epi16(v10, v11), qw1));
+
+      // _mm_srai_epi32: Shifts the 4 signed 32-bit integers in A right by count bits while shifting in the sign bit.
+      t0 = _mm_srai_epi32(_mm_add_epi32(t0, qdelta), W_BITS); // adding qdelta is for rounding closest int
+      t1 = _mm_srai_epi32(_mm_add_epi32(t1, qdelta), W_BITS);
+
+      // compute 8xres:
+      // _mm_packs_epi32: Packs the 8 signed 32-bit integers from a and b into signed 16-bit integers and saturates.
+      diff = _mm_subs_epi16(_mm_packs_epi32(t0, t1), diff);
+
+      // load gradient dX and dY, both are aligned!
+      v00 = _mm_load_si128((const __m128i*)(ref_patch_dx + y*patch_size)); // [dx1, dx2, dx3, dx4 ...]
+      v01 = _mm_load_si128((const __m128i*)(ref_patch_dy + y*patch_size)); // [dy1, dy2, dy3, dy4 ...]
+
+      // _mm_mulhi_epi16: Multiplies the 8 signed 16-bit integers from a by the 8 signed 16-bit integers from b.
+      v10 = _mm_mullo_epi16(v00, diff); // Packs the lower 16 bits of the 8 signed 32-bit results. [15:0]
+      v11 = _mm_mulhi_epi16(v00, diff); // Packs the upper 16 bits of the 8 signed 32-bit results. [31:16]
+
+      // _mm_unpacklo_epi16: Interleaves the lower 4 signed or unsigned 16-bit integers with the lower 4 signed or unsigned 16-bit integers in b.
+      v00 = _mm_unpacklo_epi16(v10, v11);
+      v10 = _mm_unpackhi_epi16(v10, v11);
+
+      // convert to float and add to dx
+      qb0 = _mm_add_ps(qb0, _mm_cvtepi32_ps(v00));
+      qb0 = _mm_add_ps(qb0, _mm_cvtepi32_ps(v10));
+
+      // same with dY
+      v10 = _mm_mullo_epi16(v01, diff);
+      v11 = _mm_mulhi_epi16(v01, diff);
+      v00 = _mm_unpacklo_epi16(v10, v11);
+      v10 = _mm_unpackhi_epi16(v10, v11);
+      qb1 = _mm_add_ps(qb1, _mm_cvtepi32_ps(v00));
+      qb1 = _mm_add_ps(qb1, _mm_cvtepi32_ps(v10));
+    }
+
+    float __attribute__((__aligned__(16))) buf[4];
+    _mm_store_ps(buf, qb0);
+    b1 += buf[0]+buf[1]+buf[2]+buf[3];
+    _mm_store_ps(buf, qb1);
+    b2 += buf[0]+buf[1]+buf[2]+buf[3];
+#endif
+
+    // compute -A^-1*b
+    update_u = ((A12*b2 - A22*b1) * Dinv)*2; // * 2 to compensate because above, we did not compute the derivative correctly
+    update_v = ((A12*b1 - A11*b2) * Dinv)*2;
+    u += update_u;
+    v += update_v;
+
+#if SUBPIX_VERBOSE
+    cout << "Iter " << iter << ":"
+         << "\t u=" << u << ", v=" << v
+         << "\t update = " << update_u << ", " << update_v
+         << "\t new chi2 = " << new_chi2 << endl;
+#endif
+
+    if(update_u*update_u+update_v*update_v < min_update_squared)
+    {
+#if SUBPIX_VERBOSE
+      cout << "converged." << endl;
+#endif
+      converged=true;
+      break;
+    }
+    chi2 = new_chi2;
+  }
+
+  cur_px_estimate << u, v;
+  return converged;
+}
+
+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)
+{
+  const int halfpatch_size = 4;
+  const int patch_size = 8;
+  const int patch_area = 64;
+  bool converged=false;
+  const int W_BITS = 14;
+
+  // compute derivative of template and prepare inverse compositional
+  int16_t __attribute__((__aligned__(16))) ref_patch_dx[patch_area];
+  int16_t __attribute__((__aligned__(16))) ref_patch_dy[patch_area];
+  
+  // compute gradient and hessian
+  const int ref_step = patch_size+2;
+  int16_t* it_dx = ref_patch_dx;
+  int16_t* it_dy = ref_patch_dy;
+  Matrix3f H; H.setZero();
+  for(int y=0; y<patch_size; ++y)
+  {
+    uint8_t* it = ref_patch_with_border + (y+1)*ref_step + 1;
+    for(int x=0; x<patch_size; ++x, ++it, ++it_dx, ++it_dy)
+    {
+      *it_dx = static_cast<int16_t>(it[1] - it[-1]);
+      *it_dy = static_cast<int16_t>(it[ref_step] - it[-ref_step]); // divide by 2 missing
+      Vector3f J(*it_dx, *it_dy, 1.0f);
+      H += J*J.transpose();
+    }
+  }
+  Matrix3f Hinv = H.inverse();
+  float mean_diff = 0.0;
+
+  // Compute pixel location in new image:
+  float u = cur_px_estimate.x();
+  float v = cur_px_estimate.y();
+
+  // termination condition
+  const float min_update_squared = 0.03*0.03;
+  const int cur_step = cur_img.step.p[0];
+  Vector3f update;
+  Vector3f Jres;
+  for(int iter = 0; iter<n_iter; ++iter)
+  {
+    int u_r = floor(u);
+    int v_r = floor(v);
+    if(u_r < halfpatch_size || v_r < halfpatch_size || u_r >= cur_img.cols-halfpatch_size || v_r >= cur_img.rows-halfpatch_size)
+      break;
+
+    if(isnan(u) || isnan(v)) // TODO very rarely this can happen, maybe H is singular? should not be at corner.. check
+      return false;
+
+    float subpix_x = u-u_r;
+    float subpix_y = v-v_r;
+    float b1=0, b2=0;
+
+#ifdef __ARM_NEON__
+    const int SHIFT_BITS = 7;
+    const uint16_t wTL = static_cast<uint16_t>((1.0f-subpix_x)*(1.0f-subpix_y)*(1<<SHIFT_BITS));
+    const uint16_t wTR = static_cast<uint16_t>(subpix_x*(1.0f-subpix_y)*(1<<SHIFT_BITS));
+    const uint16_t wBL = static_cast<uint16_t>((1.0f-subpix_x)*subpix_y*(1<<SHIFT_BITS));
+    const uint16_t wBR = static_cast<uint16_t>((1 << SHIFT_BITS) - wTL - wTR - wBL);
+
+    // initialize result to zero
+    int32x4_t vb1 = vdupq_n_s32(0);
+    int32x4_t vb2 = vdupq_n_s32(0);
+    int16x8_t vmean_diff = vdupq_n_s16( (int16_t) (mean_diff+0.5) );
+    int16x8_t vres_sum = vdupq_n_s16(0);
+    for(int y=0; y<patch_size; ++y)
+    {
+      const uint8_t* it  = (const uint8_t*) cur_img.data + (v_r+y-halfpatch_size)*cur_step + u_r-halfpatch_size;
+
+      // load and convert from uint8 to uint16
+      uint16x8_t v00 = vmovl_u8( vld1_u8( it ) );
+      uint16x8_t v01 = vmovl_u8( vld1_u8( it + 1 ) );
+      uint16x8_t v10 = vmovl_u8( vld1_u8( it + cur_step ) );
+      uint16x8_t v11 = vmovl_u8( vld1_u8( it + cur_step + 1 ) );
+
+      // vector multiply by scalar
+      v00 = vmulq_n_u16( v00, wTL );
+      v01 = vmulq_n_u16( v01, wTR );
+      v10 = vmulq_n_u16( v10, wBL );
+      v11 = vmulq_n_u16( v11, wBR );
+
+      // add all results together
+      v00 = vaddq_u16( v00, vaddq_u16( v01, vaddq_u16( v10, v11 ) ) );
+
+      // descale: shift right by constant
+      v00 = vshrq_n_u16(v00, SHIFT_BITS);
+
+      // compute difference between reference and interpolated patch,
+      // use reinterpet-cast to make signed [-255,255]
+      int16x8_t res = vsubq_s16(vreinterpretq_s16_u16(v00), vreinterpretq_s16_u16(vmovl_u8(vld1_u8( ref_patch + y*8 ))));
+
+      // correct res with mean difference
+      res = vaddq_s16(res, vmean_diff);
+
+      // compute sum of the residual
+      vres_sum = vaddq_s16(vres_sum, res); 
+
+      // Vector multiply accumulate long: vmla -> Vr[i] := Va[i] + Vb[i] * Vc[i]
+      // int32x4_t  vmlal_s16(int32x4_t a, int16x4_t b, int16x4_t c);    // VMLAL.S16 q0,d0,d0
+      int16x8_t grad = vld1q_s16(ref_patch_dx + y*patch_size);
+      vb1 = vmlal_s16(vb1, vget_low_s16(grad), vget_low_s16(res));
+      vb1 = vmlal_s16(vb1, vget_high_s16(grad), vget_high_s16(res));
+
+      grad = vld1q_s16(ref_patch_dy + y*patch_size);
+      vb2 = vmlal_s16(vb2, vget_low_s16(grad), vget_low_s16(res));
+      vb2 = vmlal_s16(vb2, vget_high_s16(grad), vget_high_s16(res));
+    }
+
+    // finally, sum results of vb1, vb2 and vres_sum
+    int32x2_t tmp;
+    tmp = vpadd_s32(vget_low_s32(vb1), vget_high_s32(vb1));
+    Jres[0] = -vget_lane_s32(tmp, 0) - vget_lane_s32(tmp, 1);
+
+    tmp = vpadd_s32(vget_low_s32(vb2), vget_high_s32(vb2));
+    Jres[1] = -vget_lane_s32(tmp, 0) - vget_lane_s32(tmp, 1);
+
+    int32x4_t vres_sum1 = vpaddlq_s16(vres_sum);
+    tmp = vpadd_s32(vget_low_s32(vres_sum1), vget_high_s32(vres_sum1));
+    Jres[2] = -vget_lane_s32(tmp, 0) - vget_lane_s32(tmp, 1);
+#endif
+
+    update = Hinv * Jres * 2; // * 2 to compensate because above, we did not compute the derivative correctly
+    u += update[0];
+    v += update[1];
+    mean_diff += update[2];
+
+#if SUBPIX_VERBOSE
+    cout << "Iter " << iter << ":"
+         << "\t u=" << u << ", v=" << v
+         << "\t update = " << update[0] << ", " << update[1] << endl;
+#endif
+
+    if(update[0]*update[0]+update[1]*update[1] < min_update_squared)
+    {
+#if SUBPIX_VERBOSE
+      cout << "converged." << endl;
+#endif
+      converged=true;
+      break;
+    }
+  }
+
+  cur_px_estimate << u, v;
+  return converged;
+}
+
+} // namespace feature_alignment
+} // namespace svo

CPU_version/svo/src/feature_detection.cpp

@@ -0,0 +1,118 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <svo/feature_detection.h>
+#include <svo/feature.h>
+#include <fast/fast.h>
+#include <vikit/vision.h>
+
+namespace svo {
+namespace feature_detection {
+
+AbstractDetector::AbstractDetector(
+    const int img_width,
+    const int img_height,
+    const int cell_size,
+    const int n_pyr_levels) :
+        cell_size_(cell_size),
+        n_pyr_levels_(n_pyr_levels),
+        grid_n_cols_(ceil(static_cast<double>(img_width)/cell_size_)),
+        grid_n_rows_(ceil(static_cast<double>(img_height)/cell_size_)),
+        grid_occupancy_(grid_n_cols_*grid_n_rows_, false)
+{}
+
+void AbstractDetector::resetGrid()
+{
+  std::fill(grid_occupancy_.begin(), grid_occupancy_.end(), false);
+}
+
+void AbstractDetector::setExistingFeatures(const Features& fts)
+{
+  std::for_each(fts.begin(), fts.end(), [&](Feature* i){
+    grid_occupancy_.at(
+        static_cast<int>(i->px[1]/cell_size_)*grid_n_cols_
+        + static_cast<int>(i->px[0]/cell_size_)) = true;
+  });
+}
+
+void AbstractDetector::setGridOccpuancy(const Vector2d& px)
+{
+  grid_occupancy_.at(
+      static_cast<int>(px[1]/cell_size_)*grid_n_cols_
+    + static_cast<int>(px[0]/cell_size_)) = true;
+}
+
+FastDetector::FastDetector(
+    const int img_width,
+    const int img_height,
+    const int cell_size,
+    const int n_pyr_levels) :
+        AbstractDetector(img_width, img_height, cell_size, n_pyr_levels)
+{}
+
+void FastDetector::detect(
+    Frame* frame,
+    const ImgPyr& img_pyr,
+    const double detection_threshold,
+    Features& fts)
+{
+  Corners corners(grid_n_cols_*grid_n_rows_, Corner(0,0,detection_threshold,0,0.0f));
+  for(int L=0; L<n_pyr_levels_; ++L)
+  {
+    const int scale = (1<<L);
+    vector<fast::fast_xy> fast_corners;
+#if __SSE2__
+      fast::fast_corner_detect_10_sse2(
+          (fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols,
+          img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners);
+#elif HAVE_FAST_NEON
+      fast::fast_corner_detect_9_neon(
+          (fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols,
+          img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners);
+#else
+      fast::fast_corner_detect_10(
+          (fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols,
+          img_pyr[L].rows, img_pyr[L].cols, 20, fast_corners);
+#endif
+    vector<int> scores, nm_corners;
+    fast::fast_corner_score_10((fast::fast_byte*) img_pyr[L].data, img_pyr[L].cols, fast_corners, 20, scores);
+    fast::fast_nonmax_3x3(fast_corners, scores, nm_corners);
+
+    for(auto it=nm_corners.begin(), ite=nm_corners.end(); it!=ite; ++it)
+    {
+      fast::fast_xy& xy = fast_corners.at(*it);
+      const int k = static_cast<int>((xy.y*scale)/cell_size_)*grid_n_cols_
+                  + static_cast<int>((xy.x*scale)/cell_size_);
+      if(grid_occupancy_[k])
+        continue;
+      const float score = vk::shiTomasiScore(img_pyr[L], xy.x, xy.y);
+      if(score > corners.at(k).score)
+        corners.at(k) = Corner(xy.x*scale, xy.y*scale, score, L, 0.0f);
+    }
+  }
+
+  // Create feature for every corner that has high enough corner score
+  std::for_each(corners.begin(), corners.end(), [&](Corner& c) {
+    if(c.score > detection_threshold)
+      fts.push_back(new Feature(frame, Vector2d(c.x, c.y), c.level));
+  });
+
+  resetGrid();
+}
+
+} // namespace feature_detection
+} // namespace svo
+

CPU_version/svo/src/frame.cpp

@@ -0,0 +1,190 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <stdexcept>
+#include <svo/frame.h>
+#include <svo/feature.h>
+#include <svo/point.h>
+#include <svo/config.h>
+#include <vikit/math_utils.h>
+#include <vikit/vision.h>
+#include <vikit/performance_monitor.h>
+#include <fast/fast.h>
+
+namespace svo {
+
+int Frame::frame_counter_ = 0;
+
+Frame::Frame(vk::AbstractCamera* cam, const cv::Mat& img, double timestamp) :
+    id_(frame_counter_++),
+    timestamp_(timestamp),
+    cam_(cam),
+    key_pts_(5),
+    is_keyframe_(false),
+    v_kf_(NULL)
+{
+  initFrame(img);
+}
+
+Frame::~Frame()
+{
+  std::for_each(fts_.begin(), fts_.end(), [&](Feature* i){delete i;});
+}
+
+void Frame::initFrame(const cv::Mat& img)
+{
+  // check image
+  if(img.empty() || img.type() != CV_8UC1 || img.cols != cam_->width() || img.rows != cam_->height())
+    throw std::runtime_error("Frame: provided image has not the same size as the camera model or image is not grayscale");
+
+  // Set keypoints to NULL
+  std::for_each(key_pts_.begin(), key_pts_.end(), [&](Feature* ftr){ ftr=NULL; });
+
+  // Build Image Pyramid
+  frame_utils::createImgPyramid(img, max(Config::nPyrLevels(), Config::kltMaxLevel()+1), img_pyr_);
+}
+
+void Frame::setKeyframe()
+{
+  is_keyframe_ = true;
+  setKeyPoints();
+}
+
+void Frame::addFeature(Feature* ftr)
+{
+  fts_.push_back(ftr);
+}
+
+void Frame::setKeyPoints()
+{
+  for(size_t i = 0; i < 5; ++i)
+    if(key_pts_[i] != NULL)
+      if(key_pts_[i]->point == NULL)
+        key_pts_[i] = NULL;
+
+  std::for_each(fts_.begin(), fts_.end(), [&](Feature* ftr){ if(ftr->point != NULL) checkKeyPoints(ftr); });
+}
+
+void Frame::checkKeyPoints(Feature* ftr)
+{
+  const int cu = cam_->width()/2;
+  const int cv = cam_->height()/2;
+
+  // center pixel
+  if(key_pts_[0] == NULL)
+    key_pts_[0] = ftr;
+  else if(std::max(std::fabs(ftr->px[0]-cu), std::fabs(ftr->px[1]-cv))
+        < std::max(std::fabs(key_pts_[0]->px[0]-cu), std::fabs(key_pts_[0]->px[1]-cv)))
+    key_pts_[0] = ftr;
+
+  if(ftr->px[0] >= cu && ftr->px[1] >= cv)
+  {
+    if(key_pts_[1] == NULL)
+      key_pts_[1] = ftr;
+    else if((ftr->px[0]-cu) * (ftr->px[1]-cv)
+          > (key_pts_[1]->px[0]-cu) * (key_pts_[1]->px[1]-cv))
+      key_pts_[1] = ftr;
+  }
+  if(ftr->px[0] >= cu && ftr->px[1] < cv)
+  {
+    if(key_pts_[2] == NULL)
+      key_pts_[2] = ftr;
+    else if((ftr->px[0]-cu) * (ftr->px[1]-cv)
+          > (key_pts_[2]->px[0]-cu) * (key_pts_[2]->px[1]-cv))
+      key_pts_[2] = ftr;
+  }
+  if(ftr->px[0] < cv && ftr->px[1] < cv)
+  {
+    if(key_pts_[3] == NULL)
+      key_pts_[3] = ftr;
+    else if((ftr->px[0]-cu) * (ftr->px[1]-cv)
+          > (key_pts_[3]->px[0]-cu) * (key_pts_[3]->px[1]-cv))
+      key_pts_[3] = ftr;
+  }
+  if(ftr->px[0] < cv && ftr->px[1] >= cv)
+  {
+    if(key_pts_[4] == NULL)
+      key_pts_[4] = ftr;
+    else if((ftr->px[0]-cu) * (ftr->px[1]-cv)
+          > (key_pts_[4]->px[0]-cu) * (key_pts_[4]->px[1]-cv))
+      key_pts_[4] = ftr;
+  }
+}
+
+void Frame::removeKeyPoint(Feature* ftr)
+{
+  bool found = false;
+  std::for_each(key_pts_.begin(), key_pts_.end(), [&](Feature*& i){
+    if(i == ftr) {
+      i = NULL;
+      found = true;
+    }
+  });
+  if(found)
+    setKeyPoints();
+}
+
+bool Frame::isVisible(const Vector3d& xyz_w) const
+{
+  Vector3d xyz_f = T_f_w_*xyz_w;
+  if(xyz_f.z() < 0.0)
+    return false; // point is behind the camera
+  Vector2d px = f2c(xyz_f);
+  if(px[0] >= 0.0 && px[1] >= 0.0 && px[0] < cam_->width() && px[1] < cam_->height())
+    return true;
+  return false;
+}
+
+
+/// Utility functions for the Frame class
+namespace frame_utils {
+
+void createImgPyramid(const cv::Mat& img_level_0, int n_levels, ImgPyr& pyr)
+{
+  pyr.resize(n_levels);
+  pyr[0] = img_level_0;
+  for(int i=1; i<n_levels; ++i)
+  {
+    pyr[i] = cv::Mat(pyr[i-1].rows/2, pyr[i-1].cols/2, CV_8U);
+    vk::halfSample(pyr[i-1], pyr[i]);
+  }
+}
+
+bool getSceneDepth(const Frame& frame, double& depth_mean, double& depth_min)
+{
+  vector<double> depth_vec;
+  depth_vec.reserve(frame.fts_.size());
+  depth_min = std::numeric_limits<double>::max();
+  for(auto it=frame.fts_.begin(), ite=frame.fts_.end(); it!=ite; ++it)
+  {
+    if((*it)->point != NULL)
+    {
+      const double z = frame.w2f((*it)->point->pos_).z();
+      depth_vec.push_back(z);
+      depth_min = fmin(z, depth_min);
+    }
+  }
+  if(depth_vec.empty())
+  {
+    SVO_WARN_STREAM("Cannot set scene depth. Frame has no point-observations!");
+    return false;
+  }
+  depth_mean = vk::getMedian(depth_vec);
+  return true;
+}
+
+} // namespace frame_utils
+} // namespace svo

CPU_version/svo/src/frame_handler_base.cpp

@@ -0,0 +1,199 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <vikit/abstract_camera.h>
+#include <stdlib.h>
+#include <Eigen/StdVector>
+#include <boost/bind.hpp>
+#include <fstream>
+#include <svo/frame_handler_base.h>
+#include <svo/config.h>
+#include <svo/feature.h>
+#include <svo/matcher.h>
+#include <svo/map.h>
+#include <svo/point.h>
+
+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");
+}
+
+FrameHandlerBase::~FrameHandlerBase()
+{
+  SVO_INFO_STREAM("SVO destructor invoked");
+#ifdef SVO_TRACE
+  delete g_permon;
+#endif
+}
+
+bool FrameHandlerBase::startFrameProcessingCommon(const double timestamp)
+{
+  if(set_start_)
+  {
+    resetAll();
+    stage_ = STAGE_FIRST_FRAME;
+  }
+
+  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();
+  return true;
+}
+
+int FrameHandlerBase::finishFrameProcessingCommon(
+    const size_t update_id,
+    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);
+
+  // save processing time to calculate fps
+  acc_frame_timings_.push_back(timer_.stop());
+  if(stage_ == STAGE_DEFAULT_FRAME)
+    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
+
+  if(dropout == RESULT_FAILURE &&
+      (stage_ == STAGE_DEFAULT_FRAME || stage_ == STAGE_RELOCALIZING ))
+  {
+    stage_ = STAGE_RELOCALIZING;
+    tracking_quality_ = TRACKING_INSUFFICIENT;
+  }
+  else if (dropout == RESULT_FAILURE)
+    resetAll();
+  if(set_reset_)
+    resetAll();
+
+  return 0;
+}
+
+void FrameHandlerBase::resetCommon()
+{
+  map_.reset();
+  stage_ = STAGE_PAUSED;
+  set_reset_ = false;
+  set_start_ = false;
+  tracking_quality_ = TRACKING_INSUFFICIENT;
+  num_obs_last_ = 0;
+  SVO_INFO_STREAM("RESET");
+}
+
+void FrameHandlerBase::setTrackingQuality(const size_t num_observations)
+{
+  tracking_quality_ = TRACKING_GOOD;
+  if(num_observations < Config::qualityMinFts())
+  {
+    SVO_WARN_STREAM_THROTTLE(0.5, "Tracking less than "<< Config::qualityMinFts() <<" features!");
+    tracking_quality_ = TRACKING_INSUFFICIENT;
+  }
+  const int feature_drop = static_cast<int>(std::min(num_obs_last_, Config::maxFts())) - num_observations;
+  if(feature_drop > Config::qualityMaxFtsDrop())
+  {
+    SVO_WARN_STREAM("Lost "<< feature_drop <<" features!");
+    tracking_quality_ = TRACKING_INSUFFICIENT;
+  }
+}
+
+bool ptLastOptimComparator(Point* lhs, Point* rhs)
+{
+  return (lhs->last_structure_optim_ < rhs->last_structure_optim_);
+}
+
+void FrameHandlerBase::optimizeStructure(
+    FramePtr frame,
+    size_t max_n_pts,
+    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);
+  }
+  max_n_pts = min(max_n_pts, pts.size());
+  nth_element(pts.begin(), pts.begin() + max_n_pts, pts.end(), ptLastOptimComparator);
+  for(deque<Point*>::iterator it=pts.begin(); it!=pts.begin()+max_n_pts; ++it)
+  {
+    (*it)->optimize(max_iter);
+    (*it)->last_structure_optim_ = frame->id_;
+  }
+}
+
+
+} // namespace svo

CPU_version/svo/src/frame_handler_mono.cpp

@@ -0,0 +1,307 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <svo/config.h>
+#include <svo/frame_handler_mono.h>
+#include <svo/map.h>
+#include <svo/frame.h>
+#include <svo/feature.h>
+#include <svo/point.h>
+#include <svo/pose_optimizer.h>
+#include <svo/sparse_img_align.h>
+#include <vikit/performance_monitor.h>
+#include <svo/depth_filter.h>
+#ifdef USE_BUNDLE_ADJUSTMENT
+#include <svo/bundle_adjustment.h>
+#endif
+
+namespace svo {
+
+FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Sophus::SE3& SE_init) :
+  FrameHandlerBase(),
+  cam_(cam),
+  reprojector_(cam_, map_),
+  depth_filter_(NULL)
+{
+  initialize();
+  imu_integPtr_=std::make_unique<Imu_Integration>(SE_init);
+}
+
+void FrameHandlerMono::initialize()
+{
+  feature_detection::DetectorPtr feature_detector(
+      new feature_detection::FastDetector(
+          cam_->width(), cam_->height(), Config::gridSize(), Config::nPyrLevels()));
+  DepthFilter::callback_t depth_filter_cb = boost::bind(
+      &MapPointCandidates::newCandidatePoint, &map_.point_candidates_, _1, _2);
+  depth_filter_ = new DepthFilter(feature_detector, depth_filter_cb);
+  depth_filter_->startThread();
+}
+
+FrameHandlerMono::~FrameHandlerMono()
+{
+  delete depth_filter_;
+}
+
+void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp)
+{
+  if(!startFrameProcessingCommon(timestamp))
+    return;
+
+  // some cleanup from last iteration, can't do before because of visualization
+  core_kfs_.clear();
+  overlap_kfs_.clear();
+
+  // create new frame
+  new_frame_.reset(new Frame(cam_, img.clone(), timestamp));
+
+  // process frame
+  UpdateResult res = RESULT_FAILURE;
+  if(stage_ == STAGE_DEFAULT_FRAME)
+    res = processFrame();
+  else if(stage_ == STAGE_SECOND_FRAME)
+    res = processSecondFrame();
+  else if(stage_ == STAGE_FIRST_FRAME)
+    res = processFirstFrame();
+  else if(stage_ == STAGE_RELOCALIZING)
+    res = relocalizeFrame(SE3(Matrix3d::Identity(), Vector3d::Zero()),
+                          map_.getClosestKeyframe(last_frame_));
+
+  // set last frame
+  last_frame_ = new_frame_;
+  new_frame_.reset();
+  // finish processing
+  finishFrameProcessingCommon(last_frame_->id_, res, last_frame_->nObs());
+}
+
+FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame()
+{
+  new_frame_->T_f_w_ = SE3(Matrix3d::Identity(), Vector3d::Zero());
+  if(klt_homography_init_.addFirstFrame(new_frame_) == initialization::FAILURE)
+    return RESULT_NO_KEYFRAME;
+  new_frame_->setKeyframe();
+  map_.addKeyframe(new_frame_);
+  stage_ = STAGE_SECOND_FRAME;
+  SVO_INFO_STREAM("Init: Selected first frame.");
+  return RESULT_IS_KEYFRAME;
+}
+
+FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
+{
+  initialization::InitResult res = klt_homography_init_.addSecondFrame(new_frame_);
+  if(res == initialization::FAILURE)
+    return RESULT_FAILURE;
+  else 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);
+
+  // add frame to map
+  map_.addKeyframe(new_frame_);
+  stage_ = STAGE_DEFAULT_FRAME;
+  klt_homography_init_.reset();
+  SVO_INFO_STREAM("Init: Selected second frame, triangulated initial map.");
+  return RESULT_IS_KEYFRAME;
+}
+
+FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
+{
+  // Set initial pose TODO use prior
+  new_frame_->T_f_w_ = last_frame_->T_f_w_;
+
+  // sparse image align
+  SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),
+                           30, SparseImgAlign::GaussNewton, false, false);
+  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;
+  }
+
+  // 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()))
+      return RESULT_FAILURE;
+
+
+  std::cerr<<"Reprojected points after opmization: "<<sfba_n_edges_final<<'\n';
+  if(sfba_n_edges_final < Config::qualityMinFts())
+    return RESULT_FAILURE;
+
+  // structure optimization
+  //SVO_START_TIMER("point_optimizer");
+  optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
+  //SVO_STOP_TIMER("point_optimizer");
+
+  // select keyframe
+  core_kfs_.insert(new_frame_);
+  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;
+  frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min);
+  if(!needNewKf(depth_mean) || tracking_quality_ == TRACKING_BAD)
+  {
+    depth_filter_->addFrame(new_frame_);
+    return RESULT_NO_KEYFRAME;
+  }
+  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<<"}");
+  }
+#endif
+
+  // init new depth-filters
+  depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min);
+
+  // if limited number of keyframes, remove the one furthest apart
+  if(Config::maxNKfs() > 2 && map_.size() >= Config::maxNKfs())
+  {
+    FramePtr furthest_frame = map_.getFurthestKeyframe(new_frame_->pos());
+    depth_filter_->removeKeyframe(furthest_frame); // TODO this interrupts the mapper thread, maybe we can solve this better
+    map_.safeDeleteFrame(furthest_frame);
+  }
+
+  // add keyframe to map
+  map_.addKeyframe(new_frame_);
+  std::cerr<<"Svo pipline was finished\n";
+  return RESULT_IS_KEYFRAME;
+}
+
+FrameHandlerMono::UpdateResult FrameHandlerMono::relocalizeFrame(
+    const SE3& T_cur_ref,
+    FramePtr ref_keyframe)
+{
+  if(ref_keyframe == nullptr)
+  {
+    return RESULT_FAILURE;
+  }
+  SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),
+                           30, SparseImgAlign::GaussNewton, false, false);
+  size_t img_align_n_tracked = img_align.run(ref_keyframe, new_frame_);
+  usleep(2000);
+  if(img_align_n_tracked > Config::qualityMinFts())
+  {
+    SE3 T_f_w_last = last_frame_->T_f_w_;
+    last_frame_ = ref_keyframe;
+    FrameHandlerMono::UpdateResult res = processFrame();
+    if(res != RESULT_FAILURE)
+    {
+      stage_ = STAGE_DEFAULT_FRAME;
+    }
+    else
+      new_frame_->T_f_w_ = T_f_w_last; // reset to last well localized pose
+    return res;
+  }
+  return RESULT_FAILURE;
+}
+
+bool FrameHandlerMono::relocalizeFrameAtPose(
+    const int keyframe_id,
+    const SE3& T_f_kf,
+    const cv::Mat& img,
+    const double timestamp)
+{
+  FramePtr ref_keyframe;
+  if(!map_.getKeyframeById(keyframe_id, ref_keyframe))
+    return false;
+  new_frame_.reset(new Frame(cam_, img.clone(), timestamp));
+  UpdateResult res = relocalizeFrame(T_f_kf, ref_keyframe);
+  if(res != RESULT_FAILURE) {
+    last_frame_ = new_frame_;
+    return true;
+  }
+  return false;
+}
+
+void FrameHandlerMono::resetAll()
+{
+  resetCommon();
+  last_frame_.reset();
+  new_frame_.reset();
+  core_kfs_.clear();
+  overlap_kfs_.clear();
+  depth_filter_->reset();
+}
+
+void FrameHandlerMono::setFirstFrame(const FramePtr& first_frame)
+{
+  resetAll();
+  last_frame_ = first_frame;
+  last_frame_->setKeyframe();
+  map_.addKeyframe(last_frame_);
+  stage_ = STAGE_DEFAULT_FRAME;
+}
+
+bool FrameHandlerMono::needNewKf(double scene_depth_mean)
+{
+  for(auto it=overlap_kfs_.begin(), ite=overlap_kfs_.end(); it!=ite; ++it)
+  {
+    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)
+      return false;
+  }
+  return true;
+}
+
+void FrameHandlerMono::setCoreKfs(size_t n_closest)
+{
+  size_t n = min(n_closest, overlap_kfs_.size()-1);
+  std::partial_sort(overlap_kfs_.begin(), overlap_kfs_.begin()+n, overlap_kfs_.end(),
+                    boost::bind(&pair<FramePtr, size_t>::second, _1) >
+                    boost::bind(&pair<FramePtr, size_t>::second, _2));
+  std::for_each(overlap_kfs_.begin(), overlap_kfs_.end(), [&](pair<FramePtr,size_t>& i){ core_kfs_.insert(i.first); });
+}
+
+} // namespace svo

CPU_version/svo/src/imu_integration.cpp

@@ -0,0 +1,151 @@
+//
+// Created by gfuse on 4/9/21.
+//
+//#define NDEBUG
+#include "svo/imu_integration.h"
+#include <memory>
+#include <utility>
+#include "svo/feature.h"
+#include "svo/point.h"
+#include "svo/svoVisionFactor.h"
+#include <gtsam/nonlinear/Marginals.h>
+#include <gtsam/geometry/Pose3.h>
+#include <vikit/params_helper.h>
+#include <svo/config.h>
+
+Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
+    graphPtr=std::make_shared<gtsam::NonlinearFactorGraph>();
+    valuesPtr=std::make_shared<gtsam::Values>();
+    parameterPtr = gtsam::PreintegratedCombinedMeasurements::Params::MakeSharedD();// TODO check the IMU frame
+    parameterPtr->accelerometerCovariance=gtsam::I_3x3 * svo::Config::ACC_Noise(); // acc white noise in continuous
+    parameterPtr->gyroscopeCovariance=gtsam::I_3x3 * svo::Config::GYO_Noise();// gyro white noise in continuous
+    parameterPtr->integrationCovariance=gtsam::I_3x3 * svo::Config::IUC(); // integration uncertainty continuous
+    parameterPtr->biasAccCovariance = gtsam::I_3x3 * svo::Config::ABC();   // acc bias in continuous
+    parameterPtr->biasOmegaCovariance = gtsam::I_3x3 * svo::Config::GBC();  // gyro bias in continuous
+    parameterPtr->biasAccOmegaInt = gtsam::I_6x6 * svo::Config::EBP();  // error in the bias used for preintegration
+    imu_biasPtr=std::make_shared<gtsam::imuBias::ConstantBias>(gtsam::Vector3(1e-8,1e-8,1e-8),gtsam::Vector3(1e-8,1e-8,1e-8));
+    preintegratedPtr = std::make_shared<gtsam::PreintegratedCombinedMeasurements>(parameterPtr,*imu_biasPtr);
+    ProjectNoisePtr = gtsam::noiseModel::Isotropic::Sigma(2, svo::Config::NRP()); // the noise used for projection
+    optimizerParamPtr=std::make_shared<gtsam::ISAM2Params>();
+    optimizerParamPtr->relinearizeThreshold = 0.1;
+    optimizerParamPtr->relinearizeSkip = 1;
+    //optimizerParamPtr->evaluateNonlinearError=true;
+    assert(preintegratedPtr);
+}
+Imu_Integration::~Imu_Integration(){
+
+}
+bool Imu_Integration::update(double* imu){
+    if(syn)return false;
+    if(imu_factor_id<1)return false;
+    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);
+    }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());
+        t_1=in;
+        ++imu_n;
+    }
+    return true;
+
+}
+bool Imu_Integration::reset(gtsam::ISAM2& optimizer,boost::shared_ptr<svo::Frame>& new_frame){
+    gtsam::Values result = optimizer.calculateEstimate();
+    // 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)));
+    preintegratedPtr->resetIntegrationAndSetBias(*imu_biasPtr);
+    ++imu_factor_id;
+    syn=false;
+    new_frame->T_f_w_=Sophus::SE3(statePtr->R(),statePtr->t());
+    new_frame->Cov_=optimizer.marginalCovariance(P(imu_factor_id-1));
+    graphPtr->resize(0);
+    valuesPtr->clear();
+    valuesPtr->insert(P(imu_factor_id-1),statePtr->pose());
+    valuesPtr->insert(V(imu_factor_id-1),statePtr->v());
+    valuesPtr->insert(B(imu_factor_id-1),*imu_biasPtr);
+    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)));
+    return true;
+}
+bool Imu_Integration::reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost::shared_ptr<svo::Frame>& new_frame){
+    gtsam::Values result = optimizer.optimize();
+    // 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)));
+    preintegratedPtr->resetIntegrationAndSetBias(*imu_biasPtr);
+    ++imu_factor_id;
+    syn=false;
+    gtsam::Marginals marginals(*graphPtr,result);
+    new_frame->T_f_w_=Sophus::SE3(statePtr->R(),statePtr->t());
+    new_frame->Cov_=marginals.marginalCovariance(P(imu_factor_id-1));
+    graphPtr->resize(0);
+    valuesPtr->clear();
+    valuesPtr->insert(P(imu_factor_id-1),statePtr->pose());
+    valuesPtr->insert(V(imu_factor_id-1),statePtr->v());
+    valuesPtr->insert(B(imu_factor_id-1),*imu_biasPtr);
+    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)));
+    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));
+    }else{
+        if(imu_n<1)return false;
+        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), 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;
+    }
+
+    num_obs=0;
+    for(auto&& f:new_frame->fts_){
+       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::ISAM2 optimizer(*optimizerParamPtr);
+    //optimizer.update(*graphPtr, *valuesPtr);
+    // Each call to iSAM2 update(*) performs one iteration of the iterative
+    // nonlinear solver. If accuracy is desired at the expense of time,
+    // update(*) can be called additional times to perform multiple optimizer
+    // iterations every step.
+    //optimizer.update();
+   // optimizer.print();
+    gtsam::LevenbergMarquardtOptimizer optimizer(*graphPtr, *valuesPtr);
+
+    reset(optimizer,new_frame);
+    // 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;
+        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)
+        {
+            // we don't need to delete a reference in the point since it was not created yet
+            f->point = NULL;
+            --num_obs;
+        }
+    }
+    return true;
+}

CPU_version/svo/src/initialization.cpp

@@ -0,0 +1,199 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <svo/config.h>
+#include <svo/frame.h>
+#include <svo/point.h>
+#include <svo/feature.h>
+#include <svo/initialization.h>
+#include <svo/feature_detection.h>
+#include <vikit/math_utils.h>
+#include <vikit/homography.h>
+
+namespace svo {
+namespace initialization {
+
+InitResult KltHomographyInit::addFirstFrame(FramePtr frame_ref)
+{
+  reset();
+  detectFeatures(frame_ref, px_ref_, f_ref_);
+  if(px_ref_.size() < 100)
+  {
+    SVO_WARN_STREAM_THROTTLE(2.0, "First image has less than 100 features. Retry in more textured environment.");
+    return FAILURE;
+  }
+  frame_ref_ = frame_ref;
+  px_cur_.insert(px_cur_.begin(), px_ref_.begin(), px_ref_.end());
+  return SUCCESS;
+}
+
+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;
+
+  double disparity = vk::getMedian(disparities_);
+  SVO_INFO_STREAM("Init: KLT "<<disparity<<"px average disparity.");
+  if(disparity < Config::initMinDisparity())
+    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;
+  }
+
+  // Rescale the map such that the mean scene depth is equal to the specified scale
+  vector<double> depth_vec;
+  for(size_t i=0; i<xyz_in_cur_.size(); ++i)
+    depth_vec.push_back((xyz_in_cur_[i]).z());
+  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()));
+
+  // 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)
+    {
+      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_ref(new Feature(frame_ref_.get(), new_point, px_ref, f_ref_[*it], 0));
+      frame_ref_->addFeature(ftr_ref);
+      new_point->addFrameRef(ftr_ref);
+    }
+  }
+  return SUCCESS;
+}
+
+void KltHomographyInit::reset()
+{
+  px_cur_.clear();
+  frame_ref_.reset();
+}
+
+void detectFeatures(
+    FramePtr frame,
+    vector<cv::Point2f>& px_vec,
+    vector<Vector3d>& f_vec)
+{
+  Features new_features;
+  feature_detection::FastDetector detector(
+      frame->img().cols, frame->img().rows, Config::gridSize(), Config::nPyrLevels());
+  detector.detect(frame.get(), frame->img_pyr_, Config::triangMinCornerScore(), new_features);
+
+  // 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;
+  });
+}
+
+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)
+{
+  const double klt_win_size = 30.0;
+  const int klt_max_iter = 30;
+  const double klt_eps = 0.001;
+  vector<uchar> status;
+  vector<float> error;
+  vector<float> min_eig_vec;
+  cv::TermCriteria termcrit(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, klt_max_iter, klt_eps);
+  cv::calcOpticalFlowPyrLK(frame_ref->img_pyr_[0], frame_cur->img_pyr_[0],
+                           px_ref, px_cur,
+                           status, error,
+                           cv::Size2i(klt_win_size, klt_win_size),
+                           4, termcrit, cv::OPTFLOW_USE_INITIAL_FLOW);
+
+  vector<cv::Point2f>::iterator px_ref_it = px_ref.begin();
+  vector<cv::Point2f>::iterator px_cur_it = px_cur.begin();
+  vector<Vector3d>::iterator f_ref_it = f_ref.begin();
+  f_cur.clear(); f_cur.reserve(px_cur.size());
+  disparities.clear(); disparities.reserve(px_cur.size());
+  for(size_t i=0; px_ref_it != px_ref.end(); ++i)
+  {
+    if(!status[i])
+    {
+      px_ref_it = px_ref.erase(px_ref_it);
+      px_cur_it = px_cur.erase(px_cur_it);
+      f_ref_it = f_ref.erase(f_ref_it);
+      continue;
+    }
+    f_cur.push_back(frame_cur->c2f(px_cur_it->x, px_cur_it->y));
+    disparities.push_back(Vector2d(px_ref_it->x - px_cur_it->x, px_ref_it->y - px_cur_it->y).norm());
+    ++px_ref_it;
+    ++px_cur_it;
+    ++f_ref_it;
+  }
+}
+
+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)
+{
+  vector<Vector2d > uv_ref(f_ref.size());
+  vector<Vector2d > uv_cur(f_cur.size());
+  for(size_t i=0, i_max=f_ref.size(); i<i_max; ++i)
+  {
+    uv_ref[i] = vk::project2d(f_ref[i]);
+    uv_cur[i] = vk::project2d(f_cur[i]);
+  }
+  vk::Homography Homography(uv_ref, uv_cur, focal_length, reprojection_threshold);
+  Homography.computeSE3fromMatches();
+  vector<int> outliers;
+  vk::computeInliers(f_cur, f_ref,
+                     Homography.T_c2_from_c1.rotation_matrix(), Homography.T_c2_from_c1.translation(),
+                     reprojection_threshold, focal_length,
+                     xyz_in_cur, inliers, outliers);
+  T_cur_from_ref = Homography.T_c2_from_c1;
+}
+
+
+} // namespace initialization
+} // namespace svo

CPU_version/svo/src/map.cpp

@@ -0,0 +1,368 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <set>
+#include <svo/map.h>
+#include <svo/point.h>
+#include <svo/frame.h>
+#include <svo/feature.h>
+#include <boost/bind.hpp>
+
+namespace svo {
+
+Map::Map() {}
+
+Map::~Map()
+{
+  reset();
+  SVO_INFO_STREAM("Map destructed");
+}
+
+void Map::reset()
+{
+  keyframes_.clear();
+  point_candidates_.reset();
+  emptyTrash();
+}
+
+bool Map::safeDeleteFrame(FramePtr frame)
+{
+  bool found = false;
+  for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it)
+  {
+    if(*it == frame)
+    {
+      std::for_each((*it)->fts_.begin(), (*it)->fts_.end(), [&](Feature* ftr){
+        removePtFrameRef(it->get(), ftr);
+      });
+      keyframes_.erase(it);
+      found = true;
+      break;
+    }
+  }
+
+  point_candidates_.removeFrameCandidates(frame);
+
+  if(found)
+    return true;
+
+  SVO_ERROR_STREAM("Tried to delete Keyframe in map which was not there.");
+  return false;
+}
+
+void Map::removePtFrameRef(Frame* frame, Feature* ftr)
+{
+  if(ftr->point == NULL)
+    return; // mappoint may have been deleted in a previous ref. removal
+  Point* pt = ftr->point;
+  ftr->point = NULL;
+  if(pt->obs_.size() <= 2)
+  {
+    // If the references list of mappoint has only size=2, delete mappoint
+    safeDeletePoint(pt);
+    return;
+  }
+  pt->deleteFrameRef(frame);  // Remove reference from map_point
+  frame->removeKeyPoint(ftr); // Check if mp was keyMp in keyframe
+}
+
+void Map::safeDeletePoint(Point* pt)
+{
+  // Delete references to mappoints in all keyframes
+  std::for_each(pt->obs_.begin(), pt->obs_.end(), [&](Feature* ftr){
+    ftr->point=NULL;
+    ftr->frame->removeKeyPoint(ftr);
+  });
+  pt->obs_.clear();
+
+  // Delete mappoint
+  deletePoint(pt);
+}
+
+void Map::deletePoint(Point* pt)
+{
+  pt->type_ = Point::TYPE_DELETED;
+  trash_points_.push_back(pt);
+}
+
+void Map::addKeyframe(FramePtr new_keyframe)
+{
+  keyframes_.push_back(new_keyframe);
+}
+
+void Map::getCloseKeyframes(
+    const FramePtr& frame,
+    std::list< std::pair<FramePtr,double> >& close_kfs) const
+{
+  for(auto kf : keyframes_)
+  {
+    // check if kf has overlaping field of view with frame, use therefore KeyPoints
+    for(auto keypoint : kf->key_pts_)
+    {
+      if(keypoint == nullptr)
+        continue;
+
+      if(frame->isVisible(keypoint->point->pos_))
+      {
+        close_kfs.push_back(
+            std::make_pair(
+                kf, (frame->T_f_w_.translation()-kf->T_f_w_.translation()).norm()));
+        break; // this keyframe has an overlapping field of view -> add to close_kfs
+      }
+    }
+  }
+}
+
+FramePtr Map::getClosestKeyframe(const FramePtr& frame) const
+{
+  list< pair<FramePtr,double> > close_kfs;
+  getCloseKeyframes(frame, close_kfs);
+  if(close_kfs.empty())
+  {
+    return nullptr;
+  }
+
+
+  // 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));
+
+  if(close_kfs.front().first != frame)
+    return close_kfs.front().first;
+  close_kfs.pop_front();
+  return close_kfs.front().first;
+}
+
+FramePtr Map::getFurthestKeyframe(const Vector3d& pos) const
+{
+  FramePtr furthest_kf;
+  double maxdist = 0.0;
+  for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it)
+  {
+    double dist = ((*it)->pos()-pos).norm();
+    if(dist > maxdist) {
+      maxdist = dist;
+      furthest_kf = *it;
+    }
+  }
+  return furthest_kf;
+}
+
+bool Map::getKeyframeById(const int id, FramePtr& frame) const
+{
+  bool found = false;
+  for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it)
+    if((*it)->id_ == id) {
+      found = true;
+      frame = *it;
+      break;
+    }
+  return found;
+}
+
+void Map::transform(const Matrix3d& R, const Vector3d& t, const double& s)
+{
+  for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it)
+  {
+    Vector3d pos = s*R*(*it)->pos() + t;
+    Matrix3d rot = R*(*it)->T_f_w_.rotation_matrix().inverse();
+    (*it)->T_f_w_ = SE3(rot, pos).inverse();
+    for(auto ftr=(*it)->fts_.begin(); ftr!=(*it)->fts_.end(); ++ftr)
+    {
+      if((*ftr)->point == NULL)
+        continue;
+      if((*ftr)->point->last_published_ts_ == -1000)
+        continue;
+      (*ftr)->point->last_published_ts_ = -1000;
+      (*ftr)->point->pos_ = s*R*(*ftr)->point->pos_ + t;
+    }
+  }
+}
+
+void Map::emptyTrash()
+{
+  std::for_each(trash_points_.begin(), trash_points_.end(), [&](Point*& pt){
+    delete pt;
+    pt=NULL;
+  });
+  trash_points_.clear();
+  point_candidates_.emptyTrash();
+}
+
+MapPointCandidates::MapPointCandidates()
+{}
+
+MapPointCandidates::~MapPointCandidates()
+{
+  reset();
+}
+
+void MapPointCandidates::newCandidatePoint(Point* point, double depth_sigma2)
+{
+  point->type_ = Point::TYPE_CANDIDATE;
+  boost::unique_lock<boost::mutex> lock(mut_);
+  candidates_.push_back(PointCandidate(point, point->obs_.front()));
+}
+
+void MapPointCandidates::addCandidatePointToFrame(FramePtr frame)
+{
+  boost::unique_lock<boost::mutex> lock(mut_);
+  PointCandidateList::iterator it=candidates_.begin();
+  while(it != candidates_.end())
+  {
+    if(it->first->obs_.front()->frame == frame.get())
+    {
+      // insert feature in the frame
+      it->first->type_ = Point::TYPE_UNKNOWN;
+      it->first->n_failed_reproj_ = 0;
+      it->second->frame->addFeature(it->second);
+      it = candidates_.erase(it);
+    }
+    else
+      ++it;
+  }
+}
+
+bool MapPointCandidates::deleteCandidatePoint(Point* point)
+{
+  boost::unique_lock<boost::mutex> lock(mut_);
+  for(auto it=candidates_.begin(), ite=candidates_.end(); it!=ite; ++it)
+  {
+    if(it->first == point)
+    {
+      deleteCandidate(*it);
+      candidates_.erase(it);
+      return true;
+    }
+  }
+  return false;
+}
+
+void MapPointCandidates::removeFrameCandidates(FramePtr frame)
+{
+  boost::unique_lock<boost::mutex> lock(mut_);
+  auto it=candidates_.begin();
+  while(it!=candidates_.end())
+  {
+    if(it->second->frame == frame.get())
+    {
+      deleteCandidate(*it);
+      it = candidates_.erase(it);
+    }
+    else
+      ++it;
+  }
+}
+
+void MapPointCandidates::reset()
+{
+  boost::unique_lock<boost::mutex> lock(mut_);
+  std::for_each(candidates_.begin(), candidates_.end(), [&](PointCandidate& c){
+    delete c.first;
+    delete c.second;
+  });
+  candidates_.clear();
+}
+
+void MapPointCandidates::deleteCandidate(PointCandidate& c)
+{
+  // camera-rig: another frame might still be pointing to the candidate point
+  // therefore, we can't delete it right now.
+  delete c.second; c.second=NULL;
+  c.first->type_ = Point::TYPE_DELETED;
+  trash_points_.push_back(c.first);
+}
+
+void MapPointCandidates::emptyTrash()
+{
+  std::for_each(trash_points_.begin(), trash_points_.end(), [&](Point*& p){
+    delete p; p=NULL;
+  });
+  trash_points_.clear();
+}
+
+namespace map_debug {
+
+void mapValidation(Map* map, int id)
+{
+  for(auto it=map->keyframes_.begin(); it!=map->keyframes_.end(); ++it)
+    frameValidation(it->get(), id);
+}
+
+void frameValidation(Frame* frame, int id)
+{
+  for(auto it = frame->fts_.begin(); it!=frame->fts_.end(); ++it)
+  {
+    if((*it)->point==NULL)
+      continue;
+
+    if((*it)->point->type_ == Point::TYPE_DELETED)
+      printf("ERROR DataValidation %i: Referenced point was deleted.\n", id);
+
+    if(!(*it)->point->findFrameRef(frame))
+      printf("ERROR DataValidation %i: Frame has reference but point does not have a reference back.\n", id);
+
+    pointValidation((*it)->point, id);
+  }
+  for(auto it=frame->key_pts_.begin(); it!=frame->key_pts_.end(); ++it)
+    if(*it != NULL)
+      if((*it)->point == NULL)
+        printf("ERROR DataValidation %i: KeyPoints not correct!\n", id);
+}
+
+void pointValidation(Point* point, int id)
+{
+  for(auto it=point->obs_.begin(); it!=point->obs_.end(); ++it)
+  {
+    bool found=false;
+    for(auto it_ftr=(*it)->frame->fts_.begin(); it_ftr!=(*it)->frame->fts_.end(); ++it_ftr)
+     if((*it_ftr)->point == point) {
+       found=true; break;
+     }
+    if(!found)
+      printf("ERROR DataValidation %i: Point %i has inconsistent reference in frame %i, is candidate = %i\n", id, point->id_, (*it)->frame->id_, (int) point->type_);
+  }
+}
+
+void mapStatistics(Map* map)
+{
+  // compute average number of features which each frame observes
+  size_t n_pt_obs(0);
+  for(auto it=map->keyframes_.begin(); it!=map->keyframes_.end(); ++it)
+    n_pt_obs += (*it)->nObs();
+  printf("\n\nMap Statistics: Frame avg. point obs = %f\n", (float) n_pt_obs/map->size());
+
+  // compute average number of observations that each point has
+  size_t n_frame_obs(0);
+  size_t n_pts(0);
+  std::set<Point*> points;
+  for(auto it=map->keyframes_.begin(); it!=map->keyframes_.end(); ++it)
+  {
+    for(auto ftr=(*it)->fts_.begin(); ftr!=(*it)->fts_.end(); ++ftr)
+    {
+      if((*ftr)->point == NULL)
+        continue;
+      if(points.insert((*ftr)->point).second) {
+        ++n_pts;
+        n_frame_obs += (*ftr)->point->nRefs();
+      }
+    }
+  }
+  printf("Map Statistics: Point avg. frame obs = %f\n\n", (float) n_frame_obs/n_pts);
+}
+
+} // namespace map_debug
+} // namespace svo

CPU_version/svo/src/matcher.cpp

@@ -0,0 +1,323 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <cstdlib>
+#include <vikit/abstract_camera.h>
+#include <vikit/vision.h>
+#include <vikit/math_utils.h>
+#include <vikit/patch_score.h>
+#include <svo/matcher.h>
+#include <svo/frame.h>
+#include <svo/feature.h>
+#include <svo/point.h>
+#include <svo/config.h>
+#include <svo/feature_alignment.h>
+
+namespace svo {
+
+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)
+{
+  // Compute affine warp matrix A_ref_cur
+  const int halfpatch_size = 5;
+  const Vector3d xyz_ref(f_ref*depth_ref);
+  Vector3d xyz_du_ref(cam_ref.cam2world(px_ref + Vector2d(halfpatch_size,0)*(1<<level_ref)));
+  Vector3d xyz_dv_ref(cam_ref.cam2world(px_ref + Vector2d(0,halfpatch_size)*(1<<level_ref)));
+  xyz_du_ref *= xyz_ref[2]/xyz_du_ref[2];
+  xyz_dv_ref *= xyz_ref[2]/xyz_dv_ref[2];
+  const Vector2d px_cur(cam_cur.world2cam(T_cur_ref*(xyz_ref)));
+  const Vector2d px_du(cam_cur.world2cam(T_cur_ref*(xyz_du_ref)));
+  const Vector2d px_dv(cam_cur.world2cam(T_cur_ref*(xyz_dv_ref)));
+  A_cur_ref.col(0) = (px_du - px_cur)/halfpatch_size;
+  A_cur_ref.col(1) = (px_dv - px_cur)/halfpatch_size;
+}
+
+int getBestSearchLevel(
+    const Matrix2d& A_cur_ref,
+    const int max_level)
+{
+  // Compute patch level in other image
+  int search_level = 0;
+  double D = A_cur_ref.determinant();
+  while(D > 3.0 && search_level < max_level)
+  {
+    search_level += 1;
+    D *= 0.25;
+  }
+  return search_level;
+}
+
+void warpAffine(
+    const Matrix2d& A_cur_ref,
+    const cv::Mat& img_ref,
+    const Vector2d& px_ref,
+    const int level_ref,
+    const int search_level,
+    const int halfpatch_size,
+    uint8_t* patch)
+{
+  const int patch_size = halfpatch_size*2 ;
+  const Matrix2f A_ref_cur = A_cur_ref.inverse().cast<float>();
+  if(isnan(A_ref_cur(0,0)))
+  {
+    printf("Affine warp is NaN, probably camera has no translation\n"); // TODO
+    return;
+  }
+
+  // Perform the warp on a larger patch.
+  uint8_t* patch_ptr = patch;
+  const Vector2f px_ref_pyr = px_ref.cast<float>() / (1<<level_ref);
+  for (int y=0; y<patch_size; ++y)
+  {
+    for (int x=0; x<patch_size; ++x, ++patch_ptr)
+    {
+      Vector2f px_patch(x-halfpatch_size, y-halfpatch_size);
+      px_patch *= (1<<search_level);
+      const Vector2f px(A_ref_cur*px_patch + px_ref_pyr);
+      if (px[0]<0 || px[1]<0 || px[0]>=img_ref.cols-1 || px[1]>=img_ref.rows-1)
+        *patch_ptr = 0;
+      else
+        *patch_ptr = (uint8_t) vk::interpolateMat_8u(img_ref, px[0], px[1]);
+    }
+  }
+}
+
+} // namespace warp
+
+bool depthFromTriangulation(
+    const SE3& T_search_ref,
+    const Vector3d& f_ref,
+    const Vector3d& f_cur,
+    double& depth)
+{
+  Matrix<double,3,2> A; A << T_search_ref.rotation_matrix() * f_ref, f_cur;
+  const Matrix2d AtA = A.transpose()*A;
+  if(AtA.determinant() < 0.000001)
+    return false;
+  const Vector2d depth2 = - AtA.inverse()*A.transpose()*T_search_ref.translation();
+  depth = fabs(depth2[0]);
+  return true;
+}
+
+void Matcher::createPatchFromPatchWithBorder()
+{
+  uint8_t* ref_patch_ptr = patch_;
+  for(int y=1; y<patch_size_+1; ++y, ref_patch_ptr += patch_size_)
+  {
+    uint8_t* ref_patch_border_ptr = patch_with_border_ + y*(patch_size_+2) + 1;
+    for(int x=0; x<patch_size_; ++x)
+      ref_patch_ptr[x] = ref_patch_border_ptr[x];
+  }
+}
+
+bool Matcher::findMatchDirect(
+    const Point& pt,
+    const Frame& cur_frame,
+    Vector2d& px_cur)
+{
+  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;
+
+  // warp affine
+  warp::getWarpMatrixAffine(
+      *ref_ftr_->frame->cam_, *cur_frame.cam_, ref_ftr_->px, ref_ftr_->f,
+      (ref_ftr_->frame->pos() - pt.pos_).norm(),
+      cur_frame.T_f_w_ * ref_ftr_->frame->T_f_w_.inverse(), ref_ftr_->level, A_cur_ref_);
+  search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1);
+  warp::warpAffine(A_cur_ref_, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px,
+                   ref_ftr_->level, search_level_, halfpatch_size_+1, patch_with_border_);
+  createPatchFromPatchWithBorder();
+
+  // px_cur should be set
+  Vector2d px_scaled(px_cur/(1<<search_level_));
+
+  bool success = false;
+  if(ref_ftr_->type == Feature::EDGELET)
+  {
+    Vector2d dir_cur(A_cur_ref_*ref_ftr_->grad);
+    dir_cur.normalize();
+    success = feature_alignment::align1D(
+          cur_frame.img_pyr_[search_level_], dir_cur.cast<float>(),
+          patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_);
+  }
+  else
+  {
+    success = feature_alignment::align2D(
+      cur_frame.img_pyr_[search_level_], patch_with_border_, patch_,
+      options_.align_max_iter, px_scaled);
+  }
+  px_cur = px_scaled * (1<<search_level_);
+  return success;
+}
+
+bool Matcher::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)
+{
+  SE3 T_cur_ref = cur_frame.T_f_w_ * ref_frame.T_f_w_.inverse();
+  int zmssd_best = PatchScore::threshold();
+  Vector2d uv_best;
+
+  // Compute start and end of epipolar line in old_kf for match search, on unit plane!
+  Vector2d A = vk::project2d(T_cur_ref * (ref_ftr.f*d_min));
+  Vector2d B = vk::project2d(T_cur_ref * (ref_ftr.f*d_max));
+  epi_dir_ = A - B;
+
+  // Compute affine warp matrix
+  warp::getWarpMatrixAffine(
+      *ref_frame.cam_, *cur_frame.cam_, ref_ftr.px, ref_ftr.f,
+      d_estimate, T_cur_ref, ref_ftr.level, A_cur_ref_);
+
+  // feature pre-selection
+  reject_ = false;
+  if(ref_ftr.type == Feature::EDGELET && options_.epi_search_edgelet_filtering)
+  {
+    const Vector2d grad_cur = (A_cur_ref_ * ref_ftr.grad).normalized();
+    const double cosangle = fabs(grad_cur.dot(epi_dir_.normalized()));
+    if(cosangle < options_.epi_search_edgelet_max_angle) {
+      reject_ = true;
+      return false;
+    }
+  }
+
+  search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1);
+
+  // Find length of search range on epipolar line
+  Vector2d px_A(cur_frame.cam_->world2cam(A));
+  Vector2d px_B(cur_frame.cam_->world2cam(B));
+  epi_length_ = (px_A-px_B).norm() / (1<<search_level_);
+
+  // Warp reference patch at ref_level
+  warp::warpAffine(A_cur_ref_, ref_frame.img_pyr_[ref_ftr.level], ref_ftr.px,
+                   ref_ftr.level, search_level_, halfpatch_size_+1, patch_with_border_);
+  createPatchFromPatchWithBorder();
+
+  if(epi_length_ < 2.0)
+  {
+    px_cur_ = (px_A+px_B)/2.0;
+    Vector2d px_scaled(px_cur_/(1<<search_level_));
+    bool res;
+    if(options_.align_1d)
+      res = feature_alignment::align1D(
+          cur_frame.img_pyr_[search_level_], (px_A-px_B).cast<float>().normalized(),
+          patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_);
+    else
+      res = feature_alignment::align2D(
+          cur_frame.img_pyr_[search_level_], patch_with_border_, patch_,
+          options_.align_max_iter, px_scaled);
+    if(res)
+    {
+      px_cur_ = px_scaled*(1<<search_level_);
+      if(depthFromTriangulation(T_cur_ref, ref_ftr.f, cur_frame.cam_->cam2world(px_cur_), depth))
+        return true;
+    }
+    return false;
+  }
+
+  size_t n_steps = epi_length_/0.7; // one step per pixel
+  Vector2d step = epi_dir_/n_steps;
+
+  if(n_steps > options_.max_epi_search_steps)
+  {
+    printf("WARNING: skip epipolar search: %zu evaluations, px_lenght=%f, d_min=%f, d_max=%f.\n",
+           n_steps, epi_length_, d_min, d_max);
+    return false;
+  }
+
+  // for matching, precompute sum and sum2 of warped reference patch
+  int pixel_sum = 0;
+  int pixel_sum_square = 0;
+  PatchScore patch_score(patch_);
+
+  // now we sample along the epipolar line
+  Vector2d uv = B-step;
+  Vector2i last_checked_pxi(0,0);
+  ++n_steps;
+  for(size_t i=0; i<n_steps; ++i, uv+=step)
+  {
+    Vector2d px(cur_frame.cam_->world2cam(uv));
+    Vector2i pxi(px[0]/(1<<search_level_)+0.5,
+                 px[1]/(1<<search_level_)+0.5); // +0.5 to round to closest int
+
+    if(pxi == last_checked_pxi)
+      continue;
+    last_checked_pxi = pxi;
+
+    // check if the patch is full within the new frame
+    if(!cur_frame.cam_->isInFrame(pxi, patch_size_, search_level_))
+      continue;
+
+    // TODO interpolation would probably be a good idea
+    uint8_t* cur_patch_ptr = cur_frame.img_pyr_[search_level_].data
+                             + (pxi[1]-halfpatch_size_)*cur_frame.img_pyr_[search_level_].cols
+                             + (pxi[0]-halfpatch_size_);
+    int zmssd = patch_score.computeScore(cur_patch_ptr, cur_frame.img_pyr_[search_level_].cols);
+
+    if(zmssd < zmssd_best) {
+      zmssd_best = zmssd;
+      uv_best = uv;
+    }
+  }
+
+  if(zmssd_best < PatchScore::threshold())
+  {
+    if(options_.subpix_refinement)
+    {
+      px_cur_ = cur_frame.cam_->world2cam(uv_best);
+      Vector2d px_scaled(px_cur_/(1<<search_level_));
+      bool res;
+      if(options_.align_1d)
+        res = feature_alignment::align1D(
+            cur_frame.img_pyr_[search_level_], (px_A-px_B).cast<float>().normalized(),
+            patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_);
+      else
+        res = feature_alignment::align2D(
+            cur_frame.img_pyr_[search_level_], patch_with_border_, patch_,
+            options_.align_max_iter, px_scaled);
+      if(res)
+      {
+        px_cur_ = px_scaled*(1<<search_level_);
+        if(depthFromTriangulation(T_cur_ref, ref_ftr.f, cur_frame.cam_->cam2world(px_cur_), depth))
+          return true;
+      }
+      return false;
+    }
+    px_cur_ = cur_frame.cam_->world2cam(uv_best);
+    if(depthFromTriangulation(T_cur_ref, ref_ftr.f, vk::unproject2d(uv_best).normalized(), depth))
+      return true;
+  }
+  return false;
+}
+
+} // namespace svo

CPU_version/svo/src/point.cpp

@@ -0,0 +1,179 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <stdexcept>
+#include <vikit/math_utils.h>
+#include <svo/point.h>
+#include <svo/frame.h>
+#include <svo/feature.h>
+ 
+namespace svo {
+
+int Point::point_counter_ = 0;
+
+Point::Point(const Vector3d& pos) :
+  id_(point_counter_++),
+  pos_(pos),
+  normal_set_(false),
+  n_obs_(0),
+  v_pt_(NULL),
+  last_published_ts_(0),
+  last_projected_kf_id_(-1),
+  type_(TYPE_UNKNOWN),
+  n_failed_reproj_(0),
+  n_succeeded_reproj_(0),
+  last_structure_optim_(0)
+{}
+
+Point::Point(const Vector3d& pos, Feature* ftr) :
+  id_(point_counter_++),
+  pos_(pos),
+  normal_set_(false),
+  n_obs_(1),
+  v_pt_(NULL),
+  last_published_ts_(0),
+  last_projected_kf_id_(-1),
+  type_(TYPE_UNKNOWN),
+  n_failed_reproj_(0),
+  n_succeeded_reproj_(0),
+  last_structure_optim_(0)
+{
+  obs_.push_front(ftr);
+}
+
+Point::~Point()
+{}
+
+void Point::addFrameRef(Feature* ftr)
+{
+  obs_.push_front(ftr);
+  ++n_obs_;
+}
+
+Feature* Point::findFrameRef(Frame* frame)
+{
+  for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it)
+    if((*it)->frame == frame)
+      return *it;
+  return NULL;    // no keyframe found
+}
+
+bool Point::deleteFrameRef(Frame* frame)
+{
+  for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it)
+  {
+    if((*it)->frame == frame)
+    {
+      obs_.erase(it);
+      return true;
+    }
+  }
+  return false;
+}
+
+void Point::initNormal()
+{
+  assert(!obs_.empty());
+  const Feature* ftr = obs_.back();
+  assert(ftr->frame != NULL);
+  normal_ = ftr->frame->T_f_w_.rotation_matrix().transpose()*(-ftr->f);
+  normal_information_ = DiagonalMatrix<double,3,3>(pow(20/(pos_-ftr->frame->pos()).norm(),2), 1.0, 1.0);
+  normal_set_ = true;
+}
+
+bool Point::getCloseViewObs(const Vector3d& framepos, Feature*& ftr) const
+{
+  // TODO: get frame with same point of view AND same pyramid level!
+  Vector3d obs_dir(framepos - pos_); obs_dir.normalize();
+  auto min_it=obs_.begin();
+  double min_cos_angle = 0;
+  for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it)
+  {
+    Vector3d dir((*it)->frame->pos() - pos_); dir.normalize();
+    double cos_angle = obs_dir.dot(dir);
+    if(cos_angle > min_cos_angle)
+    {
+      min_cos_angle = cos_angle;
+      min_it = it;
+    }
+  }
+  ftr = *min_it;
+  if(min_cos_angle < 0.5) // assume that observations larger than 60° are useless
+    return false;
+  return true;
+}
+
+void Point::optimize(const size_t n_iter)
+{
+  Vector3d old_point = pos_;
+  double chi2 = 0.0;
+  Matrix3d A;
+  Vector3d b;
+
+  for(size_t i=0; i<n_iter; i++)
+  {
+    A.setZero();
+    b.setZero();
+    double new_chi2 = 0.0;
+
+    // compute residuals
+    for(auto it=obs_.begin(); it!=obs_.end(); ++it)
+    {
+      Matrix23d J;
+      const Vector3d p_in_f((*it)->frame->T_f_w_ * pos_);
+      Point::jacobian_xyz2uv(p_in_f, (*it)->frame->T_f_w_.rotation_matrix(), J);
+      const Vector2d e(vk::project2d((*it)->f) - vk::project2d(p_in_f));
+      new_chi2 += e.squaredNorm();
+      A.noalias() += J.transpose() * J;
+      b.noalias() -= J.transpose() * e;
+    }
+
+    // solve linear system
+    const Vector3d dp(A.ldlt().solve(b));
+
+    // check if error increased
+    if((i > 0 && new_chi2 > chi2) || (bool) std::isnan((double)dp[0]))
+    {
+#ifdef POINT_OPTIMIZER_DEBUG
+      cout << "it " << i
+           << "\t FAILURE \t new_chi2 = " << new_chi2 << endl;
+#endif
+      pos_ = old_point; // roll-back
+      break;
+    }
+
+    // update the model
+    Vector3d new_point = pos_ + dp;
+    old_point = pos_;
+    pos_ = new_point;
+    chi2 = new_chi2;
+#ifdef POINT_OPTIMIZER_DEBUG
+    cout << "it " << i
+         << "\t Success \t new_chi2 = " << new_chi2
+         << "\t norm(b) = " << vk::norm_max(b)
+         << endl;
+#endif
+
+    // stop when converged
+    if(vk::norm_max(dp) <= EPS)
+      break;
+  }
+#ifdef POINT_OPTIMIZER_DEBUG
+  cout << endl;
+#endif
+}
+
+} // namespace svo

CPU_version/svo/src/pose_optimizer.cpp

@@ -0,0 +1,165 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <stdexcept>
+#include <svo/pose_optimizer.h>
+#include <svo/frame.h>
+#include <svo/feature.h>
+#include <svo/point.h>
+#include <vikit/robust_cost.h>
+#include <vikit/math_utils.h>
+
+namespace svo {
+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)
+{
+  // init
+  double chi2(0.0);
+  vector<double> chi2_vec_init, chi2_vec_final;
+  vk::robust_cost::TukeyWeightFunction weight_function;
+  SE3 T_old(frame->T_f_w_);
+  Matrix6d A;
+  Vector6d b;
+
+  // compute the scale of the error for robust estimation
+  std::vector<float> errors; errors.reserve(frame->fts_.size());
+  for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
+  {
+    if((*it)->point == NULL)
+      continue;
+    //Reprojection error
+    Vector2d e = vk::project2d((*it)->f)
+               - vk::project2d(frame->T_f_w_ * (*it)->point->pos_);
+    e *= 1.0 / (1<<(*it)->level);
+    errors.push_back(e.norm());
+  }
+  if(errors.empty())
+    return;
+  vk::robust_cost::MADScaleEstimator scale_estimator;
+  estimated_scale = scale_estimator.compute(errors);
+
+  num_obs = errors.size();
+  chi2_vec_init.reserve(num_obs);
+  chi2_vec_final.reserve(num_obs);
+  double scale = estimated_scale;
+  for(size_t iter=0; iter<n_iter; iter++)
+  {
+    // overwrite scale
+    if(iter == 5)
+      scale = 0.85/frame->cam_->errorMultiplier2();
+
+    b.setZero();
+    A.setZero();
+    double new_chi2(0.0);
+
+    // compute residual
+    for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
+    {
+      if((*it)->point == NULL)
+        continue;
+      Matrix26d J;
+      Vector3d xyz_f(frame->T_f_w_ * (*it)->point->pos_);
+      Frame::jacobian_xyz2uv(xyz_f, J);
+      Vector2d e = vk::project2d((*it)->f) - vk::project2d(xyz_f);
+      double sqrt_inv_cov = 1.0 / (1<<(*it)->level);
+      e *= sqrt_inv_cov;
+      if(iter == 0)
+        chi2_vec_init.push_back(e.squaredNorm()); // just for debug
+      J *= sqrt_inv_cov;
+      double weight = weight_function.value(e.norm()/scale);
+      A.noalias() += J.transpose()*J*weight;
+      b.noalias() -= J.transpose()*e*weight;
+      new_chi2 += e.squaredNorm()*weight;
+    }
+
+    // solve linear system
+    const Vector6d dT(A.ldlt().solve(b));
+
+    // check if error increased
+    if((iter > 0 && new_chi2 > chi2) || (bool) std::isnan((double)dT[0]))
+    {
+      if(verbose)
+        std::cout << "it " << iter
+                  << "\t FAILURE \t new_chi2 = " << new_chi2 << std::endl;
+      frame->T_f_w_ = T_old; // roll-back
+      break;
+    }
+
+    // update the model
+    SE3 T_new = SE3::exp(dT)*frame->T_f_w_;
+    T_old = frame->T_f_w_;
+    frame->T_f_w_ = T_new;
+    chi2 = new_chi2;
+    if(verbose)
+      std::cout << "it " << iter
+                << "\t Success \t new_chi2 = " << new_chi2
+                << "\t norm(dT) = " << vk::norm_max(dT) << std::endl;
+
+    // stop when converged
+    if(vk::norm_max(dT) <= EPS)
+      break;
+  }
+
+  // Set covariance as inverse information matrix. Optimistic estimator!
+  const double pixel_variance=1.0;
+  frame->Cov_ = pixel_variance*(A*std::pow(frame->cam_->errorMultiplier2(),2)).inverse();
+
+  // Remove Measurements with too large reprojection error
+  double reproj_thresh_scaled = reproj_thresh / frame->cam_->errorMultiplier2();
+  size_t n_deleted_refs = 0;
+  for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
+  {
+    if((*it)->point == NULL)
+      continue;
+    Vector2d e = vk::project2d((*it)->f) - vk::project2d(frame->T_f_w_ * (*it)->point->pos_);
+    double sqrt_inv_cov = 1.0 / (1<<(*it)->level);
+    e *= sqrt_inv_cov;
+    chi2_vec_final.push_back(e.squaredNorm());
+    if(e.norm() > reproj_thresh_scaled)
+    {
+      // we don't need to delete a reference in the point since it was not created yet
+      (*it)->point = NULL;
+      ++n_deleted_refs;
+    }
+  }
+
+  error_init=0.0;
+  error_final=0.0;
+  if(!chi2_vec_init.empty())
+    error_init = sqrt(vk::getMedian(chi2_vec_init))*frame->cam_->errorMultiplier2();
+  if(!chi2_vec_final.empty())
+    error_final = sqrt(vk::getMedian(chi2_vec_final))*frame->cam_->errorMultiplier2();
+
+  estimated_scale *= frame->cam_->errorMultiplier2();
+  if(verbose)
+    std::cout << "n deleted obs = " << n_deleted_refs
+              << "\t scale = " << estimated_scale
+              << "\t error init = " << error_init
+              << "\t error end = " << error_final << std::endl;
+  num_obs -= n_deleted_refs;
+}
+
+} // namespace pose_optimizer
+} // namespace svo

CPU_version/svo/src/reprojector.cpp

@@ -0,0 +1,219 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <algorithm>
+#include <stdexcept>
+#include <svo/reprojector.h>
+#include <svo/frame.h>
+#include <svo/point.h>
+#include <svo/feature.h>
+#include <svo/map.h>
+#include <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>
+
+namespace svo {
+
+Reprojector::Reprojector(vk::AbstractCamera* cam, Map& map) :
+    map_(map)
+{
+  initializeGrid(cam);
+}
+
+Reprojector::~Reprojector()
+{
+  std::for_each(grid_.cells.begin(), grid_.cells.end(), [&](Cell* c){ delete c; });
+}
+
+void Reprojector::initializeGrid(vk::AbstractCamera* cam)
+{
+  grid_.cell_size = Config::gridSize();
+  grid_.grid_n_cols = ceil(static_cast<double>(cam->width())/grid_.cell_size);
+  grid_.grid_n_rows = ceil(static_cast<double>(cam->height())/grid_.cell_size);
+  grid_.cells.resize(grid_.grid_n_cols*grid_.grid_n_rows);
+  std::for_each(grid_.cells.begin(), grid_.cells.end(), [&](Cell*& c){ c = new Cell; });
+  grid_.cell_order.resize(grid_.cells.size());
+  for(size_t i=0; i<grid_.cells.size(); ++i)
+    grid_.cell_order[i] = i;
+  random_shuffle(grid_.cell_order.begin(), grid_.cell_order.end()); // maybe we should do it at every iteration!
+}
+
+void Reprojector::resetGrid()
+{
+  n_matches_ = 0;
+  n_trials_ = 0;
+  std::for_each(grid_.cells.begin(), grid_.cells.end(), [&](Cell* c){ c->clear(); });
+}
+
+void Reprojector::reprojectMap(
+    FramePtr frame,
+    std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs)
+{
+  resetGrid();
+
+  // Identify those Keyframes which share a common field of view.
+  SVO_START_TIMER("reproject_kfs");
+  list< pair<FramePtr,double> > close_kfs;
+  map_.getCloseKeyframes(frame, close_kfs);
+
+  // 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));
+
+  // Reproject all mappoints of the closest N kfs with overlap. We only store
+  // in which grid cell the points fall.
+  size_t n = 0;
+  overlap_kfs.reserve(options_.max_n_kfs);
+  for(auto it_frame=close_kfs.begin(), ite_frame=close_kfs.end();
+      it_frame!=ite_frame && n<options_.max_n_kfs; ++it_frame, ++n)
+  {
+    FramePtr ref_frame = it_frame->first;
+    overlap_kfs.push_back(pair<FramePtr,size_t>(ref_frame,0));
+
+    // Try to reproject each mappoint that the other KF observes
+    for(auto it_ftr=ref_frame->fts_.begin(), ite_ftr=ref_frame->fts_.end();
+        it_ftr!=ite_ftr; ++it_ftr)
+    {
+      // check if the feature has a mappoint assigned
+      if((*it_ftr)->point == NULL)
+        continue;
+
+      // make sure we project a point only once
+      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++;
+    }
+  }
+  SVO_STOP_TIMER("reproject_kfs");
+
+  // Now project all point candidates
+  SVO_START_TIMER("reproject_candidates");
+  {
+    boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_);
+    auto it=map_.point_candidates_.candidates_.begin();
+    while(it!=map_.point_candidates_.candidates_.end())
+    {
+      if(!reprojectPoint(frame, it->first))
+      {
+        it->first->n_failed_reproj_ += 3;
+        if(it->first->n_failed_reproj_ > 30)
+        {
+          map_.point_candidates_.deleteCandidate(*it);
+          it = map_.point_candidates_.candidates_.erase(it);
+          continue;
+        }
+      }
+      ++it;
+    }
+  } // unlock the mutex when out of scope
+  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");
+  for(size_t i=0; i<grid_.cells.size(); ++i)
+  {
+    // we prefer good quality points over unkown quality (more likely to match)
+    // and unknown quality over candidates (position not optimized)
+    if(reprojectCell(*grid_.cells.at(grid_.cell_order[i]), frame))
+      ++n_matches_;
+    if(n_matches_ > (size_t) Config::maxFts())
+      break;
+  }
+  SVO_STOP_TIMER("feature_align");
+}
+
+bool Reprojector::pointQualityComparator(Candidate& lhs, Candidate& rhs)
+{
+  if(lhs.pt->type_ > rhs.pt->type_)
+    return true;
+  return false;
+}
+
+bool Reprojector::reprojectCell(Cell& cell, FramePtr frame)
+{
+  cell.sort(boost::bind(&Reprojector::pointQualityComparator, _1, _2));
+  Cell::iterator it=cell.begin();
+  while(it!=cell.end())
+  {
+    ++n_trials_;
+
+    if(it->pt->type_ == Point::TYPE_DELETED)
+    {
+      it = cell.erase(it);
+      continue;
+    }
+
+    bool found_match = true;
+    if(options_.find_match_direct)
+      found_match = matcher_.findMatchDirect(*it->pt, *frame, it->px);
+    if(!found_match)
+    {
+      it->pt->n_failed_reproj_++;
+      if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_failed_reproj_ > 15)
+        map_.safeDeletePoint(it->pt);
+      if(it->pt->type_ == Point::TYPE_CANDIDATE  && it->pt->n_failed_reproj_ > 30)
+        map_.point_candidates_.deleteCandidatePoint(it->pt);
+      it = cell.erase(it);
+      continue;
+    }
+    it->pt->n_succeeded_reproj_++;
+    if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_succeeded_reproj_ > 10)
+      it->pt->type_ = Point::TYPE_GOOD;
+
+    Feature* new_feature = new Feature(frame.get(), it->px, matcher_.search_level_);
+    frame->addFeature(new_feature);
+
+    // Here we add a reference in the feature to the 3D point, the other way
+    // round is only done if this frame is selected as keyframe.
+    new_feature->point = it->pt;
+
+    if(matcher_.ref_ftr_->type == Feature::EDGELET)
+    {
+      new_feature->type = Feature::EDGELET;
+      new_feature->grad = matcher_.A_cur_ref_*matcher_.ref_ftr_->grad;
+      new_feature->grad.normalize();
+    }
+
+    // If the keyframe is selected and we reproject the rest, we don't have to
+    // check this point anymore.
+    it = cell.erase(it);
+
+    // Maximum one point per cell.
+    return true;
+  }
+  return false;
+}
+
+bool Reprojector::reprojectPoint(FramePtr frame, Point* point)
+{
+  Vector2d px(frame->w2c(point->pos_));
+  if(frame->cam_->isInFrame(px.cast<int>(), 8)) // 8px is the patch size in the matcher
+  {
+    const int k = static_cast<int>(px[1]/grid_.cell_size)*grid_.grid_n_cols
+                + static_cast<int>(px[0]/grid_.cell_size);
+    grid_.cells.at(k)->push_back(Candidate(point, px));
+    return true;
+  }
+  return false;
+}
+
+} // namespace svo

CPU_version/svo/src/sparse_img_align.cpp

@@ -0,0 +1,274 @@
+// This file is part of SVO - Semi-direct Visual Odometry.
+//
+// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
+// (Robotics and Perception Group, University of Zurich, Switzerland).
+//
+// SVO is free software: you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the Free Software
+// Foundation, either version 3 of the License, or any later version.
+//
+// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <algorithm>
+#include <svo/sparse_img_align.h>
+#include <svo/frame.h>
+#include <svo/feature.h>
+#include <svo/config.h>
+#include <svo/point.h>
+#include <vikit/abstract_camera.h>
+#include <vikit/vision.h>
+#include <vikit/math_utils.h>
+
+namespace svo {
+
+SparseImgAlign::SparseImgAlign(
+    int max_level, int min_level, int n_iter,
+    Method method, bool display, bool verbose) :
+        display_(display),
+        max_level_(max_level),
+        min_level_(min_level)
+{
+  n_iter_ = n_iter;
+  n_iter_init_ = n_iter_;
+  method_ = method;
+  verbose_ = verbose;
+  eps_ = 0.000001;
+}
+
+size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame)
+{
+  reset();
+
+  if(ref_frame->fts_.empty())
+  {
+    SVO_WARN_STREAM("SparseImgAlign: no features to track!");
+    return 0;
+  }
+
+  ref_frame_ = ref_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?
+
+  SE3 T_cur_from_ref(cur_frame_->T_f_w_ * ref_frame_->T_f_w_.inverse());
+
+  for(level_=max_level_; level_>=min_level_; --level_)
+  {
+    mu_ = 0.1;
+    jacobian_cache_.setZero();
+    have_ref_patch_cache_ = false;
+    if(verbose_)
+      printf("\nPYRAMID LEVEL %i\n---------------\n", level_);
+    optimize(T_cur_from_ref);
+  }
+  cur_frame_->T_f_w_ = T_cur_from_ref * ref_frame_->T_f_w_;
+
+
+
+  return n_meas_/patch_area_;
+}
+
+Matrix<double, 6, 6> SparseImgAlign::getFisherInformation()
+{
+  double sigma_i_sq = 5e-4*255*255; // image noise
+  Matrix<double,6,6> I = H_/sigma_i_sq;
+  return I;
+}
+
+void SparseImgAlign::precomputeReferencePatches()
+{
+  const int border = patch_halfsize_+1;
+  const cv::Mat& ref_img = ref_frame_->img_pyr_.at(level_);
+  const int stride = ref_img.cols;
+  const float scale = 1.0f/(1<<level_);
+  const Vector3d ref_pos = ref_frame_->pos();
+  const double focal_length = ref_frame_->cam_->errorMultiplier2();
+  size_t feature_counter = 0;
+  std::vector<bool>::iterator visiblity_it = visible_fts_.begin();
+  for(auto it=ref_frame_->fts_.begin(), ite=ref_frame_->fts_.end();
+      it!=ite; ++it, ++feature_counter, ++visiblity_it)
+  {
+    // check if reference with patch size is within image
+    const float u_ref = (*it)->px[0]*scale;
+    const float v_ref = (*it)->px[1]*scale;
+    const int u_ref_i = floorf(u_ref);
+    const int v_ref_i = floorf(v_ref);
+    if((*it)->point == NULL || u_ref_i-border < 0 || v_ref_i-border < 0 || u_ref_i+border >= ref_img.cols || v_ref_i+border >= ref_img.rows)
+      continue;
+    *visiblity_it = true;
+
+    // cannot just take the 3d points coordinate because of the reprojection errors in the reference image!!!
+    const double depth(((*it)->point->pos_ - ref_pos).norm());
+    const Vector3d xyz_ref((*it)->f*depth);
+
+    // evaluate projection jacobian
+    Matrix<double,2,6> frame_jac;
+    Frame::jacobian_xyz2uv(xyz_ref, frame_jac);
+
+    // compute bilateral interpolation weights for reference image
+    const float subpix_u_ref = u_ref-u_ref_i;
+    const float subpix_v_ref = v_ref-v_ref_i;
+    const float w_ref_tl = (1.0-subpix_u_ref) * (1.0-subpix_v_ref);
+    const float w_ref_tr = subpix_u_ref * (1.0-subpix_v_ref);
+    const float w_ref_bl = (1.0-subpix_u_ref) * subpix_v_ref;
+    const float w_ref_br = subpix_u_ref * subpix_v_ref;
+    size_t pixel_counter = 0;
+    float* cache_ptr = reinterpret_cast<float*>(ref_patch_cache_.data) + patch_area_*feature_counter;
+    for(int y=0; y<patch_size_; ++y)
+    {
+      uint8_t* ref_img_ptr = (uint8_t*) ref_img.data + (v_ref_i+y-patch_halfsize_)*stride + (u_ref_i-patch_halfsize_);
+      for(int x=0; x<patch_size_; ++x, ++ref_img_ptr, ++cache_ptr, ++pixel_counter)
+      {
+        // precompute interpolated reference patch color
+        *cache_ptr = w_ref_tl*ref_img_ptr[0] + w_ref_tr*ref_img_ptr[1] + w_ref_bl*ref_img_ptr[stride] + w_ref_br*ref_img_ptr[stride+1];
+
+        // we use the inverse compositional: thereby we can take the gradient always at the same position
+        // get gradient of warped image (~gradient at warped position)
+        float dx = 0.5f * ((w_ref_tl*ref_img_ptr[1] + w_ref_tr*ref_img_ptr[2] + w_ref_bl*ref_img_ptr[stride+1] + w_ref_br*ref_img_ptr[stride+2])
+                          -(w_ref_tl*ref_img_ptr[-1] + w_ref_tr*ref_img_ptr[0] + w_ref_bl*ref_img_ptr[stride-1] + w_ref_br*ref_img_ptr[stride]));
+        float dy = 0.5f * ((w_ref_tl*ref_img_ptr[stride] + w_ref_tr*ref_img_ptr[1+stride] + w_ref_bl*ref_img_ptr[stride*2] + w_ref_br*ref_img_ptr[stride*2+1])
+                          -(w_ref_tl*ref_img_ptr[-stride] + w_ref_tr*ref_img_ptr[1-stride] + w_ref_bl*ref_img_ptr[0] + w_ref_br*ref_img_ptr[1]));
+
+        // cache the jacobian
+        jacobian_cache_.col(feature_counter*patch_area_ + pixel_counter) =
+            (dx*frame_jac.row(0) + dy*frame_jac.row(1))*(focal_length / (1<<level_));
+      }
+    }
+  }
+  have_ref_patch_cache_ = true;
+}
+
+double SparseImgAlign::computeResiduals(
+    const SE3& T_cur_from_ref,
+    bool linearize_system,
+    bool compute_weight_scale)
+{
+  // Warp the (cur)rent image such that it aligns with the (ref)erence image
+  const cv::Mat& cur_img = cur_frame_->img_pyr_.at(level_);
+
+  if(linearize_system && display_)
+    resimg_ = cv::Mat(cur_img.size(), CV_32F, cv::Scalar(0));
+
+  if(have_ref_patch_cache_ == false)
+    precomputeReferencePatches();
+
+  // compute the weights on the first iteration
+  std::vector<float> errors;
+  if(compute_weight_scale)
+    errors.reserve(visible_fts_.size());
+  const int stride = cur_img.cols;
+  const int border = patch_halfsize_+1;
+  const float scale = 1.0f/(1<<level_);
+  const Vector3d ref_pos(ref_frame_->pos());
+  float chi2 = 0.0;
+  size_t feature_counter = 0; // is used to compute the index of the cached jacobian
+  std::vector<bool>::iterator visiblity_it = visible_fts_.begin();
+  for(auto it=ref_frame_->fts_.begin(); it!=ref_frame_->fts_.end();
+      ++it, ++feature_counter, ++visiblity_it)
+  {
+    // check if feature is within image
+    if(!*visiblity_it)
+      continue;
+
+    // compute pixel location in cur img
+    const double depth = ((*it)->point->pos_ - ref_pos).norm();
+    const Vector3d xyz_ref((*it)->f*depth);
+    const Vector3d xyz_cur(T_cur_from_ref * xyz_ref);
+    const Vector2f uv_cur_pyr(cur_frame_->cam_->world2cam(xyz_cur).cast<float>() * scale);
+    const float u_cur = uv_cur_pyr[0];
+    const float v_cur = uv_cur_pyr[1];
+    const int u_cur_i = floorf(u_cur);
+    const int v_cur_i = floorf(v_cur);
+
+    // check if projection is within the image
+    if(u_cur_i < 0 || v_cur_i < 0 || u_cur_i-border < 0 || v_cur_i-border < 0 || u_cur_i+border >= cur_img.cols || v_cur_i+border >= cur_img.rows)
+      continue;
+
+    // compute bilateral interpolation weights for the current image
+    const float subpix_u_cur = u_cur-u_cur_i;
+    const float subpix_v_cur = v_cur-v_cur_i;
+    const float w_cur_tl = (1.0-subpix_u_cur) * (1.0-subpix_v_cur);
+    const float w_cur_tr = subpix_u_cur * (1.0-subpix_v_cur);
+    const float w_cur_bl = (1.0-subpix_u_cur) * subpix_v_cur;
+    const float w_cur_br = subpix_u_cur * subpix_v_cur;
+    float* ref_patch_cache_ptr = reinterpret_cast<float*>(ref_patch_cache_.data) + patch_area_*feature_counter;
+    size_t pixel_counter = 0; // is used to compute the index of the cached jacobian
+    for(int y=0; y<patch_size_; ++y)
+    {
+      uint8_t* cur_img_ptr = (uint8_t*) cur_img.data + (v_cur_i+y-patch_halfsize_)*stride + (u_cur_i-patch_halfsize_);
+
+      for(int x=0; x<patch_size_; ++x, ++pixel_counter, ++cur_img_ptr, ++ref_patch_cache_ptr)
+      {
+        // compute residual
+        const float intensity_cur = w_cur_tl*cur_img_ptr[0] + w_cur_tr*cur_img_ptr[1] + w_cur_bl*cur_img_ptr[stride] + w_cur_br*cur_img_ptr[stride+1];
+        const float res = intensity_cur - (*ref_patch_cache_ptr);
+
+        // used to compute scale for robust cost
+        if(compute_weight_scale)
+          errors.push_back(fabsf(res));
+
+        // robustification
+        float weight = 1.0;
+        if(use_weights_) {
+          weight = weight_function_->value(res/scale_);
+        }
+
+        chi2 += res*res*weight;
+        n_meas_++;
+
+        if(linearize_system)
+        {
+          // compute Jacobian, weighted Hessian and weighted "steepest descend images" (times error)
+          const Vector6d J(jacobian_cache_.col(feature_counter*patch_area_ + pixel_counter));
+          H_.noalias() += J*J.transpose()*weight;
+          Jres_.noalias() -= J*res*weight;
+          if(display_)
+            resimg_.at<float>((int) v_cur+y-patch_halfsize_, (int) u_cur+x-patch_halfsize_) = res/255.0;
+        }
+      }
+    }
+  }
+
+  // compute the weights on the first iteration
+  if(compute_weight_scale && iter_ == 0)
+    scale_ = scale_estimator_->compute(errors);
+
+  return chi2/n_meas_;
+}
+
+int SparseImgAlign::solve()
+{
+  x_ = H_.ldlt().solve(Jres_);
+  if((bool) std::isnan((double) x_[0]))
+    return 0;
+  return 1;
+}
+
+void SparseImgAlign::update(
+    const ModelType& T_curold_from_ref,
+    ModelType& T_curnew_from_ref)
+{
+  T_curnew_from_ref =  T_curold_from_ref * SE3::exp(-x_);
+}
+
+void SparseImgAlign::startIteration()
+{}
+
+void SparseImgAlign::finishIteration()
+{
+  if(display_)
+  {
+    cv::namedWindow("residuals", CV_WINDOW_AUTOSIZE);
+    cv::imshow("residuals", resimg_*10);
+    cv::waitKey(0);
+  }
+}
+
+} // namespace svo
+