Added method to grid map iterator to get unwrapped index.

wpfhtl · Sep 21, 2015 · 15cd09a · 15cd09a
1 parent de882e2
commit 15cd09a
Show file tree

Hide file tree

Showing 4 changed files with 35 additions and 15 deletions.
diff --git a/grid_map_core/include/grid_map_core/GridMapMath.hpp b/grid_map_core/include/grid_map_core/GridMapMath.hpp
@@ -225,6 +225,17 @@ bool incrementIndexForSubmap(Eigen::Array2i& submapIndex, Eigen::Array2i& index,
                              const Eigen::Array2i& bufferSize,
                              const Eigen::Array2i& bufferStartIndex = Eigen::Array2i::Zero());
 
+/*!
+ * Retrieve the index as unwrapped index, i.e., as the corresponding index of a
+ * grid map with no circular buffer offset.
+ * @param bufferIndex the index in the circular buffer.
+ * @param bufferSize the map buffer size.
+ * @param bufferStartIndex the map buffer start index.
+ * @return the unwrapped index.
+ */
+Index getIndexFromBufferIndex(const Index& bufferIndex, const Size& bufferSize,
+                              const Index& bufferStartIndex);
+
 /*!
  * Returns the 1d index corresponding to the 2d index for either row- or column-major format.
  * Note: Eigen is defaulting to column-major format.
@@ -233,7 +244,7 @@ bool incrementIndexForSubmap(Eigen::Array2i& submapIndex, Eigen::Array2i& index,
  * @param[in] (optional) rowMajor if the 1d index is generated for row-major format.
  * @return the 1d index.
  */
-unsigned int get1dIndexFrom2dIndex(const Eigen::Array2i& index, const Eigen::Array2i& bufferSize,
+unsigned int get1dIndexFrom2dIndex(const Index& index, const Size& bufferSize,
                                    const bool rowMajor);
 
 /*!

diff --git a/grid_map_core/include/grid_map_core/iterators/GridMapIterator.hpp b/grid_map_core/include/grid_map_core/iterators/GridMapIterator.hpp
@@ -53,6 +53,12 @@ class GridMapIterator
    */
   const Index& operator *() const;
 
+  /*!
+   * Retrieve the index as unwrapped index, i.e., as the corresponding index of a
+   * grid map with no circular buffer offset.
+   */
+  const Index getUnwrappedIndex() const;
+
   /*!
    * Increase the iterator to the next element.
    * @return a reference to the updated iterator.

diff --git a/grid_map_core/src/GridMapMath.cpp b/grid_map_core/src/GridMapMath.cpp
@@ -81,19 +81,6 @@ inline Eigen::Array2i getBufferIndexFromIndex(
   return bufferIndex;
 }
 
-inline Eigen::Array2i getIndexFromBufferIndex(
-    const Eigen::Array2i& bufferIndex,
-    const Eigen::Array2i& bufferSize,
-    const Eigen::Array2i& bufferStartIndex)
-{
-  if (checkIfStartIndexAtDefaultPosition(bufferStartIndex))
-    return bufferIndex;
-
-  Array2i index = bufferIndex - bufferStartIndex;
-  mapIndexWithinRange(index, bufferSize);
-  return index;
-}
-
 inline Eigen::Vector2d getIndexVectorFromIndex(
     const Eigen::Array2i& index,
     const Eigen::Array2i& bufferSize,
@@ -470,6 +457,17 @@ bool incrementIndexForSubmap(Eigen::Array2i& submapIndex, Eigen::Array2i& index,
   return true;
 }
 
+Index getIndexFromBufferIndex(const Index& bufferIndex, const Size& bufferSize,
+                              const Index& bufferStartIndex)
+{
+  if (checkIfStartIndexAtDefaultPosition(bufferStartIndex))
+    return bufferIndex;
+
+  Array2i index = bufferIndex - bufferStartIndex;
+  mapIndexWithinRange(index, bufferSize);
+  return index;
+}
+
 unsigned int get1dIndexFrom2dIndex(const Eigen::Array2i& index, const Eigen::Array2i& bufferSize, const bool rowMajor)
 {
   if (!rowMajor) return index(1) * bufferSize(0) + index(0);

diff --git a/grid_map_core/src/iterators/GridMapIterator.cpp b/grid_map_core/src/iterators/GridMapIterator.cpp
@@ -45,11 +45,16 @@ bool GridMapIterator::operator !=(const GridMapIterator& other) const
   return (index_ != other.index_).any();
 }
 
-const Eigen::Array2i& GridMapIterator::operator *() const
+const Index& GridMapIterator::operator *() const
 {
   return index_;
 }
 
+const Index GridMapIterator::getUnwrappedIndex() const
+{
+  return getIndexFromBufferIndex(index_, size_, startIndex_);
+}
+
 GridMapIterator& GridMapIterator::operator ++()
 {
   isPastEnd_ = !incrementIndex(index_, size_, startIndex_);