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

Conflicts:
	svo/src/reprojector.cpp

svo/CMakeLists.txt

@@ -49,7 +49,7 @@ ELSE()
 ENDIF()
 
 # Add catkin and required ROS packages
-FIND_PACKAGE(catkin REQUIRED COMPONENTS roscpp vikit_common vikit_ros)
+FIND_PACKAGE(catkin REQUIRED COMPONENTS roscpp roslib vikit_common vikit_ros)
 
 # Add plain cmake packages 
 FIND_PACKAGE(G2O REQUIRED)
@@ -62,7 +62,7 @@ FIND_PACKAGE(Boost REQUIRED COMPONENTS system thread signals)
 # Describe catkin Project
 catkin_package(
     DEPENDS Eigen OpenCV Sophus Boost fast
-    CATKIN_DEPENDS roscpp vikit_common vikit_ros
+    CATKIN_DEPENDS roscpp roslib vikit_common vikit_ros
     INCLUDE_DIRS include
     LIBRARIES svo
 )
@@ -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
@@ -127,13 +127,6 @@ TARGET_LINK_LIBRARIES(svo ${LINK_LIBS})
 ################################################################################
 # TESTS
 
-IF(DEFINED ENV{SVO_DATASET_DIR})
-  ADD_DEFINITIONS(-DTEST_DATA_DIR=\"$ENV{SVO_DATASET_DIR}\")
-ELSE()
-  ADD_DEFINITIONS(-DTEST_DATA_DIR=\"${PROJECT_SOURCE_DIR}/test/data\")
-ENDIF()
-ADD_DEFINITIONS(-DTEST_TRACE_DIR=\"${PROJECT_SOURCE_DIR}/test/results\")
-
 ADD_EXECUTABLE(test_feature_align test/test_feature_alignment.cpp)
 TARGET_LINK_LIBRARIES(test_feature_align svo)
 

svo/include/svo/frame_handler_base.h

@@ -26,13 +26,14 @@
 #include <svo/global.h>
 #include <svo/map.h>
 
-namespace vk {
-  class AbstractCamera;
-  class PerformanceMonitor;
+namespace vk
+{
+class AbstractCamera;
+class PerformanceMonitor;
 }
 
-namespace svo {
-
+namespace svo
+{
 class Point;
 class Matcher;
 class DepthFilter;
@@ -41,9 +42,22 @@ class DepthFilter;
 class FrameHandlerBase : boost::noncopyable
 {
 public:
-  enum Stage { PAUSED, FIRST_FRAME, SECOND_FRAME, DEFAULT_FRAME };
-  enum TrackingQuality { INSUFFICIENT, BAD, GOOD };
-  enum UpdateResult { NO_KEYFRAME, IS_KEYFRAME, FAILURE };
+  enum Stage {
+    STAGE_PAUSED,
+    STAGE_FIRST_FRAME,
+    STAGE_SECOND_FRAME,
+    STAGE_DEFAULT_FRAME
+  };
+  enum TrackingQuality {
+    TRACKING_INSUFFICIENT,
+    TRACKING_BAD,
+    TRACKING_GOOD
+  };
+  enum UpdateResult {
+    RESULT_NO_KEYFRAME,
+    RESULT_IS_KEYFRAME,
+    RESULT_FAILURE
+  };
 
   FrameHandlerBase();
 
@@ -73,9 +87,6 @@ public:
   /// Get the number of feature observations of the last frame.
   size_t lastNumObservations() const { return num_obs_last_; }
 
-  /// Access the depth filter.
-  DepthFilter* depthFilter() const { return depth_filter_; }
-
 protected:
   Stage stage_;                 //!< Current stage of the algorithm.
   bool set_reset_;              //!< Flag that the user can set. Will reset the system before the next iteration.
@@ -87,16 +98,15 @@ protected:
   vk::RingBuffer<size_t> acc_num_obs_;          //!< Number of observed features of the last 10 frames, used to give some user feedback on the tracking performance.
   size_t num_obs_last_;                         //!< Number of observations in the previous frame.
   TrackingQuality tracking_quality_;            //!< An estimate of the tracking quality based on the number of tracked features.
-  DepthFilter* depth_filter_;                   //!< Depth estimation algorithm runs in a parallel thread and is used to initialize new 3D points.
 
   /// Before a frame is processed, this function is called.
-  bool startFrameProcessingCommon();
+  bool startFrameProcessingCommon(const double timestamp);
 
   /// When a frame is finished processing, this function is called.
   int finishFrameProcessingCommon(
-      size_t update_id,
-      UpdateResult dropout,
-      size_t num_observations);
+      const size_t update_id,
+      const UpdateResult dropout,
+      const size_t num_observations);
 
   /// Reset the map and frame handler to start from scratch.
   void resetCommon();

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 {
@@ -47,14 +47,18 @@ public:
   const vector<cv::Point2f>& initFeatureTrackRefPx() const { return klt_homography_init_.px_ref_; }
   const vector<cv::Point2f>& initFeatureTrackCurPx() const { return klt_homography_init_.px_cur_; }
 
+  /// Access the depth filter.
+  DepthFilter* depthFilter() const { return depth_filter_; }
+
 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.
   vector< pair<FramePtr,size_t> > overlap_kfs_; //!< All keyframes with overlapping field of view. the paired number specifies how many common mappoints are observed TODO: why vector!?
   initialization::KltHomographyInit klt_homography_init_; //!< Used to estimate pose of the first two keyframes by estimating a homography.
+  DepthFilter* depth_filter_;                   //!< Depth estimation algorithm runs in a parallel thread and is used to initialize new 3D points.
 
   /// Processes the first frame and sets it as a keyframe.
   UpdateResult processFirstFrame();

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

@@ -60,7 +60,7 @@ protected:
   int min_level_;                 //!< finest pyramid level for the alignment.
 
   // cache:
-  Matrix<double, 6, Dynamic, ColMajor> jacobian_cache_; //!< the first row is a flag whether the jacobian has been computed.
+  Matrix<double, 6, Dynamic, ColMajor> jacobian_cache_;
   bool have_ref_patch_cache_;
   cv::Mat ref_patch_cache_;
   std::vector<bool> visible_fts_;

svo/package.xml

@@ -14,11 +14,13 @@
 
   <!-- Dependencies needed to compile this package. -->
   <build_depend>roscpp</build_depend>
+  <build_depend>roslib</build_depend>
   <build_depend>vikit_common</build_depend>
   <build_depend>vikit_ros</build_depend>
 
   <!-- Dependencies needed after this package is compiled. -->
   <run_depend>roscpp</run_depend>
+  <run_depend>roslib</run_depend>
   <run_depend>vikit_common</run_depend>
   <run_depend>vikit_ros</run_depend>
 </package>

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);
   }
 }

svo/test/test_depth_filter.cpp

@@ -32,6 +32,7 @@
 #include <svo/feature.h>
 #include <svo/point.h>
 #include <svo/config.h>
+#include "test_utils.h"
 
 namespace {
 
@@ -151,14 +152,15 @@ void DepthFilterTest::testReconstruction(
   }
 
   // trace error
-  std::string trace_name(std::string(TEST_TRACE_DIR) + "/depth_filter_" + experiment_name + ".txt");
+  std::string trace_dir(svo::test_utils::getTraceDir());
+  std::string trace_name(trace_dir + "/depth_filter_" + experiment_name + ".txt");
   std::ofstream ofs(trace_name.c_str());
   for(std::list<ConvergedSeed>::iterator i=results_.begin(); i!=results_.end(); ++i)
     ofs << i->x_ << ", " << i->y_ << ", " << fabs(i->error_) << std::endl;
   ofs.close();
   
   // trace convergence rate
-  trace_name = std::string(TEST_TRACE_DIR) + "/depth_filter_" + experiment_name + "_convergence.txt";
+  trace_name = trace_dir + "/depth_filter_" + experiment_name + "_convergence.txt";
   ofs.open(trace_name.c_str());
   for(std::list<size_t>::iterator it=n_converged_per_iteration.begin();
       it!=n_converged_per_iteration.end(); ++it)
@@ -166,7 +168,7 @@ void DepthFilterTest::testReconstruction(
   ofs.close();
 
   // write ply file for pointcloud visualization in Meshlab
-  trace_name = std::string(TEST_TRACE_DIR) + "/depth_filter_" + experiment_name + ".ply";
+  trace_name = trace_dir + "/depth_filter_" + experiment_name + ".ply";
   ofs.open(trace_name.c_str());
   ofs << "ply" << std::endl
       << "format ascii 1.0" << std::endl
@@ -193,7 +195,7 @@ void DepthFilterTest::testReconstruction(
 int main(int argc, char** argv)
 {
   DepthFilterTest test;
-  std::string dataset_dir(std::string(TEST_DATA_DIR) + "/flying_room_1_rig_1");
+  std::string dataset_dir(svo::test_utils::getDatasetDir() + "/flying_room_1_rig_1");
   std::string experiment_name("synth_fast_2d");
   test.testReconstruction(dataset_dir, experiment_name);
   return 0;

svo/test/test_feature_alignment.cpp

@@ -19,6 +19,7 @@
 #include <svo/feature_alignment.h>
 #include <vikit/timer.h>
 #include <vikit/aligned_mem.h>
+#include "test_utils.h"
 #if __ARM_NEON__
 #include <arm_neon.h>
 #endif
@@ -52,7 +53,8 @@ void generateRefPatchNoWarpInterpolate(const cv::Mat& img, const Vector2d& px,
 
 int main(int argc, char **argv)
 {
-  std::string img_name(std::string(TEST_DATA_DIR) + "/flying_room_1_rig_1/img/frame_000001_0.png");
+
+  std::string img_name(svo::test_utils::getDatasetDir() + "/flying_room_1_rig_1/img/frame_000001_0.png");
   printf("Loading image '%s'\n", img_name.c_str());
   cv::Mat img(cv::imread(img_name, 0));
   assert(img.type() == CV_8UC1);

svo/test/test_feature_detection.cpp

@@ -23,6 +23,7 @@
 #include <vikit/vision.h>
 #include <vikit/abstract_camera.h>
 #include <vikit/atan_camera.h>
+#include "test_utils.h"
 
 namespace {
 
@@ -31,7 +32,7 @@ using namespace std;
 
 void testCornerDetector()
 {
-  std::string img_name(std::string(TEST_DATA_DIR) + "/flying_room_1_rig_1/img/frame_000001_0.png");
+  std::string img_name(svo::test_utils::getDatasetDir() + "/flying_room_1_rig_1/img/frame_000001_0.png");
   printf("Loading image '%s'\n", img_name.c_str());
   cv::Mat img(cv::imread(img_name, 0));
   assert(img.type() == CV_8UC1 && !img.empty());

svo/test/test_matcher.cpp

@@ -22,6 +22,7 @@
 #include <svo/frame.h>
 #include <svo/config.h>
 #include <svo/feature.h>
+#include "test_utils.h"
 
 namespace {
 
@@ -33,7 +34,7 @@ class MatcherTest {
     cam_ = new vk::PinholeCamera(752, 480, 217.083701215, 217.083701215, 376, 240);
 
     // load images
-    std::string dataset_dir(std::string(TEST_DATA_DIR) + "/flying_room_1_rig_1");
+    std::string dataset_dir(svo::test_utils::getDatasetDir() + "/flying_room_1_rig_1");
     std::string img_name(dataset_dir+"/img/frame_000071_0.png");
     printf("Loading image '%s'\n", img_name.c_str());
     cv::Mat img_ref(cv::imread(img_name, 0));
@@ -112,31 +113,32 @@ void MatcherTest::testEpipolarSearchFullImg()
   }
 
   // compute mean, median and variance of error in converged area
-  printf("n converged:  \t %zu (ref: 194472)\n", n_converged);
-  printf("mean error:   \t %f (ref: 1.0848)\n", sum_error*100/n_converged);
+  printf("n converged:  \t %zu mm (ref: 169936)\n", n_converged);
+  printf("mean error:   \t %f mm (ref: 1.912410)\n", sum_error*100/n_converged);
   std::vector<double>::iterator it = errors.begin()+0.5*errors.size();
   std::nth_element(errors.begin(), it, errors.end());
-  printf("50-percentile: \t %f (ref: 0.1343)\n", *it*100);
+  printf("50-percentile: \t %f mm (ref: 1.462241)\n", *it*100);
   it = errors.begin()+0.8*errors.size();
   std::nth_element(errors.begin(), it, errors.end());
-  printf("80-percentile: \t %f (ref: 0.3151)\n", *it*100);
+  printf("80-percentile: \t %f mm (ref: 2.620957)\n", *it*100);
   it = errors.begin()+0.95*errors.size();
   std::nth_element(errors.begin(), it, errors.end());
-  printf("95-percentile: \t %f (ref: 1.3753)\n", *it*100);
+  printf("95-percentile: \t %f mm (ref: 4.154051)\n", *it*100);
 
   // save results to file
   std::ofstream output_stream;
-  std::string output_filename(std::string(TEST_TRACE_DIR) + "/depthmap.bin");
+  std::string trace_dir(svo::test_utils::getTraceDir());
+  std::string output_filename(trace_dir + "/depthmap.bin");
   output_stream.open(output_filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc);
   output_stream.write((const char *)depthmap.data, depthmap.rows*depthmap.cols*sizeof(float));
   output_stream.close();
 
-  output_filename.assign(std::string(TEST_TRACE_DIR) + "/depthmap_error.bin");
+  output_filename.assign(trace_dir + "/depthmap_error.bin");
   output_stream.open(output_filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc);
   output_stream.write((const char *)error_map.data, error_map.rows*error_map.cols*sizeof(float));
   output_stream.close();
 
-  output_filename.assign(std::string(TEST_TRACE_DIR) + "/depthmap_mask.bin");
+  output_filename.assign(trace_dir + "/depthmap_mask.bin");
   output_stream.open(output_filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc);
   output_stream.write((const char *)mask.data, mask.rows*mask.cols*sizeof(uint8_t));
   output_stream.close();

svo/test/test_pipeline.cpp

@@ -28,6 +28,7 @@
 #include <opencv2/opencv.hpp>
 #include <sophus/se3.h>
 #include <iostream>
+#include "test_utils.h"
 
 namespace svo {
 
@@ -44,8 +45,6 @@ public:
 
 BenchmarkNode::BenchmarkNode()
 {
-  Config::traceName() = "benchmark";
-  Config::traceDir() =  std::string(TEST_TRACE_DIR)+"/..";
   cam_ = new vk::PinholeCamera(752, 480, 217.083701215, 217.083701215, 376, 240);
   vo_ = new svo::FrameHandlerMono(cam_);
   vo_->start();
@@ -62,8 +61,9 @@ void BenchmarkNode::runFromFolder()
   for(int img_id = 1; img_id < 310; ++img_id)
   {
     // load image
-	std::stringstream ss;
-    ss << std::string(TEST_DATA_DIR) << "/flying_room_1_rig_1/img/frame_" << std::setw( 6 ) << std::setfill( '0' ) << img_id << "_0.png";
+    std::stringstream ss;
+    ss << svo::test_utils::getDatasetDir() << "/flying_room_1_rig_1/img/frame_"
+       << std::setw( 6 ) << std::setfill( '0' ) << img_id << "_0.png";
     if(img_id == 1)
       std::cout << "reading image " << ss.str() << std::endl;
     cv::Mat img(cv::imread(ss.str().c_str(), 0));
@@ -75,8 +75,8 @@ void BenchmarkNode::runFromFolder()
     // display tracking quality
     if(vo_->lastFrame() != NULL)
     	std::cout << "Frame-Id: " << vo_->lastFrame()->id_ << " \t"
-    		      << "#Features: " << vo_->lastNumObservations() << " \t"
-    		      << "Proc. Time: " << vo_->lastProcessingTime()*1000 << "ms \n";
+                  << "#Features: " << vo_->lastNumObservations() << " \t"
+                  << "Proc. Time: " << vo_->lastProcessingTime()*1000 << "ms \n";
   }
 }
 

svo/test/test_pose_optimizer.cpp

@@ -25,6 +25,7 @@
 #include <svo/config.h>
 #include <svo/feature_detection.h>
 #include <svo/pose_optimizer.h>
+#include "test_utils.h"
 
 namespace {
 
@@ -39,7 +40,7 @@ class PoseOptimizerTest {
     cam_ = new vk::PinholeCamera(752, 480, 217.083701215, 217.083701215, 376, 240);
 
     // load image
-    std::string dataset_dir(std::string(TEST_DATA_DIR) + "/flying_room_1_rig_1");
+    std::string dataset_dir(svo::test_utils::getDatasetDir() + "/flying_room_1_rig_1");
     std::string img_name(dataset_dir+"/img/frame_000001_0.png");
     printf("Loading image '%s'\n", img_name.c_str());
     cv::Mat img(cv::imread(img_name, 0));

svo/test/test_sparse_img_align.cpp

@@ -26,6 +26,7 @@
 #include <svo/point.h>
 #include <svo/feature.h>
 #include <svo/config.h>
+#include "test_utils.h"
 
 namespace {
 
@@ -68,7 +69,8 @@ void SparseImgAlignTest::testSequence(
   std::list<double> translation_error;
 
   Sophus::SE3 T_prev_w, T_prevgt_w;
-  std::string trace_name(std::string(TEST_TRACE_DIR) + "/sparse_img_align_" + experiment_name + "_trans_estimate.txt");
+  std::string trace_dir(svo::test_utils::getTraceDir());
+  std::string trace_name(trace_dir + "/sparse_img_align_" + experiment_name + "_trans_estimate.txt");
   std::ofstream ofs(trace_name.c_str());
   for(int i=0; iter != sequence.end() && i<30; ++iter, ++i)
   {
@@ -142,7 +144,7 @@ void SparseImgAlignTest::testSequence(
   ofs.close();
 
   // trace error
-  trace_name = std::string(TEST_TRACE_DIR) + "/sparse_img_align_" + experiment_name + "_trans_error.txt";
+  trace_name = trace_dir + "/sparse_img_align_" + experiment_name + "_trans_error.txt";
   ofs.open(trace_name.c_str());
   for(std::list<double>::iterator it=translation_error.begin(); it!=translation_error.end(); ++it)
     ofs << *it << std::endl;
@@ -155,7 +157,7 @@ void SparseImgAlignTest::testSequence(
 int main(int argc, char** argv)
 {
   std::string experiment_name("flying_room_1_rig_1_fast_minlev0");
-  std::string dataset_dir(std::string(TEST_DATA_DIR) + "/flying_room_1_rig_1");
+  std::string dataset_dir(svo::test_utils::getDatasetDir() + "/flying_room_1_rig_1");
   svo::feature_detection::FastDetector detector;
   svo::Config::triangMinCornerScore() = 20;
   svo::Config::kltMinLevel() = 0;

svo/test/test_utils.h

@@ -0,0 +1,47 @@
+// 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 TEST_UTILS_H_
+#define TEST_UTILS_H_
+
+#include <string.h>
+#include <cstdlib> // for getenv
+#include <ros/package.h>
+#include <vikit/params_helper.h>
+
+namespace svo {
+namespace test_utils {
+
+std::string getDatasetDir()
+{
+  std::string dataset_dir(ros::package::getPath("svo")+"/test/data");
+  const char* env_dir = std::getenv("SVO_DATASET_DIR");
+  if(env_dir != NULL)
+    dataset_dir = std::string(env_dir);
+  return dataset_dir;
+}
+
+std::string getTraceDir()
+{
+  std::string default_dir(ros::package::getPath("svo")+"/test/results");
+  return vk::getParam<std::string>("svo/trace_dir", default_dir);
+}
+
+} // namespace test_utils
+} // namespace svo
+
+
+#endif // TEST_UTILS_H_