From 8cf1504f7cca7be288b3d455a5638083a3c21ac1 Mon Sep 17 00:00:00 2001 From: Christian Forster Date: Mon, 31 Mar 2014 19:31:11 +0200 Subject: [PATCH] combined reprojection functions in new reprojector class --- svo/CMakeLists.txt | 4 +- svo/include/svo/frame_handler_mono.h | 4 +- svo/include/svo/reprojection.h | 98 --------------- svo/include/svo/reprojector.h | 98 +++++++++++++++ svo/src/frame_handler_base.cpp | 1 - svo/src/frame_handler_mono.cpp | 14 +-- svo/src/{reprojection.cpp => reprojector.cpp} | 115 ++++++++---------- 7 files changed, 160 insertions(+), 174 deletions(-) delete mode 100644 svo/include/svo/reprojection.h create mode 100644 svo/include/svo/reprojector.h rename svo/src/{reprojection.cpp => reprojector.cpp} (63%) diff --git a/svo/CMakeLists.txt b/svo/CMakeLists.txt index 3c55cef..ae995f4 100644 --- a/svo/CMakeLists.txt +++ b/svo/CMakeLists.txt @@ -2,7 +2,7 @@ # user build settings SET(DEBUG FALSE) -SET(TRACE FALSE) +SET(TRACE TRUE) SET(HAVE_G2O FALSE) ################################################################################ @@ -109,7 +109,7 @@ LIST(APPEND SOURCEFILES src/pose_optimizer.cpp src/initialization.cpp src/matcher.cpp - src/reprojection.cpp + src/reprojector.cpp src/feature_alignment.cpp src/feature_detection.cpp src/depth_filter.cpp diff --git a/svo/include/svo/frame_handler_mono.h b/svo/include/svo/frame_handler_mono.h index f15afaf..2887713 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 { @@ -49,7 +49,7 @@ public: protected: vk::AbstractCamera* cam_; //!< Camera model, can be ATAN, Pinhole or Ocam (see vikit). - reprojection::Grid reprojection_grid_; //!< Grid used for reprojection to make sure we have evenly distributed features. + Reprojector reprojector_; //!< Projects points from other keyframes into the current frame FramePtr new_frame_; //!< Current frame. FramePtr last_frame_; //!< Last frame, not necessarily a keyframe. set core_kfs_; //!< Keyframes in the closer neighbourhood. 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..b2f4a20 --- /dev/null +++ b/svo/include/svo/reprojector.h @@ -0,0 +1,98 @@ +// 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 + Options() + : max_n_kfs(10) + {} + } options_; + + size_t n_matches_; + size_t n_trials_; + + Reprojector(vk::AbstractCamera* cam, Map& map); + + ~Reprojector(); + + /// Project points from the map into the image. First finds keyframes with + /// overlapping field of view and projects only those map-points. + void reprojectMap( + FramePtr frame, + std::vector< std::pair >& 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/src/frame_handler_base.cpp b/svo/src/frame_handler_base.cpp index 3a8bad7..5a36bbc 100644 --- a/svo/src/frame_handler_base.cpp +++ b/svo/src/frame_handler_base.cpp @@ -25,7 +25,6 @@ #include #include #include -#include #include namespace svo diff --git a/svo/src/frame_handler_mono.cpp b/svo/src/frame_handler_mono.cpp index b0846c0..441a262 100644 --- a/svo/src/frame_handler_mono.cpp +++ b/svo/src/frame_handler_mono.cpp @@ -33,10 +33,9 @@ namespace svo { FrameHandlerMono:: FrameHandlerMono(vk::AbstractCamera* cam) : FrameHandlerBase(), - cam_(cam) -{ - reprojection_grid_.initialize(cam_); -} + cam_(cam), + reprojector_(cam_, map_) +{} FrameHandlerMono:: ~FrameHandlerMono() @@ -141,11 +140,10 @@ 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 = "< > 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)); @@ -100,7 +98,7 @@ void reprojectMap( if((*it_ftr)->point->last_projected_kf_id_ == frame->id_) continue; (*it_ftr)->point->last_projected_kf_id_ = frame->id_; - if(reprojectPoint(grid, frame, (*it_ftr)->point)) + if(reprojectPoint(frame, (*it_ftr)->point)) overlap_kfs.back().second++; } } @@ -109,17 +107,17 @@ void reprojectMap( // Now project all point candidates SVO_START_TIMER("reproject_candidates"); { - boost::unique_lock 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; } } @@ -131,38 +129,32 @@ void reprojectMap( // Now we go through each grid cell and select one point to match. // At the end, we should have at maximum one reprojected point per cell. SVO_START_TIMER("feature_align"); - n_matches=0; n_trials=0; - for(size_t i=0; i (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::DELETED) { @@ -176,9 +168,9 @@ bool reprojectCell( { it->pt->n_failed_reproj_++; if(it->pt->type_ == Point::UNKNOWN_QUALITY && it->pt->n_failed_reproj_ > 15) - map.safeDeletePoint(it->pt); + map_.safeDeletePoint(it->pt); if(it->pt->type_ == Point::CANDIDATE && it->pt->n_failed_reproj_ > 30) - candidate_points.deleteCandidatePoint(it->pt); + map_.point_candidates_.deleteCandidatePoint(it->pt); it = cell.erase(it); continue; } @@ -201,20 +193,17 @@ bool reprojectCell( return false; } -bool reprojectPoint( - 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