[octree] use PCL_ERROR instead of assert (#5321)

* [refactor] octree: use PCL_ERROR instead of assert

* PCL_THROW_EXCEPTION for invalid resolution

* revert to assert for min_child_idx

* use assert for performance

* use assert for performance reason

* use assert for performance reason

* use assert to circumvent "error: reference to local variable 'node' returned [-Werror=return-local-addr]"

* [doc] add doc for child_idx_arg

* use more assert

* Revert some changes in octree_pointcloud.hpp

* Revert some changes

* Revert some changes

* Re-add include

---------

Co-authored-by: Markus Vieth <39675748+mvieth@users.noreply.github.com>

Author: keineahnung2345

octree/include/pcl/octree/impl/octree2buf_base.hpp

@@ -71,7 +71,12 @@ Octree2BufBase<LeafContainerT, BranchContainerT>::setMaxVoxelIndex(
 {
   uindex_t treeDepth;
 
-  assert(max_voxel_index_arg > 0);
+  if (max_voxel_index_arg <= 0) {
+    PCL_ERROR("[pcl::octree::Octree2BufBase::setMaxVoxelIndex] Max voxel index (%lu) "
+              "must be > 0!\n",
+              max_voxel_index_arg);
+    return;
+  }
 
   // tree depth == amount of bits of maxVoxels
   treeDepth =
@@ -88,7 +93,12 @@ template <typename LeafContainerT, typename BranchContainerT>
 void
 Octree2BufBase<LeafContainerT, BranchContainerT>::setTreeDepth(uindex_t depth_arg)
 {
-  assert(depth_arg > 0);
+  if (depth_arg <= 0) {
+    PCL_ERROR(
+        "[pcl::octree::Octree2BufBase::setTreeDepth] Tree depth (%lu) must be > 0!\n",
+        depth_arg);
+    return;
+  }
 
   // set octree depth
   octree_depth_ = depth_arg;

octree/include/pcl/octree/impl/octree_base.hpp

@@ -71,7 +71,12 @@ OctreeBase<LeafContainerT, BranchContainerT>::setMaxVoxelIndex(
 {
   uindex_t tree_depth;
 
-  assert(max_voxel_index_arg > 0);
+  if (max_voxel_index_arg <= 0) {
+    PCL_ERROR("[pcl::octree::OctreeBase::setMaxVoxelIndex] Max voxel index (%lu) must "
+              "be > 0!\n",
+              max_voxel_index_arg);
+    return;
+  }
 
   // tree depth == bitlength of maxVoxels
   tree_depth =
@@ -86,8 +91,18 @@ template <typename LeafContainerT, typename BranchContainerT>
 void
 OctreeBase<LeafContainerT, BranchContainerT>::setTreeDepth(uindex_t depth_arg)
 {
-  assert(depth_arg > 0);
-  assert(depth_arg <= OctreeKey::maxDepth);
+  if (depth_arg <= 0) {
+    PCL_ERROR("[pcl::octree::OctreeBase::setTreeDepth] Tree depth (%lu) must be > 0!\n",
+              depth_arg);
+    return;
+  }
+  if (depth_arg > OctreeKey::maxDepth) {
+    PCL_ERROR("[pcl::octree::OctreeBase::setTreeDepth] Tree depth (%lu) must be <= max "
+              "depth(%lu)!\n",
+              depth_arg,
+              OctreeKey::maxDepth);
+    return;
+  }
 
   // set octree depth
   octree_depth_ = depth_arg;

octree/include/pcl/octree/impl/octree_pointcloud.hpp

@@ -66,7 +66,11 @@ pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>
 , bounding_box_defined_(false)
 , max_objs_per_leaf_(0)
 {
-  assert(resolution > 0.0f);
+  if (resolution <= 0.0) {
+    PCL_THROW_EXCEPTION(InitFailedException,
+                        "[pcl::octree::OctreePointCloud::OctreePointCloud] Resolution "
+                            << resolution << " must be > 0!");
+  }
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////
@@ -339,7 +343,12 @@ pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>
   PointT max_pt;
 
   // bounding box cannot be changed once the octree contains elements
-  assert(this->leaf_count_ == 0);
+  if (this->leaf_count_ != 0) {
+    PCL_ERROR("[pcl::octree::OctreePointCloud::defineBoundingBox] Leaf count (%lu) "
+              "must be 0\n",
+              this->leaf_count_);
+    return;
+  }
 
   pcl::getMinMax3D(*input_, min_pt, max_pt);
 
@@ -372,7 +381,12 @@ pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>
                       const double max_z_arg)
 {
   // bounding box cannot be changed once the octree contains elements
-  assert(this->leaf_count_ == 0);
+  if (this->leaf_count_ != 0) {
+    PCL_ERROR("[pcl::octree::OctreePointCloud::defineBoundingBox] Leaf count (%lu) "
+              "must be 0\n",
+              this->leaf_count_);
+    return;
+  }
 
   min_x_ = std::min(min_x_arg, max_x_arg);
   min_y_ = std::min(min_y_arg, max_y_arg);
@@ -400,7 +414,12 @@ pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>
                       const double max_z_arg)
 {
   // bounding box cannot be changed once the octree contains elements
-  assert(this->leaf_count_ == 0);
+  if (this->leaf_count_ != 0) {
+    PCL_ERROR("[pcl::octree::OctreePointCloud::defineBoundingBox] Leaf count (%lu) "
+              "must be 0\n",
+              this->leaf_count_);
+    return;
+  }
 
   min_x_ = std::min(0.0, max_x_arg);
   min_y_ = std::min(0.0, max_y_arg);
@@ -426,7 +445,12 @@ pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>
     defineBoundingBox(const double cubeLen_arg)
 {
   // bounding box cannot be changed once the octree contains elements
-  assert(this->leaf_count_ == 0);
+  if (this->leaf_count_ != 0) {
+    PCL_ERROR("[pcl::octree::OctreePointCloud::defineBoundingBox] Leaf count (%lu) "
+              "must be 0\n",
+              this->leaf_count_);
+    return;
+  }
 
   min_x_ = std::min(0.0, cubeLen_arg);
   min_y_ = std::min(0.0, cubeLen_arg);

octree/include/pcl/octree/octree2buf_base.h

@@ -89,8 +89,8 @@ class BufferedBranchNode : public OctreeNode {
   }
 
   /** \brief Get child pointer in current branch node
-   *  \param buffer_arg: buffer selector
-   *  \param index_arg: index of child in node
+   *  \param buffer_arg: buffer selector, must be less than 2
+   *  \param index_arg: index of child in node, must be less than 8
    *  \return pointer to child node
    */
   inline OctreeNode*
@@ -101,8 +101,8 @@ class BufferedBranchNode : public OctreeNode {
   }
 
   /** \brief Set child pointer in current branch node
-   *  \param buffer_arg: buffer selector
-   *  \param index_arg: index of child in node
+   *  \param buffer_arg: buffer selector, must be less than 2
+   *  \param index_arg: index of child in node, must be less than 8
    *  \param newNode_arg: pointer to new child node
    */
   inline void
@@ -115,8 +115,8 @@ class BufferedBranchNode : public OctreeNode {
   }
 
   /** \brief Check if branch is pointing to a particular child node
-   *  \param buffer_arg: buffer selector
-   *  \param index_arg: index of child in node
+   *  \param buffer_arg: buffer selector, must be less than 2
+   *  \param index_arg: index of child in node, must be less than 8
    *  \return "true" if pointer to child node exists; "false" otherwise
    */
   inline bool

octree/include/pcl/octree/octree_container.h

@@ -38,6 +38,7 @@
 
 #pragma once
 
+#include <pcl/console/print.h>
 #include <pcl/types.h>
 
 #include <cassert>
@@ -161,7 +162,8 @@ class OctreeContainerEmpty : public OctreeContainerBase {
   index_t
   getPointIndex() const override
   {
-    assert("getPointIndex: undefined point index");
+    PCL_ERROR(
+        "[pcl::octree::OctreeContainerBase::getPointIndex] Undefined point index!\n");
     return -1;
   }
 

octree/include/pcl/octree/octree_nodes.h

@@ -216,7 +216,7 @@ class OctreeBranchNode : public OctreeNode {
   ~OctreeBranchNode() override = default;
 
   /** \brief Access operator.
-   *  \param child_idx_arg: index to child node
+   *  \param child_idx_arg: index to child node, must be less than 8
    *  \return OctreeNode pointer
    * */
   inline OctreeNode*&
@@ -227,7 +227,7 @@ class OctreeBranchNode : public OctreeNode {
   }
 
   /** \brief Get pointer to child
-   *  \param child_idx_arg: index to child node
+   *  \param child_idx_arg: index to child node, must be less than 8
    *  \return OctreeNode pointer
    * */
   inline OctreeNode*
@@ -238,6 +238,7 @@ class OctreeBranchNode : public OctreeNode {
   }
 
   /** \brief Get pointer to child
+   *  \param index: index to child node, must be less than 8
    *  \return OctreeNode pointer
    * */
   inline void
@@ -248,7 +249,7 @@ class OctreeBranchNode : public OctreeNode {
   }
 
   /** \brief Check if branch is pointing to a particular child node
-   *  \param child_idx_arg: index to child node
+   *  \param child_idx_arg: index to child node, must be less than 8
    *  \return "true" if pointer to child node exists; "false" otherwise
    * */
   inline bool

octree/include/pcl/octree/octree_pointcloud.h

@@ -164,7 +164,11 @@ class OctreePointCloud : public OctreeT {
   setResolution(double resolution_arg)
   {
     // octree needs to be empty to change its resolution
-    assert(this->leaf_count_ == 0);
+    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;
 
@@ -416,7 +420,11 @@ class OctreePointCloud : public OctreeT {
   inline void
   enableDynamicDepth(std::size_t maxObjsPerLeaf)
   {
-    assert(this->leaf_count_ == 0);
+    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;