diff --git a/grid_map_core/CMakeLists.txt b/grid_map_core/CMakeLists.txt index 749c250..dc48960 100644 --- a/grid_map_core/CMakeLists.txt +++ b/grid_map_core/CMakeLists.txt @@ -34,6 +34,7 @@ add_library(${PROJECT_NAME} src/iterators/SpiralIterator.cpp src/iterators/PolygonIterator.cpp src/iterators/LineIterator.cpp + src/iterators/TraversalGridLineIterator.cpp src/iterators/SlidingWindowIterator.cpp ) diff --git a/grid_map_core/include/grid_map_core/iterators/TraversalGridLineIterator.hpp b/grid_map_core/include/grid_map_core/iterators/TraversalGridLineIterator.hpp new file mode 100644 index 0000000..d1194b7 --- /dev/null +++ b/grid_map_core/include/grid_map_core/iterators/TraversalGridLineIterator.hpp @@ -0,0 +1,135 @@ +/* + * LineIterator.hpp + * + * Created on: Nov 13, 2014 + * Author: Péter Fankhauser + * Institute: ETH Zurich, ANYbotics + */ + +#ifndef GRID_MAP_CORE__ITERATORS__TRAVERSALGRIDLINEITERATOR_HPP_ +#define GRID_MAP_CORE__ITERATORS__TRAVERSALGRIDLINEITERATOR_HPP_ + +#include +#include + +#include "grid_map_core/GridMap.hpp" +#include "grid_map_core/iterators/SubmapIterator.hpp" + +namespace grid_map +{ + +/*! + * Iterator class to iterate over a line in the map. + * Based on Bresenham Line Drawing algorithm. + */ +class TraversalGridLineIterator +{ +public: + /*! + * Constructor. + * @param gridMap the grid map to iterate on. + * @param start the starting point of the line. + * @param end the ending point of the line. + * @throw std::invalid_argument if start and end impose an ill conditioned line iteration. + */ + TraversalGridLineIterator(const grid_map::GridMap & gridMap, const Position & start, const Position & end); + + /*! + * Constructor. + * @param gridMap the grid map to iterate on. + * @param start the starting index of the line. + * @param end the ending index of the line. + */ + TraversalGridLineIterator(const grid_map::GridMap & gridMap, const Index & start, const Index & end); + + /*! + * Assignment operator. + * @param iterator the iterator to copy data from. + * @return a reference to *this. + */ + TraversalGridLineIterator & operator=(const TraversalGridLineIterator & other); + + /*! + * Compare to another iterator. + * @return whether the current iterator points to a different address than the other one. + */ + bool operator!=(const TraversalGridLineIterator & other) const; + + /*! + * Dereference the iterator with const. + * @return the value to which the iterator is pointing. + */ + const Index & operator*() const; + + /*! + * Increase the iterator to the next element. + * @return a reference to the updated iterator. + */ + TraversalGridLineIterator & operator++(); + + /*! + * Indicates if iterator is past end. + * @return true if iterator is out of scope, false if end has not been reached. + */ + bool isPastEnd() const; + +private: + /*! + * Construct function. + * @param gridMap the grid map to iterate on. + * @param start the starting index of the line. + * @param end the ending index of the line. + * @return true if successful, false otherwise. + */ + bool initialize(const grid_map::GridMap & gridMap, const Index & start, const Index & end); + + /*! + * Computes the parameters requires for the line drawing algorithm. + */ + void initializeIterationParameters(); + + /*! + * Finds the index of a position on a line within the limits of the map. + * @param[in] gridMap the grid map that defines the map boundaries. + * @param[in] start the position that will be limited to the map range. + * @param[in] end the ending position of the line. + * @param[out] index the index of the moved start position. + * @return true if successful, false otherwise. + */ + bool getIndexLimitedToMapRange( + const grid_map::GridMap & gridMap, const Position & start, + const Position & end, Index & index); + + //! Current index. + Index index_; + + //! Starting index of the line. + Index start_; + + //! Ending index of the line. + Index end_; + + //! Current cell number. + unsigned int iCell_; + + //! Number of cells in the line. + unsigned int nCells_; + + //! Helper variables for Bresenham Line Drawing algorithm. + std::queue nextIndex_; + Size increment1_, increment2_; + int denominator_, numerator_, numeratorAdd_; + + //! Map information needed to get position from iterator. + Length mapLength_; + Position mapPosition_; + double resolution_; + Size bufferSize_; + Index bufferStartIndex_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace grid_map +#endif // GRID_MAP_CORE__ITERATORS__TRAVERSALGRIDLINEITERATOR_HPP_ diff --git a/grid_map_core/include/grid_map_core/iterators/iterators.hpp b/grid_map_core/include/grid_map_core/iterators/iterators.hpp index e54f9dd..4f01cd8 100644 --- a/grid_map_core/include/grid_map_core/iterators/iterators.hpp +++ b/grid_map_core/include/grid_map_core/iterators/iterators.hpp @@ -16,6 +16,7 @@ #include "grid_map_core/iterators/EllipseIterator.hpp" #include "grid_map_core/iterators/SpiralIterator.hpp" #include "grid_map_core/iterators/LineIterator.hpp" +#include "grid_map_core/iterators/TraversalGridLineIterator.hpp" #include "grid_map_core/iterators/PolygonIterator.hpp" #include "grid_map_core/iterators/SlidingWindowIterator.hpp" diff --git a/grid_map_core/src/iterators/TraversalGridLineIterator.cpp b/grid_map_core/src/iterators/TraversalGridLineIterator.cpp new file mode 100644 index 0000000..86835a3 --- /dev/null +++ b/grid_map_core/src/iterators/TraversalGridLineIterator.cpp @@ -0,0 +1,177 @@ +#include "grid_map_core/iterators/TraversalGridLineIterator.hpp" +#include +#include + +#include "grid_map_core/GridMapMath.hpp" + +namespace grid_map +{ + +TraversalGridLineIterator::TraversalGridLineIterator( + const grid_map::GridMap & gridMap, const Position & start, + const Position & end) +{ + Index startIndex, endIndex; + if (getIndexLimitedToMapRange(gridMap, start, end, startIndex) && + getIndexLimitedToMapRange(gridMap, end, start, endIndex)) + { + initialize(gridMap, startIndex, endIndex); + } else { + throw std::invalid_argument("Failed to construct TraversalGridLineIterator."); + } +} + +TraversalGridLineIterator::TraversalGridLineIterator( + const grid_map::GridMap & gridMap, const Index & start, + const Index & end) +{ + initialize(gridMap, start, end); +} + +TraversalGridLineIterator & TraversalGridLineIterator::operator=(const TraversalGridLineIterator & other) +{ + index_ = other.index_; + start_ = other.start_; + end_ = other.end_; + iCell_ = other.iCell_; + nCells_ = other.nCells_; + mapLength_ = other.mapLength_; + mapPosition_ = other.mapPosition_; + resolution_ = other.resolution_; + bufferSize_ = other.bufferSize_; + bufferStartIndex_ = other.bufferStartIndex_; + return *this; +} + +bool TraversalGridLineIterator::operator!=(const TraversalGridLineIterator & other) const +{ + return (index_ != other.index_).any(); +} + +const Index & TraversalGridLineIterator::operator*() const +{ + assert(!nextIndex_.empty()); + return nextIndex_.front(); +} + +TraversalGridLineIterator & TraversalGridLineIterator::operator++() +{ + if (!nextIndex_.empty()) { + nextIndex_.pop(); + } + // If queue is not empty return first index + if (!nextIndex_.empty()) + { + return *this; + } + if (iCell_ < nCells_) + { + numerator_ += numeratorAdd_; // Increase the numerator by the top of the fraction. + if (numerator_ >= denominator_) { + numerator_ -= denominator_; + const Index unwrappedIndex = + getIndexFromBufferIndex(index_, bufferSize_, bufferStartIndex_) + increment1_; + index_ = getBufferIndexFromIndex(unwrappedIndex, bufferSize_, bufferStartIndex_); + nextIndex_.push(index_); + const Index unwrappedIndex2 = + getIndexFromBufferIndex(index_, bufferSize_, bufferStartIndex_) - increment1_; + Index additional_index_ = getBufferIndexFromIndex(unwrappedIndex2, bufferSize_, bufferStartIndex_); + nextIndex_.push(additional_index_); + } + const Index unwrappedIndex = + getIndexFromBufferIndex(index_, bufferSize_, bufferStartIndex_) + increment2_; + index_ = getBufferIndexFromIndex(unwrappedIndex, bufferSize_, bufferStartIndex_); + nextIndex_.push(index_); +} +++iCell_; +return *this; +} + +bool TraversalGridLineIterator::isPastEnd() const +{ + return nextIndex_.empty(); +} + +bool TraversalGridLineIterator::initialize( + const grid_map::GridMap & gridMap, const Index & start, + const Index & end) +{ + start_ = start; + end_ = end; + mapLength_ = gridMap.getLength(); + mapPosition_ = gridMap.getPosition(); + resolution_ = gridMap.getResolution(); + bufferSize_ = gridMap.getSize(); + bufferStartIndex_ = gridMap.getStartIndex(); + initializeIterationParameters(); + return true; +} + +bool TraversalGridLineIterator::getIndexLimitedToMapRange( + const grid_map::GridMap & gridMap, + const Position & start, const Position & end, + Index & index) +{ + Position newStart = start; + Vector direction = (end - start).normalized(); + while (!gridMap.getIndex(newStart, index)) { + newStart += (gridMap.getResolution() - std::numeric_limits::epsilon()) * direction; + if ((end - newStart).norm() < + gridMap.getResolution() - std::numeric_limits::epsilon()) + { + return false; + } + } + return true; +} + +void TraversalGridLineIterator::initializeIterationParameters() +{ + iCell_ = 0; + index_ = start_; + nextIndex_.push(index_); + + const Index unwrappedStart = getIndexFromBufferIndex(start_, bufferSize_, bufferStartIndex_); + const Index unwrappedEnd = getIndexFromBufferIndex(end_, bufferSize_, bufferStartIndex_); + const Size delta = (unwrappedEnd - unwrappedStart).abs(); + + if (unwrappedEnd.x() >= unwrappedStart.x()) { + // x-values increasing. + increment1_.x() = 1; + increment2_.x() = 1; + } else { + // x-values decreasing. + increment1_.x() = -1; + increment2_.x() = -1; + } + + if (unwrappedEnd.y() >= unwrappedStart.y()) { + // y-values increasing. + increment1_.y() = 1; + increment2_.y() = 1; + } else { + // y-values decreasing. + increment1_.y() = -1; + increment2_.y() = -1; + } + + if (delta.x() >= delta.y()) { + // There is at least one x-value for every y-value. + increment1_.x() = 0; // Do not change the x when numerator >= denominator. + increment2_.y() = 0; // Do not change the y for every iteration. + denominator_ = delta.x(); + numerator_ = delta.x() / 2; + numeratorAdd_ = delta.y(); + nCells_ = delta.x() + 1; // There are more x-values than y-values. + } else { + // There is at least one y-value for every x-value + increment2_.x() = 0; // Do not change the x for every iteration. + increment1_.y() = 0; // Do not change the y when numerator >= denominator. + denominator_ = delta.y(); + numerator_ = delta.y() / 2; + numeratorAdd_ = delta.x(); + nCells_ = delta.y() + 1; // There are more y-values than x-values. + } +} + +} // namespace grid_map