diff --git a/README.md b/README.md index 36cf767..f8e5dae 100644 --- a/README.md +++ b/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}}, - journal = {IEEE International Conference on Robotics and Automation (ICRA)}, + title = {{SVO}: Fast Semi-Direct Monocular Visual Odometry}, + booktitle = {IEEE International Conference on Robotics and Automation (ICRA)}, year = {2014} } diff --git a/svo/CMakeLists.txt b/svo/CMakeLists.txt index 7612a94..ae995f4 100644 --- a/svo/CMakeLists.txt +++ b/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) diff --git a/svo/include/svo/frame_handler_base.h b/svo/include/svo/frame_handler_base.h index 51286c7..66d4595 100644 --- a/svo/include/svo/frame_handler_base.h +++ b/svo/include/svo/frame_handler_base.h @@ -26,13 +26,14 @@ #include #include -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 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(); diff --git a/svo/include/svo/frame_handler_mono.h b/svo/include/svo/frame_handler_mono.h index f15afaf..9024394 100644 --- a/svo/include/svo/frame_handler_mono.h +++ b/svo/include/svo/frame_handler_mono.h @@ -19,7 +19,7 @@ #include #include -#include +#include #include namespace svo { @@ -47,14 +47,18 @@ public: const vector& initFeatureTrackRefPx() const { return klt_homography_init_.px_ref_; } const vector& 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 core_kfs_; //!< Keyframes in the closer neighbourhood. vector< pair > 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(); diff --git a/svo/include/svo/reprojection.h b/svo/include/svo/reprojection.h deleted file mode 100644 index a6ce153..0000000 --- a/svo/include/svo/reprojection.h +++ /dev/null @@ -1,98 +0,0 @@ -// This file is part of SVO - Semi-direct Visual Odometry. -// -// Copyright (C) 2014 Christian Forster -// (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 . - -#ifndef SVO_REPROJECTION_H_ -#define SVO_REPROJECTION_H_ - -#include -#include - -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 > Cell; -typedef std::vector CandidateGrid; - -/// The grid stores a set of candidate matches. For every grid cell we try to find one match. -struct Grid -{ - CandidateGrid cells; - vector 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 >& 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_ diff --git a/svo/include/svo/reprojector.h b/svo/include/svo/reprojector.h new file mode 100644 index 0000000..cc6df72 --- /dev/null +++ b/svo/include/svo/reprojector.h @@ -0,0 +1,100 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (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 . + +#ifndef SVO_REPROJECTION_H_ +#define SVO_REPROJECTION_H_ + +#include +#include + +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 >& 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 > Cell; + typedef std::vector CandidateGrid; + + /// The grid stores a set of candidate matches. For every grid cell we try to find one match. + struct Grid + { + CandidateGrid cells; + vector 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_ diff --git a/svo/include/svo/sparse_img_align.h b/svo/include/svo/sparse_img_align.h index 7df0456..17cb3a1 100644 --- a/svo/include/svo/sparse_img_align.h +++ b/svo/include/svo/sparse_img_align.h @@ -60,7 +60,7 @@ protected: int min_level_; //!< finest pyramid level for the alignment. // cache: - Matrix jacobian_cache_; //!< the first row is a flag whether the jacobian has been computed. + Matrix jacobian_cache_; bool have_ref_patch_cache_; cv::Mat ref_patch_cache_; std::vector visible_fts_; diff --git a/svo/package.xml b/svo/package.xml index c2997c2..d51d015 100644 --- a/svo/package.xml +++ b/svo/package.xml @@ -14,11 +14,13 @@ roscpp + roslib vikit_common vikit_ros roscpp + roslib vikit_common vikit_ros \ No newline at end of file diff --git a/svo/src/frame_handler_base.cpp b/svo/src/frame_handler_base.cpp index 991268c..c3568d5 100644 --- a/svo/src/frame_handler_base.cpp +++ b/svo/src/frame_handler_base.cpp @@ -25,26 +25,23 @@ #include #include #include -#include -#include - -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: "<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(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 pts; for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it) diff --git a/svo/src/frame_handler_mono.cpp b/svo/src/frame_handler_mono.cpp index 1c4c814..5804057 100644 --- a/svo/src/frame_handler_mono.cpp +++ b/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 = "<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(), diff --git a/svo/src/reprojection.cpp b/svo/src/reprojector.cpp similarity index 58% rename from svo/src/reprojection.cpp rename to svo/src/reprojector.cpp index 61de512..c0a0bc1 100644 --- a/svo/src/reprojection.cpp +++ b/svo/src/reprojector.cpp @@ -16,7 +16,7 @@ #include #include -#include +#include #include #include #include @@ -29,60 +29,59 @@ #include 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(cam->width())/cell_size); - grid_n_rows = ceil(static_cast(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; iclear(); }); + grid_.cell_size = Config::gridSize(); + grid_.grid_n_cols = ceil(static_cast(cam->width())/grid_.cell_size); + grid_.grid_n_rows = ceil(static_cast(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; iclear(); }); +} + +void Reprojector::reprojectMap( FramePtr frame, - Grid& grid, - MapPointCandidates& candidate_points, - std::vector< std::pair >& overlap_kfs, - size_t& n_matches, - size_t& n_trials) + std::vector< std::pair >& overlap_kfs) { - resetGrid(grid); + resetGrid(); // Identify those Keyframes which share a common field of view. + SVO_START_TIMER("reproject_kfs"); list< pair > 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::second, _1) < boost::bind(&std::pair::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 && nfirst; overlap_kfs.push_back(pair(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 lock(candidate_points.mut_); - auto it=candidate_points.candidates_.begin(); - while(it!=candidate_points.candidates_.end()) + boost::unique_lock 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 (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(), 8)) // 8px is the patch size in the matcher { - const int k = static_cast(px[1]/grid.cell_size)*grid.grid_n_cols + static_cast(px[0]/grid.cell_size); - grid.cells.at(k)->push_back(Candidate(point, px)); + const int k = static_cast(px[1]/grid_.cell_size)*grid_.grid_n_cols + + static_cast(px[0]/grid_.cell_size); + grid_.cells.at(k)->push_back(Candidate(point, px)); return true; } return false; } -} // namespace reprojection } // namespace svo diff --git a/svo/src/sparse_img_align.cpp b/svo/src/sparse_img_align.cpp index c4a2afd..6dc97c8 100644 --- a/svo/src/sparse_img_align.cpp +++ b/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 & J) +void frameJac(const Vector3d & xyz, Matrix & 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<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 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<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); } } diff --git a/svo/test/test_depth_filter.cpp b/svo/test/test_depth_filter.cpp index 545d9bc..a53947d 100644 --- a/svo/test/test_depth_filter.cpp +++ b/svo/test/test_depth_filter.cpp @@ -32,6 +32,7 @@ #include #include #include +#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::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::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; diff --git a/svo/test/test_feature_alignment.cpp b/svo/test/test_feature_alignment.cpp index 04bddf7..a7352e0 100644 --- a/svo/test/test_feature_alignment.cpp +++ b/svo/test/test_feature_alignment.cpp @@ -19,6 +19,7 @@ #include #include #include +#include "test_utils.h" #if __ARM_NEON__ #include #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); diff --git a/svo/test/test_feature_detection.cpp b/svo/test/test_feature_detection.cpp index 00e21a2..6fa3723 100644 --- a/svo/test/test_feature_detection.cpp +++ b/svo/test/test_feature_detection.cpp @@ -23,6 +23,7 @@ #include #include #include +#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()); diff --git a/svo/test/test_matcher.cpp b/svo/test/test_matcher.cpp index 3138d48..c81c1fe 100644 --- a/svo/test/test_matcher.cpp +++ b/svo/test/test_matcher.cpp @@ -22,6 +22,7 @@ #include #include #include +#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::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(); diff --git a/svo/test/test_pipeline.cpp b/svo/test/test_pipeline.cpp index d328bf1..364b089 100644 --- a/svo/test/test_pipeline.cpp +++ b/svo/test/test_pipeline.cpp @@ -28,6 +28,7 @@ #include #include #include +#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"; } } diff --git a/svo/test/test_pose_optimizer.cpp b/svo/test/test_pose_optimizer.cpp index 16f3dfc..8e98eac 100644 --- a/svo/test/test_pose_optimizer.cpp +++ b/svo/test/test_pose_optimizer.cpp @@ -25,6 +25,7 @@ #include #include #include +#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)); diff --git a/svo/test/test_sparse_img_align.cpp b/svo/test/test_sparse_img_align.cpp index b4f5f3b..f719f72 100644 --- a/svo/test/test_sparse_img_align.cpp +++ b/svo/test/test_sparse_img_align.cpp @@ -26,6 +26,7 @@ #include #include #include +#include "test_utils.h" namespace { @@ -68,7 +69,8 @@ void SparseImgAlignTest::testSequence( std::list 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::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; diff --git a/svo/test/test_utils.h b/svo/test/test_utils.h new file mode 100644 index 0000000..5793bb7 --- /dev/null +++ b/svo/test/test_utils.h @@ -0,0 +1,47 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (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 . + +#ifndef TEST_UTILS_H_ +#define TEST_UTILS_H_ + +#include +#include // for getenv +#include +#include + +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("svo/trace_dir", default_dir); +} + +} // namespace test_utils +} // namespace svo + + +#endif // TEST_UTILS_H_ diff --git a/svo_analysis/README.md b/svo_analysis/README.md new file mode 100644 index 0000000..a6cc2a7 --- /dev/null +++ b/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. \ No newline at end of file diff --git a/svo_analysis/experiments/.gitignore b/svo_analysis/experiments/.gitignore new file mode 100644 index 0000000..5b74354 --- /dev/null +++ b/svo_analysis/experiments/.gitignore @@ -0,0 +1,2 @@ +*.yaml +!.gitignore diff --git a/svo_analysis/experiments/asl2_acc.yaml b/svo_analysis/experiments/asl2_acc.yaml index 0b5bf3b..9e05b2e 100644 --- a/svo_analysis/experiments/asl2_acc.yaml +++ b/svo_analysis/experiments/asl2_acc.yaml @@ -1,2 +1,3 @@ dataset: asl_vicon_d2 -param_settings: vo_accurate \ No newline at end of file +param_settings: vo_accurate +experiment_label: 'svo acc' \ No newline at end of file diff --git a/svo_analysis/experiments/asl2_fast.yaml b/svo_analysis/experiments/asl2_fast.yaml index 34285b5..529b6bf 100644 --- a/svo_analysis/experiments/asl2_fast.yaml +++ b/svo_analysis/experiments/asl2_fast.yaml @@ -1,2 +1,3 @@ dataset: asl_vicon_d2 -param_settings: vo_fast \ No newline at end of file +param_settings: vo_fast +experiment_label: 'svo fast' \ No newline at end of file diff --git a/svo_analysis/experiments/flying_room_1_fast.yaml b/svo_analysis/experiments/flying_room_1_fast.yaml index 018e1fe..20a9f9d 100644 --- a/svo_analysis/experiments/flying_room_1_fast.yaml +++ b/svo_analysis/experiments/flying_room_1_fast.yaml @@ -1,2 +1,3 @@ dataset: flying_room_1_rig_1 -param_settings: vo_fast \ No newline at end of file +param_settings: vo_fast +experiment_label: 'svo fast' \ No newline at end of file diff --git a/svo_analysis/scripts/benchmark.py b/svo_analysis/scripts/benchmark.py index 4cebf88..19688ce 100755 --- a/svo_analysis/scripts/benchmark.py +++ b/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 - analyse_trajectory.analyse_trajectory(params['trace_dir']) + evaluate.evaluate_dataset(params['trace_dir']) diff --git a/svo_analysis/scripts/comparison.py b/svo_analysis/scripts/comparison.py old mode 100644 new mode 100755 index 1bc219e..5605fe3 --- a/svo_analysis/scripts/comparison.py +++ b/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 plotTrajectory(ax, filename, label, color, linewidth): +def plot_trajectory(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' - file = open(trans_error_filename) - 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) + trans_error = np.loadtxt(os.path.join(results_dir, exp, 'translation_error.txt')) + trans_error[:,0] = trans_error[:,0]-trans_error[0,0] 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' - file = open(rot_error_filename) - 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) + rot_error = np.loadtxt(os.path.join(results_dir, exp, 'orientation_error.txt')) + rot_error[:,0] = rot_error[:,0]-rot_error[0,0] 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)) - ax_scale = fig_scale.add_subplot(111, xlabel='time [s]', ylabel='scale change [\%]') - - for exp in experiments: + if plot_scale_drift: + fig_scale = plt.figure(figsize=(8,2.5)) + ax_scale = fig_scale.add_subplot(111, xlabel='time [s]', ylabel='scale change [\%]') - # load dataset parameters - params_stream = open(results_dir+'/'+exp+'/params.yaml') - params = yaml.load(params_stream) + for exp in experiments: + + # 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 - scale_drift_filename = results_dir+'/'+exp+'/scale_drift.txt' - file = open(scale_drift_filename) - data = file.read() - lines = data.replace(","," ").replace("\t"," ").split("\n") - 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') + 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(os.path.join(comparison_dir, 'scale_drift.pdf')) # ------------------------------------------------------------------------------ # trajectory - - 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]) - - 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+'/20130906_2149_ptam_i7_asl2/traj_estimate_rotated.txt', 'PTAM', 'r', 1) - - # show detail: - axins = zoomed_inset_axes(ax_traj, 5.0, loc=1) # zoom = 6 - plotTrajectory(axins, '/home/cforster/Datasets/asl_vicon_d2/groundtruth_filtered.txt', 'Groundtruth', 'k', 1.5) - plotTrajectory(axins, results_dir+'/20130911_2229_nslam_i7_asl2_fast/traj_estimate_rotated.txt', 'Fast', 'g', 1) - plotTrajectory(axins, results_dir+'/20130906_2149_ptam_i7_asl2/traj_estimate_rotated.txt', 'PTAM', 'r', 1) - axins.set_xlim(1.2, 1.70) - 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') +# 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]) +# +# 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+'/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') +# 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') - # ------------------------------------------------------------------------------ - # number of matches +if __name__ == '__main__': - # 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', ylim=[0,220]) -# ax.plot(np.arange(len(n_fts_1))/25.0, n_fts_1, 'b-', label='Accurate') -# ax.plot(np.arange(len(n_fts_2))/25.0, n_fts_2, 'g-', label='Fast') -# ax.legend(bbox_to_anchor=[1, 0], loc='lower right', ncol=2) -# fig.tight_layout() -# fig.savefig('../results/number_of_features2.pdf', bbox_inches="tight") -# + default_name = time.strftime("%Y%m%d_%H%M", time.localtime())+'_comparison' + parser = argparse.ArgumentParser(description='Compare results.') + parser.add_argument('result_directories', nargs='+', help='list of result directories to compare') + parser.add_argument('--name', help='name of the comparison', default=default_name) + args = parser.parse_args() + + # create folder for comparison results + 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) - - \ No newline at end of file + + + diff --git a/svo_analysis/scripts/evaluate.py b/svo_analysis/scripts/evaluate.py index 4cff109..eb9fe60 100755 --- a/svo_analysis/scripts/evaluate.py +++ b/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 - parser = argparse.ArgumentParser() - 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)) + # read tracefile + data = csv.reader(open(os.path.join(trace_dir, 'trace.csv'))) 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 - rc('font',**{'family':'serif','serif':['Cardo']}) - rc('text', usetex=True) + # generate plots + analyse_logs.analyse_logs(D, trace_dir) + analyse_timing.analyse_timing(D, trace_dir) + analyse_trajectory.analyse_trajectory(trace_dir) - analyse_logs.analyse_logs(D, results_dir+'/'+args.trace_name) - analyse_timing.analyse_timing(D, results_dir+'/'+args.trace_name) \ No newline at end of file +if __name__=="__main__": + 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) \ No newline at end of file diff --git a/svo_analysis/src/svo_analysis/analyse_logs.py b/svo_analysis/src/svo_analysis/analyse_logs.py index 2f368ec..4e40bfe 100755 --- a/svo_analysis/src/svo_analysis/analyse_logs.py +++ b/svo_analysis/src/svo_analysis/analyse_logs.py @@ -1,16 +1,10 @@ #!/usr/bin/python -import argparse -import csv +import os 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() - plt.plot(np.arange(n_frames), D['repr_n_mps'][is_frame], 'r-', label='reprojected points, avg = %(i).2f'%{'i':mean_n_reproj_points}) - 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}) - plt.legend() - plt.tight_layout() - plt.title('Reprojected points asdf') - plt.savefig(trace_name+'_reprojection.pdf', bbox_inches="tight") - - # plot number of triangulations -# plt.figure(3) -# 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 = plt.figure(figsize=(8,3)) + ax = fig.add_subplot(111, xlabel='time [s]') + ax.plot(D['timestamp'][is_frame], D['repr_n_mps'][is_frame], 'r-', + label='Reprojected Points, avg = %.2f'%mean_n_reproj_points) + ax.plot(D['timestamp'][is_frame], D['repr_n_new_references'][is_frame], 'b-', + label='Feature Matches, avg = %.2f'%mean_n_reproj_matches) + ax.plot(D['timestamp'][is_frame], D['sfba_n_edges_final'][is_frame], 'g-', + label='Points after Optimization, avg = %.2f'%mean_n_edges_final) + ax.set_ylim(bottom=0) + ax.legend(loc='lower right') 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(np.arange(n_frames)[0:t_max]/fps, D['sfba_error_init'][is_frame][0:t_max], 'r-', label='Initial 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.plot(D['timestamp'][is_frame], D['sfba_error_init'][is_frame], 'r-', label='Initial error') + ax.plot(D['timestamp'][is_frame], D['sfba_error_final'][is_frame], '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() - 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') - plt.fill(np.arange(n_frames+1), np.append(D['klt_n_tracked'][is_frame],0), 'b-', label='KLT Matches') - plt.plot(np.arange(n_frames), D['sfba_n_edges_final'][is_frame], 'r-', label='References after Optimization', linewidth=2.0) - plt.legend() - plt.tight_layout() - plt.title('Pose Optimization') - plt.savefig(trace_name+'_img_n_references.pdf', bbox_inches="tight") + # ---------------------------------------------------------------------------- + # plot number of candidate points + fig = plt.figure(figsize=(8,3)) + ax = fig.add_subplot(111, xlabel='time [s]') + ax.plot(D['timestamp'][is_frame], D['n_candidates'][is_frame], 'r-', label='Candidate Points') + fig.tight_layout() + fig.savefig(os.path.join(trace_dir,'candidate_points.pdf'), bbox_inches="tight") - # plot number of seeds - plt.figure() - has_seed = np.argwhere(D['seeds_n_tot'] >= 0) - plt.fill(np.arange(len(has_seed)+1), np.append(D['seeds_n_tot'][has_seed],0), label='nSeeds', edgecolor='none') - 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') - plt.fill(np.arange(len(has_seed)+1), np.append(D['seeds_n_updates'][has_seed],0), label='nUpdates', edgecolor='none') - plt.legend() - 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) + # ---------------------------------------------------------------------------- + # plot number of candidate points + fig = plt.figure(figsize=(8,2)) + ax = fig.add_subplot(111, xlabel='time [s]', ylabel='px') + ax.plot(D['timestamp'][is_frame], D['sfba_thresh'][is_frame], 'r-', label='Threshold') + fig.tight_layout() + fig.savefig(os.path.join(trace_dir,'optimization_thresh.pdf'), bbox_inches="tight") diff --git a/svo_analysis/src/svo_analysis/analyse_timing.py b/svo_analysis/src/svo_analysis/analyse_timing.py index b0d2199..0dfc1bc 100755 --- a/svo_analysis/src/svo_analysis/analyse_timing.py +++ b/svo_analysis/src/svo_analysis/analyse_timing.py @@ -1,97 +1,68 @@ #!/usr/bin/python -import argparse -import csv +import os 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(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], D['tot_time'][is_frame]*1000, 'g-', label='total time [ms]') + 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['t_reproject'][is_frame]*1000, - D['t_sparse_img_align'][is_frame]*1000, - D['t_pyramid_creation'][is_frame]*1000 - ], 0,'', vert=0) # + D['pose_optimizer'][is_frame]*1000 + D['point_optimizer'][is_frame]*1000, + D['reproject'][is_frame]*1000, + D['sparse_img_align'][is_frame]*1000, + D['pyramid_creation'][is_frame]*1000 + ], 0,'', vert=0) boxplot_labels = [ - r'\textbf{Total Motion Estimation: %2.2fms}' % np.median(D['t_tot_time'][is_frame]*1000), -# 'Local BA (KF only): %.2fms ' % np.median(D['t_local_ba'][is_kf]*1000), - 'Refinement: %2.2fms' % np.median(D['t_pose_optimizer'][is_frame]*1000 + D['t_point_optimizer'][is_frame]*1000), - 'Feature Alignment: %2.2fms' % np.median(D['t_reproject'][is_frame]*1000), - 'Sparse Image Alignment: %2.2fms' % np.median(D['t_sparse_img_align'][is_frame]*1000), - 'Pyramid Creation: %2.2fms' % np.median(D['t_pyramid_creation'][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['local_ba'][is_kf]*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['reproject'][is_frame]*1000), + 'Sparse Image Alignment: %2.2fms' % np.median(D['sparse_img_align'][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]') - ax.plot(D['t_reproject']*1000, 'r', label='reproject') - ax.plot(D['t_reproject_find_pts']*1000, 'g', label='reproject find pts') # ok! - ax.plot(D['t_reproject_best']*1000, 'm', label='reproject best') - ax.plot(D['t_reproject_candidates']*1000, 'b', label='reproject candidates') - ax.legend() + # ---------------------------------------------------------------------------- + # plot boxplot reprojection + fig = plt.figure(figsize=(6,2)) + ax = fig.add_subplot(111, xlabel='Processing time [ms]') + ax.boxplot([ D['reproject'][is_frame]*1000, + D['feature_align'][is_frame]*1000, + 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") - - -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) - - \ No newline at end of file + fig.savefig(os.path.join(trace_dir,'timing_reprojection.pdf'), bbox_inches="tight") \ No newline at end of file diff --git a/svo_ros/src/benchmark_node.cpp b/svo_ros/src/benchmark_node.cpp index 13ee94b..d8cc31e 100644 --- a/svo_ros/src/benchmark_node.cpp +++ b/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 diff --git a/svo_ros/src/visualizer.cpp b/svo_ros/src/visualizer.cpp index d8aa72f..eda01c6 100644 --- a/svo_ros/src/visualizer.cpp +++ b/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& px_ref(slam.initFeatureTrackRefPx()); const vector& 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; diff --git a/svo_ros/src/vo_node.cpp b/svo_ros/src/vo_node.cpp index 27b7ef0..9dc31dd 100644 --- a/svo_ros/src/vo_node.cpp +++ b/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 }