debug GPU version

GPU_version/svo/src/atan_camera.cpp

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

GPU_version/svo/src/bundle_adjustment.cpp

@@ -14,7 +14,7 @@
 // You should have received a copy of the GNU General Public License
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
-#include <vikit/math_utils.h>
+#include <gpu_svo/math_utils.h>
 #include <boost/thread.hpp>
 #include <g2o/core/sparse_optimizer.h>
 #include <g2o/core/block_solver.h>

GPU_version/svo/src/config.cpp

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

GPU_version/svo/src/depth_filter.cpp

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

GPU_version/svo/src/feature_alignment.cpp

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

GPU_version/svo/src/feature_detection.cpp

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

GPU_version/svo/src/frame.cpp

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

GPU_version/svo/src/frame_handler_base.cpp

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

GPU_version/svo/src/frame_handler_mono.cpp

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

GPU_version/svo/src/homography.cpp

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

GPU_version/svo/src/imu_integration.cpp

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

GPU_version/svo/src/initialization.cpp

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

GPU_version/svo/src/matcher.cpp

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

GPU_version/svo/src/math_utils.cpp

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

GPU_version/svo/src/point.cpp

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

GPU_version/svo/src/pose_optimizer.cpp

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

GPU_version/svo/src/reprojector.cpp

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

GPU_version/svo/src/robust_cost.cpp

@@ -0,0 +1,162 @@
+/**
+* This file is part of dvo.
+*
+* Copyright 2012 Christian Kerl <christian.kerl@in.tum.de> (Technical University of Munich)
+* For more information see <http://vision.in.tum.de/data/software/dvo>.
+*
+* dvo is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* dvo is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with dvo. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include <numeric>
+#include <algorithm>
+#include <gpu_svo/robust_cost.h>
+#include <gpu_svo/math_utils.h>
+
+namespace vk {
+namespace robust_cost {
+
+const float TDistributionScaleEstimator::INITIAL_SIGMA = 5.0f;
+const float TDistributionScaleEstimator::DEFAULT_DOF = 5.0f;
+
+TDistributionScaleEstimator::
+TDistributionScaleEstimator(const float dof) :
+  dof_(dof),
+  initial_sigma_(INITIAL_SIGMA)
+{}
+
+float TDistributionScaleEstimator::
+compute(std::vector<float>& errors) const
+{
+  float initial_lamda = 1.0f / (initial_sigma_ * initial_sigma_);
+  int num = 0;
+  float lambda = initial_lamda;
+  int iterations = 0;
+  do
+  {
+    ++iterations;
+    initial_lamda = lambda;
+    num = 0;
+    lambda = 0.0f;
+
+    for(std::vector<float>::iterator it=errors.begin(); it!=errors.end(); ++it)
+    {
+      if(std::isfinite(*it))
+      {
+        ++num;
+        const float error2 = (*it)*(*it);
+        lambda += error2 * ( (dof_ + 1.0f) / (dof_ + initial_lamda * error2) );
+      }
+    }
+    lambda = float(num) / lambda;
+  } while(std::abs(lambda - initial_lamda) > 1e-3);
+
+  return std::sqrt(1.0f / lambda);
+}
+
+const float MADScaleEstimator::NORMALIZER = 1.48f; // 1 / 0.6745
+
+float MADScaleEstimator::
+compute(std::vector<float>& errors) const
+{
+  // error must be in absolute values!
+  return NORMALIZER * vk::getMedian(errors);
+}
+
+float NormalDistributionScaleEstimator::
+compute(std::vector<float>& errors) const
+{
+  const float mean = std::accumulate(errors.begin(), errors.end(), 0)/errors.size();
+  float var = 0.0;
+  std::for_each(errors.begin(), errors.end(), [&](const float d) {
+      var += (d - mean) * (d - mean);
+  });
+  return std::sqrt(var); // return standard deviation
+}
+
+const float TukeyWeightFunction::DEFAULT_B = 4.6851f;
+
+TukeyWeightFunction::TukeyWeightFunction(const float b)
+{
+  configure(b);
+}
+
+float TukeyWeightFunction::value(const float& x) const
+{
+  const float x_square = x * x;
+  if(x_square <= b_square)
+  {
+    const float tmp = 1.0f - x_square / b_square;
+    return tmp * tmp;
+  }
+  else
+  {
+    return 0.0f;
+  }
+}
+
+void TukeyWeightFunction::
+configure(const float& param)
+{
+  b_square = param * param;
+}
+
+const float TDistributionWeightFunction::DEFAULT_DOF = 5.0f;
+
+TDistributionWeightFunction::
+TDistributionWeightFunction(const float dof)
+{
+  configure(dof);
+}
+
+float TDistributionWeightFunction::
+value(const float & x) const
+{
+  return ((dof_ + 1.0f) / (dof_ + (x * x)));
+}
+
+void TDistributionWeightFunction::
+configure(const float& param)
+{
+  dof_ = param;
+  normalizer_ = dof_ / (dof_ + 1.0f);
+}
+
+const float HuberWeightFunction::DEFAULT_K = 1.345f;
+
+HuberWeightFunction::
+HuberWeightFunction(const float k)
+{
+  configure(k);
+}
+
+void HuberWeightFunction::
+configure(const float& param)
+{
+  k = param;
+}
+
+float HuberWeightFunction::
+value(const float& t) const
+{
+  const float t_abs = std::abs(t);
+  if(t_abs < k)
+    return 1.0f;
+  else
+    return k / t_abs;
+}
+
+} // namespace robust_cost
+} // namespace vk
+
+

GPU_version/svo/src/sparse_img_align.cpp

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

GPU_version/svo/src/vision.cpp

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