298 lines
7.1 KiB
C++
298 lines
7.1 KiB
C++
/*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* Point Cloud Library (PCL) - www.pointclouds.org
|
|
* Copyright (c) 2012-, Open Perception, Inc.
|
|
*
|
|
* All rights reserved.
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions
|
|
* are met:
|
|
*
|
|
* * Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* * Redistributions in binary form must reproduce the above
|
|
* copyright notice, this list of conditions and the following
|
|
* disclaimer in the documentation and/or other materials provided
|
|
* with the distribution.
|
|
* * Neither the name of the copyright holder(s) nor the names of its
|
|
* contributors may be used to endorse or promote products derived
|
|
* from this software without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#ifdef __GNUC__
|
|
#pragma GCC system_header
|
|
#endif
|
|
|
|
#include <pcl/memory.h>
|
|
|
|
#include <cstring>
|
|
#include <iostream> // for size_t, operator<<, endl, cout
|
|
#include <vector>
|
|
|
|
namespace pcl {
|
|
|
|
/** Implementation of a high-dimensional gaussian filtering using the permutohedral
|
|
* lattice.
|
|
*
|
|
* \author Christian Potthast (potthast@usc.edu)
|
|
*
|
|
* Adams_fasthigh-dimensional
|
|
* author = {Andrew Adams and Jongmin Baek and Myers Abraham Davis},
|
|
* title = {Fast high-dimensional filtering using the permutohedral lattice},
|
|
* booktitle = {Computer Graphics Forum (EG 2010 Proceedings},
|
|
* year = {},
|
|
* pages = {2010}
|
|
* }
|
|
*/
|
|
|
|
class Permutohedral {
|
|
protected:
|
|
struct Neighbors {
|
|
int n1, n2;
|
|
Neighbors(int n1 = 0, int n2 = 0) : n1(n1), n2(n2) {}
|
|
};
|
|
|
|
public:
|
|
/** Constructor for Permutohedral class. */
|
|
Permutohedral();
|
|
|
|
/** Deconstructor for Permutohedral class. */
|
|
~Permutohedral(){};
|
|
|
|
/** Initialization. */
|
|
void
|
|
init(const std::vector<float>& feature, const int feature_dimension, const int N);
|
|
|
|
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
|
|
initOLD(const std::vector<float>& feature, const int feature_dimension, const int N);
|
|
|
|
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;
|
|
|
|
void
|
|
debug();
|
|
|
|
/** Pseudo radnom generator. */
|
|
inline std::size_t
|
|
generateHashKey(const std::vector<short>& k)
|
|
{
|
|
std::size_t r = 0;
|
|
for (int i = 0; i < d_; i++) {
|
|
r += k[i];
|
|
r *= 1664525;
|
|
// r *= 5;
|
|
}
|
|
return r; // % (2* N_ * (d_+1));
|
|
}
|
|
|
|
public:
|
|
/// Number of variables
|
|
int N_;
|
|
|
|
std::vector<Neighbors> blur_neighbors_;
|
|
|
|
/// Size of sparse discretized space
|
|
int M_;
|
|
|
|
/// Dimension of feature
|
|
int d_;
|
|
|
|
std::vector<float> offset_;
|
|
std::vector<float> offsetTMP_;
|
|
std::vector<float> barycentric_;
|
|
|
|
Neighbors* blur_neighborsOLD_;
|
|
int* offsetOLD_;
|
|
float* barycentricOLD_;
|
|
std::vector<float> baryOLD_;
|
|
|
|
public:
|
|
PCL_MAKE_ALIGNED_OPERATOR_NEW
|
|
};
|
|
|
|
class HashTableOLD {
|
|
// Don't copy!
|
|
HashTableOLD(const HashTableOLD& o)
|
|
: key_size_(o.key_size_), filled_(0), capacity_(o.capacity_)
|
|
{
|
|
table_ = new int[capacity_];
|
|
keys_ = new short[(capacity_ / 2 + 10) * key_size_];
|
|
memset(table_, -1, capacity_ * sizeof(int));
|
|
}
|
|
|
|
protected:
|
|
std::size_t key_size_, filled_, capacity_;
|
|
short* keys_;
|
|
int* table_;
|
|
|
|
void
|
|
grow()
|
|
{
|
|
std::cout << "GROW" << std::endl;
|
|
|
|
// Swap out the old memory
|
|
short* old_keys = keys_;
|
|
int* old_table = table_;
|
|
int old_capacity = static_cast<int>(capacity_);
|
|
capacity_ *= 2;
|
|
// Allocate the new memory
|
|
keys_ = new short[(old_capacity + 10) * key_size_];
|
|
table_ = new int[capacity_];
|
|
memset(table_, -1, capacity_ * sizeof(int));
|
|
memcpy(keys_, old_keys, filled_ * key_size_ * sizeof(short));
|
|
|
|
// Reinsert each element
|
|
for (int i = 0; i < old_capacity; i++)
|
|
if (old_table[i] >= 0) {
|
|
int e = old_table[i];
|
|
std::size_t h = hash(old_keys + (getKey(e) - keys_)) % capacity_;
|
|
for (; table_[h] >= 0; h = h < capacity_ - 1 ? h + 1 : 0) {
|
|
};
|
|
table_[h] = e;
|
|
}
|
|
|
|
delete[] old_keys;
|
|
delete[] old_table;
|
|
}
|
|
|
|
std::size_t
|
|
hash(const short* k)
|
|
{
|
|
std::size_t r = 0;
|
|
for (std::size_t i = 0; i < key_size_; i++) {
|
|
r += k[i];
|
|
r *= 1664525;
|
|
}
|
|
return r;
|
|
}
|
|
|
|
public:
|
|
explicit HashTableOLD(int key_size, int n_elements)
|
|
: key_size_(key_size), filled_(0), capacity_(2 * n_elements)
|
|
{
|
|
table_ = new int[capacity_];
|
|
keys_ = new short[(capacity_ / 2 + 10) * key_size_];
|
|
memset(table_, -1, capacity_ * sizeof(int));
|
|
}
|
|
|
|
~HashTableOLD()
|
|
{
|
|
delete[] keys_;
|
|
delete[] table_;
|
|
}
|
|
|
|
int
|
|
size() const
|
|
{
|
|
return static_cast<int>(filled_);
|
|
}
|
|
|
|
void
|
|
reset()
|
|
{
|
|
filled_ = 0;
|
|
memset(table_, -1, capacity_ * sizeof(int));
|
|
}
|
|
|
|
int
|
|
find(const short* k, bool create = false)
|
|
{
|
|
if (2 * filled_ >= capacity_)
|
|
grow();
|
|
// Get the hash value
|
|
std::size_t h = hash(k) % capacity_;
|
|
// Find the element with he right key, using linear probing
|
|
while (1) {
|
|
int e = table_[h];
|
|
if (e == -1) {
|
|
if (create) {
|
|
// Insert a new key and return the new id
|
|
for (std::size_t i = 0; i < key_size_; i++)
|
|
keys_[filled_ * key_size_ + i] = k[i];
|
|
return table_[h] = static_cast<int>(filled_++);
|
|
}
|
|
else
|
|
return -1;
|
|
}
|
|
// Check if the current key is The One
|
|
bool good = true;
|
|
for (std::size_t i = 0; i < key_size_ && good; i++)
|
|
if (keys_[e * key_size_ + i] != k[i])
|
|
good = false;
|
|
if (good)
|
|
return e;
|
|
// Continue searching
|
|
h++;
|
|
if (h == capacity_)
|
|
h = 0;
|
|
}
|
|
}
|
|
|
|
const short*
|
|
getKey(int i) const
|
|
{
|
|
return keys_ + i * key_size_;
|
|
}
|
|
};
|
|
|
|
/*
|
|
class HashTable
|
|
{
|
|
public:
|
|
HashTable ( int N ) : N_ (2 * N) {};
|
|
|
|
find (const std::vector<short> &k, bool create = false;)
|
|
{
|
|
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
protected:
|
|
std::multimap<std::size_t, int> table_;
|
|
|
|
std::vector<std::vector<short> > keys;
|
|
//keys.reserve ( (d_+1) * N_ );
|
|
// number of elements
|
|
int N_;
|
|
};*/
|
|
|
|
} // namespace pcl
|