octree_pointcloud_adjacency.h
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2012, Jeremie Papon
*
* 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.
*
* Author : jpapon@gmail.com
* Email : jpapon@gmail.com
*/
#pragma once
#include <pcl/octree/boost.h>
#include <pcl/octree/octree_pointcloud.h>
#include <pcl/octree/octree_pointcloud_adjacency_container.h>
#include <set>
#include <list>
namespace pcl
{
namespace octree
{
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b Octree pointcloud voxel class which maintains adjacency information for its voxels.
*
* This pointcloud octree class generates an octree from a point cloud (zero-copy). The octree pointcloud is
* initialized with its voxel resolution. Its bounding box is automatically adjusted or can be predefined.
*
* The OctreePointCloudAdjacencyContainer class can be used to store data in leaf nodes.
*
* An optional transform function can be provided which changes how the voxel grid is computed - this can be used to,
* for example, make voxel bins larger as they increase in distance from the origin (camera).
* \note See SupervoxelClustering for an example of how to provide a transform function.
*
* If used in academic work, please cite:
*
* - J. Papon, A. Abramov, M. Schoeler, F. Woergoetter
* Voxel Cloud Connectivity Segmentation - Supervoxels from PointClouds
* In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR) 2013
*
* \ingroup octree
* \author Jeremie Papon (jpapon@gmail.com) */
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT,
typename LeafContainerT = OctreePointCloudAdjacencyContainer<PointT>,
typename BranchContainerT = OctreeContainerEmpty>
class OctreePointCloudAdjacency : public OctreePointCloud<PointT, LeafContainerT, BranchContainerT>
{
public:
using OctreeBaseT = OctreeBase<LeafContainerT, BranchContainerT>;
using OctreeAdjacencyT = OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT>;
using Ptr = shared_ptr<OctreeAdjacencyT>;
using ConstPtr = shared_ptr<const OctreeAdjacencyT>;
using OctreePointCloudT = OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeBaseT>;
using LeafNode = typename OctreePointCloudT::LeafNode;
using BranchNode = typename OctreePointCloudT::BranchNode;
using PointCloud = pcl::PointCloud<PointT>;
using PointCloudPtr = typename PointCloud::Ptr;
using PointCloudConstPtr = typename PointCloud::ConstPtr;
// BGL graph
using VoxelAdjacencyList = boost::adjacency_list<boost::setS, boost::setS, boost::undirectedS, PointT, float>;
using VoxelID = typename VoxelAdjacencyList::vertex_descriptor;
using EdgeID = typename VoxelAdjacencyList::edge_descriptor;
// Leaf vector - pointers to all leaves
using LeafVectorT = std::vector<LeafContainerT *>;
// Fast leaf iterators that don't require traversing tree
using iterator = typename LeafVectorT::iterator;
using const_iterator = typename LeafVectorT::const_iterator;
inline iterator begin () { return (leaf_vector_.begin ()); }
inline iterator end () { return (leaf_vector_.end ()); }
inline LeafContainerT* at (std::size_t idx) { return leaf_vector_.at (idx); }
// Size of neighbors
inline std::size_t size () const { return leaf_vector_.size (); }
/** \brief Constructor.
*
* \param[in] resolution_arg Octree resolution at lowest octree level (voxel size) */
OctreePointCloudAdjacency (const double resolution_arg);
/** \brief Adds points from cloud to the octree.
*
* \note This overrides addPointsFromInputCloud() from the OctreePointCloud class. */
void
addPointsFromInputCloud ();
/** \brief Gets the leaf container for a given point.
*
* \param[in] point_arg Point to search for
*
* \returns Pointer to the leaf container - null if no leaf container found. */
LeafContainerT*
getLeafContainerAtPoint (const PointT& point_arg) const;
/** \brief Computes an adjacency graph of voxel relations.
*
* \warning This slows down rapidly as cloud size increases due to the number of edges.
*
* \param[out] voxel_adjacency_graph Boost Graph Library Adjacency graph of the voxel touching relationships.
* Vertices are PointT, edges represent touching, and edge lengths are the distance between the points. */
void
computeVoxelAdjacencyGraph (VoxelAdjacencyList &voxel_adjacency_graph);
/** \brief Sets a point transform (and inverse) used to transform the space of the input cloud.
*
* This is useful for changing how adjacency is calculated - such as relaxing the adjacency criterion for
* points further from the camera.
*
* \param[in] transform_func A boost:function pointer to the transform to be used. The transform must have one
* parameter (a point) which it modifies in place. */
void
setTransformFunction (std::function<void (PointT &p)> transform_func)
{
transform_func_ = transform_func;
}
/** \brief Tests whether input point is occluded from specified camera point by other voxels.
*
* \param[in] point_arg Point to test for
* \param[in] camera_pos Position of camera, defaults to origin
*
* \returns True if path to camera is blocked by a voxel, false otherwise. */
bool
testForOcclusion (const PointT& point_arg, const PointXYZ &camera_pos = PointXYZ (0, 0, 0));
protected:
/** \brief Add point at index from input pointcloud dataset to octree.
*
* \param[in] point_idx_arg The index representing the point in the dataset given by setInputCloud() to be added
*
* \note This virtual implementation allows the use of a transform function to compute keys. */
void
addPointIdx (const int point_idx_arg) override;
/** \brief Fills in the neighbors fields for new voxels.
*
* \param[in] key_arg Key of the voxel to check neighbors for
* \param[in] leaf_container Pointer to container of the leaf to check neighbors for */
void
computeNeighbors (OctreeKey &key_arg, LeafContainerT* leaf_container);
/** \brief Generates octree key for specified point (uses transform if provided).
*
* \param[in] point_arg Point to generate key for
* \param[out] key_arg Resulting octree key */
void
genOctreeKeyforPoint (const PointT& point_arg, OctreeKey& key_arg) const;
private:
/** \brief Add point at given index from input point cloud to octree.
*
* Index will be also added to indices vector. This functionality is not enabled for adjacency octree. */
using OctreePointCloudT::addPointFromCloud;
/** \brief Add point simultaneously to octree and input point cloud.
*
* This functionality is not enabled for adjacency octree. */
using OctreePointCloudT::addPointToCloud;
using OctreePointCloudT::input_;
using OctreePointCloudT::resolution_;
using OctreePointCloudT::min_x_;
using OctreePointCloudT::min_y_;
using OctreePointCloudT::min_z_;
using OctreePointCloudT::max_x_;
using OctreePointCloudT::max_y_;
using OctreePointCloudT::max_z_;
/// Local leaf pointer vector used to make iterating through leaves fast.
LeafVectorT leaf_vector_;
std::function<void (PointT &p)> transform_func_;
};
}
}
// Note: Do not precompile this octree type because it is typically used with custom leaf containers.
#include <pcl/octree/impl/octree_pointcloud_adjacency.hpp>
pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
pcl::octree::OctreePointCloudAdjacency::end
iterator end()
Definition: octree_pointcloud_adjacency.h:110
pcl::octree::OctreePointCloudAdjacency::setTransformFunction
void setTransformFunction(std::function< void(PointT &p)> transform_func)
Sets a point transform (and inverse) used to transform the space of the input cloud.
Definition: octree_pointcloud_adjacency.h:152
pcl::octree::OctreePointCloud::resolution_
double resolution_
Octree resolution.
Definition: octree_pointcloud.h:521
pcl::octree::OctreePointCloudAdjacency::computeNeighbors
void computeNeighbors(OctreeKey &key_arg, LeafContainerT *leaf_container)
Fills in the neighbors fields for new voxels.
Definition: octree_pointcloud_adjacency.hpp:160
pcl::octree::OctreePointCloudAdjacency::VoxelID
typename VoxelAdjacencyList::vertex_descriptor VoxelID
Definition: octree_pointcloud_adjacency.h:99
pcl::octree::OctreePointCloud
Octree pointcloud class
Definition: octree_pointcloud.h:72
pcl::octree::OctreePointCloudAdjacency::Ptr
shared_ptr< OctreeAdjacencyT > Ptr
Definition: octree_pointcloud_adjacency.h:86
pcl::octree::OctreeKey
Octree key class
Definition: octree_key.h:50
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: projection_matrix.h:52
pcl::octree::OctreePointCloud::addPointToCloud
void addPointToCloud(const PointT &point_arg, PointCloudPtr cloud_arg)
Add point simultaneously to octree and input point cloud.
Definition: octree_pointcloud.hpp:98
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:623
pcl::octree::OctreePointCloudAdjacency::EdgeID
typename VoxelAdjacencyList::edge_descriptor EdgeID
Definition: octree_pointcloud_adjacency.h:100
pcl::octree::OctreePointCloudAdjacency::addPointsFromInputCloud
void addPointsFromInputCloud()
Adds points from cloud to the octree.
Definition: octree_pointcloud_adjacency.hpp:63
pcl::octree::OctreePointCloud::min_y_
double min_y_
Definition: octree_pointcloud.h:527
pcl::octree::OctreePointCloudAdjacency::size
std::size_t size() const
Definition: octree_pointcloud_adjacency.h:114
pcl::octree::OctreeLeafNode
Abstract octree leaf class
Definition: octree_nodes.h:96
pcl::octree::OctreePointCloudAdjacency::iterator
typename LeafVectorT::iterator iterator
Definition: octree_pointcloud_adjacency.h:106
pcl::octree::OctreePointCloudAdjacency::genOctreeKeyforPoint
void genOctreeKeyforPoint(const PointT &point_arg, OctreeKey &key_arg) const
Generates octree key for specified point (uses transform if provided).
Definition: octree_pointcloud_adjacency.hpp:112
pcl::octree::OctreePointCloudAdjacency::PointCloudConstPtr
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: octree_pointcloud_adjacency.h:95
pcl::octree::OctreeBase
Octree class.
Definition: octree_base.h:61
pcl::octree::OctreePointCloudAdjacency::PointCloudPtr
typename PointCloud::Ptr PointCloudPtr
Definition: octree_pointcloud_adjacency.h:94
pcl::PointXYZ
A point structure representing Euclidean xyz coordinates.
Definition: point_types.hpp:291
pcl::octree::OctreeBranchNode
Abstract octree branch class
Definition: octree_nodes.h:203
pcl::octree::OctreePointCloudAdjacency::LeafVectorT
std::vector< LeafContainerT * > LeafVectorT
Definition: octree_pointcloud_adjacency.h:103
pcl::octree::OctreePointCloud::min_z_
double min_z_
Definition: octree_pointcloud.h:530
pcl::octree::OctreePointCloudAdjacency::addPointIdx
void addPointIdx(const int point_idx_arg) override
Add point at index from input pointcloud dataset to octree.
Definition: octree_pointcloud_adjacency.hpp:141
pcl::octree::OctreePointCloud::max_x_
double max_x_
Definition: octree_pointcloud.h:525
pcl::octree::OctreePointCloud::BranchNode
typename OctreeT::BranchNode BranchNode
Definition: octree_pointcloud.h:78
pcl::octree::OctreePointCloudAdjacency
Octree pointcloud voxel class which maintains adjacency information for its voxels.
Definition: octree_pointcloud_adjacency.h:78
pcl::octree::OctreePointCloudAdjacency::at
LeafContainerT * at(std::size_t idx)
Definition: octree_pointcloud_adjacency.h:111
pcl::octree::OctreePointCloud::addPointFromCloud
void addPointFromCloud(const int point_idx_arg, IndicesPtr indices_arg)
Add point at given index from input point cloud to octree.
Definition: octree_pointcloud.hpp:89
pcl::octree::OctreePointCloud::max_z_
double max_z_
Definition: octree_pointcloud.h:531
pcl::octree::OctreePointCloudAdjacency::begin
iterator begin()
Definition: octree_pointcloud_adjacency.h:109
pcl::octree::OctreePointCloud::max_y_
double max_y_
Definition: octree_pointcloud.h:528
pcl::octree::OctreePointCloudAdjacency::OctreePointCloudAdjacency
OctreePointCloudAdjacency(const double resolution_arg)
Constructor.
Definition: octree_pointcloud_adjacency.hpp:54
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:415
pcl::octree::OctreePointCloudAdjacency::const_iterator
typename LeafVectorT::const_iterator const_iterator
Definition: octree_pointcloud_adjacency.h:107
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:416
pcl::octree::OctreePointCloudAdjacency::getLeafContainerAtPoint
LeafContainerT * getLeafContainerAtPoint(const PointT &point_arg) const
Gets the leaf container for a given point.
Definition: octree_pointcloud_adjacency.hpp:200
pcl::octree::OctreePointCloudAdjacency::testForOcclusion
bool testForOcclusion(const PointT &point_arg, const PointXYZ &camera_pos=PointXYZ(0, 0, 0))
Tests whether input point is occluded from specified camera point by other voxels.
Definition: octree_pointcloud_adjacency.hpp:259
pcl::octree::OctreePointCloudAdjacency::computeVoxelAdjacencyGraph
void computeVoxelAdjacencyGraph(VoxelAdjacencyList &voxel_adjacency_graph)
Computes an adjacency graph of voxel relations.
Definition: octree_pointcloud_adjacency.hpp:215
pcl::octree::OctreePointCloudAdjacency::ConstPtr
shared_ptr< const OctreeAdjacencyT > ConstPtr
Definition: octree_pointcloud_adjacency.h:87
pcl::octree::OctreePointCloud::input_
PointCloudConstPtr input_
Pointer to input point cloud dataset.
Definition: octree_pointcloud.h:512
pcl::octree::OctreePointCloudAdjacency::VoxelAdjacencyList
boost::adjacency_list< boost::setS, boost::setS, boost::undirectedS, PointT, float > VoxelAdjacencyList
Definition: octree_pointcloud_adjacency.h:98
pcl::octree::OctreePointCloud::LeafNode
typename OctreeT::LeafNode LeafNode
Definition: octree_pointcloud.h:77
pcl::shared_ptr
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
Definition: pcl_macros.h:90
pcl::octree::OctreePointCloud::min_x_
double min_x_
Definition: octree_pointcloud.h:524