diff --git a/include/vision/processor/active_search.h b/include/vision/processor/active_search.h index bda60ffc3c9ce0778b24db4999a61feef75e7532..7f00e5501fe89b876da0cf78da27915c789621f7 100644 --- a/include/vision/processor/active_search.h +++ b/include/vision/processor/active_search.h @@ -203,6 +203,7 @@ class ActiveSearchGrid { /** * \brief Add a projected pixel to the grid. + * If the cell is blocked, unblock and add. * \param _x the x-coordinate of the pixel to add. * \param _y the y-coordinate of the pixel to add. */ @@ -211,6 +212,7 @@ class ActiveSearchGrid { /** * \brief Add a projected pixel to the grid. + * If the cell is blocked, unblock and add. * \param _pix the pixel to add as an Eigen 2-vector with any Scalar type (can be a non-integer). */ template<typename Scalar> @@ -218,6 +220,7 @@ class ActiveSearchGrid { /** * \brief Add a projected pixel to the grid. + * If the cell is blocked, unblock and add. * \param _pix the pixel to add as a cv::KeyPoint. */ void hitCell(const cv::KeyPoint& _pix); @@ -230,18 +233,17 @@ class ActiveSearchGrid { bool pickRoi(cv::Rect & _roi); /** - * \brief Call this after pickRoi if no point was found in the roi - * in order to avoid searching again in it. - * \param _roi the ROI where nothing was found + * \brief Block a cell known to be empty in order to avoid searching again in it. + * \param _cell the cell where nothing was found */ - void blockCell(const cv::Rect & _roi); + void blockCell(const Eigen::Vector2i & _cell); /** * \brief Call this after pickRoi if no point was found in the roi * in order to avoid searching again in it. - * \param _cell the cell where nothing was found + * \param _roi the ROI where nothing was found */ - void blockCell(const Vector2i & _cell); + void blockCell(const cv::Rect & _roi); private: @@ -271,6 +273,12 @@ class ActiveSearchGrid { */ void cell2roi(const Eigen::Vector2i & _cell, cv::Rect& _roi); + /** + * \brief True if the cell is blocked + * \param _cell the queried cell + */ + bool isCellBlocked(const Eigen::Vector2i& _cell); + }; inline void ActiveSearchGrid::clear() @@ -303,8 +311,8 @@ inline void ActiveSearchGrid::hitCell(const Scalar _x, const Scalar _y) if (cell(0) < 0 || cell(1) < 0 || cell(0) >= grid_size_(0) || cell(1) >= grid_size_(1)) return; - if (projections_count_(cell(0), cell(1)) == -1) - projections_count_(cell(0), cell(1)) = 0; + if (isCellBlocked(cell)) + projections_count_(cell(0), cell(1)) = 0; // unblock cell: it becomes empty projections_count_(cell(0), cell(1))++; } diff --git a/src/processor/active_search.cpp b/src/processor/active_search.cpp index a316781a633b81766b249b7df2b8b54cd85fc09d..e9006dfe617cac74802c11da0dcbf82d90524368 100644 --- a/src/processor/active_search.cpp +++ b/src/processor/active_search.cpp @@ -76,7 +76,8 @@ bool ActiveSearchGrid::pickEmptyCell(Eigen::Vector2i & _cell) { for (int j = 1; j < grid_size_(1) - 1; j++) { cell0(0) = i; cell0(1) = j; - if (projections_count_(i, j) == 0) { + if (projections_count_(i, j) == 0) // cell in empty AND not blocked + { empty_cells_tile_tmp_(0,k) = i; //may be done in a better way empty_cells_tile_tmp_(1,k) = j; k++; @@ -125,6 +126,11 @@ bool ActiveSearchGrid::pickRoi(cv::Rect & _roi) { return false; } +void ActiveSearchGrid::blockCell(const cv::Rect & _cell) +{ + projections_count_(_cell(0), _cell(1)) = -1; +} + void ActiveSearchGrid::blockCell(const cv::Rect & _roi) { Eigen::Vector2i pix; @@ -134,9 +140,9 @@ void ActiveSearchGrid::blockCell(const cv::Rect & _roi) blockCell(cell); } -void ActiveSearchGrid::blockCell(const cv::Rect & _cell) +bool ActiveSearchGrid::isCellBlocked (const Eigen::Vector2i& _cell) { - projections_count_(_cell(0), _cell(1)) = -1; + return projections_count_(_cell(0), _cell(1)) < 0; }