Merge branch 'dev' of https://github.com/uzh-rpg/rpg_svo into dev

Conflicts:
	svo/src/reprojector.cpp

svo/src/frame_handler_base.cpp

@@ -25,26 +25,23 @@
 #include <svo/matcher.h>
 #include <svo/map.h>
 #include <svo/point.h>
-#include <svo/reprojection.h>
-#include <svo/depth_filter.h>
- 
-namespace svo {
+
+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_(PAUSED),
+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_(BAD),
-  depth_filter_(NULL)
+  tracking_quality_(TRACKING_BAD)
 {
 #ifdef SVO_TRACE
   // Initialize Performance Monitor
@@ -52,10 +49,14 @@ FrameHandlerBase() :
   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");
@@ -72,35 +73,26 @@ FrameHandlerBase() :
   g_permon->init(Config::traceName(), Config::traceDir());
 #endif
 
-  // create depth filter and set callback
-  feature_detection::DetectorPtr feature_detector(new feature_detection::FastDetector());
-  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();
-
   SVO_INFO_STREAM("NanoSlam initialized");
 }
 
-FrameHandlerBase::
-~FrameHandlerBase()
+FrameHandlerBase::~FrameHandlerBase()
 {
   SVO_INFO_STREAM("SVO destructor invoked.");
-  delete depth_filter_;
-  SVO_INFO_STREAM("SVO destructed.");
 }
 
-bool FrameHandlerBase::
-startFrameProcessingCommon()
+bool FrameHandlerBase::startFrameProcessingCommon(const double timestamp)
 {
   if(set_start_)
   {
     resetAll();
-    stage_ = FIRST_FRAME;
+    stage_ = STAGE_FIRST_FRAME;
   }
 
-  if(stage_ == PAUSED)
+  if(stage_ == STAGE_PAUSED)
     return false;
 
+  SVO_LOG(timestamp);
   SVO_DEBUG_STREAM("New Frame");
   SVO_START_TIMER("tot_time");
   timer_.start();
@@ -110,15 +102,17 @@ startFrameProcessingCommon()
   return true;
 }
 
-int FrameHandlerBase::
-finishFrameProcessingCommon(size_t update_id, UpdateResult dropout, size_t num_observations)
+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_ == DEFAULT_FRAME)
+  if(stage_ == STAGE_DEFAULT_FRAME)
     acc_num_obs_.push_back(num_observations);
   num_obs_last_ = num_observations;
   SVO_STOP_TIMER("tot_time");
@@ -133,43 +127,40 @@ finishFrameProcessingCommon(size_t update_id, UpdateResult dropout, size_t num_o
   #endif
 
   // reset if failure or requested
-  if(dropout == FAILURE || set_reset_)
+  if(dropout == RESULT_FAILURE || set_reset_)
     resetAll();
   return 0;
 }
 
-void FrameHandlerBase::
-resetCommon()
+void FrameHandlerBase::resetCommon()
 {
   map_.reset();
-  stage_ = PAUSED;
+  stage_ = STAGE_PAUSED;
   set_reset_ = false;
   set_start_ = false;
-  tracking_quality_ = BAD;
-  depth_filter_->reset();
+  tracking_quality_ = TRACKING_BAD;
   num_obs_last_ = 0;
   SVO_INFO_STREAM("RESET");
 }
 
-void FrameHandlerBase::
-setTrackingQuality(const size_t num_observations)
+void FrameHandlerBase::setTrackingQuality(const size_t num_observations)
 {
-  tracking_quality_ = GOOD;
+  tracking_quality_ = TRACKING_GOOD;
   if(num_observations < Config::qualityMinFts())
   {
     ROS_WARN_STREAM_THROTTLE(0.5, "Tracking less than "<< Config::qualityMinFts() <<" features!");
-    tracking_quality_ = BAD;
+    tracking_quality_ = TRACKING_BAD;
   }
   const int feature_drop = static_cast<int>(num_obs_last_) - num_observations;
   if(feature_drop > Config::qualityMaxFtsDrop())
   {
     ROS_WARN_STREAM("Lost "<< feature_drop <<" features!");
-    tracking_quality_ = BAD;
+    tracking_quality_ = TRACKING_BAD;
   }
   if(num_observations < 20)
   {
     ROS_ERROR_STREAM("Tracking less than 20 features!");
-    tracking_quality_ = INSUFFICIENT;
+    tracking_quality_ = TRACKING_INSUFFICIENT;
   }
 }
 
@@ -178,8 +169,10 @@ 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)
+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)

svo/src/frame_handler_mono.cpp

@@ -30,22 +30,27 @@
 
 namespace svo {
 
-FrameHandlerMono::
-FrameHandlerMono(vk::AbstractCamera* cam) :
+FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam) :
   FrameHandlerBase(),
-  cam_(cam)
+  cam_(cam),
+  reprojector_(cam_, map_),
+  depth_filter_(NULL)
 {
-  reprojection_grid_.initialize(cam_);
+  // create depth filter and set callback
+  feature_detection::DetectorPtr feature_detector(new feature_detection::FastDetector());
+  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()
-{}
-
-void FrameHandlerMono::
-addImage(const cv::Mat& img, double timestamp)
+FrameHandlerMono::~FrameHandlerMono()
 {
-  if(!startFrameProcessingCommon())
+  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
@@ -58,12 +63,12 @@ addImage(const cv::Mat& img, double timestamp)
   SVO_STOP_TIMER("pyramid_creation");
 
   // process frame
-  UpdateResult res = FAILURE;
-  if(stage_ == DEFAULT_FRAME)
+  UpdateResult res = RESULT_FAILURE;
+  if(stage_ == STAGE_DEFAULT_FRAME)
     res = processFrame();
-  else if(stage_ == SECOND_FRAME)
+  else if(stage_ == STAGE_SECOND_FRAME)
     res = processSecondFrame();
-  else if(stage_ == FIRST_FRAME)
+  else if(stage_ == STAGE_FIRST_FRAME)
     res = processFirstFrame();
 
   // set last frame
@@ -74,37 +79,34 @@ addImage(const cv::Mat& img, double timestamp)
   finishFrameProcessingCommon(last_frame_->id_, res, last_frame_->nObs());
 }
 
-void FrameHandlerMono::
-setFirstFrame(const FramePtr& first_frame)
+void FrameHandlerMono::setFirstFrame(const FramePtr& first_frame)
 {
   resetAll();
   last_frame_ = first_frame;
   last_frame_->setKeyframe();
   map_.addKeyframe(last_frame_);
-  stage_ = DEFAULT_FRAME;
+  stage_ = STAGE_DEFAULT_FRAME;
 }
 
-FrameHandlerMono::UpdateResult FrameHandlerMono::
-processFirstFrame()
+FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame()
 {
   new_frame_->T_f_w_ = T_f_w_init_;
   if(klt_homography_init_.addFirstFrame(new_frame_) == initialization::FAILURE)
-    return NO_KEYFRAME;
+    return RESULT_NO_KEYFRAME;
   new_frame_->setKeyframe();
   map_.addKeyframe(new_frame_);
-  stage_ = SECOND_FRAME;
+  stage_ = STAGE_SECOND_FRAME;
   SVO_INFO_STREAM("Init: Selected first frame.");
-  return IS_KEYFRAME;
+  return RESULT_IS_KEYFRAME;
 }
 
-FrameHandlerMono::UpdateResult FrameHandlerMono::
-processSecondFrame()
+FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
 {
   initialization::InitResult res = klt_homography_init_.addSecondFrame(new_frame_);
   if(res == initialization::FAILURE)
-    return FAILURE;
+    return RESULT_FAILURE;
   else if(res == initialization::NO_KEYFRAME)
-    return NO_KEYFRAME;
+    return RESULT_NO_KEYFRAME;
 
   // two-frame bundle adjustment
 #ifdef USE_BUNDLE_ADJUSTMENT
@@ -118,14 +120,13 @@ processSecondFrame()
   // add frame to map
   new_frame_->setKeyframe();
   map_.addKeyframe(new_frame_);
-  stage_ = DEFAULT_FRAME;
+  stage_ = STAGE_DEFAULT_FRAME;
   klt_homography_init_.reset();
   SVO_INFO_STREAM("Init: Selected second frame, triangulated initial map.");
-  return IS_KEYFRAME;
+  return RESULT_IS_KEYFRAME;
 }
 
-FrameHandlerMono::UpdateResult FrameHandlerMono::
-processFrame()
+FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
 {
   // Set initial pose TODO use prior
   new_frame_->T_f_w_ = last_frame_->T_f_w_;
@@ -141,16 +142,15 @@ processFrame()
 
   // map reprojection & feature alignment
   SVO_START_TIMER("reproject");
-  size_t repr_n_new_references, repr_n_mps;
-  reprojection::reprojectMap(
-      map_, new_frame_, reprojection_grid_, map_.point_candidates_,
-      overlap_kfs_, repr_n_new_references, repr_n_mps);
+  reprojector_.reprojectMap(new_frame_, overlap_kfs_);
   SVO_STOP_TIMER("reproject");
+  const size_t repr_n_new_references = reprojector_.n_matches_;
+  const size_t repr_n_mps = reprojector_.n_trials_;
   SVO_LOG2(repr_n_mps, repr_n_new_references);
   SVO_DEBUG_STREAM("Reprojection:\t nPoints = "<<repr_n_mps<<"\t \t nMatches = "<<repr_n_new_references);
   if(repr_n_new_references == 0) {
     SVO_ERROR_STREAM("Not enough matched features.");
-    return FAILURE;
+    return RESULT_FAILURE;
   }
 
   // pose optimization
@@ -165,7 +165,7 @@ processFrame()
   SVO_DEBUG_STREAM("PoseOptimizer:\t ErrInit = "<<sfba_error_init<<"px\t thresh = "<<sfba_thresh);
   SVO_DEBUG_STREAM("PoseOptimizer:\t ErrFin. = "<<sfba_error_final<<"px\t nObsFin. = "<<sfba_n_edges_final);
   if(sfba_n_edges_final < 10)
-    return FAILURE;
+    return RESULT_FAILURE;
 
   // structure optimization
   SVO_START_TIMER("point_optimizer");
@@ -174,14 +174,14 @@ processFrame()
 
   // select keyframe
   setTrackingQuality(sfba_n_edges_final);
-  if(tracking_quality_ == INSUFFICIENT)
-    return FAILURE;
+  if(tracking_quality_ == TRACKING_INSUFFICIENT)
+    return RESULT_FAILURE;
   double depth_mean, depth_min;
   frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min);
-  if(!needNewKf(depth_mean) || tracking_quality_ == BAD)
+  if(!needNewKf(depth_mean) || tracking_quality_ == TRACKING_BAD)
   {
     depth_filter_->addFrame(new_frame_);
-    return NO_KEYFRAME;
+    return RESULT_NO_KEYFRAME;
   }
 
   // new keyframe selected
@@ -224,21 +224,20 @@ processFrame()
   new_frame_->setKeyframe();
   map_.addKeyframe(new_frame_);
 
-  return IS_KEYFRAME;
+  return RESULT_IS_KEYFRAME;
 }
 
-void FrameHandlerMono::
-resetAll()
+void FrameHandlerMono::resetAll()
 {
   resetCommon();
   last_frame_.reset();
   new_frame_.reset();
   core_kfs_.clear();
   overlap_kfs_.clear();
+  depth_filter_->reset();
 }
 
-bool FrameHandlerMono::
-needNewKf(double scene_depth_mean)
+bool FrameHandlerMono::needNewKf(double scene_depth_mean)
 {
   for(auto it=overlap_kfs_.begin(), ite=overlap_kfs_.end(); it!=ite; ++it)
   {
@@ -251,8 +250,7 @@ needNewKf(double scene_depth_mean)
   return true;
 }
 
-void FrameHandlerMono::
-setCoreKfs(size_t n_closest)
+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(),

svo/src/reprojector.cpp

@@ -16,7 +16,7 @@
 
 #include <algorithm>
 #include <stdexcept>
-#include <svo/reprojection.h>
+#include <svo/reprojector.h>
 #include <svo/frame.h>
 #include <svo/point.h>
 #include <svo/feature.h>
@@ -29,60 +29,59 @@
 #include <vikit/timer.h>
 
 namespace svo {
-namespace reprojection {
 
-Grid::Grid()
-{}
-
-void Grid::initialize(vk::AbstractCamera* cam)
+Reprojector::Reprojector(vk::AbstractCamera* cam, Map& map) :
+    map_(map)
 {
-  cell_size = Config::gridSize();
-  grid_n_cols = ceil(static_cast<double>(cam->width())/cell_size);
-  grid_n_rows = ceil(static_cast<double>(cam->height())/cell_size);
-  cells.resize(grid_n_cols*grid_n_rows);
-  std::for_each(cells.begin(), cells.end(), [&](Cell*& c){ c = new Cell; });
-  cell_order.resize(cells.size());
-  for(size_t i=0; i<cells.size(); ++i)
-    cell_order[i] = i;
-  random_shuffle(cell_order.begin(), cell_order.end()); // maybe we should do it at every iteration!
+  initializeGrid(cam);
 }
 
-Grid::~Grid()
+Reprojector::~Reprojector()
 {
-  std::for_each(cells.begin(), cells.end(), [&](Cell* c){ delete c; });
-  cells.clear();
+  std::for_each(grid_.cells.begin(), grid_.cells.end(), [&](Cell* c){ delete c; });
 }
 
-void resetGrid(Grid& grid)
+void Reprojector::initializeGrid(vk::AbstractCamera* cam)
 {
-  std::for_each(grid.cells.begin(), grid.cells.end(), [&](Cell* c){ c->clear(); });
+  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 reprojectMap(
-    Map& map,
+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,
-    Grid& grid,
-    MapPointCandidates& candidate_points,
-    std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs,
-    size_t& n_matches,
-    size_t& n_trials)
+    std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs)
 {
-  resetGrid(grid);
+  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);
+  map_.getCloseKeyframes(frame, close_kfs);
 
-  // Sort KFs with overlap according to their closeness // TODO is this still necessary?
+  // 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, max_n = 10;
-  overlap_kfs.reserve(max_n);
+  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<max_n; ++it_frame, ++n)
+      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));
@@ -99,64 +98,63 @@ void reprojectMap(
       if((*it_ftr)->point->last_projected_kf_id_ == frame->id_)
         continue;
       (*it_ftr)->point->last_projected_kf_id_ = frame->id_;
-      if(reprojectPoint(grid, frame, (*it_ftr)->point))
+      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(candidate_points.mut_);
-    auto it=candidate_points.candidates_.begin();
-    while(it!=candidate_points.candidates_.end())
+    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(grid, frame, it->first))
+      if(!reprojectPoint(frame, it->first))
       {
         it->first->n_failed_reproj_ += 3;
         if(it->first->n_failed_reproj_ > 30)
         {
-          candidate_points.deleteCandidate(*it);
-          it = candidate_points.candidates_.erase(it);
+          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.
-  n_matches=0; n_trials=0;
-  for(size_t i=0; i<grid.cells.size(); ++i)
+  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, map, candidate_points, n_trials))
-      ++n_matches;
-    if(n_matches > (size_t) Config::maxFts())
+    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 pointQualityComparator(Candidate& lhs, Candidate& rhs)
+bool Reprojector::pointQualityComparator(Candidate& lhs, Candidate& rhs)
 {
   if(lhs.pt->type_ > rhs.pt->type_)
     return true;
   return false;
 }
 
-bool reprojectCell(
-    Cell& cell,
-    FramePtr frame,
-    Map& map,
-    MapPointCandidates& candidate_points,
-    size_t& n_trials)
+bool Reprojector::reprojectCell(Cell& cell, FramePtr frame)
 {
-  cell.sort(boost::bind(&pointQualityComparator, _1, _2));
+  cell.sort(boost::bind(&Reprojector::pointQualityComparator, _1, _2));
   Cell::iterator it=cell.begin();
   while(it!=cell.end())
   {
-    ++n_trials;
+    ++n_trials_;
 
     if(it->pt->type_ == Point::TYPE_DELETED)
     {
@@ -165,14 +163,16 @@ bool reprojectCell(
     }
 
     int feature_level;
-    bool found_match = matcher::findMatchDirect(*it->pt, *frame, it->px, feature_level);
+    bool found_match = true;
+    if(options_.find_match_direct)
+      found_match = matcher::findMatchDirect(*it->pt, *frame, it->px, feature_level);
     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);
+        map_.safeDeletePoint(it->pt);
       if(it->pt->type_ == Point::TYPE_CANDIDATE  && it->pt->n_failed_reproj_ > 30)
-        candidate_points.deleteCandidatePoint(it->pt);
+        map_.point_candidates_.deleteCandidatePoint(it->pt);
       it = cell.erase(it);
       continue;
     }
@@ -195,20 +195,17 @@ bool reprojectCell(
   return false;
 }
 
-bool reprojectPoint(
-    Grid& grid,
-    FramePtr frame,
-    Point* point)
+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));
+    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 reprojection
 } // namespace svo

svo/src/sparse_img_align.cpp

@@ -26,12 +26,12 @@
 
 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)
+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_;
@@ -40,8 +40,7 @@ SparseImgAlign(int max_level, int min_level, int n_iter,
   eps_ = 0.000001;
 }
 
-size_t SparseImgAlign::
-run(FramePtr ref_frame, FramePtr cur_frame)
+size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame)
 {
   reset();
 
@@ -55,7 +54,7 @@ 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);
+  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());
 
@@ -74,9 +73,7 @@ run(FramePtr ref_frame, FramePtr cur_frame)
 }
 
 // TODO take jacobian from Frame Class!
-void
-frameJac(const Vector3d & xyz,
-         Matrix<double,2,6> & J)
+void frameJac(const Vector3d & xyz, Matrix<double,2,6> & J)
 {
   const double x = xyz[0];
   const double y = xyz[1];
@@ -98,8 +95,7 @@ frameJac(const Vector3d & xyz,
   J(1,5) = x*z_inv;             // x/z
 }
 
-void SparseImgAlign::
-precomputeReferencePatches()
+void SparseImgAlign::precomputeReferencePatches()
 {
   const int border = patch_halfsize_+1;
   const cv::Mat& ref_img = ref_frame_->img_pyr_.at(level_);
@@ -155,22 +151,23 @@ precomputeReferencePatches()
 
         // 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_));
+           - (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)
+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& ref_img = ref_frame_->img_pyr_.at(level_);
   const cv::Mat& cur_img = cur_frame_->img_pyr_.at(level_);
 
   if(linearize_system && display_)
-    resimg_ = cv::Mat(ref_img.size(), CV_32F, cv::Scalar(0));
+    resimg_ = cv::Mat(cur_img.size(), CV_32F, cv::Scalar(0));
 
   if(have_ref_patch_cache_ == false)
     precomputeReferencePatches();
@@ -179,7 +176,7 @@ computeResiduals(const SE3& T_cur_from_ref, bool linearize_system, bool compute_
   std::vector<float> errors;
   if(compute_weight_scale)
     errors.reserve(visible_fts_.size());
-  const int stride = ref_img.cols;
+  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());
@@ -259,8 +256,7 @@ computeResiduals(const SE3& T_cur_from_ref, bool linearize_system, bool compute_
   return chi2/n_meas_;
 }
 
-int SparseImgAlign::
-solve()
+int SparseImgAlign::solve()
 {
   x_ = H_.ldlt().solve(Jres_);
   if((bool) std::isnan((double) x_[0]))
@@ -268,26 +264,22 @@ solve()
   return 1;
 }
 
-void SparseImgAlign::
-update(const ModelType& T_curold_from_ref,  ModelType& T_curnew_from_ref)
+void SparseImgAlign::update(
+    const ModelType& T_curold_from_ref,
+    ModelType& T_curnew_from_ref)
 {
-  // the jacobian is computed in the coordinate frame of the reference image
-  // T_ref_from_curnew = T_ref_from_curold * SE3::exp(x_).inverse();
-  // but our state is in the reference frame of the new image, thus using (AB)^-1 = B^-1*A^-1
-  T_curnew_from_ref = SE3::exp(x_) * T_curold_from_ref;
+  T_curnew_from_ref =  T_curold_from_ref * SE3::exp(-x_);
 }
 
-void SparseImgAlign::
-startIteration()
+void SparseImgAlign::startIteration()
 {}
 
-void SparseImgAlign::
-finishIteration()
+void SparseImgAlign::finishIteration()
 {
   if(display_)
   {
     cv::namedWindow("residuals", CV_WINDOW_AUTOSIZE);
-    cv::imshow("residuals", resimg_*4);
+    cv::imshow("residuals", resimg_*10);
     cv::waitKey(0);
   }
 }