Add slightly modified LineIterator for maxheight calculation

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
 )
 

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 <Eigen/Core>
+#include <queue>
+
+#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<Index> 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_

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"
 

grid_map_core/src/iterators/TraversalGridLineIterator.cpp

@@ -0,0 +1,177 @@
+#include "grid_map_core/iterators/TraversalGridLineIterator.hpp"
+#include <iostream>
+#include <limits>
+
+#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<double>::epsilon()) * direction;
+    if ((end - newStart).norm() <
+      gridMap.getResolution() - std::numeric_limits<double>::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