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

Conflicts:
	svo/src/reprojector.cpp

README.md

@@ -91,7 +91,7 @@ Now we are ready to build SVO.
 Clone it into your catkin workspace
 
     cd catkin_ws/src
-    git clone https://github.com/uzh-rpg/rpg_slam.git
+    git clone https://github.com/uzh-rpg/rpg_svo.git
 
 If you installed g2o then set `HAVE_G2O` in `svo/CMakeLists.txt` to TRUE.
 Then build
@@ -165,9 +165,9 @@ Citing
 
 If you use SVO in an academic context, please cite the following publication:
 
-    @article{Forster2014ICRA,
+    @inproceedings{Forster2014ICRA,
       author = {Forster, Christian and Pizzoli, Matia and Scaramuzza, Davide},
-      title = {{SVO: Fast Semi-Direct Monocular Visual Odometry}},
+      title = {{SVO}: Fast Semi-Direct Monocular Visual Odometry},
-      journal = {IEEE International Conference on Robotics and Automation (ICRA)},
+      booktitle = {IEEE International Conference on Robotics and Automation (ICRA)},
       year = {2014}
     }

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 {
+namespace vk
-  class AbstractCamera;
+{
-  class PerformanceMonitor;
+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 Stage {
-  enum TrackingQuality { INSUFFICIENT, BAD, GOOD };
+    STAGE_PAUSED,
-  enum UpdateResult { NO_KEYFRAME, IS_KEYFRAME, FAILURE };
+    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,
+      const size_t update_id,
-      UpdateResult dropout,
+      const UpdateResult dropout,
-      size_t num_observations);
+      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::FrameHandlerBase() :
-FrameHandlerBase() :
+  stage_(STAGE_PAUSED),
-  stage_(PAUSED),
   set_reset_(false),
   set_start_(false),
   acc_frame_timings_(10),
   acc_num_obs_(10),
   num_obs_last_(0),
-  tracking_quality_(BAD),
+  tracking_quality_(TRACKING_BAD)
-  depth_filter_(NULL)
 {
 #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::
+bool FrameHandlerBase::startFrameProcessingCommon(const double timestamp)
-startFrameProcessingCommon()
 {
   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::
+int FrameHandlerBase::finishFrameProcessingCommon(
-finishFrameProcessingCommon(size_t update_id, UpdateResult dropout, size_t num_observations)
+    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::
+void FrameHandlerBase::resetCommon()
-resetCommon()
 {
   map_.reset();
-  stage_ = PAUSED;
+  stage_ = STAGE_PAUSED;
   set_reset_ = false;
   set_start_ = false;
-  tracking_quality_ = BAD;
+  tracking_quality_ = TRACKING_BAD;
-  depth_filter_->reset();
   num_obs_last_ = 0;
   SVO_INFO_STREAM("RESET");
 }
 
-void FrameHandlerBase::
+void FrameHandlerBase::setTrackingQuality(const size_t num_observations)
-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::
+void FrameHandlerBase::optimizeStructure(
-optimizeStructure(FramePtr frame, size_t max_n_pts, int max_iter)
+    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::FrameHandlerMono(vk::AbstractCamera* cam) :
-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::~FrameHandlerMono()
-~FrameHandlerMono()
-{}
-
-void FrameHandlerMono::
-addImage(const cv::Mat& img, double timestamp)
 {
-  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;
+  UpdateResult res = RESULT_FAILURE;
-  if(stage_ == DEFAULT_FRAME)
+  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::
+void FrameHandlerMono::setFirstFrame(const FramePtr& first_frame)
-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::
+FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame()
-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::
+FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
-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::
+FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
-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;
+  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_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)
+  if(tracking_quality_ == TRACKING_INSUFFICIENT)
-    return FAILURE;
+    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::
+void FrameHandlerMono::resetAll()
-resetAll()
 {
   resetCommon();
   last_frame_.reset();
   new_frame_.reset();
   core_kfs_.clear();
   overlap_kfs_.clear();
+  depth_filter_->reset();
 }
 
-bool FrameHandlerMono::
+bool FrameHandlerMono::needNewKf(double scene_depth_mean)
-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::
+void FrameHandlerMono::setCoreKfs(size_t n_closest)
-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()
+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));
@@ -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_);
+    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;
         }
       }
       ++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;
+  SVO_START_TIMER("feature_align");
-  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::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(
+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

svo/src/sparse_img_align.cpp

@@ -26,12 +26,12 @@
 
 namespace svo {
 
-SparseImgAlign::
+SparseImgAlign::SparseImgAlign(
-SparseImgAlign(int max_level, int min_level, int n_iter,
+    int max_level, int min_level, int n_iter,
-               Method method, bool display, bool verbose) :
+    Method method, bool display, bool verbose) :
-    display_(display),
+        display_(display),
-    max_level_(max_level),
+        max_level_(max_level),
-    min_level_(min_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::
+size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame)
-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
+void frameJac(const Vector3d & xyz, Matrix<double,2,6> & J)
-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::
+void SparseImgAlign::precomputeReferencePatches()
-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::
+double SparseImgAlign::computeResiduals(
-computeResiduals(const SE3& T_cur_from_ref, bool linearize_system, bool compute_weight_scale)
+    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::
+int SparseImgAlign::solve()
-solve()
 {
   x_ = H_.ldlt().solve(Jres_);
   if((bool) std::isnan((double) x_[0]))
@@ -268,26 +264,22 @@ solve()
   return 1;
 }
 
-void SparseImgAlign::
+void SparseImgAlign::update(
-update(const ModelType& T_curold_from_ref,  ModelType& T_curnew_from_ref)
+    const ModelType& T_curold_from_ref,
+    ModelType& T_curnew_from_ref)
 {
-  // the jacobian is computed in the coordinate frame of the reference image
+  T_curnew_from_ref =  T_curold_from_ref * SE3::exp(-x_);
-  // 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;
 }
 
-void SparseImgAlign::
+void SparseImgAlign::startIteration()
-startIteration()
 {}
 
-void SparseImgAlign::
+void SparseImgAlign::finishIteration()
-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("n converged:  \t %zu mm (ref: 169936)\n", n_converged);
-  printf("mean error:   \t %f (ref: 1.0848)\n", sum_error*100/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;
+    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";
+    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"
+                  << "#Features: " << vo_->lastNumObservations() << " \t"
-    		      << "Proc. Time: " << vo_->lastProcessingTime()*1000 << "ms \n";
+                  << "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_

svo_analysis/README.md

@@ -0,0 +1,27 @@
+## SVO Analysis
+
+#### Download Test Data
+
+Create a folder where you will store all datasets, e.g.:
+
+    mkdir Datasets
+
+In your `.bashrc` script, create a new environment variable pointing to the dataset folder:
+
+    export SVO_DATASET_DIR=${HOME}/Datasets
+
+Source your new `.bashrc` script, go to the new dataset folder and download the test data:
+
+    source `~/.bashrc`
+    cd ${SVO_DATASET_DIR}
+    wget http://rpg.ifi.uzh.ch/datasets/flying_room_1_rig_1.tar.gz -O - | tar -xz
+
+
+#### Run Benchmark
+
+In SVO you need to set the `TRACE` flag to `TRUE` and recompile SVO.
+Afterwards run
+
+    rosrun svo_analysis benchmark.py flying_room_1_fast
+
+The script will create a new folder in `svo_analysis/results/`, run svo on the dataset and save the tracefile in this folder and at the end it will run some scripts to generate plots from the tracefile.

svo_analysis/experiments/.gitignore

@@ -0,0 +1,2 @@
+*.yaml
+!.gitignore

svo_analysis/experiments/asl2_acc.yaml

@@ -1,2 +1,3 @@
 dataset: asl_vicon_d2
-param_settings: vo_accurate
+param_settings: vo_accurate
+experiment_label: 'svo acc'

svo_analysis/experiments/asl2_fast.yaml

@@ -1,2 +1,3 @@
 dataset: asl_vicon_d2
-param_settings: vo_fast
+param_settings: vo_fast
+experiment_label: 'svo fast'

svo_analysis/experiments/flying_room_1_fast.yaml

@@ -1,2 +1,3 @@
 dataset: flying_room_1_rig_1
-param_settings: vo_fast
+param_settings: vo_fast
+experiment_label: 'svo fast'

svo_analysis/scripts/benchmark.py

@@ -10,9 +10,9 @@ import yaml
 import rospkg
 import argparse
 import time
-import svo_analysis.analyse_trajectory as analyse_trajectory
 import vikit_py.cpu_info as cpu_info
 import vikit_py.ros_node as ros_node
+import evaluate
 
 if __name__=="__main__":
   
@@ -23,6 +23,8 @@ if __name__=="__main__":
   parser.add_argument('experiment_file', help='experiment file in svo_analysis/experiments folder')
   parser.add_argument('--name', help='experiment name')
   parser.add_argument('--evaluate', help='evaluate tracefile after running SVO', action='store_true')
+  parser.add_argument('--version', help='version of svo to evaluate', default='svo_ros')
+  parser.add_argument('--executable', help='the executable to be called', default='benchmark')
   args = parser.parse_args()
 
   # load experiment parameters
@@ -33,7 +35,7 @@ if __name__=="__main__":
   expParams['platform'] = cpu_info.get_cpu_info()
   expParams['experiment_name'] = args.name
   if(expParams['experiment_name'] == None):
-    expParams['experiment_name'] = expParams['time']+'_svo_' + args.experiment_file
+    expParams['experiment_name'] = expParams['time']+'_'+args.version+'_' + args.experiment_file
 
   # load dataset parameters
   expParams['dataset_directory'] = os.path.join(rospkg.RosPack().get_path('Datasets'), expParams['dataset'])
@@ -63,12 +65,11 @@ if __name__=="__main__":
     outfile.write(yaml.dump(params, default_flow_style=False))
     
   # execute ros node
-  node = ros_node.RosNode('svo_ros','benchmark')
+  node = ros_node.RosNode(args.version, args.executable)
   node.run(params)
   
   # TODO: check if it is a synthetic dataset 
-  # plot trajectory error
+  evaluate.evaluate_dataset(params['trace_dir'])
-  analyse_trajectory.analyse_trajectory(params['trace_dir'])
   
   
   

svo_analysis/scripts/comparison.py

@@ -1,32 +1,32 @@
-# -*- coding: utf-8 -*-
+#!/usr/bin/python
 
+import os
+import sys
+import time
+import rospkg
 import numpy as np
 import matplotlib.pyplot as plt
 import yaml
+import argparse
 from matplotlib import rc
+
+# tell matplotlib to use latex font
+rc('font',**{'family':'serif','serif':['Cardo']})
+rc('text', usetex=True)    
+  
 from mpl_toolkits.axes_grid1.inset_locator import zoomed_inset_axes
 from mpl_toolkits.axes_grid1.inset_locator import mark_inset
 
-
+def plot_trajectory(ax, filename, label, color, linewidth):
-def plotTrajectory(ax, filename, label, color, linewidth):
   file = open(filename)
   data = file.read()
   lines = data.replace(","," ").replace("\t"," ").split("\n")
   trajectory = np.array([[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"], dtype=np.float64)
   ax.plot(trajectory[:,1], trajectory[:,2], label=label, color=color, linewidth=linewidth)
-  
 
-if __name__ == '__main__':
+def compare_results(experiments, results_dir, comparison_dir, 
+                    plot_scale_drift = False):
   
-  results_dir = '../results'
-  experiments = ['20130911_2113_nslam_i7_asl2_acc',
-                 '20130911_2229_nslam_i7_asl2_fast',
-                 '20130906_2149_ptam_i7_asl2']
-                 
-  # tell matplotlib to use latex font
-  rc('font',**{'family':'serif','serif':['Cardo']})
-  rc('text', usetex=True)               
-
   # ------------------------------------------------------------------------------
   # position error 
   fig_poserr = plt.figure(figsize=(8,6))
@@ -37,28 +37,25 @@ if __name__ == '__main__':
   for exp in experiments:
     
     # load dataset parameters
-    params_stream = open(results_dir+'/'+exp+'/params.yaml')
+    params_stream = open(os.path.join(results_dir, exp, 'params.yaml'))
     params = yaml.load(params_stream)
   
     # plot translation error
-    trans_error_filename = results_dir+'/'+exp+'/translation_error.txt'
+    trans_error = np.loadtxt(os.path.join(results_dir, exp, 'translation_error.txt'))
-    file = open(trans_error_filename)
+    trans_error[:,0] = trans_error[:,0]-trans_error[0,0]
-    data = file.read()
-    lines = data.replace(","," ").replace("\t"," ").split("\n")
-    trans_error = np.array([[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"], dtype=np.float64)
     ax_poserr_x.plot(trans_error[:,0], trans_error[:,1], label=params['experiment_label'])
     ax_poserr_y.plot(trans_error[:,0], trans_error[:,2])
     ax_poserr_z.plot(trans_error[:,0], trans_error[:,3])
     
   ax_poserr_x.set_xlim([0, trans_error[-1,0]+4])
-  ax_poserr_y.set_xlim([0, trans_error[-1,0]+4])
+  ax_poserr_y.set_xlim([0, trans_error[-1,0]+4]) 
   ax_poserr_z.set_xlim([0, trans_error[-1,0]+4])
   ax_poserr_x.legend(bbox_to_anchor=[0, 0], loc='lower left', ncol=3)
   ax_poserr_x.grid()
   ax_poserr_y.grid()
   ax_poserr_z.grid()
   fig_poserr.tight_layout()
-  fig_poserr.savefig('../results/translation_error.pdf')
+  fig_poserr.savefig(os.path.join(comparison_dir, 'translation_error.pdf'))
   
   # ------------------------------------------------------------------------------
   # orientation error 
@@ -70,15 +67,12 @@ if __name__ == '__main__':
   for exp in experiments:
     
     # load dataset parameters
-    params_stream = open(results_dir+'/'+exp+'/params.yaml')
+    params_stream = open(os.path.join(results_dir, exp, 'params.yaml'))
     params = yaml.load(params_stream)
   
     # plot translation error
-    rot_error_filename = results_dir+'/'+exp+'/orientation_error.txt'
+    rot_error = np.loadtxt(os.path.join(results_dir, exp, 'orientation_error.txt'))
-    file = open(rot_error_filename)
+    rot_error[:,0] = rot_error[:,0]-rot_error[0,0]
-    data = file.read()
-    lines = data.replace(","," ").replace("\t"," ").split("\n")
-    rot_error = np.array([[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"], dtype=np.float64)
     ax_roterr_r.plot(rot_error[:,0], rot_error[:,3], label=params['experiment_label'])
     ax_roterr_p.plot(rot_error[:,0], rot_error[:,2])
     ax_roterr_y.plot(rot_error[:,0], rot_error[:,1])
@@ -91,76 +85,66 @@ if __name__ == '__main__':
   ax_roterr_p.grid()
   ax_roterr_y.grid()
   fig_roterr.tight_layout()
-  fig_roterr.savefig('../results/orientation_error.pdf')
+  fig_roterr.savefig(os.path.join(comparison_dir, 'orientation_error.pdf'))
   
   # ------------------------------------------------------------------------------
   # scale error 
-  fig_scale = plt.figure(figsize=(8,2.5))
+  if plot_scale_drift:
-  ax_scale = fig_scale.add_subplot(111, xlabel='time [s]', ylabel='scale change [\%]')
+    fig_scale = plt.figure(figsize=(8,2.5))
-  
+    ax_scale = fig_scale.add_subplot(111, xlabel='time [s]', ylabel='scale change [\%]')
-  for exp in experiments:
     
-    # load dataset parameters
+    for exp in experiments:
-    params_stream = open(results_dir+'/'+exp+'/params.yaml')
+      
-    params = yaml.load(params_stream)
+      # load dataset parameters
+      params = yaml.load(open(os.path.join(results_dir, exp, 'params.yaml')))
+    
+      # plot translation error
+      scale_drift = open(os.path.join(results_dir, exp, 'scale_drift.txt'))
+      scale_drift[:,0] = scale_drift[:,0]-scale_drift[0,0]
+      ax_scale.plot(scale_drift[:,0], scale_drift[:,1], label=params['experiment_label'])
   
-    # plot translation error
+    ax_scale.set_xlim([0, rot_error[-1,0]+4])
-    scale_drift_filename = results_dir+'/'+exp+'/scale_drift.txt'
+    ax_scale.legend(bbox_to_anchor=[0, 1], loc='upper left', ncol=3)
-    file = open(scale_drift_filename)
+    ax_scale.grid()
-    data = file.read()
+    fig_scale.tight_layout()
-    lines = data.replace(","," ").replace("\t"," ").split("\n")
+    fig_scale.savefig(os.path.join(comparison_dir, 'scale_drift.pdf'))
-    scale_drift = np.array([[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"], dtype=np.float64)
-    ax_scale.plot(scale_drift[:,0], scale_drift[:,1], label=params['experiment_label'])
-
-  ax_scale.set_xlim([0, rot_error[-1,0]+4])
-  ax_scale.legend(bbox_to_anchor=[0, 1], loc='upper left', ncol=3)
-  ax_scale.grid()
-  fig_scale.tight_layout()
-  fig_scale.savefig('../results/scale_drift.pdf')
   
   # ------------------------------------------------------------------------------
   # trajectory
   
-
+#   fig_traj = plt.figure(figsize=(8,4.8))
-  fig_traj = plt.figure(figsize=(8,4.8))
+#   ax_traj = fig_traj.add_subplot(111, xlabel='x [m]', ylabel='y [m]', aspect='equal', xlim=[-3.1, 4], ylim=[-1.5, 2.6])
-  ax_traj = fig_traj.add_subplot(111, xlabel='x [m]', ylabel='y [m]', aspect='equal', xlim=[-3.1, 4], ylim=[-1.5, 2.6])
+# 
-
+#   plotTrajectory(ax_traj, '/home/cforster/Datasets/asl_vicon_d2/groundtruth_filtered.txt', 'Groundtruth', 'k', 1.5)
-  plotTrajectory(ax_traj, '/home/cforster/Datasets/asl_vicon_d2/groundtruth_filtered.txt', 'Groundtruth', 'k', 1.5)
+#   plotTrajectory(ax_traj, results_dir+'/20130911_2229_nslam_i7_asl2_fast/traj_estimate_rotated.txt', 'Fast', 'g', 1)
-  plotTrajectory(ax_traj, results_dir+'/20130911_2229_nslam_i7_asl2_fast/traj_estimate_rotated.txt', 'Fast', 'g', 1)
+#   plotTrajectory(ax_traj, results_dir+'/20130906_2149_ptam_i7_asl2/traj_estimate_rotated.txt', 'PTAM', 'r', 1)
-  plotTrajectory(ax_traj, results_dir+'/20130906_2149_ptam_i7_asl2/traj_estimate_rotated.txt', 'PTAM', 'r', 1)
+# 
-
+#   mark_inset(ax_traj, axins, loc1=2, loc2=4, fc="none", ec='b')
-  # show detail:
+#   plt.draw()
-  axins = zoomed_inset_axes(ax_traj, 5.0, loc=1) # zoom = 6
+#   plt.show() 
-  plotTrajectory(axins, '/home/cforster/Datasets/asl_vicon_d2/groundtruth_filtered.txt', 'Groundtruth', 'k', 1.5)
+#   ax_traj.legend(bbox_to_anchor=[1, 0], loc='lower right', ncol=3)
-  plotTrajectory(axins, results_dir+'/20130911_2229_nslam_i7_asl2_fast/traj_estimate_rotated.txt', 'Fast', 'g', 1)
+#   ax_traj.grid()
-  plotTrajectory(axins, results_dir+'/20130906_2149_ptam_i7_asl2/traj_estimate_rotated.txt', 'PTAM', 'r', 1)
+#   fig_traj.tight_layout()
-  axins.set_xlim(1.2, 1.70)
+#   fig_traj.savefig('../results/trajectory_asl.pdf')
-  axins.set_ylim(-0.8, -0.3)
-  axins.patch.set_facecolor('0.9')
-  plt.xticks(visible=False)
-  plt.yticks(visible=False)
-
-  mark_inset(ax_traj, axins, loc1=2, loc2=4, fc="none", ec='b')
-  plt.draw()
-  plt.show()
-  ax_traj.legend(bbox_to_anchor=[1, 0], loc='lower right', ncol=3)
-  ax_traj.grid()
-  fig_traj.tight_layout()
-  fig_traj.savefig('../results/trajectory_asl.pdf')
   
   
-  # ------------------------------------------------------------------------------
+if __name__ == '__main__':
-  # number of matches
   
-  # plot number of klt tracked and matched points
+  default_name = time.strftime("%Y%m%d_%H%M", time.localtime())+'_comparison'
-#  fig = plt.figure(figsize=(8,2))
+  parser = argparse.ArgumentParser(description='Compare results.')
-#  ax = fig.add_subplot(111, xlabel='time [s]', ylabel='no. features', ylim=[0,220])
+  parser.add_argument('result_directories', nargs='+', help='list of result directories to compare')
-#  ax.plot(np.arange(len(n_fts_1))/25.0, n_fts_1, 'b-', label='Accurate')
+  parser.add_argument('--name', help='name of the comparison', default=default_name)
-#  ax.plot(np.arange(len(n_fts_2))/25.0, n_fts_2, 'g-', label='Fast')
+  args = parser.parse_args()
-#  ax.legend(bbox_to_anchor=[1, 0], loc='lower right', ncol=2)
+ 
-#  fig.tight_layout()
+  # create folder for comparison results
-#  fig.savefig('../results/number_of_features2.pdf', bbox_inches="tight")
+  results_dir = os.path.join(rospkg.RosPack().get_path('svo_analysis'), 'results')
-#  
+  comparison_dir = os.path.join(results_dir, args.name)
+  if not os.path.exists(comparison_dir):
+    os.makedirs(comparison_dir)
+    
+  # run comparison
+  compare_results(args.result_directories, results_dir, comparison_dir) 
   
-  
+ 
-  
+                 
+           

svo_analysis/scripts/evaluate.py

@@ -5,26 +5,19 @@ Created on Sat Aug 10 18:21:47 2013
 @author: Christian Forster
 """
 
-import argparse
+import os
 import csv
 import numpy as np
-from matplotlib import rc
+import rospkg
+import argparse
 import svo_analysis.analyse_logs as analyse_logs
 import svo_analysis.analyse_timing as analyse_timing
+import svo_analysis.analyse_trajectory as analyse_trajectory
 
-if __name__=="__main__":
+def evaluate_dataset(trace_dir):
   
-  # parse command line 
+  # read tracefile
-  parser = argparse.ArgumentParser()
+  data = csv.reader(open(os.path.join(trace_dir, 'trace.csv')))
-  parser.add_argument('--trace_name', help='svo tracefile with timings and logs', default='20131121_0930_px7')
-  args = parser.parse_args()
-
-  # config
-  results_dir = 'results/'+args.trace_name
-  trace_file = results_dir+'/'+args.trace_name+'.csv'
-
-  # tracefile
-  data = csv.reader(open(trace_file))
   fields = data.next()
   D = dict()
   for field in fields:
@@ -39,9 +32,19 @@ if __name__=="__main__":
   for field, value in D.items():
     D[field] = np.array(D[field])
 
-  # tell matplotlib to use latex font
+  # generate plots
-  rc('font',**{'family':'serif','serif':['Cardo']})
+  analyse_logs.analyse_logs(D, trace_dir)
-  rc('text', usetex=True)
+  analyse_timing.analyse_timing(D, trace_dir)
+  analyse_trajectory.analyse_trajectory(trace_dir)
 
-  analyse_logs.analyse_logs(D, results_dir+'/'+args.trace_name)
+if __name__=="__main__":
-  analyse_timing.analyse_timing(D, results_dir+'/'+args.trace_name)
+  parser = argparse.ArgumentParser(description='''
+  Evaluates tracefiles of SVO and generates the plots.
+  ''')
+  parser.add_argument('experiment_name', help='directory name of the tracefiles')
+  args = parser.parse_args()
+
+  trace_dir = os.path.join(rospkg.RosPack().get_path('svo_analysis'), 
+                           'results',
+                           args.experiment_name)
+  evaluate_dataset(trace_dir)

svo_analysis/src/svo_analysis/analyse_logs.py

@@ -1,16 +1,10 @@
 #!/usr/bin/python
 
-import argparse
+import os
-import csv
 import numpy as np
 import matplotlib.pyplot as plt
-from matplotlib import rc
-
-# tell matplotlib to use latex font
-rc('font',**{'family':'serif','serif':['Cardo']})
-rc('text', usetex=True)
   
-def analyse_logs(D, trace_name):
+def analyse_logs(D, trace_dir):
 
   # identify measurements which result from normal frames and which from keyframes
   is_kf = np.argwhere( (D['dropout'] == 1) & (D['repr_n_mps'] >= 0))
@@ -19,114 +13,56 @@ def analyse_logs(D, trace_name):
   n_frames = len(is_frame)
   n_kfs = len(is_kf)
   n_nokf = len(is_nokf)
-  fps = 25.0
   
+  # set initial time to zero
+  D['timestamp'] = D['timestamp'] - D['timestamp'][0]
+  
+  # ----------------------------------------------------------------------------
   # plot number of reprojected points
   mean_n_reproj_points = np.mean(D['repr_n_mps'][is_frame]);
   mean_n_reproj_matches = np.mean(D['repr_n_new_references'][is_frame]);
+  mean_n_edges_final = np.mean(D['sfba_n_edges_final'][is_frame]);
 
-  plt.figure()
+  fig = plt.figure(figsize=(8,3))
-  plt.plot(np.arange(n_frames), D['repr_n_mps'][is_frame], 'r-', label='reprojected points, avg = %(i).2f'%{'i':mean_n_reproj_points})
+  ax = fig.add_subplot(111, xlabel='time [s]')
-  plt.plot(np.arange(n_frames), D['repr_n_new_references'][is_frame], 'b-', label='reprojected matches, avg = %(i).2f'%{'i':mean_n_reproj_matches})
+  ax.plot(D['timestamp'][is_frame], D['repr_n_mps'][is_frame], 'r-',
-  plt.legend()
+          label='Reprojected Points, avg = %.2f'%mean_n_reproj_points)
-  plt.tight_layout()
+  ax.plot(D['timestamp'][is_frame], D['repr_n_new_references'][is_frame], 'b-', 
-  plt.title('Reprojected points asdf')
+          label='Feature Matches, avg = %.2f'%mean_n_reproj_matches)
-  plt.savefig(trace_name+'_reprojection.pdf', bbox_inches="tight")
+  ax.plot(D['timestamp'][is_frame], D['sfba_n_edges_final'][is_frame], 'g-', 
-
+          label='Points after Optimization, avg = %.2f'%mean_n_edges_final)
-  # plot number of triangulations
+  ax.set_ylim(bottom=0)
-#  plt.figure(3)
+  ax.legend(loc='lower right')
-#  plt.plot(np.arange(n_kfs), D['triang_n_points'][is_kf], 'g-', label='triangulated points, avg %0.2f'%np.mean(D['triang_n_points'][is_kf]))
-#  plt.plot(np.arange(n_kfs), D['triang_n_refs'][is_kf], 'b-', label='new references, avg %0.2f'%np.mean(D['triang_n_refs'][is_kf]))
-#  plt.plot(np.arange(n_kfs), D['triang_n_fails'][is_kf], 'r-', label='failures, avg %0.2f'%np.mean(D['triang_n_fails'][is_kf]))
-#  plt.plot(np.arange(n_kfs), D['triang_n_add_refs'][is_kf], 'm-', label='additional refs, avg %0.2f'%np.mean(D['triang_n_add_refs'][is_kf]))
-#  plt.legend()
-#  plt.title('Triangulation')
-#  plt.savefig(trace_name+'_img_triangulation.png', bbox_inches="tight")
-
-  # plot number of klt tracked and matched points
-  fig = plt.figure(figsize=(8,2))
-  ax = fig.add_subplot(111, xlabel='time [s]', ylabel='no. features')
-  ax.plot(np.arange(n_frames)/fps, D['repr_n_new_references'][is_frame], 'b-', label='number of successful matches')
   fig.tight_layout()
-  fig.savefig(trace_name+'number_of_features.pdf', bbox_inches="tight")
+  fig.savefig(os.path.join(trace_dir,'num_reprojected.pdf'), bbox_inches="tight")
 
+  # ----------------------------------------------------------------------------
   # plot median error before and after pose-optimzation and bundle adjustment
   init_error_avg = np.mean(D['sfba_error_init'][is_frame])
   opt1_avg = np.mean(D['sfba_error_final'][is_frame])
 
   fig = plt.figure(figsize=(8,2))
   ax = fig.add_subplot(111, xlabel='time [s]', ylabel='error [px]')
-  t_max = n_frames
+  ax.plot(D['timestamp'][is_frame], D['sfba_error_init'][is_frame], 'r-', label='Initial error')
-  ax.plot(np.arange(n_frames)[0:t_max]/fps, D['sfba_error_init'][is_frame][0:t_max], 'r-', label='Initial error')
+  ax.plot(D['timestamp'][is_frame], D['sfba_error_final'][is_frame], 'b-', label='Final error')
-  ax.plot(np.arange(n_frames)[0:t_max]/fps, D['sfba_error_final'][is_frame][0:t_max], 'b-', label='Final error')
   ax.legend(ncol=2)  
-  ax.set_xlim([0, t_max/fps+4])
-  #ax.set_ylim([0, 0.8])
   fig.tight_layout()
-  fig.savefig(trace_name+'reprojection_error.pdf', bbox_inches="tight")
+  fig.savefig(os.path.join(trace_dir,'reprojection_error.pdf'), bbox_inches="tight")
   print 'average reprojection error improvement: ' + str(init_error_avg - opt1_avg)
 
-  # plot number of references
+  # ----------------------------------------------------------------------------
-  plt.figure()
+  # plot number of candidate points
-  plt.fill(np.arange(n_frames+1), np.append(D['repr_n_new_references'][is_frame]+D['klt_n_matches'][is_frame],0), 'g-', label='Reprojection')
+  fig = plt.figure(figsize=(8,3))
-  plt.fill(np.arange(n_frames+1), np.append(D['klt_n_tracked'][is_frame],0), 'b-', label='KLT Matches')
+  ax = fig.add_subplot(111, xlabel='time [s]')
-  plt.plot(np.arange(n_frames), D['sfba_n_edges_final'][is_frame], 'r-', label='References after Optimization', linewidth=2.0)
+  ax.plot(D['timestamp'][is_frame], D['n_candidates'][is_frame], 'r-', label='Candidate Points')
-  plt.legend()
+  fig.tight_layout()
-  plt.tight_layout()
+  fig.savefig(os.path.join(trace_dir,'candidate_points.pdf'), bbox_inches="tight")
-  plt.title('Pose Optimization')
-  plt.savefig(trace_name+'_img_n_references.pdf', bbox_inches="tight")
   
-  # plot number of seeds
+  # ----------------------------------------------------------------------------
-  plt.figure()
+  # plot number of candidate points
-  has_seed = np.argwhere(D['seeds_n_tot'] >= 0)
+  fig = plt.figure(figsize=(8,2))
-  plt.fill(np.arange(len(has_seed)+1), np.append(D['seeds_n_tot'][has_seed],0), label='nSeeds', edgecolor='none') 
+  ax = fig.add_subplot(111, xlabel='time [s]', ylabel='px')
-  plt.fill(np.arange(len(has_seed)+1), np.append(D['seeds_n_updates'][has_seed]+D['seeds_n_failed_matches'][has_seed],0), label='nFailedMatches', edgecolor='none')
+  ax.plot(D['timestamp'][is_frame], D['sfba_thresh'][is_frame], 'r-', label='Threshold')
-  plt.fill(np.arange(len(has_seed)+1), np.append(D['seeds_n_updates'][has_seed],0), label='nUpdates', edgecolor='none') 
+  fig.tight_layout()
-  plt.legend()
+  fig.savefig(os.path.join(trace_dir,'optimization_thresh.pdf'), bbox_inches="tight")
-  plt.tight_layout()
-  plt.title('Seeds')
-  plt.savefig(trace_name+'_seeds.pdf', bbox_inches="tight")
-
-
-  plt.figure()
-  plt.plot(D['n_candidates'])
-  plt.title('Candidates')
-  plt.show()
-  
-#  plt.figure()
-#  plt.plot())
-
-if __name__=="__main__":
-
-  dataset='20131003_1937_nslam_i7_asl2_fast'
-  n_align_frames = 10
-  save = True
-  results_dir = '../results/'+dataset
-  logfile = results_dir+'/'+dataset+'_0.csv'
-  
-  # parse command line 
-  parser = argparse.ArgumentParser(description='''
-  This script computes the absolute trajectory error from the ground truth trajectory and the estimated trajectory.
-  ''')
-  parser.add_argument('--trace_file', help='nanoslam tracefile with timings and logs', default=logfile)
-  parser.add_argument('--trace_name', help='name of the tracefile, used for saving figures etc.', default=results_dir+'/')
-  args = parser.parse_args()
-
-  # tracefile
-  data = csv.reader(open(args.trace_file))
-  fields = data.next()
-  D = dict()
-  for field in fields:
-    D[field] = list()
-
-  # fill dictionary with column values
-  for row in data:
-    for (field, value) in zip(fields, row):
-      D[field].append(float(value))
-
-  # change dictionary values from list to numpy array for easier manipulation
-  for field, value in D.items():
-    D[field] = np.array(D[field])
-
-  analyse_logs(D, args.trace_name)
 

svo_analysis/src/svo_analysis/analyse_timing.py

@@ -1,97 +1,68 @@
 #!/usr/bin/python
 
-import argparse
+import os
-import csv
 import numpy as np
 import matplotlib.pyplot as plt
-from matplotlib import rc
 
-def analyse_timing(D, trace_name):
+def analyse_timing(D, trace_dir):
 
   # identify measurements which result from normal frames and which from keyframes
   is_frame = np.argwhere(D['repr_n_mps'] >= 0)
   n_frames = len(is_frame)
+  
+  # set initial time to zero
+  D['timestamp'] = D['timestamp'] - D['timestamp'][0]
 
+  # ----------------------------------------------------------------------------
   # plot total time for frame processing
-  avg_time = np.mean(D['t_tot_time'][is_frame])*1000;
+  avg_time = np.mean(D['tot_time'][is_frame])*1000;
-
   fig = plt.figure(figsize=(8, 3))
   ax = fig.add_subplot(111, ylabel='processing time [ms]', xlabel='time [s]')
-  ax.plot(np.arange(n_frames), D['t_tot_time'][is_frame]*1000, 'g-', label='total time [ms]')
+  ax.plot(D['timestamp'][is_frame], D['tot_time'][is_frame]*1000, 'g-', label='total time [ms]')
-  ax.plot(np.arange(n_frames), np.ones(n_frames)*avg_time, 'b--', label=str('%(time).1fms mean time' % {'time': avg_time}))
+  ax.plot(D['timestamp'][is_frame], np.ones(n_frames)*avg_time, 'b--', label=str('%(time).1fms mean time' % {'time': avg_time}))
   ax.legend()
   fig.tight_layout()
-  fig.savefig(trace_name+'_img_tot_processing_time.pdf', bbox_inches="tight")
+  fig.savefig(os.path.join(trace_dir,'timing.pdf'), bbox_inches="tight")
   
+  # ----------------------------------------------------------------------------
+  # plot boxplot
   fig = plt.figure(figsize=(6,2))
   ax = fig.add_subplot(111, xlabel='Processing time [ms]')
   ax.boxplot([ 
-                D['t_tot_time'][is_frame]*1000,
+                D['tot_time'][is_frame]*1000,
 #                D['t_local_ba'][is_kf]*1000,
-                D['t_pose_optimizer'][is_frame]*1000 + D['t_point_optimizer'][is_frame]*1000,
+                D['pose_optimizer'][is_frame]*1000 + D['point_optimizer'][is_frame]*1000,
-                D['t_reproject'][is_frame]*1000,
+                D['reproject'][is_frame]*1000,
-                D['t_sparse_img_align'][is_frame]*1000,
+                D['sparse_img_align'][is_frame]*1000,
-                D['t_pyramid_creation'][is_frame]*1000
+                D['pyramid_creation'][is_frame]*1000
-              ], 0,'', vert=0) # 
+              ], 0,'', vert=0) 
                
   boxplot_labels = [
-                    r'\textbf{Total Motion Estimation: %2.2fms}' % np.median(D['t_tot_time'][is_frame]*1000),
+                    r'\textbf{Total Motion Estimation: %2.2fms}' % np.median(D['tot_time'][is_frame]*1000),
-#                    'Local BA (KF only): %.2fms ' % np.median(D['t_local_ba'][is_kf]*1000),
+#                    'Local BA (KF only): %.2fms ' % np.median(D['local_ba'][is_kf]*1000),
-                    'Refinement: %2.2fms' % np.median(D['t_pose_optimizer'][is_frame]*1000 + D['t_point_optimizer'][is_frame]*1000),
+                    'Refinement: %2.2fms' % np.median(D['pose_optimizer'][is_frame]*1000 + D['point_optimizer'][is_frame]*1000),
-                    'Feature Alignment: %2.2fms' % np.median(D['t_reproject'][is_frame]*1000),
+                    'Feature Alignment: %2.2fms' % np.median(D['reproject'][is_frame]*1000),
-                    'Sparse Image Alignment: %2.2fms' % np.median(D['t_sparse_img_align'][is_frame]*1000),
+                    'Sparse Image Alignment: %2.2fms' % np.median(D['sparse_img_align'][is_frame]*1000),
-                    'Pyramid Creation: %2.2fms' % np.median(D['t_pyramid_creation'][is_frame]*1000) ]
+                    'Pyramid Creation: %2.2fms' % np.median(D['pyramid_creation'][is_frame]*1000) ]
                    
   ax.set_yticks(np.arange(len(boxplot_labels))+1)
   ax.set_yticklabels(boxplot_labels)
   fig.tight_layout()
-  fig.savefig(trace_name+'_timing_boxplot.pdf', bbox_inches="tight")
+  fig.savefig(os.path.join(trace_dir,'timing_boxplot.pdf'), bbox_inches="tight")
   
-  fig = plt.figure(figsize=(8, 5))
+  # ----------------------------------------------------------------------------
-  ax = fig.add_subplot(111, ylabel='time [ms]')
+  # plot boxplot reprojection
-  ax.plot(D['t_reproject']*1000, 'r', label='reproject')
+  fig = plt.figure(figsize=(6,2))
-  ax.plot(D['t_reproject_find_pts']*1000, 'g', label='reproject find pts') # ok!
+  ax = fig.add_subplot(111, xlabel='Processing time [ms]')
-  ax.plot(D['t_reproject_best']*1000, 'm', label='reproject best')
+  ax.boxplot([ D['reproject'][is_frame]*1000,
-  ax.plot(D['t_reproject_candidates']*1000, 'b', label='reproject candidates')
+               D['feature_align'][is_frame]*1000,
-  ax.legend()
+               D['reproject_candidates'][is_frame]*1000,
+               D['reproject_kfs'][is_frame]*1000 ], 0, '', vert=0) 
+  boxplot_labels = [r'\textbf{Total Reprojection: %2.2fms}' % np.median(D['reproject'][is_frame]*1000),
+                     'Feature Alignment: %2.2fms' % np.median(D['feature_align'][is_frame]*1000),
+                     'Reproject Candidates: %2.2fms' % np.median(D['reproject_candidates'][is_frame]*1000),
+                     'Reproject Keyframes: %2.2fms' % np.median(D['reproject_kfs'][is_frame]*1000) ]             
+  ax.set_yticks(np.arange(len(boxplot_labels))+1)
+  ax.set_yticklabels(boxplot_labels)
   fig.tight_layout()
-  fig.savefig(trace_name+'_reprojection.pdf', bbox_inches="tight")
+  fig.savefig(os.path.join(trace_dir,'timing_reprojection.pdf'), bbox_inches="tight")
-
-
-if __name__=="__main__":
-
-  # config
-  dataset='20131003_1937_nslam_i7_asl2_fast'
-  results_dir = '../results/'+dataset
-  tracefile = results_dir + '/'+dataset+'_0.csv'
-  
-  # parse command line
-  parser = argparse.ArgumentParser(description='''
-  This script computes the absolute trajectory error from the ground truth trajectory and the estimated trajectory.
-  ''')
-  parser.add_argument('--trace_file', help='nanoslam tracefile with timings and logs', default=tracefile)
-  parser.add_argument('--trace_name', help='name of the tracefile, used for saving figures etc.', default=results_dir)
-  args = parser.parse_args()
-  results_dir = args.trace_name
-  
-  # tracefile
-  data = csv.reader(open(args.trace_file))
-  fields = data.next()
-  D = dict()
-  for field in fields:
-    D[field] = list()
-
-  # fill dictionary with column values
-  for row in data:
-    for (field, value) in zip(fields, row):
-      D[field].append(float(value))
-
-  # change dictionary values from list to numpy array for easier manipulation
-  for field, value in D.items():
-    D[field] = np.array(D[field])
-
-  # tell matplotlib to use latex font
-  rc('font',**{'family':'serif','serif':['Cardo']})
-  rc('text', usetex=True)
-
-  

svo_ros/src/benchmark_node.cpp

@@ -141,7 +141,7 @@ void BenchmarkNode::runBenchmark(const std::string& dataset_dir)
     visualizer_.publishMinimal(img, vo_->lastFrame(), *vo_, it->timestamp_);
 
     // write pose to tracefile
-    if(vo_->stage() == svo::FrameHandlerMono::DEFAULT_FRAME)
+    if(vo_->stage() == svo::FrameHandlerMono::STAGE_DEFAULT_FRAME)
       tracePose(vo_->lastFrame()->T_f_w_.inverse(), it->timestamp_);
   }
 }
@@ -216,7 +216,7 @@ void BenchmarkNode::runBlenderBenchmark(const std::string& dataset_dir)
     // Add image to VO
     vo_->addImage(img, it->timestamp_);
 
-    if(vo_->stage() != svo::FrameHandlerMono::DEFAULT_FRAME)
+    if(vo_->stage() != svo::FrameHandlerMono::STAGE_DEFAULT_FRAME)
       continue;
 
     // Compute pose error and trace to file

svo_ros/src/visualizer.cpp

@@ -110,7 +110,7 @@ publishMinimal(const cv::Mat& img, const FramePtr& frame, const FrameHandlerMono
     cv::cvtColor(frame->img_pyr_[img_pub_level_], img_rgb, CV_GRAY2RGB);
 
     // during initialization, draw lines
-    if(slam.stage() == FrameHandlerBase::SECOND_FRAME)
+    if(slam.stage() == FrameHandlerBase::STAGE_SECOND_FRAME)
     {
       const vector<cv::Point2f>& px_ref(slam.initFeatureTrackRefPx());
       const vector<cv::Point2f>& px_cur(slam.initFeatureTrackCurPx());
@@ -148,7 +148,7 @@ publishMinimal(const cv::Mat& img, const FramePtr& frame, const FrameHandlerMono
     img_msg.encoding = sensor_msgs::image_encodings::BGR8;
     pub_images_.publish(img_msg.toImageMsg());
   }
-  else if(frame == NULL && pub_images_.getNumSubscribers() > 0 && slam.stage() == FrameHandlerBase::PAUSED)
+  else if(frame == NULL && pub_images_.getNumSubscribers() > 0 && slam.stage() == FrameHandlerBase::STAGE_PAUSED)
   {
     // display image when slam is not running
     cv_bridge::CvImage img_msg;

svo_ros/src/vo_node.cpp

@@ -110,13 +110,13 @@ imgCb(const sensor_msgs::ImageConstPtr& msg)
   vo_->addImage(img, msg->header.stamp.toSec());
   visualizer_.publishMinimal(img, vo_->lastFrame(), *vo_, msg->header.stamp.toSec());
 
-  if(publish_markers_ && vo_->stage() != FrameHandlerBase::PAUSED)
+  if(publish_markers_ && vo_->stage() != FrameHandlerBase::STAGE_PAUSED)
     visualizer_.visualizeMarkers(vo_->lastFrame(), vo_->coreKeyframes(), vo_->map());
 
   if(publish_dense_input_)
     visualizer_.exportToDense(vo_->lastFrame());
 
-  if(vo_->stage() == FrameHandlerMono::PAUSED)
+  if(vo_->stage() == FrameHandlerMono::STAGE_PAUSED)
     usleep(100000); // avoid busy loop when paused
 }