Go to the documentation of this file.00001
00004
00005
00006
00007
00008 #include <cost_map_core/iterators/ellipse_iterator.hpp>
00009 #include <grid_map_core/GridMapMath.hpp>
00010 #include <math.h>
00011 #include <Eigen/Geometry>
00012
00013 using namespace std;
00014
00015 namespace cost_map {
00016
00017 EllipseIterator::EllipseIterator(const CostMap& gridMap, const Position& center, const Length& length, const double rotation)
00018 : center_(center)
00019 {
00020 semiAxisSquare_ = (0.5 * length).square();
00021 double sinRotation = sin(rotation);
00022 double cosRotation = cos(rotation);
00023 transformMatrix_ << cosRotation, sinRotation, sinRotation, -cosRotation;
00024 mapLength_ = gridMap.getLength();
00025 mapPosition_ = gridMap.getPosition();
00026 resolution_ = gridMap.getResolution();
00027 bufferSize_ = gridMap.getSize();
00028 bufferStartIndex_ = gridMap.getStartIndex();
00029 Index submapStartIndex;
00030 Index submapBufferSize;
00031 findSubmapParameters(center, length, rotation, submapStartIndex, submapBufferSize);
00032 internalIterator_ = std::shared_ptr<SubmapIterator>(new SubmapIterator(gridMap, submapStartIndex, submapBufferSize));
00033 if(!isInside()) ++(*this);
00034 }
00035
00036 EllipseIterator& EllipseIterator::operator =(const EllipseIterator& other)
00037 {
00038 center_ = other.center_;
00039 semiAxisSquare_ = other.semiAxisSquare_;
00040 transformMatrix_ = other.transformMatrix_;
00041 internalIterator_ = other.internalIterator_;
00042 mapLength_ = other.mapLength_;
00043 mapPosition_ = other.mapPosition_;
00044 resolution_ = other.resolution_;
00045 bufferSize_ = other.bufferSize_;
00046 bufferStartIndex_ = other.bufferStartIndex_;
00047 return *this;
00048 }
00049
00050 bool EllipseIterator::operator !=(const EllipseIterator& other) const
00051 {
00052 return (internalIterator_ != other.internalIterator_);
00053 }
00054
00055 const Eigen::Array2i& EllipseIterator::operator *() const
00056 {
00057 return *(*internalIterator_);
00058 }
00059
00060 EllipseIterator& EllipseIterator::operator ++()
00061 {
00062 ++(*internalIterator_);
00063 if (internalIterator_->isPastEnd()) return *this;
00064
00065 for ( ; !internalIterator_->isPastEnd(); ++(*internalIterator_)) {
00066 if (isInside()) break;
00067 }
00068
00069 return *this;
00070 }
00071
00072 bool EllipseIterator::isPastEnd() const
00073 {
00074 return internalIterator_->isPastEnd();
00075 }
00076
00077 bool EllipseIterator::isInside() const
00078 {
00079 Position position;
00080 grid_map::getPositionFromIndex(position, *(*internalIterator_), mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
00081 double value = ((transformMatrix_ * (position - center_)).array().square() / semiAxisSquare_).sum();
00082 return (value <= 1);
00083 }
00084
00085 void EllipseIterator::findSubmapParameters(const Position& center, const Length& length, const double rotation,
00086 Index& startIndex, Size& bufferSize) const
00087 {
00088 const Eigen::Rotation2Dd rotationMatrix(rotation);
00089 Eigen::Vector2d u = rotationMatrix * Eigen::Vector2d(length(0), 0.0);
00090 Eigen::Vector2d v = rotationMatrix * Eigen::Vector2d(0.0, length(1));
00091 const Length boundingBoxHalfLength = (u.cwiseAbs2() + v.cwiseAbs2()).array().sqrt();
00092 Position topLeft = center.array() + boundingBoxHalfLength;
00093 Position bottomRight = center.array() - boundingBoxHalfLength;
00094 grid_map::boundPositionToRange(topLeft, mapLength_, mapPosition_);
00095 grid_map::boundPositionToRange(bottomRight, mapLength_, mapPosition_);
00096 grid_map::getIndexFromPosition(startIndex, topLeft, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
00097 Index endIndex;
00098 grid_map::getIndexFromPosition(endIndex, bottomRight, mapLength_, mapPosition_, resolution_, bufferSize_, bufferStartIndex_);
00099 bufferSize = endIndex - startIndex + Eigen::Array2i::Ones();
00100 }
00101
00102 }
00103