simple_octree.h
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2012, Willow Garage, 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 Willow Garage, Inc. 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.
*
*
*/
/*
* simple_octree.h
*
* Created on: Mar 11, 2013
* Author: papazov
*/
#ifndef SIMPLE_OCTREE_H_
#define SIMPLE_OCTREE_H_
#include <pcl/pcl_exports.h>
#include <set>
namespace pcl
{
namespace recognition
{
template<typename NodeData, typename NodeDataCreator, typename Scalar = float>
class PCL_EXPORTS SimpleOctree
{
public:
class Node
{
public:
Node ();
virtual~ Node ();
inline void
setCenter (const Scalar *c);
inline void
setBounds (const Scalar *b);
inline const Scalar*
getCenter () const { return center_;}
inline const Scalar*
getBounds () const { return bounds_;}
inline void
getBounds (Scalar b[6]) const { memcpy (b, bounds_, 6*sizeof (Scalar));}
inline Node*
getChild (int id) { return &children_[id];}
inline Node*
getChildren () { return children_;}
inline void
setData (const NodeData& src){ *data_ = src;}
inline NodeData&
getData (){ return *data_;}
inline const NodeData&
getData () const { return *data_;}
inline Node*
getParent (){ return parent_;}
inline float
getRadius () const { return radius_;}
inline bool
hasData (){ return static_cast<bool> (data_);}
inline bool
hasChildren (){ return static_cast<bool> (children_);}
inline const std::set<Node*>&
getNeighbors () const { return (full_leaf_neighbors_);}
inline void
deleteChildren ();
inline void
deleteData ();
friend class SimpleOctree;
protected:
void
setData (NodeData* data){ if ( data_ ) delete data_; data_ = data;}
inline bool
createChildren ();
/** \brief Make this and 'node' neighbors by inserting each node in the others node neighbor set. Nothing happens
* of either of the nodes has no data. */
inline void
makeNeighbors (Node* node);
inline void
setParent (Node* parent){ parent_ = parent;}
/** \brief Computes the "radius" of the node which is half the diagonal length. */
inline void
computeRadius ();
protected:
NodeData *data_;
Scalar center_[3], bounds_[6];
Node *parent_, *children_;
Scalar radius_;
std::set<Node*> full_leaf_neighbors_;
};
public:
SimpleOctree ();
virtual ~SimpleOctree ();
void
clear ();
/** \brief Creates an empty octree with bounds at least as large as the ones provided as input and with leaf
* size equal to 'voxel_size'. */
void
build (const Scalar* bounds, Scalar voxel_size, NodeDataCreator* node_data_creator);
/** \brief Creates the leaf containing p = (x, y, z) and returns a pointer to it, however, only if p lies within
* the octree bounds! A more general version which allows p to be out of bounds is not implemented yet. The method
* returns NULL if p is not within the root bounds. If the leaf containing p already exists nothing happens and
* method just returns a pointer to the leaf. Note that for a new created leaf, the method also creates its data
* object. */
inline Node*
createLeaf (Scalar x, Scalar y, Scalar z);
/** \brief Since the leaves are aligned in a rectilinear grid, each leaf has a unique id. The method returns the full
* leaf, i.e., the one having a data object, with id [i, j, k] or NULL is no such leaf exists. */
inline Node*
getFullLeaf (int i, int j, int k);
/** \brief Returns a pointer to the full leaf, i.e., one having a data pbject, containing p = (x, y, z) or NULL if no such leaf exists. */
inline Node*
getFullLeaf (Scalar x, Scalar y, Scalar z);
inline std::vector<Node*>&
getFullLeaves () { return full_leaves_;}
inline const std::vector<Node*>&
getFullLeaves () const { return full_leaves_;}
inline Node*
getRoot (){ return root_;}
inline const Scalar*
getBounds () const { return (bounds_);}
inline void
getBounds (Scalar b[6]) const { memcpy (b, bounds_, 6*sizeof (Scalar));}
inline Scalar
getVoxelSize () const { return voxel_size_;}
protected:
inline void
insertNeighbors (Node* node);
protected:
Scalar voxel_size_, bounds_[6];
int tree_levels_;
Node* root_;
std::vector<Node*> full_leaves_;
NodeDataCreator* node_data_creator_;
};
} // namespace recognition
} // namespace pcl
#include <pcl/recognition/impl/ransac_based/simple_octree.hpp>
#endif /* SIMPLE_OCTREE_H_ */
pcl::recognition::SimpleOctree::root_
Node * root_
Definition: simple_octree.h:202
pcl::recognition::SimpleOctree::getFullLeaves
const std::vector< Node * > & getFullLeaves() const
Definition: simple_octree.h:181
pcl::recognition::SimpleOctree::Node::setData
void setData(NodeData *data)
Definition: simple_octree.h:122
pcl::recognition::SimpleOctree::Node
Definition: simple_octree.h:60
pcl::recognition::SimpleOctree::tree_levels_
int tree_levels_
Definition: simple_octree.h:201
pcl
Definition: cloud_iterator.h:46
pcl::recognition::SimpleOctree::Node::full_leaf_neighbors_
std::set< Node * > full_leaf_neighbors_
Definition: simple_octree.h:144
pcl::recognition::SimpleOctree::Node::getCenter
const Scalar * getCenter() const
Definition: simple_octree.h:74
pcl::recognition::SimpleOctree::Node::getRadius
float getRadius() const
Definition: simple_octree.h:101
pcl::recognition::SimpleOctree::Node::setData
void setData(const NodeData &src)
Definition: simple_octree.h:89
pcl::recognition::SimpleOctree::Node::getData
NodeData & getData()
Definition: simple_octree.h:92
pcl::recognition::SimpleOctree::getRoot
Node * getRoot()
Definition: simple_octree.h:184
pcl::recognition::SimpleOctree::getVoxelSize
Scalar getVoxelSize() const
Definition: simple_octree.h:193
pcl::recognition::SimpleOctree::getBounds
const Scalar * getBounds() const
Definition: simple_octree.h:187
pcl::recognition::SimpleOctree::Node::getData
const NodeData & getData() const
Definition: simple_octree.h:95
pcl::recognition::SimpleOctree::node_data_creator_
NodeDataCreator * node_data_creator_
Definition: simple_octree.h:204
pcl::recognition::SimpleOctree::Node::getBounds
void getBounds(Scalar b[6]) const
Definition: simple_octree.h:80
pcl::recognition::SimpleOctree::Node::getChild
Node * getChild(int id)
Definition: simple_octree.h:83
pcl::recognition::SimpleOctree::Node::hasData
bool hasData()
Definition: simple_octree.h:104
pcl::recognition::SimpleOctree::Node::hasChildren
bool hasChildren()
Definition: simple_octree.h:107
pcl::recognition::SimpleOctree::Node::setParent
void setParent(Node *parent)
Definition: simple_octree.h:133
pcl::recognition::SimpleOctree::Node::data_
NodeData * data_
Definition: simple_octree.h:140
pcl::recognition::SimpleOctree::Node::getChildren
Node * getChildren()
Definition: simple_octree.h:86
pcl::recognition::SimpleOctree::voxel_size_
Scalar voxel_size_
Definition: simple_octree.h:200
pcl::recognition::SimpleOctree::Node::getBounds
const Scalar * getBounds() const
Definition: simple_octree.h:77
pcl::recognition::SimpleOctree
Definition: simple_octree.h:57
pcl::recognition::SimpleOctree::Node::parent_
Node * parent_
Definition: simple_octree.h:142
pcl::recognition::SimpleOctree::full_leaves_
std::vector< Node * > full_leaves_
Definition: simple_octree.h:203
pcl::recognition::SimpleOctree::getFullLeaves
std::vector< Node * > & getFullLeaves()
Definition: simple_octree.h:178
pcl::recognition::SimpleOctree::Node::getNeighbors
const std::set< Node * > & getNeighbors() const
Definition: simple_octree.h:110
pcl::recognition::SimpleOctree::getBounds
void getBounds(Scalar b[6]) const
Definition: simple_octree.h:190
pcl::recognition::SimpleOctree::Node::getParent
Node * getParent()
Definition: simple_octree.h:98
pcl::recognition::SimpleOctree::Node::radius_
Scalar radius_
Definition: simple_octree.h:143