Loading...
Searching...
No Matches
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/octree_pointcloud.h>
#include <pcl/octree/octree_pointcloud_adjacency_container.h>
#include <boost/graph/adjacency_list.hpp> // for adjacency_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(uindex_t 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::max_x_;
using OctreePointCloudT::max_y_;
using OctreePointCloudT::max_z_;
using OctreePointCloudT::min_x_;
using OctreePointCloudT::min_y_;
using OctreePointCloudT::min_z_;
using OctreePointCloudT::resolution_;
/// Local leaf pointer vector used to make iterating through leaves fast.
LeafVectorT leaf_vector_;
std::function<void(PointT& p)> transform_func_;
};
} // namespace octree
} // namespace pcl
// 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::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition point_cloud.h:173
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition point_cloud.h:413
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition point_cloud.h:414
pcl::octree::OctreeBase
Octree class.
Definition octree_base.h:62
pcl::octree::OctreeKey
Octree key class
Definition octree_key.h:54
pcl::octree::OctreePointCloudAdjacency
Octree pointcloud voxel class which maintains adjacency information for its voxels.
Definition octree_pointcloud_adjacency.h:79
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:173
pcl::octree::OctreePointCloudAdjacency::VoxelID
typename VoxelAdjacencyList::vertex_descriptor VoxelID
Definition octree_pointcloud_adjacency.h:101
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:121
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:270
pcl::octree::OctreePointCloudAdjacency::OctreeAdjacencyT
OctreePointCloudAdjacency< PointT, LeafContainerT, BranchContainerT > OctreeAdjacencyT
Definition octree_pointcloud_adjacency.h:85
pcl::octree::OctreePointCloudAdjacency::PointCloudConstPtr
typename PointCloud::ConstPtr PointCloudConstPtr
Definition octree_pointcloud_adjacency.h:96
pcl::octree::OctreePointCloudAdjacency::Ptr
shared_ptr< OctreeAdjacencyT > Ptr
Definition octree_pointcloud_adjacency.h:86
pcl::octree::OctreePointCloudAdjacency::PointCloudPtr
typename PointCloud::Ptr PointCloudPtr
Definition octree_pointcloud_adjacency.h:95
pcl::octree::OctreePointCloudAdjacency::computeVoxelAdjacencyGraph
void computeVoxelAdjacencyGraph(VoxelAdjacencyList &voxel_adjacency_graph)
Computes an adjacency graph of voxel relations.
Definition octree_pointcloud_adjacency.hpp:224
pcl::octree::OctreePointCloudAdjacency::getLeafContainerAtPoint
LeafContainerT * getLeafContainerAtPoint(const PointT &point_arg) const
Gets the leaf container for a given point.
Definition octree_pointcloud_adjacency.hpp:208
pcl::octree::OctreePointCloudAdjacency::addPointsFromInputCloud
void addPointsFromInputCloud()
Adds points from cloud to the octree.
Definition octree_pointcloud_adjacency.hpp:66
pcl::octree::OctreePointCloudAdjacency::at
LeafContainerT * at(std::size_t idx)
Definition octree_pointcloud_adjacency.h:122
pcl::octree::OctreePointCloudAdjacency::iterator
typename LeafVectorT::iterator iterator
Definition octree_pointcloud_adjacency.h:108
pcl::octree::OctreePointCloudAdjacency::EdgeID
typename VoxelAdjacencyList::edge_descriptor EdgeID
Definition octree_pointcloud_adjacency.h:102
pcl::octree::OctreePointCloudAdjacency::const_iterator
typename LeafVectorT::const_iterator const_iterator
Definition octree_pointcloud_adjacency.h:109
pcl::octree::OctreePointCloudAdjacency::begin
iterator begin()
Definition octree_pointcloud_adjacency.h:112
pcl::octree::OctreePointCloudAdjacency::OctreeBaseT
OctreeBase< LeafContainerT, BranchContainerT > OctreeBaseT
Definition octree_pointcloud_adjacency.h:82
pcl::octree::OctreePointCloudAdjacency::addPointIdx
void addPointIdx(uindex_t point_idx_arg) override
Add point at index from input pointcloud dataset to octree.
Definition octree_pointcloud_adjacency.hpp:150
pcl::octree::OctreePointCloudAdjacency::LeafVectorT
std::vector< LeafContainerT * > LeafVectorT
Definition octree_pointcloud_adjacency.h:105
pcl::octree::OctreePointCloudAdjacency::ConstPtr
shared_ptr< const OctreeAdjacencyT > ConstPtr
Definition octree_pointcloud_adjacency.h:87
pcl::octree::OctreePointCloudAdjacency::end
iterator end()
Definition octree_pointcloud_adjacency.h:117
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:171
pcl::octree::OctreePointCloudAdjacency::PointCloud
pcl::PointCloud< PointT > PointCloud
Definition octree_pointcloud_adjacency.h:94
pcl::octree::OctreePointCloudAdjacency::BranchNode
typename OctreePointCloudT::BranchNode BranchNode
Definition octree_pointcloud_adjacency.h:92
pcl::octree::OctreePointCloudAdjacency::LeafNode
typename OctreePointCloudT::LeafNode LeafNode
Definition octree_pointcloud_adjacency.h:91
pcl::octree::OctreePointCloudAdjacency::size
std::size_t size() const
Definition octree_pointcloud_adjacency.h:129
pcl::octree::OctreePointCloudAdjacency::VoxelAdjacencyList
boost::adjacency_list< boost::setS, boost::setS, boost::undirectedS, PointT, float > VoxelAdjacencyList
Definition octree_pointcloud_adjacency.h:100
pcl::octree::OctreePointCloud
Octree pointcloud class
Definition octree_pointcloud.h:72
pcl::octree::OctreePointCloud::min_y_
double min_y_
Definition octree_pointcloud.h:595
pcl::octree::OctreePointCloud::max_z_
double max_z_
Definition octree_pointcloud.h:599
pcl::octree::OctreePointCloud::input_
PointCloudConstPtr input_
Pointer to input point cloud dataset.
Definition octree_pointcloud.h:580
pcl::octree::OctreePointCloud::max_y_
double max_y_
Definition octree_pointcloud.h:596
pcl::octree::OctreePointCloud::addPointFromCloud
void addPointFromCloud(uindex_t point_idx_arg, IndicesPtr indices_arg)
Add point at given index from input point cloud to octree.
Definition octree_pointcloud.hpp:106
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:120
pcl::octree::OctreePointCloud::LeafNode
typename OctreeT::LeafNode LeafNode
Definition octree_pointcloud.h:76
pcl::octree::OctreePointCloud::min_x_
double min_x_
Definition octree_pointcloud.h:592
pcl::octree::OctreePointCloud::BranchNode
typename OctreeT::BranchNode BranchNode
Definition octree_pointcloud.h:77
pcl::octree::OctreePointCloud::min_z_
double min_z_
Definition octree_pointcloud.h:598
pcl::octree::OctreePointCloud::resolution_
double resolution_
Octree resolution.
Definition octree_pointcloud.h:589
pcl::octree::OctreePointCloud::max_x_
double max_x_
Definition octree_pointcloud.h:593
pcl
Definition convolution.h:46
pcl::uindex_t
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
Definition types.h:120
pcl::PointXYZ
A point structure representing Euclidean xyz coordinates.
Definition point_types.hpp:322
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition point_types.hpp:594