snark
pin_screen.h
1 // This file is part of snark, a generic and flexible library
2 // for robotics research.
3 //
4 // Copyright (C) 2011 The University of Sydney
5 //
6 // snark is free software; you can redistribute it and/or
7 // modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation; either
9 // version 3 of the License, or (at your option) any later version.
10 //
11 // snark is distributed in the hope that it will be useful, but WITHOUT ANY
12 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
13 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
14 // for more details.
15 //
16 // You should have received a copy of the GNU Lesser General Public
17 // License along with snark. If not, see <http://www.gnu.org/licenses/>.
18 
19 #ifndef SNARK_PERCEPTION_PIN_SCREEN_HEADER_GUARD_
20 #define SNARK_PERCEPTION_PIN_SCREEN_HEADER_GUARD_
21 
22 #include <cassert>
23 #include <map>
24 #include <Eigen/Core>
25 
26 namespace snark {
27 
32 template < typename T >
33 class pin_screen
34 {
35  public:
37  typedef T value_type;
38 
40  typedef Eigen::Matrix< std::size_t, 1, 3 > index_type;
41 
43  typedef Eigen::Matrix< std::size_t, 1, 2 > size_type;
44 
46  typedef std::map< std::size_t, T > column_type;
47 
49  pin_screen( std::size_t size1 , std::size_t size2 );
50 
52  pin_screen( size_type size );
53 
55  size_type size() const { return size_type( m_grid.rows(), m_grid.cols() ); }
56 
59  const column_type& column( std::size_t i , std::size_t j ) const { return m_grid( i, j ); }
60 
62  std::size_t height( std::size_t i , std::size_t j ) const;
63 
65  bool exists( std::size_t i , std::size_t j , std::size_t k ) const;
66 
68  bool exists( const index_type& i ) const { return exists( i[0], i[1], i[2] ); }
69 
71  T* find( std::size_t i , std::size_t j , std::size_t k );
72 
74  T* find( const index_type& i ) { return find( i[0], i[1], i[2] ); }
75 
77  const T* find( std::size_t i , std::size_t j , std::size_t k ) const;
78 
80  const T* find( const index_type& i ) const { return find( i[0], i[1], i[2] ); }
81 
83  T& operator()( std::size_t i , std::size_t j , std::size_t k );
84 
86  const T& operator()( std::size_t i , std::size_t j , std::size_t k ) const;
87 
92  T& operator()( const index_type& i ) { return operator()( i[0], i[1], i[2] ); }
93 
95  const T& operator()( const index_type& i ) const { return operator()( i[0], i[1], i[2] ); }
96 
98  T& touch( std::size_t i , std::size_t j , std::size_t k ) { return m_grid( i, j )[k]; }
99 
101  T& touch( const index_type& i ) { return touch( i[0], i[1], i[2] ); }
102 
104  void erase( std::size_t i, std::size_t j, std::size_t k );
105 
107  void erase( const index_type& i ) { return erase( i[0], i[1], i[2] ); }
108 
110  void clear();
111 
113  class iterator;
114 
116  class const_iterator;
117 
119  iterator begin();
120 
122  const_iterator begin() const;
123 
125  iterator end();
126 
128  const_iterator end() const;
129 
132  class neighbourhood_iterator;
133 
134  private:
135  friend class iterator;
136  friend class const_iterator;
137  typedef ::Eigen::Matrix< column_type, ::Eigen::Dynamic, ::Eigen::Dynamic > GridType;
138  GridType m_grid;
139 };
140 
141 template < typename T >
142 class pin_screen< T >::const_iterator
143 {
144  public:
146  typedef T value_type;
147 
149  typedef typename pin_screen< T >::index_type index_type;
150 
152  typedef typename pin_screen< T >::size_type size_type;
153 
155  enum { Dimensions = 3 };
156 
158  bool operator==( const const_iterator& rhs ) const { return m_grid == rhs.m_grid && m_column == rhs.m_column && m_it == rhs.m_it; }
159  bool operator!=( const const_iterator& rhs ) const { return !operator==( rhs ); }
160  bool operator<( const const_iterator& rhs ) const
161  {
162  assert( m_grid == rhs.m_grid );
163  if (m_column[0] < rhs.m_column[0]) return true;
164  if (m_column[0] > rhs.m_column[0]) return false;
165 
166  if (m_column[1] < rhs.m_column[1]) return true;
167  if (m_column[1] > rhs.m_column[1]) return false;
168 
169  if (m_it->first < rhs.m_it->first) return true;
170  if (m_it->first > rhs.m_it->first) return false;
171 
172  return false;
173 
174  // partially ordered operator, which does not seem to work for RB tree containers
175  //return m_column[0] < rhs.m_column[0] || m_column[1] < rhs.m_column[1] || m_it->first < rhs.m_it->first;
176  }
177  const T& operator*() const { return m_it->second; }
178  const T* operator->() const { return &m_it->second; }
179  index_type operator()() const { return index_type( m_column[0], m_column[1], m_it->first ); }
180  const const_iterator& operator++()
181  {
182  if( m_it != ( *m_grid )( m_column[0], m_column[1] ).end() ) { ++m_it; }
183  while( m_it == ( *m_grid )( m_column[0], m_column[1] ).end() )
184  {
185  ++m_column[1];
186  if( m_column[1] >= std::size_t( m_grid->cols() ) )
187  {
188  ++m_column[0];
189  if( m_column[0] >= std::size_t( m_grid->rows() ) ) { return *this; }
190  m_column[1] = 0;
191  }
192  m_it = ( *m_grid )( m_column[0], m_column[1] ).begin();
193  }
194  return *this;
195  }
196 
197  const_iterator() : m_column( 0, 0 ) {}
198 
199  protected:
200  friend class pin_screen< T >;
201  friend class pin_screen< T >::iterator;
202  const GridType* m_grid;
203  size_type m_column;
204  typename pin_screen< T >::column_type::const_iterator m_it;
205 };
206 
208 template < typename T >
209 class pin_screen< T >::iterator
210 {
211  public:
213  typedef T value_type;
214 
216  typedef typename pin_screen< T >::index_type index_type;
217 
219  typedef typename pin_screen< T >::size_type size_type;
220 
222  enum { Dimensions = 3 };
223 
225  bool operator==( const iterator& rhs ) const { return m_grid == rhs.m_grid && m_column == rhs.m_column && m_it == rhs.m_it; }
226  bool operator!=( const iterator& rhs ) const { return !operator==( rhs ); }
227  bool operator<( const const_iterator& rhs ) const
228  {
229  assert( m_grid == rhs.m_grid );
230  if (m_column[0] < rhs.m_column[0]) return true;
231  if (m_column[0] > rhs.m_column[0]) return false;
232 
233  if (m_column[1] < rhs.m_column[1]) return true;
234  if (m_column[1] > rhs.m_column[1]) return false;
235 
236  if (m_it->first < rhs.m_it->first) return true;
237  if (m_it->first > rhs.m_it->first) return false;
238 
239  return false;
240 
241  // partially ordered operator, which does not seem to work for RB tree containers
242  //return m_column[0] < rhs.m_column[0] || m_column[1] < rhs.m_column[1] || m_it->first < rhs.m_it->first;
243  }
244  T& operator*() { return m_it->second; }
245  T* operator->() { return &m_it->second; }
246  const T& operator*() const { return m_it->second; }
247  const T* operator->() const { return &m_it->second; }
248  index_type operator()() const { return index_type( m_column[0], m_column[1], m_it->first ); }
249  const iterator& operator++()
250  {
251  if( m_it != ( *m_grid )( m_column[0], m_column[1] ).end() ) { ++m_it; }
252  while( m_it == ( *m_grid )( m_column[0], m_column[1] ).end() )
253  {
254  ++m_column[1];
255  if( m_column[1] >= std::size_t( m_grid->cols() ) )
256  {
257  ++m_column[0];
258  if( m_column[0] >= std::size_t( m_grid->rows() ) ) { return *this; }
259  m_column[1] = 0;
260  }
261  m_it = ( *m_grid )( m_column[0], m_column[1] ).begin();
262  }
263  return *this;
264  }
265 
266  operator const_iterator() const
267  {
268  const_iterator it;
269  it.m_grid = m_grid;
270  it.m_column = m_column;
271  it.m_it = m_it;
272  return it;
273  }
274 
275  iterator() : m_column( 0, 0 ) {}
276 
277  protected:
278  friend class pin_screen< T >;
279  GridType* m_grid;
280  size_type m_column;
281  typename pin_screen< T >::column_type::iterator m_it;
282 };
283 
284 template < typename T >
285 inline typename pin_screen< T >::iterator pin_screen< T >::begin()
286 {
287  iterator it;
288  it.m_grid = &m_grid;
289  it.m_it = m_grid( 0, 0 ).begin();
290  if( m_grid( 0, 0 ).empty() ) { ++it; }
291  return it;
292 }
293 
294 template < typename T >
295 inline typename pin_screen< T >::const_iterator pin_screen< T >::begin() const
296 {
297  const_iterator it;
298  it.m_grid = &m_grid;
299  it.m_it = m_grid( 0, 0 ).begin();
300  if( m_grid( 0, 0 ).empty() ) { ++it; }
301  return it;
302 }
303 
304 template < typename T >
305 inline typename pin_screen< T >::iterator pin_screen< T >::end()
306 {
307  iterator it;
308  it.m_grid = &m_grid;
309  it.m_column = size_type( m_grid.rows(), m_grid.cols() );
310  it.m_it = m_grid( m_grid.rows() - 1, m_grid.cols() - 1 ).end();
311  return it;
312 }
313 
314 template < typename T >
315 inline typename pin_screen< T >::const_iterator pin_screen< T >::end() const
316 {
317  const_iterator it;
318  it.m_grid = &m_grid;
319  it.m_column = size_type( m_grid.rows(), m_grid.cols() );
320  it.m_it = m_grid( m_grid.rows() - 1, m_grid.cols() - 1 ).end();
321  return it;
322 }
323 
325 template < typename T >
326 class pin_screen< T >::neighbourhood_iterator : public pin_screen< T >::iterator
327 {
328  public:
330  typedef typename pin_screen< T >::neighbourhood_iterator iterator;
331 
333  typedef typename pin_screen< T >::iterator::index_type index_type;
334 
336  const neighbourhood_iterator& operator++();
337 
339  //const iterator& Centre() const { return m_center; }
340 
342  static neighbourhood_iterator begin( const typename pin_screen< T >::iterator& center );
343 
345  static neighbourhood_iterator end( const typename pin_screen< T >::iterator& center );
346 
347  private:
348  //iterator m_center;
349  index_type m_center;
350  index_type m_begin;
351  index_type m_end;
352  using pin_screen< T >::iterator::m_grid;
353  using pin_screen< T >::iterator::m_column;
354  using pin_screen< T >::iterator::m_it;
355  void Init( const typename pin_screen< T >::iterator& center );
356 };
357 
358 template < typename T >
359 inline void pin_screen< T >::neighbourhood_iterator::Init( const typename pin_screen< T >::iterator& center )
360 {
361  m_grid = center.m_grid;
362  m_center = center();
363  m_begin[0] = m_center[0] - ( m_center[0] > 0 ? 1 : 0 );
364  m_begin[1] = m_center[1] - ( m_center[1] > 0 ? 1 : 0 );
365  m_begin[2] = m_center[2] - ( m_center[2] > 0 ? 1 : 0 );
366  m_end[0] = m_center[0] + 1 + ( m_center[0] < std::size_t( m_grid->rows() ) - 1 ? 1 : 0 );
367  m_end[1] = m_center[1] + 1 + ( m_center[1] < std::size_t( m_grid->cols() ) - 1 ? 1 : 0 );
368  m_end[2] = m_center[2] + 1 + 1; // pin screen can grow upwards without limits
369 }
370 
371 template < typename T >
372 inline const typename pin_screen< T >::neighbourhood_iterator& pin_screen< T >::neighbourhood_iterator::operator++()
373 {
374  if( m_it != ( *m_grid )( m_column[0], m_column[1] ).end() && m_it->first < m_end[2] ) { ++m_it; }
375  while( m_it == ( *m_grid )( m_column[0], m_column[1] ).end() || m_it->first >= m_end[2] || this->operator()() == m_center )
376  {
377  ++m_column[1];
378  if( m_column[1] >= m_end[1] )
379  {
380  ++m_column[0];
381  if( m_column[0] >= m_end[0] )
382  {
383  m_it = ( *( m_grid ) )( m_end[0] - 1, m_end[1] - 1 ).end();
384  return *this;
385  }
386  m_column[1] = m_begin[1];
387  }
388  m_it = m_begin[2] == 0
389  ? ( *m_grid )( m_column[0], m_column[1] ).begin()
390  : ( *m_grid )( m_column[0], m_column[1] ).upper_bound( m_begin[2] - 1 );
391  }
392  return *this;
393 }
394 
395 template < typename T >
396 inline typename pin_screen< T >::neighbourhood_iterator pin_screen< T >::neighbourhood_iterator::begin( const typename pin_screen< T >::iterator& center )
397 {
398  neighbourhood_iterator it;
399  it.Init( center );
400  for( it.m_column[0] = it.m_begin[0]; it.m_column[0] < it.m_end[0]; ++it.m_column[0] )
401  {
402  for( it.m_column[1] = it.m_begin[1]; it.m_column[1] < it.m_end[1]; ++it.m_column[1] )
403  {
404  if( it.m_begin[2] == 0 )
405  {
406  it.m_it = ( *( it.m_grid ) )( it.m_column[0], it.m_column[1] ).begin();
407  }
408  else
409  {
410  it.m_it = ( *( it.m_grid ) )( it.m_column[0], it.m_column[1] ).upper_bound( it.m_begin[2] - 1 );
411  }
412  for( ; it.m_it != ( *( it.m_grid ) )( it.m_column[0], it.m_column[1] ).end() && it.m_it->first < it.m_end[2]; ++it.m_it )
413  {
414  if( it() != it.m_center ) { return it; }
415  }
416  }
417  }
418  it.m_it = ( *( it.m_grid ) )( it.m_end[0] - 1, it.m_end[1] - 1 ).end();
419  return it;
420 }
421 
422 template < typename T >
423 inline typename pin_screen< T >::neighbourhood_iterator pin_screen< T >::neighbourhood_iterator::end( const typename pin_screen< T >::iterator& center )
424 {
425  neighbourhood_iterator it;
426  it.Init( center );
427  it.m_column[0] = it.m_end[0];
428  it.m_column[1] = it.m_end[1];
429  it.m_it = ( *( it.m_grid ) )( it.m_end[0] - 1, it.m_end[1] - 1 ).end();
430  return it;
431 }
432 
433 template < typename T >
434 inline pin_screen< T >::pin_screen( std::size_t size1 , std::size_t size2 )
435  : m_grid( size1, size2 )
436 {
437 }
438 
439 template < typename T >
440 inline pin_screen< T >::pin_screen( typename pin_screen< T >::size_type size )
441  : m_grid( size[0], size[1] )
442 {
443 }
444 
445 template < typename T >
446 inline std::size_t pin_screen< T >::height( std::size_t i , std::size_t j ) const
447 {
448  return m_grid( i, j ).empty() ? 0 : m_grid( i, j ).rbegin()->first;
449 }
450 
451 template < typename T >
452 inline bool pin_screen< T >::exists( std::size_t i , std::size_t j , std::size_t k ) const
453 {
454  return m_grid( i, j ).find( k ) != m_grid( i, j ).end();
455 }
456 
457 template < typename T >
458 inline T* pin_screen< T >::find( std::size_t i , std::size_t j , std::size_t k )
459 {
460  typename column_type::iterator it( m_grid( i, j ).find( k ) );
461  return it == m_grid( i, j ).end() ? NULL : &it->second;
462 }
463 
464 template < typename T >
465 inline const T* pin_screen< T >::find( std::size_t i, std::size_t j, std::size_t k ) const
466 {
467  typename column_type::const_iterator it( m_grid( i, j ).find( k ) );
468  return it == m_grid( i, j ).end() ? NULL : &it->second;
469 }
470 
471 template < typename T >
472 inline T& pin_screen< T >::operator() ( std::size_t i, std::size_t j, std::size_t k )
473 {
474  return touch( i, j, k );
475 }
476 
477 template < typename T >
478 inline const T& pin_screen< T >::operator() ( std::size_t i, std::size_t j, std::size_t k ) const
479 {
480  return *find( i, j, k );
481 }
482 
483 template< typename T >
484 inline void pin_screen< T >::erase( std::size_t i, std::size_t j, std::size_t k )
485 {
486  m_grid( i, j ).erase( k );
487 }
488 
489 template< typename T >
490 inline void pin_screen< T >::clear()
491 {
492  for( std::size_t i = 0; i < m_grid.rows(); ++i )
493  {
494  for( std::size_t j = 0; j < m_grid.cols(); ++j )
495  {
496  m_grid( i, j ).clear();
497  }
498  }
499 }
500 
501 } // namespace snark {
502 
503 #endif // #ifndef SNARK_PERCEPTION_PIN_SCREEN_HEADER_GUARD_
Generated on Fri Nov 16 2012 12:42:42 for snark by   doxygen 1.8.1.2