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