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