249 lines
9.0 KiB
C
249 lines
9.0 KiB
C
|
/*
|
||
|
|
* 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>
|