SlidingWindowIterator.cpp
Go to the documentation of this file.
00001 /*
00002  * SlidingWindowIterator.cpp
00003  *
00004  *  Created on: Aug 17, 2017
00005  *      Author: Péter Fankhauser
00006  *   Institute: ETH Zurich, Robotic Systems Lab
00007  */
00008 
00009 #include "grid_map_core/iterators/SlidingWindowIterator.hpp"
00010 #include "grid_map_core/GridMapMath.hpp"
00011 
00012 #include <iostream>
00013 
00014 namespace grid_map {
00015 
00016 SlidingWindowIterator::SlidingWindowIterator(const GridMap& gridMap, const std::string& layer,
00017                                              const EdgeHandling& edgeHandling, const size_t windowSize)
00018     : GridMapIterator(gridMap),
00019       edgeHandling_(edgeHandling),
00020       data_(gridMap[layer])
00021 {
00022   windowSize_ = windowSize;
00023   setup(gridMap);
00024 }
00025 
00026 SlidingWindowIterator::SlidingWindowIterator(const SlidingWindowIterator* other)
00027     : GridMapIterator(other),
00028       edgeHandling_(other->edgeHandling_),
00029       data_(other->data_)
00030 {
00031   windowSize_ = other->windowSize_;
00032   windowMargin_ = other->windowMargin_;
00033 }
00034 
00035 void SlidingWindowIterator::setWindowLength(const GridMap& gridMap, const double windowLength)
00036 {
00037   windowSize_ = std::round(windowLength / gridMap.getResolution());
00038   if (windowSize_ % 2 != 1) ++windowSize_;
00039   setup(gridMap);
00040 }
00041 
00042 SlidingWindowIterator& SlidingWindowIterator::operator ++()
00043 {
00044   if (edgeHandling_ == EdgeHandling::INSIDE) {
00045     while (!isPastEnd()) {
00046       GridMapIterator::operator++();
00047       if (dataInsideMap()) break;
00048     }
00049   } else {
00050     GridMapIterator::operator++();
00051   }
00052   return *this;
00053 }
00054 
00055 const Matrix SlidingWindowIterator::getData() const
00056 {
00057   const Index centerIndex(*(*this));
00058   const Index windowMargin(Index::Constant(windowMargin_));
00059   const Index originalTopLeftIndex(centerIndex - windowMargin);
00060   Index topLeftIndex(originalTopLeftIndex);
00061   boundIndexToRange(topLeftIndex, size_);
00062   Index bottomRightIndex(centerIndex + windowMargin);
00063   boundIndexToRange(bottomRightIndex, size_);
00064   const Size adjustedWindowSize(bottomRightIndex - topLeftIndex + Size::Ones());
00065 
00066   switch (edgeHandling_) {
00067     case EdgeHandling::INSIDE:
00068     case EdgeHandling::CROP:
00069       return data_.block(topLeftIndex(0), topLeftIndex(1), adjustedWindowSize(0), adjustedWindowSize(1));
00070     case EdgeHandling::EMPTY:
00071     case EdgeHandling::MEAN:
00072       const Matrix data = data_.block(topLeftIndex(0), topLeftIndex(1), adjustedWindowSize(0), adjustedWindowSize(1));
00073       Matrix returnData(windowSize_, windowSize_);
00074       if (edgeHandling_ == EdgeHandling::EMPTY) returnData.setConstant(NAN);
00075       else if (edgeHandling_ == EdgeHandling::MEAN) returnData.setConstant(data.meanOfFinites());
00076       const Index topLeftIndexShift(topLeftIndex - originalTopLeftIndex);
00077       returnData.block(topLeftIndexShift(0), topLeftIndexShift(1), adjustedWindowSize(0), adjustedWindowSize(1)) =
00078           data_.block(topLeftIndex(0), topLeftIndex(1), adjustedWindowSize(0), adjustedWindowSize(1));
00079       return returnData;
00080   }
00081   return Matrix::Zero(0, 0);
00082 }
00083 
00084 void SlidingWindowIterator::setup(const GridMap& gridMap)
00085 {
00086   if (!gridMap.isDefaultStartIndex()) throw std::runtime_error(
00087       "SlidingWindowIterator cannot be used with grid maps that don't have a default buffer start index.");
00088   if (windowSize_ % 2 == 0) throw std::runtime_error(
00089       "SlidingWindowIterator has a wrong window size!");
00090   windowMargin_ = (windowSize_ - 1) / 2;
00091 
00092   if (edgeHandling_ == EdgeHandling::INSIDE) {
00093     if (!dataInsideMap()) {
00094       operator++();
00095     }
00096   }
00097 }
00098 
00099 bool SlidingWindowIterator::dataInsideMap() const
00100 {
00101   const Index centerIndex(*(*this));
00102   const Index windowMargin(Index::Constant(windowMargin_));
00103   const Index topLeftIndex(centerIndex - windowMargin);
00104   const Index bottomRightIndex(centerIndex + windowMargin);
00105   return checkIfIndexInRange(topLeftIndex, size_) && checkIfIndexInRange(bottomRightIndex, size_);
00106 }
00107 
00108 } /* namespace grid_map */


grid_map_core
Author(s): Péter Fankhauser
autogenerated on Mon Oct 9 2017 03:09:16