/* * Software License Agreement (BSD License) * * Point Cloud Library (PCL) - www.pointclouds.org * Copyright (c) 2010-2011, 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. * * $Id$ */ #pragma once #include #include #include #include namespace pcl { namespace octree { ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** \brief @b Octree pointcloud class * \note Octree implementation for pointclouds. Only indices are stored by the octree * leaf nodes (zero-copy). * \note The octree pointcloud class needs to be initialized with its voxel resolution. * Its bounding box is automatically adjusted * \note according to the pointcloud dimension or it can be predefined. * \note Note: The tree depth equates to the resolution and the bounding box dimensions * of the octree. * \tparam PointT: type of point used in pointcloud * \tparam LeafContainerT: leaf node container * \tparam BranchContainerT: branch node container * \tparam OctreeT: octree implementation * \ingroup octree * \author Julius Kammerl * (julius@kammerl.de) */ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template > class OctreePointCloud : public OctreeT { public: using Base = OctreeT; using LeafNode = typename OctreeT::LeafNode; using BranchNode = typename OctreeT::BranchNode; /** \brief Octree pointcloud constructor. * \param[in] resolution_arg octree resolution at lowest octree level */ OctreePointCloud(const double resolution_arg); // public typedefs using IndicesPtr = shared_ptr; using IndicesConstPtr = shared_ptr; using PointCloud = pcl::PointCloud; using PointCloudPtr = typename PointCloud::Ptr; using PointCloudConstPtr = typename PointCloud::ConstPtr; // public typedefs for single/double buffering using SingleBuffer = OctreePointCloud>; // using DoubleBuffer = OctreePointCloud >; // Boost shared pointers using Ptr = shared_ptr>; using ConstPtr = shared_ptr< const OctreePointCloud>; // Eigen aligned allocator using AlignedPointTVector = std::vector>; using AlignedPointXYZVector = std::vector>; /** \brief Provide a pointer to the input data set. * \param[in] cloud_arg the const boost shared pointer to a PointCloud message * \param[in] indices_arg the point indices subset that is to be used from \a cloud - * if 0 the whole point cloud is used */ inline void setInputCloud(const PointCloudConstPtr& cloud_arg, const IndicesConstPtr& indices_arg = IndicesConstPtr()) { input_ = cloud_arg; indices_ = indices_arg; } /** \brief Get a pointer to the vector of indices used. * \return pointer to vector of indices used. */ inline IndicesConstPtr const getIndices() const { return (indices_); } /** \brief Get a pointer to the input point cloud dataset. * \return pointer to pointcloud input class. */ inline PointCloudConstPtr getInputCloud() const { return (input_); } /** \brief Set the search epsilon precision (error bound) for nearest neighbors * searches. * \param[in] eps precision (error bound) for nearest neighbors searches */ inline void setEpsilon(double eps) { epsilon_ = eps; } /** \brief Get the search epsilon precision (error bound) for nearest neighbors * searches. */ inline double getEpsilon() const { return (epsilon_); } /** \brief Set/change the octree voxel resolution * \param[in] resolution_arg side length of voxels at lowest tree level */ inline void setResolution(double resolution_arg) { // octree needs to be empty to change its resolution if (this->leaf_count_ > 0) { PCL_ERROR("[pcl::octree::OctreePointCloud::setResolution] Octree needs to be " "empty to change its resolution(leaf count should must be 0)!\n"); return; } resolution_ = resolution_arg; getKeyBitSize(); } /** \brief Get octree voxel resolution * \return voxel resolution at lowest tree level */ inline double getResolution() const { return (resolution_); } /** \brief Get the maximum depth of the octree. * \return depth_arg: maximum depth of octree * */ inline uindex_t getTreeDepth() const { return this->octree_depth_; } /** \brief Add points from input point cloud to octree. */ void addPointsFromInputCloud(); /** \brief Add point at given index from input point cloud to octree. Index will be * also added to indices vector. * \param[in] point_idx_arg index of point to be added * \param[in] indices_arg pointer to indices vector of the dataset (given by \a * setInputCloud) */ void addPointFromCloud(uindex_t point_idx_arg, IndicesPtr indices_arg); /** \brief Add point simultaneously to octree and input point cloud. * \param[in] point_arg point to be added * \param[in] cloud_arg pointer to input point cloud dataset (given by \a * setInputCloud) */ void addPointToCloud(const PointT& point_arg, PointCloudPtr cloud_arg); /** \brief Add point simultaneously to octree and input point cloud. A corresponding * index will be added to the indices vector. * \param[in] point_arg point to be added * \param[in] cloud_arg pointer to input point cloud dataset (given by \a * setInputCloud) * \param[in] indices_arg pointer to indices vector of the dataset (given by \a * setInputCloud) */ void addPointToCloud(const PointT& point_arg, PointCloudPtr cloud_arg, IndicesPtr indices_arg); /** \brief Check if voxel at given point exist. * \param[in] point_arg point to be checked * \return "true" if voxel exist; "false" otherwise */ bool isVoxelOccupiedAtPoint(const PointT& point_arg) const; /** \brief Delete the octree structure and its leaf nodes. * */ void deleteTree() { // reset bounding box min_x_ = max_x_ = min_y_ = max_y_ = min_z_ = max_z_ = 0; this->bounding_box_defined_ = false; OctreeT::deleteTree(); } /** \brief Check if voxel at given point coordinates exist. * \param[in] point_x_arg X coordinate of point to be checked * \param[in] point_y_arg Y coordinate of point to be checked * \param[in] point_z_arg Z coordinate of point to be checked * \return "true" if voxel exist; "false" otherwise */ bool isVoxelOccupiedAtPoint(const double point_x_arg, const double point_y_arg, const double point_z_arg) const; /** \brief Check if voxel at given point from input cloud exist. * \param[in] point_idx_arg point to be checked * \return "true" if voxel exist; "false" otherwise */ bool isVoxelOccupiedAtPoint(const index_t& point_idx_arg) const; /** \brief Get a PointT vector of centers of all occupied voxels. * \param[out] voxel_center_list_arg results are written to this vector of PointT * elements * \return number of occupied voxels */ uindex_t getOccupiedVoxelCenters(AlignedPointTVector& voxel_center_list_arg) const; /** \brief Get a PointT vector of centers of voxels intersected by a line segment. * This returns a approximation of the actual intersected voxels by walking * along the line with small steps. Voxels are ordered, from closest to * furthest w.r.t. the origin. * \param[in] origin origin of the line segment * \param[in] end end of the line segment * \param[out] voxel_center_list results are written to this vector of PointT elements * \param[in] precision determines the size of the steps: step_size = * octree_resolution x precision * \return number of intersected voxels */ uindex_t getApproxIntersectedVoxelCentersBySegment(const Eigen::Vector3f& origin, const Eigen::Vector3f& end, AlignedPointTVector& voxel_center_list, float precision = 0.2); /** \brief Delete leaf node / voxel at given point * \param[in] point_arg point addressing the voxel to be deleted. */ void deleteVoxelAtPoint(const PointT& point_arg); /** \brief Delete leaf node / voxel at given point from input cloud * \param[in] point_idx_arg index of point addressing the voxel to be deleted. */ void deleteVoxelAtPoint(const index_t& point_idx_arg); ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Bounding box methods ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** \brief Investigate dimensions of pointcloud data set and define corresponding * bounding box for octree. */ void defineBoundingBox(); /** \brief Define bounding box for octree * \note Bounding box cannot be changed once the octree contains elements. * \param[in] min_x_arg X coordinate of lower bounding box corner * \param[in] min_y_arg Y coordinate of lower bounding box corner * \param[in] min_z_arg Z coordinate of lower bounding box corner * \param[in] max_x_arg X coordinate of upper bounding box corner * \param[in] max_y_arg Y coordinate of upper bounding box corner * \param[in] max_z_arg Z coordinate of upper bounding box corner */ void defineBoundingBox(const double min_x_arg, const double min_y_arg, const double min_z_arg, const double max_x_arg, const double max_y_arg, const double max_z_arg); /** \brief Define bounding box for octree * \note Lower bounding box point is set to (0, 0, 0) * \note Bounding box cannot be changed once the octree contains elements. * \param[in] max_x_arg X coordinate of upper bounding box corner * \param[in] max_y_arg Y coordinate of upper bounding box corner * \param[in] max_z_arg Z coordinate of upper bounding box corner */ void defineBoundingBox(const double max_x_arg, const double max_y_arg, const double max_z_arg); /** \brief Define bounding box cube for octree * \note Lower bounding box corner is set to (0, 0, 0) * \note Bounding box cannot be changed once the octree contains elements. * \param[in] cubeLen_arg side length of bounding box cube. */ void defineBoundingBox(const double cubeLen_arg); /** \brief Get bounding box for octree * \note Bounding box cannot be changed once the octree contains elements. * \param[in] min_x_arg X coordinate of lower bounding box corner * \param[in] min_y_arg Y coordinate of lower bounding box corner * \param[in] min_z_arg Z coordinate of lower bounding box corner * \param[in] max_x_arg X coordinate of upper bounding box corner * \param[in] max_y_arg Y coordinate of upper bounding box corner * \param[in] max_z_arg Z coordinate of upper bounding box corner */ void getBoundingBox(double& min_x_arg, double& min_y_arg, double& min_z_arg, double& max_x_arg, double& max_y_arg, double& max_z_arg) const; /** \brief Calculates the squared diameter of a voxel at given tree depth * \param[in] tree_depth_arg depth/level in octree * \return squared diameter */ double getVoxelSquaredDiameter(uindex_t tree_depth_arg) const; /** \brief Calculates the squared diameter of a voxel at leaf depth * \return squared diameter */ inline double getVoxelSquaredDiameter() const { return getVoxelSquaredDiameter(this->octree_depth_); } /** \brief Calculates the squared voxel cube side length at given tree depth * \param[in] tree_depth_arg depth/level in octree * \return squared voxel cube side length */ double getVoxelSquaredSideLen(uindex_t tree_depth_arg) const; /** \brief Calculates the squared voxel cube side length at leaf level * \return squared voxel cube side length */ inline double getVoxelSquaredSideLen() const { return getVoxelSquaredSideLen(this->octree_depth_); } /** \brief Generate bounds of the current voxel of an octree iterator * \param[in] iterator: octree iterator * \param[out] min_pt lower bound of voxel * \param[out] max_pt upper bound of voxel */ inline void getVoxelBounds(const OctreeIteratorBase& iterator, Eigen::Vector3f& min_pt, Eigen::Vector3f& max_pt) const { this->genVoxelBoundsFromOctreeKey(iterator.getCurrentOctreeKey(), iterator.getCurrentOctreeDepth(), min_pt, max_pt); } /** \brief Enable dynamic octree structure * \note Leaf nodes are kept as close to the root as possible and are only expanded * if the number of DataT objects within a leaf node exceeds a fixed limit. * \param maxObjsPerLeaf: maximum number of DataT objects per leaf * */ inline void enableDynamicDepth(std::size_t maxObjsPerLeaf) { if (this->leaf_count_ > 0) { PCL_ERROR("[pcl::octree::OctreePointCloud::enableDynamicDepth] Leaf count should " "must be 0!\n"); return; } max_objs_per_leaf_ = maxObjsPerLeaf; this->dynamic_depth_enabled_ = max_objs_per_leaf_ > 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 * \a setInputCloud to be added */ virtual void addPointIdx(uindex_t point_idx_arg); /** \brief Add point at index from input pointcloud dataset to octree * \param[in] leaf_node to be expanded * \param[in] parent_branch parent of leaf node to be expanded * \param[in] child_idx child index of leaf node (in parent branch) * \param[in] depth_mask of leaf node to be expanded */ void expandLeafNode(LeafNode* leaf_node, BranchNode* parent_branch, unsigned char child_idx, uindex_t depth_mask); /** \brief Get point at index from input pointcloud dataset * \param[in] index_arg index representing the point in the dataset given by \a * setInputCloud * \return PointT from input pointcloud dataset */ const PointT& getPointByIndex(uindex_t index_arg) const; /** \brief Find octree leaf node at a given point * \param[in] point_arg query point * \return pointer to leaf node. If leaf node does not exist, pointer is 0. */ LeafContainerT* findLeafAtPoint(const PointT& point_arg) const { OctreeKey key; // generate key for point this->genOctreeKeyforPoint(point_arg, key); return (this->findLeaf(key)); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Protected octree methods based on octree keys ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** \brief Define octree key setting and octree depth based on defined bounding box. */ void getKeyBitSize(); /** \brief Grow the bounding box/octree until point fits * \param[in] point_idx_arg point that should be within bounding box; */ void adoptBoundingBoxToPoint(const PointT& point_idx_arg); /** \brief Checks if given point is within the bounding box of the octree * \param[in] point_idx_arg point to be checked for bounding box violations * \return "true" - no bound violation */ inline bool isPointWithinBoundingBox(const PointT& point_idx_arg) const { return (!((point_idx_arg.x < min_x_) || (point_idx_arg.y < min_y_) || (point_idx_arg.z < min_z_) || (point_idx_arg.x >= max_x_) || (point_idx_arg.y >= max_y_) || (point_idx_arg.z >= max_z_))); } /** \brief Generate octree key for voxel at a given point * \param[in] point_arg the point addressing a voxel * \param[out] key_arg write octree key to this reference */ void genOctreeKeyforPoint(const PointT& point_arg, OctreeKey& key_arg) const; /** \brief Generate octree key for voxel at a given point * \param[in] point_x_arg X coordinate of point addressing a voxel * \param[in] point_y_arg Y coordinate of point addressing a voxel * \param[in] point_z_arg Z coordinate of point addressing a voxel * \param[out] key_arg write octree key to this reference */ void genOctreeKeyforPoint(const double point_x_arg, const double point_y_arg, const double point_z_arg, OctreeKey& key_arg) const; /** \brief Virtual method for generating octree key for a given point index. * \note This method enables to assign indices to leaf nodes during octree * deserialization. * \param[in] data_arg index value representing a point in the dataset given by \a * setInputCloud * \param[out] key_arg write octree key to this reference \return "true" - octree keys * are assignable */ virtual bool genOctreeKeyForDataT(const index_t& data_arg, OctreeKey& key_arg) const; /** \brief Generate a point at center of leaf node voxel * \param[in] key_arg octree key addressing a leaf node. * \param[out] point_arg write leaf node voxel center to this point reference */ void genLeafNodeCenterFromOctreeKey(const OctreeKey& key_arg, PointT& point_arg) const; /** \brief Generate a point at center of octree voxel at given tree level * \param[in] key_arg octree key addressing an octree node. * \param[in] tree_depth_arg octree depth of query voxel * \param[out] point_arg write leaf node center point to this reference */ void genVoxelCenterFromOctreeKey(const OctreeKey& key_arg, uindex_t tree_depth_arg, PointT& point_arg) const; /** \brief Generate bounds of an octree voxel using octree key and tree depth * arguments * \param[in] key_arg octree key addressing an octree node. * \param[in] tree_depth_arg octree depth of query voxel * \param[out] min_pt lower bound of voxel * \param[out] max_pt upper bound of voxel */ void genVoxelBoundsFromOctreeKey(const OctreeKey& key_arg, uindex_t tree_depth_arg, Eigen::Vector3f& min_pt, Eigen::Vector3f& max_pt) const; /** \brief Recursively search the tree for all leaf nodes and return a vector of voxel * centers. * \param[in] node_arg current octree node to be explored * \param[in] key_arg octree key addressing a leaf node. * \param[out] voxel_center_list_arg results are written to this vector of PointT * elements * \return number of voxels found */ uindex_t getOccupiedVoxelCentersRecursive(const BranchNode* node_arg, const OctreeKey& key_arg, AlignedPointTVector& voxel_center_list_arg) const; ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // Globals ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** \brief Pointer to input point cloud dataset. */ PointCloudConstPtr input_; /** \brief A pointer to the vector of point indices to use. */ IndicesConstPtr indices_; /** \brief Epsilon precision (error bound) for nearest neighbors searches. */ double epsilon_{0.0}; /** \brief Octree resolution. */ double resolution_; // Octree bounding box coordinates double min_x_{0.0}; double max_x_; double min_y_{0.0}; double max_y_; double min_z_{0.0}; double max_z_; /** \brief Flag indicating if octree has defined bounding box. */ bool bounding_box_defined_{false}; /** \brief Amount of DataT objects per leafNode before expanding branch * \note zero indicates a fixed/maximum depth octree structure * **/ std::size_t max_objs_per_leaf_{0}; }; } // namespace octree } // namespace pcl #ifdef PCL_NO_PRECOMPILE #include #endif