harray2d.h
Go to the documentation of this file.
1 #ifndef HARRAY2D_H
2 #define HARRAY2D_H
3 #include <set>
4 #include <gmapping/utils/point.h>
7 
8 namespace GMapping {
9 
10 template <class Cell>
11 class HierarchicalArray2D: public Array2D<autoptr< Array2D<Cell> > >{
12  public:
13  typedef std::set< point<int>, pointcomparator<int> > PointSet;
14  HierarchicalArray2D(int xsize, int ysize, int patchMagnitude=5);
18  void resize(int ixmin, int iymin, int ixmax, int iymax);
19  inline int getPatchSize() const {return m_patchMagnitude;}
20  inline int getPatchMagnitude() const {return m_patchMagnitude;}
21 
22  inline const Cell& cell(int x, int y) const;
23  inline Cell& cell(int x, int y);
24  inline bool isAllocated(int x, int y) const;
25  inline AccessibilityState cellState(int x, int y) const ;
26  inline IntPoint patchIndexes(int x, int y) const;
27 
28  inline const Cell& cell(const IntPoint& p) const { return cell(p.x,p.y); }
29  inline Cell& cell(const IntPoint& p) { return cell(p.x,p.y); }
30  inline bool isAllocated(const IntPoint& p) const { return isAllocated(p.x,p.y);}
31  inline AccessibilityState cellState(const IntPoint& p) const { return cellState(p.x,p.y); }
32  inline IntPoint patchIndexes(const IntPoint& p) const { return patchIndexes(p.x,p.y);}
33 
34  inline void setActiveArea(const PointSet&, bool patchCoords=false);
35  const PointSet& getActiveArea() const {return m_activeArea; }
36  inline void allocActiveArea();
37  protected:
38  virtual Array2D<Cell> * createPatch(const IntPoint& p) const;
39  PointSet m_activeArea;
42 };
43 
44 template <class Cell>
45 HierarchicalArray2D<Cell>::HierarchicalArray2D(int xsize, int ysize, int patchMagnitude)
46  :Array2D<autoptr< Array2D<Cell> > >::Array2D((xsize>>patchMagnitude), (ysize>>patchMagnitude)){
47  m_patchMagnitude=patchMagnitude;
48  m_patchSize=1<<m_patchMagnitude;
49 }
50 
51 template <class Cell>
53  :Array2D<autoptr< Array2D<Cell> > >::Array2D((hg.m_xsize>>hg.m_patchMagnitude), (hg.m_ysize>>hg.m_patchMagnitude)) // added by cyrill: if you have a resize error, check this again
54 {
55  this->m_xsize=hg.m_xsize;
56  this->m_ysize=hg.m_ysize;
57  this->m_cells=new autoptr< Array2D<Cell> >*[this->m_xsize];
58  for (int x=0; x<this->m_xsize; x++){
59  this->m_cells[x]=new autoptr< Array2D<Cell> >[this->m_ysize];
60  for (int y=0; y<this->m_ysize; y++)
61  this->m_cells[x][y]=hg.m_cells[x][y];
62  }
63  this->m_patchMagnitude=hg.m_patchMagnitude;
64  this->m_patchSize=hg.m_patchSize;
65 }
66 
67 template <class Cell>
68 void HierarchicalArray2D<Cell>::resize(int xmin, int ymin, int xmax, int ymax){
69  int xsize=xmax-xmin;
70  int ysize=ymax-ymin;
71  autoptr< Array2D<Cell> > ** newcells=new autoptr< Array2D<Cell> > *[xsize];
72  for (int x=0; x<xsize; x++){
73  newcells[x]=new autoptr< Array2D<Cell> >[ysize];
74  for (int y=0; y<ysize; y++){
75  newcells[x][y]=autoptr< Array2D<Cell> >(0);
76  }
77  }
78  int dx= xmin < 0 ? 0 : xmin;
79  int dy= ymin < 0 ? 0 : ymin;
80  int Dx=xmax<this->m_xsize?xmax:this->m_xsize;
81  int Dy=ymax<this->m_ysize?ymax:this->m_ysize;
82  for (int x=dx; x<Dx; x++){
83  for (int y=dy; y<Dy; y++){
84  newcells[x-xmin][y-ymin]=this->m_cells[x][y];
85  }
86  delete [] this->m_cells[x];
87  }
88  delete [] this->m_cells;
89  this->m_cells=newcells;
90  this->m_xsize=xsize;
91  this->m_ysize=ysize;
92 }
93 
94 template <class Cell>
96 // Array2D<autoptr< Array2D<Cell> > >::operator=(hg);
97  if (this->m_xsize!=hg.m_xsize || this->m_ysize!=hg.m_ysize){
98  for (int i=0; i<this->m_xsize; i++)
99  delete [] this->m_cells[i];
100  delete [] this->m_cells;
101  this->m_xsize=hg.m_xsize;
102  this->m_ysize=hg.m_ysize;
103  this->m_cells=new autoptr< Array2D<Cell> >*[this->m_xsize];
104  for (int i=0; i<this->m_xsize; i++)
105  this->m_cells[i]=new autoptr< Array2D<Cell> > [this->m_ysize];
106  }
107  for (int x=0; x<this->m_xsize; x++)
108  for (int y=0; y<this->m_ysize; y++)
109  this->m_cells[x][y]=hg.m_cells[x][y];
110 
111  m_activeArea.clear();
112  m_patchMagnitude=hg.m_patchMagnitude;
113  m_patchSize=hg.m_patchSize;
114  return *this;
115 }
116 
117 
118 template <class Cell>
120  m_activeArea.clear();
121  for (PointSet::const_iterator it= aa.begin(); it!=aa.end(); it++){
122  IntPoint p;
123  if (patchCoords)
124  p=*it;
125  else
126  p=patchIndexes(*it);
127  m_activeArea.insert(p);
128  }
129 }
130 
131 template <class Cell>
133  return new Array2D<Cell>(1<<m_patchMagnitude, 1<<m_patchMagnitude);
134 }
135 
136 
137 template <class Cell>
139  if (this->isInside(patchIndexes(x,y)))
140  if(isAllocated(x,y))
141  return (AccessibilityState)((int)Inside|(int)Allocated);
142  else
143  return Inside;
144  return Outside;
145 }
146 
147 template <class Cell>
149  for (PointSet::const_iterator it= m_activeArea.begin(); it!=m_activeArea.end(); it++){
150  const autoptr< Array2D<Cell> >& ptr=this->m_cells[it->x][it->y];
151  Array2D<Cell>* patch=0;
152  if (!ptr){
153  patch=createPatch(*it);
154  } else{
155  patch=new Array2D<Cell>(*ptr);
156  }
157  this->m_cells[it->x][it->y]=autoptr< Array2D<Cell> >(patch);
158  }
159 }
160 
161 template <class Cell>
162 bool HierarchicalArray2D<Cell>::isAllocated(int x, int y) const{
163  IntPoint c=patchIndexes(x,y);
164  autoptr< Array2D<Cell> >& ptr=this->m_cells[c.x][c.y];
165  return (ptr != 0);
166 }
167 
168 template <class Cell>
170  if (x>=0 && y>=0)
171  return IntPoint(x>>m_patchMagnitude, y>>m_patchMagnitude);
172  return IntPoint(-1, -1);
173 }
174 
175 template <class Cell>
177  IntPoint c=patchIndexes(x,y);
178  assert(this->isInside(c.x, c.y));
179  if (!this->m_cells[c.x][c.y]){
180  Array2D<Cell>* patch=createPatch(IntPoint(x,y));
181  this->m_cells[c.x][c.y]=autoptr< Array2D<Cell> >(patch);
182  //cerr << "!!! FATAL: your dick is going to fall down" << endl;
183  }
184  autoptr< Array2D<Cell> >& ptr=this->m_cells[c.x][c.y];
185  return (*ptr).cell(IntPoint(x-(c.x<<m_patchMagnitude),y-(c.y<<m_patchMagnitude)));
186 }
187 
188 template <class Cell>
189 const Cell& HierarchicalArray2D<Cell>::cell(int x, int y) const{
190  assert(isAllocated(x,y));
191  IntPoint c=patchIndexes(x,y);
192  const autoptr< Array2D<Cell> >& ptr=this->m_cells[c.x][c.y];
193  return (*ptr).cell(IntPoint(x-(c.x<<m_patchMagnitude),y-(c.y<<m_patchMagnitude)));
194 }
195 
196 };
197 
198 #endif
199 
std::set< point< int >, pointcomparator< int > > PointSet
Definition: harray2d.h:13
Cell & cell(const IntPoint &p)
Definition: harray2d.h:29
int getPatchMagnitude() const
Definition: harray2d.h:20
point< int > IntPoint
Definition: point.h:201
void resize(int ixmin, int iymin, int ixmax, int iymax)
Definition: harray2d.h:68
IntPoint patchIndexes(int x, int y) const
Definition: harray2d.h:169
bool isAllocated(const IntPoint &p) const
Definition: harray2d.h:30
AccessibilityState cellState(const IntPoint &p) const
Definition: harray2d.h:31
HierarchicalArray2D & operator=(const HierarchicalArray2D &hg)
Definition: harray2d.h:95
void setActiveArea(const PointSet &, bool patchCoords=false)
Definition: harray2d.h:119
virtual Array2D< Cell > * createPatch(const IntPoint &p) const
Definition: harray2d.h:132
AccessibilityState
Definition: accessstate.h:5
const Cell & cell(const IntPoint &p) const
Definition: harray2d.h:28
unsigned int c
Definition: gfs2stream.cpp:41
AccessibilityState cellState(int x, int y) const
Definition: harray2d.h:138
IntPoint patchIndexes(const IntPoint &p) const
Definition: harray2d.h:32
HierarchicalArray2D(int xsize, int ysize, int patchMagnitude=5)
Definition: harray2d.h:45
bool isAllocated(int x, int y) const
Definition: harray2d.h:162
const PointSet & getActiveArea() const
Definition: harray2d.h:35
Cell ** m_cells
Definition: array2d.h:37
const Cell & cell(int x, int y) const
Definition: harray2d.h:189


openslam_gmapping
Author(s): Cyrill Stachniss, Udo Frese, Giorgio Grisetti, Wolfram Burgard
autogenerated on Mon Feb 28 2022 22:59:20