src/lib/iterators/ellipse_iterator.cpp
Go to the documentation of this file.
1 
4 /*****************************************************************************
5 ** Includes
6 *****************************************************************************/
7 
10 #include <math.h>
11 #include <Eigen/Geometry>
12 
13 using namespace std;
14 
15 namespace cost_map {
16 
17 EllipseIterator::EllipseIterator(const CostMap& gridMap, const Position& center, const Length& length, const double rotation)
18  : center_(center)
19 {
20  semiAxisSquare_ = (0.5 * length).square();
21  double sinRotation = sin(rotation);
22  double cosRotation = cos(rotation);
23  transformMatrix_ << cosRotation, sinRotation, sinRotation, -cosRotation;
24  mapLength_ = gridMap.getLength();
25  mapPosition_ = gridMap.getPosition();
26  resolution_ = gridMap.getResolution();
27  bufferSize_ = gridMap.getSize();
28  bufferStartIndex_ = gridMap.getStartIndex();
29  Index submapStartIndex;
30  Index submapBufferSize;
31  findSubmapParameters(center, length, rotation, submapStartIndex, submapBufferSize);
32  internalIterator_ = std::shared_ptr<SubmapIterator>(new SubmapIterator(gridMap, submapStartIndex, submapBufferSize));
33  if(!isInside()) ++(*this);
34 }
35 
37 {
38  center_ = other.center_;
42  mapLength_ = other.mapLength_;
43  mapPosition_ = other.mapPosition_;
44  resolution_ = other.resolution_;
45  bufferSize_ = other.bufferSize_;
47  return *this;
48 }
49 
51 {
52  return (internalIterator_ != other.internalIterator_);
53 }
54 
55 const Eigen::Array2i& EllipseIterator::operator *() const
56 {
57  return *(*internalIterator_);
58 }
59 
61 {
62  ++(*internalIterator_);
63  if (internalIterator_->isPastEnd()) return *this;
64 
65  for ( ; !internalIterator_->isPastEnd(); ++(*internalIterator_)) {
66  if (isInside()) break;
67  }
68 
69  return *this;
70 }
71 
73 {
74  return internalIterator_->isPastEnd();
75 }
76 
78 {
79  Position position;
81  double value = ((transformMatrix_ * (position - center_)).array().square() / semiAxisSquare_).sum();
82  return (value <= 1);
83 }
84 
85 void EllipseIterator::findSubmapParameters(const Position& center, const Length& length, const double rotation,
86  Index& startIndex, Size& bufferSize) const
87 {
88  const Eigen::Rotation2Dd rotationMatrix(rotation);
89  Eigen::Vector2d u = rotationMatrix * Eigen::Vector2d(length(0), 0.0);
90  Eigen::Vector2d v = rotationMatrix * Eigen::Vector2d(0.0, length(1));
91  const Length boundingBoxHalfLength = (u.cwiseAbs2() + v.cwiseAbs2()).array().sqrt();
92  Position topLeft = center.array() + boundingBoxHalfLength;
93  Position bottomRight = center.array() - boundingBoxHalfLength;
97  Index endIndex;
99  bufferSize = endIndex - startIndex + Eigen::Array2i::Ones();
100 }
101 
102 } /* namespace cost_map */
103 
const Length & getLength() const
void findSubmapParameters(const Position &center, const Length &length, const double rotation, Index &startIndex, Size &bufferSize) const
Position center_
Position of the circle center;.
Length mapLength_
Map information needed to get position from iterator.
const Size & getSize() const
double getResolution() const
grid_map::Size Size
Definition: common.hpp:46
bool getIndexFromPosition(Index &index, const Position &position, const Length &mapLength, const Position &mapPosition, const double &resolution, const Size &bufferSize, const Index &bufferStartIndex=Index::Zero())
Eigen::Matrix2d transformMatrix_
Sine and cosine values of the rotation angle as transformation matrix.
Eigen::Array2d semiAxisSquare_
Square length of the semi axis.
bool getPosition(const Index &index, Position &position) const
grid_map::Index Index
Definition: common.hpp:45
std::shared_ptr< SubmapIterator > internalIterator_
Grid submap iterator. // TODO Think of using unique_ptr instead.
grid_map::Position Position
Definition: common.hpp:41
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 Index & getStartIndex() const
void boundPositionToRange(Position &position, const Length &mapLength, const Position &mapPosition)
grid_map::Length Length
Definition: common.hpp:47
bool operator!=(const EllipseIterator &other) const
EllipseIterator & operator=(const EllipseIterator &other)


cost_map_core
Author(s): Daniel Stonier
autogenerated on Mon Jun 10 2019 13:03:41