Point Cloud Library (PCL)  1.11.0
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 #pragma once
39 
40 #ifdef __GNUC__
41 #pragma GCC system_header
42 #endif
43 
44 #include <pcl/common/eigen.h>
45 #include <pcl/memory.h>
46 #include <pcl/pcl_macros.h>
47 
48 #include <boost/intrusive/hashtable.hpp>
49 
50 #include <cassert>
51 #include <cmath>
52 #include <cstdio>
53 #include <cstdlib>
54 #include <cstring>
55 #include <map>
56 #include <vector>
57 
58 namespace pcl {
59 
60 /** Implementation of a high-dimensional gaussian filtering using the permutohedral
61  * lattice.
62  *
63  * \author Christian Potthast (potthast@usc.edu)
64  *
65  * Adams_fasthigh-dimensional
66  * author = {Andrew Adams and Jongmin Baek and Myers Abraham Davis},
67  * title = {Fast high-dimensional filtering using the permutohedral lattice},
68  * booktitle = {Computer Graphics Forum (EG 2010 Proceedings},
69  * year = {},
70  * pages = {2010}
71  * }
72  */
73 
75 protected:
76  struct Neighbors {
77  int n1, n2;
78  Neighbors(int n1 = 0, int n2 = 0) : n1(n1), n2(n2) {}
79  };
80 
81 public:
82  /** Constructor for Permutohedral class. */
83  Permutohedral();
84 
85  /** Deconstructor for Permutohedral class. */
87 
88  /** Initialization. */
89  void
90  init(const std::vector<float>& feature, const int feature_dimension, const int N);
91 
92  void
93  compute(std::vector<float>& out,
94  const std::vector<float>& in,
95  int value_size,
96  int in_offset = 0,
97  int out_offset = 0,
98  int in_size = -1,
99  int out_size = -1) const;
100 
101  void
102  initOLD(const std::vector<float>& feature, const int feature_dimension, const int N);
103 
104  void
105  computeOLD(std::vector<float>& out,
106  const std::vector<float>& in,
107  int value_size,
108  int in_offset = 0,
109  int out_offset = 0,
110  int in_size = -1,
111  int out_size = -1) const;
112 
113  void
114  debug();
115 
116  /** Pseudo radnom generator. */
117  inline std::size_t
118  generateHashKey(const std::vector<short>& k)
119  {
120  std::size_t r = 0;
121  for (int i = 0; i < d_; i++) {
122  r += k[i];
123  r *= 1664525;
124  // r *= 5;
125  }
126  return r; // % (2* N_ * (d_+1));
127  }
128 
129 public:
130  /// Number of variables
131  int N_;
132 
133  std::vector<Neighbors> blur_neighbors_;
134 
135  /// Size of sparse discretized space
136  int M_;
137 
138  /// Dimension of feature
139  int d_;
140 
141  std::vector<float> offset_;
142  std::vector<float> offsetTMP_;
143  std::vector<float> barycentric_;
144 
148  std::vector<float> baryOLD_;
149 
150 public:
152 };
153 
155  // Don't copy!
156  HashTableOLD(const HashTableOLD& o)
158  {
159  table_ = new int[capacity_];
160  keys_ = new short[(capacity_ / 2 + 10) * key_size_];
161  memset(table_, -1, capacity_ * sizeof(int));
162  }
163 
164 protected:
165  std::size_t key_size_, filled_, capacity_;
166  short* keys_;
167  int* table_;
168 
169  void
171  {
172  std::cout << "GROW" << std::endl;
173 
174  // Swap out the old memory
175  short* old_keys = keys_;
176  int* old_table = table_;
177  int old_capacity = static_cast<int>(capacity_);
178  capacity_ *= 2;
179  // Allocate the new memory
180  keys_ = new short[(old_capacity + 10) * key_size_];
181  table_ = new int[capacity_];
182  memset(table_, -1, capacity_ * sizeof(int));
183  memcpy(keys_, old_keys, filled_ * key_size_ * sizeof(short));
184 
185  // Reinsert each element
186  for (int i = 0; i < old_capacity; i++)
187  if (old_table[i] >= 0) {
188  int e = old_table[i];
189  std::size_t h = hash(old_keys + (getKey(e) - keys_)) % capacity_;
190  for (; table_[h] >= 0; h = h < capacity_ - 1 ? h + 1 : 0) {
191  };
192  table_[h] = e;
193  }
194 
195  delete[] old_keys;
196  delete[] old_table;
197  }
198 
199  std::size_t
200  hash(const short* k)
201  {
202  std::size_t r = 0;
203  for (std::size_t i = 0; i < key_size_; i++) {
204  r += k[i];
205  r *= 1664525;
206  }
207  return r;
208  }
209 
210 public:
211  explicit HashTableOLD(int key_size, int n_elements)
212  : key_size_(key_size), filled_(0), capacity_(2 * n_elements)
213  {
214  table_ = new int[capacity_];
215  keys_ = new short[(capacity_ / 2 + 10) * key_size_];
216  memset(table_, -1, capacity_ * sizeof(int));
217  }
218 
220  {
221  delete[] keys_;
222  delete[] table_;
223  }
224 
225  int
226  size() const
227  {
228  return static_cast<int>(filled_);
229  }
230 
231  void
233  {
234  filled_ = 0;
235  memset(table_, -1, capacity_ * sizeof(int));
236  }
237 
238  int
239  find(const short* k, bool create = false)
240  {
241  if (2 * filled_ >= capacity_)
242  grow();
243  // Get the hash value
244  std::size_t h = hash(k) % capacity_;
245  // Find the element with he right key, using linear probing
246  while (1) {
247  int e = table_[h];
248  if (e == -1) {
249  if (create) {
250  // Insert a new key and return the new id
251  for (std::size_t i = 0; i < key_size_; i++)
252  keys_[filled_ * key_size_ + i] = k[i];
253  return table_[h] = static_cast<int>(filled_++);
254  }
255  else
256  return -1;
257  }
258  // Check if the current key is The One
259  bool good = true;
260  for (std::size_t i = 0; i < key_size_ && good; i++)
261  if (keys_[e * key_size_ + i] != k[i])
262  good = false;
263  if (good)
264  return e;
265  // Continue searching
266  h++;
267  if (h == capacity_)
268  h = 0;
269  }
270  }
271 
272  const short*
273  getKey(int i) const
274  {
275  return keys_ + i * key_size_;
276  }
277 };
278 
279 /*
280 class HashTable
281 {
282  public:
283  HashTable ( int N ) : N_ (2 * N) {};
284 
285  find (const std::vector<short> &k, bool create = false;)
286  {
287 
288 
289 
290 
291 
292  }
293 
294 
295 
296  protected:
297  std::multimap<std::size_t, int> table_;
298 
299  std::vector<std::vector<short> > keys;
300  //keys.reserve ( (d_+1) * N_ );
301  // number of elements
302  int N_;
303 };*/
304 
305 } // namespace pcl
Implementation of a high-dimensional gaussian filtering using the permutohedral lattice.
Definition: permutohedral.h:74
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
std::vector< Neighbors > blur_neighbors_
void initOLD(const std::vector< float > &feature, const int feature_dimension, const int N)
std::vector< float > offset_
HashTableOLD(int key_size, int n_elements)
std::size_t capacity_
Permutohedral()
Constructor for Permutohedral class.
int size() const
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: memory.h:63
int find(const short *k, bool create=false)
std::vector< float > baryOLD_
int N_
Number of variables.
void init(const std::vector< float > &feature, const int feature_dimension, const int N)
Initialization.
~Permutohedral()
Deconstructor for Permutohedral class.
Definition: permutohedral.h:86
std::vector< float > offsetTMP_
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
std::size_t filled_
std::size_t hash(const short *k)
std::size_t generateHashKey(const std::vector< short > &k)
Pseudo radnom generator.
std::vector< float > barycentric_
Neighbors(int n1=0, int n2=0)
Definition: permutohedral.h:78
const short * getKey(int i) const
int d_
Dimension of feature.
Neighbors * blur_neighborsOLD_
int M_
Size of sparse discretized space.
std::size_t key_size_