Point Cloud Library (PCL)  1.9.1
permutohedral.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2012-, Open Perception, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 
38 #ifndef PCL_ML_PERMUTOHEDRAL_H_
39 #define PCL_ML_PERMUTOHEDRAL_H_
40 
41 #ifdef __GNUC__
42 #pragma GCC system_header
43 #endif
44 
45 #include <vector>
46 #include <map>
47 #include <pcl/common/eigen.h>
48 #include <boost/intrusive/hashtable.hpp>
49 
50 // TODO: SWAP with Boost intrusive hash table
51 #include <cstdlib>
52 #include <cstring>
53 #include <cassert>
54 #include <cstdio>
55 #include <cmath>
56 
57 namespace pcl
58 {
59  /** \brief Implementation of a high-dimensional gaussian filtering using the permutohedral lattice
60  * \author Christian Potthast (potthast@usc.edu)
61  *
62  * Adams_fasthigh-dimensional
63  * author = {Andrew Adams and Jongmin Baek and Myers Abraham Davis},
64  * title = {Fast high-dimensional filtering using the permutohedral lattice},
65  * booktitle = {Computer Graphics Forum (EG 2010 Proceedings},
66  * year = {},
67  * pages = {2010}
68  * }
69  */
71  {
72  protected:
73  struct Neighbors
74  {
75  int n1, n2;
76  Neighbors (int n1 = 0, int n2 = 0) : n1 (n1), n2 (n2) {}
77  };
78 
79  public:
80 
81  /** \brief Constructor for Permutohedral class */
82  Permutohedral ();
83 
84  /** \brief Deconstructor for Permutohedral class */
86 
87  /** \brief initialization */
88  void
89  init (const std::vector<float> &feature, const int feature_dimension, const int N);
90 
91  void
92  compute (std::vector<float> &out, const std::vector<float> &in,
93  int value_size,
94  int in_offset=0, int out_offset=0,
95  int in_size = -1, int out_size = -1) const;
96  void
97  initOLD (const std::vector<float> &feature, const int feature_dimension, const int N);
98 
99  void
100  computeOLD (std::vector<float> &out, const std::vector<float> &in,
101  int value_size,
102  int in_offset=0, int out_offset=0,
103  int in_size = -1, int out_size = -1) const;
104 
105  void
106  debug ();
107 
108  // Pseudo radnom generator
109  inline
110  size_t generateHashKey (const std::vector<short> &k)
111  {
112  size_t r = 0;
113  for (int i = 0; i < d_; i++)
114  {
115  r += k[i];
116  r *= 1664525;
117  //r *= 5;
118  }
119  return r;// % (2* N_ * (d_+1));
120  }
121 
122  public:
123 
124  /** \brief Number of variables */
125  int N_;
126 
127  std::vector<Neighbors> blur_neighbors_;
128 
129  /** \brief size of sparse discretized space */
130  int M_;
131 
132  /** \brief dimension of feature */
133  int d_;
134 
135  std::vector<float> offset_;
136  std::vector<float> offsetTMP_;
137  std::vector<float> barycentric_;
138 
140  int * offsetOLD_;
142  std::vector<float> baryOLD_;
143 
144  public:
145  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
146 
147  };
148 
150  {
151  // Don't copy!
153  table_ = new int[ capacity_ ];
154  keys_ = new short[ (capacity_/2+10)*key_size_ ];
155  memset( table_, -1, capacity_*sizeof(int) );
156  }
157  protected:
159  short * keys_;
160  int * table_;
161  void grow(){
162  std::cout << "GROW" << std::endl;
163 
164  // Swap out the old memory
165  short * old_keys = keys_;
166  int * old_table = table_;
167  int old_capacity = static_cast<int> (capacity_);
168  capacity_ *= 2;
169  // Allocate the new memory
170  keys_ = new short[ (old_capacity+10)*key_size_ ];
171  table_ = new int[ capacity_ ];
172  memset( table_, -1, capacity_*sizeof(int) );
173  memcpy( keys_, old_keys, filled_*key_size_*sizeof(short) );
174 
175  // Reinsert each element
176  for( int i=0; i<old_capacity; i++ )
177  if (old_table[i] >= 0){
178  int e = old_table[i];
179  size_t h = hash( old_keys+(getKey(e)-keys_) ) % capacity_;
180  for (; table_[h] >= 0; h = h<capacity_-1 ? h+1 : 0) { };
181  table_[h] = e;
182  }
183 
184  delete [] old_keys;
185  delete [] old_table;
186  }
187  size_t hash( const short * k ) {
188  size_t r = 0;
189  for( size_t i=0; i<key_size_; i++ ){
190  r += k[i];
191  r *= 1664525;
192  }
193  return r;
194  }
195  public:
196  explicit HashTableOLD( int key_size, int n_elements ) : key_size_ ( key_size ), filled_(0), capacity_(2*n_elements) {
197  table_ = new int[ capacity_ ];
198  keys_ = new short[ (capacity_/2+10)*key_size_ ];
199  memset( table_, -1, capacity_*sizeof(int) );
200  }
202  delete [] keys_;
203  delete [] table_;
204  }
205  int size() const {
206  return static_cast<int> (filled_);
207  }
208  void reset() {
209  filled_ = 0;
210  memset( table_, -1, capacity_*sizeof(int) );
211  }
212  int find( const short * k, bool create = false ){
213  if (2*filled_ >= capacity_) grow();
214  // Get the hash value
215  size_t h = hash( k ) % capacity_;
216  // Find the element with he right key, using linear probing
217  while(1){
218  int e = table_[h];
219  if (e==-1){
220  if (create){
221  // Insert a new key and return the new id
222  for( size_t i=0; i<key_size_; i++ )
223  keys_[ filled_*key_size_+i ] = k[i];
224  return table_[h] = static_cast<int> (filled_++);
225  }
226  else
227  return -1;
228  }
229  // Check if the current key is The One
230  bool good = true;
231  for( size_t i=0; i<key_size_ && good; i++ )
232  if (keys_[ e*key_size_+i ] != k[i])
233  good = false;
234  if (good)
235  return e;
236  // Continue searching
237  h++;
238  if (h==capacity_) h = 0;
239  }
240  }
241  const short * getKey( int i ) const{
242  return keys_+i*key_size_;
243  }
244  };
245  /*
246  class HashTable
247  {
248  public:
249  HashTable ( int N ) : N_ (2 * N) {};
250 
251  find (const std::vector<short> &k, bool create = false;)
252  {
253 
254 
255 
256 
257 
258  }
259 
260 
261 
262  protected:
263  std::multimap<size_t, int> table_;
264 
265  std::vector<std::vector<short> > keys;
266  //keys.reserve ( (d_+1) * N_ );
267  // number of elements
268  int N_;
269  };*/
270 
271 }
272 
273 #endif
pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
pcl::Permutohedral::Neighbors
Definition: permutohedral.h:73
pcl::HashTableOLD::hash
size_t hash(const short *k)
Definition: permutohedral.h:187
pcl::Permutohedral::barycentric_
std::vector< float > barycentric_
Definition: permutohedral.h:137
pcl::HashTableOLD::keys_
short * keys_
Definition: permutohedral.h:159
pcl::Permutohedral::computeOLD
void computeOLD(std::vector< float > &out, const std::vector< float > &in, int value_size, int in_offset=0, int out_offset=0, int in_size=-1, int out_size=-1) const
pcl::Permutohedral::offsetOLD_
int * offsetOLD_
Definition: permutohedral.h:140
pcl::HashTableOLD::HashTableOLD
HashTableOLD(int key_size, int n_elements)
Definition: permutohedral.h:196
pcl::Permutohedral::init
void init(const std::vector< float > &feature, const int feature_dimension, const int N)
initialization
pcl::Permutohedral::barycentricOLD_
float * barycentricOLD_
Definition: permutohedral.h:141
pcl::Permutohedral::debug
void debug()
pcl::HashTableOLD::filled_
size_t filled_
Definition: permutohedral.h:158
pcl::Permutohedral::blur_neighborsOLD_
Neighbors * blur_neighborsOLD_
Definition: permutohedral.h:139
pcl::Permutohedral::Neighbors::Neighbors
Neighbors(int n1=0, int n2=0)
Definition: permutohedral.h:76
pcl::Permutohedral::~Permutohedral
~Permutohedral()
Deconstructor for Permutohedral class.
Definition: permutohedral.h:85
pcl::Permutohedral::Permutohedral
Permutohedral()
Constructor for Permutohedral class.
pcl::Permutohedral::Neighbors::n2
int n2
Definition: permutohedral.h:75
pcl::HashTableOLD::reset
void reset()
Definition: permutohedral.h:208
pcl::HashTableOLD::getKey
const short * getKey(int i) const
Definition: permutohedral.h:241
pcl::Permutohedral::d_
int d_
dimension of feature
Definition: permutohedral.h:133
pcl::Permutohedral::compute
void compute(std::vector< float > &out, const std::vector< float > &in, int value_size, int in_offset=0, int out_offset=0, int in_size=-1, int out_size=-1) const
pcl::Permutohedral::baryOLD_
std::vector< float > baryOLD_
Definition: permutohedral.h:142
pcl::Permutohedral::generateHashKey
size_t generateHashKey(const std::vector< short > &k)
Definition: permutohedral.h:110
pcl::Permutohedral::N_
int N_
Number of variables.
Definition: permutohedral.h:125
pcl::HashTableOLD::find
int find(const short *k, bool create=false)
Definition: permutohedral.h:212
pcl::HashTableOLD::capacity_
size_t capacity_
Definition: permutohedral.h:158
pcl::HashTableOLD
Definition: permutohedral.h:149
pcl::HashTableOLD::~HashTableOLD
~HashTableOLD()
Definition: permutohedral.h:201
pcl::HashTableOLD::key_size_
size_t key_size_
Definition: permutohedral.h:158
pcl::Permutohedral::initOLD
void initOLD(const std::vector< float > &feature, const int feature_dimension, const int N)
pcl::HashTableOLD::table_
int * table_
Definition: permutohedral.h:160
pcl::Permutohedral::M_
int M_
size of sparse discretized space
Definition: permutohedral.h:130
pcl::Permutohedral::blur_neighbors_
std::vector< Neighbors > blur_neighbors_
Definition: permutohedral.h:127
pcl::HashTableOLD::grow
void grow()
Definition: permutohedral.h:161
pcl::HashTableOLD::size
int size() const
Definition: permutohedral.h:205
pcl::Permutohedral::offset_
std::vector< float > offset_
Definition: permutohedral.h:135
pcl::Permutohedral::Neighbors::n1
int n1
Definition: permutohedral.h:75
pcl::Permutohedral::offsetTMP_
std::vector< float > offsetTMP_
Definition: permutohedral.h:136
pcl::Permutohedral
Implementation of a high-dimensional gaussian filtering using the permutohedral lattice.
Definition: permutohedral.h:70