EllipseIterator.cpp
Go to the documentation of this file.
1 /*
2  * EllipseIterator.hpp
3  *
4  * Created on: Dec 2, 2015
5  * Author: Péter Fankhauser
6  * Institute: ETH Zurich, ANYbotics
7  */
8 
11 
12 #include <math.h>
13 #include <Eigen/Geometry>
14 
15 using namespace std;
16 
17 namespace grid_map {
18 
19 EllipseIterator::EllipseIterator(const GridMap& gridMap, const Position& center, const Length& length, const double rotation)
20  : center_(center)
21 {
22  semiAxisSquare_ = (0.5 * length).square();
23  double sinRotation = sin(rotation);
24  double cosRotation = cos(rotation);
25  transformMatrix_ << cosRotation, sinRotation, sinRotation, -cosRotation;
26  mapLength_ = gridMap.getLength();
27  mapPosition_ = gridMap.getPosition();
28  resolution_ = gridMap.getResolution();
29  bufferSize_ = gridMap.getSize();
30  bufferStartIndex_ = gridMap.getStartIndex();
31  Index submapStartIndex;
32  Index submapBufferSize;
33  findSubmapParameters(center, length, rotation, submapStartIndex, submapBufferSize);
34  internalIterator_ = std::shared_ptr<SubmapIterator>(new SubmapIterator(gridMap, submapStartIndex, submapBufferSize));
35  if(!isInside()) ++(*this);
36 }
37 
39 {
40  center_ = other.center_;
44  mapLength_ = other.mapLength_;
45  mapPosition_ = other.mapPosition_;
46  resolution_ = other.resolution_;
47  bufferSize_ = other.bufferSize_;
49  return *this;
50 }
51 
53 {
54  return (internalIterator_ != other.internalIterator_);
55 }
56 
57 const Eigen::Array2i& EllipseIterator::operator *() const
58 {
59  return *(*internalIterator_);
60 }
61 
63 {
64  ++(*internalIterator_);
65  if (internalIterator_->isPastEnd()) return *this;
66 
67  for ( ; !internalIterator_->isPastEnd(); ++(*internalIterator_)) {
68  if (isInside()) break;
69  }
70 
71  return *this;
72 }
73 
75 {
76  return internalIterator_->isPastEnd();
77 }
78 
80 {
81  return internalIterator_->getSubmapSize();
82 }
83 
85 {
86  Position position;
88  double value = ((transformMatrix_ * (position - center_)).array().square() / semiAxisSquare_).sum();
89  return (value <= 1);
90 }
91 
92 void EllipseIterator::findSubmapParameters(const Position& center, const Length& length, const double rotation,
93  Index& startIndex, Size& bufferSize) const
94 {
95  const Eigen::Rotation2Dd rotationMatrix(rotation);
96  Eigen::Vector2d u = rotationMatrix * Eigen::Vector2d(length(0), 0.0);
97  Eigen::Vector2d v = rotationMatrix * Eigen::Vector2d(0.0, length(1));
98  const Length boundingBoxHalfLength = (u.cwiseAbs2() + v.cwiseAbs2()).array().sqrt();
99  Position topLeft = center.array() + boundingBoxHalfLength;
100  Position bottomRight = center.array() - boundingBoxHalfLength;
104  Index endIndex;
106  bufferSize = getSubmapSizeFromCornerIndeces(startIndex, endIndex, bufferSize_, bufferStartIndex_);
107 }
108 
109 } /* namespace grid_map */
110 
const Length & getLength() const
Definition: GridMap.cpp:640
Eigen::Array2i Index
Definition: TypeDefs.hpp:22
Length mapLength_
Map information needed to get position from iterator.
const Index & getStartIndex() const
Definition: GridMap.cpp:660
bool getPosition(const Index &index, Position &position) const
Definition: GridMap.cpp:233
void findSubmapParameters(const Position &center, const Length &length, const double rotation, Index &startIndex, Size &bufferSize) const
Eigen::Array2i Size
Definition: TypeDefs.hpp:23
EllipseIterator & operator=(const EllipseIterator &other)
Position center_
Position of the circle center;.
EllipseIterator & operator++()
const Index & operator*() const
Eigen::Matrix2d transformMatrix_
Sine and cosine values of the rotation angle as transformation matrix.
bool getIndexFromPosition(Index &index, const Position &position, const Length &mapLength, const Position &mapPosition, const double &resolution, const Size &bufferSize, const Index &bufferStartIndex=Index::Zero())
double getResolution() const
Definition: GridMap.cpp:648
bool operator!=(const EllipseIterator &other) const
std::shared_ptr< SubmapIterator > internalIterator_
Grid submap iterator. // TODO Think of using unique_ptr instead.
Eigen::Vector2d Position
Definition: TypeDefs.hpp:18
Size getSubmapSizeFromCornerIndeces(const Index &topLeftIndex, const Index &bottomRightIndex, const Size &bufferSize, const Index &bufferStartIndex)
Eigen::Array2d semiAxisSquare_
Square length of the semi axis.
bool getPositionFromIndex(Position &position, const Index &index, const Length &mapLength, const Position &mapPosition, const double &resolution, const Size &bufferSize, const Index &bufferStartIndex=Index::Zero())
const Size & getSubmapSize() const
void boundPositionToRange(Position &position, const Length &mapLength, const Position &mapPosition)
Eigen::Array2d Length
Definition: TypeDefs.hpp:24
const Size & getSize() const
Definition: GridMap.cpp:652


grid_map_core
Author(s): Péter Fankhauser
autogenerated on Sun Oct 25 2020 03:14:42