combined reprojection functions in new reprojector class

svo/CMakeLists.txt

@@ -2,7 +2,7 @@
 # user build settings
 
 SET(DEBUG FALSE)
-SET(TRACE FALSE)
+SET(TRACE TRUE)
 SET(HAVE_G2O FALSE) 
 
 ################################################################################
@@ -109,7 +109,7 @@ LIST(APPEND SOURCEFILES
   src/pose_optimizer.cpp
   src/initialization.cpp
   src/matcher.cpp
-  src/reprojection.cpp
+  src/reprojector.cpp
   src/feature_alignment.cpp
   src/feature_detection.cpp
   src/depth_filter.cpp

svo/include/svo/frame_handler_mono.h

@@ -19,7 +19,7 @@
 
 #include <vikit/abstract_camera.h>
 #include <svo/frame_handler_base.h>
-#include <svo/reprojection.h>
+#include <svo/reprojector.h>
 #include <svo/initialization.h>
 
 namespace svo {
@@ -49,7 +49,7 @@ public:
 
 protected:
   vk::AbstractCamera* cam_;                     //!< Camera model, can be ATAN, Pinhole or Ocam (see vikit).
-  reprojection::Grid reprojection_grid_;        //!< Grid used for reprojection to make sure we have evenly distributed features.
+  Reprojector reprojector_;                     //!< Projects points from other keyframes into the current frame
   FramePtr new_frame_;                          //!< Current frame.
   FramePtr last_frame_;                         //!< Last frame, not necessarily a keyframe.
   set<FramePtr> core_kfs_;                      //!< Keyframes in the closer neighbourhood.

svo/include/svo/reprojection.h

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

svo/include/svo/reprojector.h

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

svo/src/frame_handler_base.cpp

@@ -25,7 +25,6 @@
 #include <svo/matcher.h>
 #include <svo/map.h>
 #include <svo/point.h>
-#include <svo/reprojection.h>
 #include <svo/depth_filter.h>
  
 namespace svo

svo/src/frame_handler_mono.cpp

@@ -33,10 +33,9 @@ namespace svo {
 FrameHandlerMono::
 FrameHandlerMono(vk::AbstractCamera* cam) :
   FrameHandlerBase(),
-  cam_(cam)
+  cam_(cam),
-{
+  reprojector_(cam_, map_)
-  reprojection_grid_.initialize(cam_);
+{}
-}
 
 FrameHandlerMono::
 ~FrameHandlerMono()
@@ -141,11 +140,10 @@ processFrame()
 
   // map reprojection & feature alignment
   SVO_START_TIMER("reproject");
-  size_t repr_n_new_references, repr_n_mps;
+  reprojector_.reprojectMap(new_frame_, overlap_kfs_);
-  reprojection::reprojectMap(
-      map_, new_frame_, reprojection_grid_, map_.point_candidates_,
-      overlap_kfs_, repr_n_new_references, repr_n_mps);
   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/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,61 +29,59 @@
 #include <vikit/timer.h>
 
 namespace svo {
-namespace reprojection {
 
-Grid::Grid()
+Reprojector::Reprojector(vk::AbstractCamera* cam, Map& map) :
-{}
+    map_(map)
-
-void Grid::initialize(vk::AbstractCamera* cam)
 {
-  cell_size = Config::gridSize();
+  initializeGrid(cam);
-  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!
 }
 
-Grid::~Grid()
+Reprojector::~Reprojector()
 {
-  std::for_each(cells.begin(), cells.end(), [&](Cell* c){ delete c; });
+  std::for_each(grid_.cells.begin(), grid_.cells.end(), [&](Cell* c){ delete c; });
-  cells.clear();
 }
 
-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(
+void Reprojector::resetGrid()
-    Map& map,
+{
+  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,
+    std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs)
-    MapPointCandidates& candidate_points,
-    std::vector< std::pair<FramePtr,std::size_t> >& overlap_kfs,
-    size_t& n_matches,
-    size_t& n_trials)
 {
-  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;
+  size_t n = 0;
-  overlap_kfs.reserve(max_n);
+  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));
@@ -100,7 +98,7 @@ 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++;
     }
   }
@@ -109,17 +107,17 @@ void reprojectMap(
   // Now project all point candidates
   SVO_START_TIMER("reproject_candidates");
   {
-    boost::unique_lock<boost::mutex> lock(candidate_points.mut_);
+    boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_);
-    auto it=candidate_points.candidates_.begin();
+    auto it=map_.point_candidates_.candidates_.begin();
-    while(it!=candidate_points.candidates_.end())
+    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);
+          map_.point_candidates_.deleteCandidate(*it);
-          it = candidate_points.candidates_.erase(it);
+          it = map_.point_candidates_.candidates_.erase(it);
           continue;
         }
       }
@@ -131,38 +129,32 @@ void reprojectMap(
   // 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");
-  n_matches=0; n_trials=0;
+  for(size_t i=0; i<grid_.cells.size(); ++i)
-  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))
+    if(reprojectCell(*grid_.cells.at(grid_.cell_order[i]), frame))
-      ++n_matches;
+      ++n_matches_;
-    if(n_matches > (size_t) Config::maxFts())
+    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(
+bool Reprojector::reprojectCell(Cell& cell, FramePtr frame)
-    Cell& cell,
-    FramePtr frame,
-    Map& map,
-    MapPointCandidates& candidate_points,
-    size_t& n_trials)
 {
-  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::DELETED)
     {
@@ -176,9 +168,9 @@ bool reprojectCell(
     {
       it->pt->n_failed_reproj_++;
       if(it->pt->type_ == Point::UNKNOWN_QUALITY && it->pt->n_failed_reproj_ > 15)
-        map.safeDeletePoint(it->pt);
+        map_.safeDeletePoint(it->pt);
       if(it->pt->type_ == Point::CANDIDATE  && it->pt->n_failed_reproj_ > 30)
-        candidate_points.deleteCandidatePoint(it->pt);
+        map_.point_candidates_.deleteCandidatePoint(it->pt);
       it = cell.erase(it);
       continue;
     }
@@ -201,20 +193,17 @@ bool reprojectCell(
   return false;
 }
 
-bool reprojectPoint(
+bool Reprojector::reprojectPoint(FramePtr frame, Point* point)
-    Grid& grid,
-    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);
+    const int k = static_cast<int>(px[1]/grid_.cell_size)*grid_.grid_n_cols
-    grid.cells.at(k)->push_back(Candidate(point, px));
+                + 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