permutohedral.h
/*
* 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 <boost/intrusive/hashtable.hpp>
#include <map>
#include <pcl/common/eigen.h>
#include <vector>
// TODO: SWAP with Boost intrusive hash table
#include <cassert>
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <pcl/pcl_macros.h>
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
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