Point Cloud Library (PCL) 1.13.1
Loading...
Searching...
No Matches
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 /** 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
104
105 /** Pseudo random 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
118public:
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
139public:
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
153protected:
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
199public:
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/*
269class 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
HashTableOLD(int key_size, int n_elements)
const short * getKey(int i) 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.
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 random 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)