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