From d976272d23991873d0781f9ea3f6e61b070b134c Mon Sep 17 00:00:00 2001
From: Yuhang Wang <83380147+TsingYiPainter@users.noreply.github.com>
Date: Thu, 3 Nov 2022 15:15:17 +0800
Subject: [PATCH] Merge pull request #22682 from TsingYiPainter:5.x

3D: Handle cases where the depth of Octree setting is too large

* Handle cases where the depth setting is too large
---
 modules/3d/include/opencv2/3d.hpp |  3 ++-
 modules/3d/src/octree.cpp         | 24 ++++++++++++++++--------
 modules/3d/test/test_octree.cpp   |  2 +-
 3 files changed, 19 insertions(+), 10 deletions(-)

diff --git a/modules/3d/include/opencv2/3d.hpp b/modules/3d/include/opencv2/3d.hpp
index 95b57d8458..e838737d1a 100644
--- a/modules/3d/include/opencv2/3d.hpp
+++ b/modules/3d/include/opencv2/3d.hpp
@@ -2548,8 +2548,9 @@ public:
     /** @brief Insert a point data to a OctreeNode.
     *
     * @param point The point data in Point3f format.
+    * @return Returns whether the insertion is successful.
     */
-    void insertPoint(const Point3f& point);
+    bool insertPoint(const Point3f& point);
 
     /** @brief Read point cloud data and create OctreeNode.
     *
diff --git a/modules/3d/src/octree.cpp b/modules/3d/src/octree.cpp
index be284a38a2..7dfc6f47fc 100644
--- a/modules/3d/src/octree.cpp
+++ b/modules/3d/src/octree.cpp
@@ -13,7 +13,7 @@ namespace cv{
 static Ptr<OctreeNode> index(const Point3f& point, Ptr<OctreeNode>& node);
 
 static bool _isPointInBound(const Point3f& _point, const Point3f& _origin, double _size);
-static void insertPointRecurse( Ptr<OctreeNode>& node,  const Point3f& point, int maxDepth);
+static bool insertPointRecurse( Ptr<OctreeNode>& node,  const Point3f& point, int maxDepth);
 bool deletePointRecurse( Ptr<OctreeNode>& node);
 
 // For Nearest neighbor search.
@@ -128,14 +128,14 @@ Octree::Octree(int _maxDepth) : p(new Impl)
 
 Octree::~Octree(){}
 
-void Octree::insertPoint(const Point3f& point)
+bool Octree::insertPoint(const Point3f& point)
 {
     if(p->rootNode.empty())
     {
         p->rootNode = new OctreeNode( 0, p->size, p->origin, -1);
     }
 
-    insertPointRecurse(p->rootNode, point, p->maxDepth);
+    return insertPointRecurse(p->rootNode, point, p->maxDepth);
 }
 
 
@@ -175,11 +175,15 @@ bool Octree::create(const std::vector<Point3f> &pointCloud, int _maxDepth)
     this->p->size = 2 * halfSize;
 
     // Insert every point in PointCloud data.
+    int cnt=0;
     for(size_t idx = 0; idx < pointCloud.size(); idx++ )
     {
-        insertPoint(pointCloud[idx]);
+        if(!insertPoint(pointCloud[idx])){
+            cnt++;
+        };
     }
 
+    CV_LOG_IF_WARNING(NULL,cnt!=0,"OverAll "<<cnt<<" points has been ignored! The number of point clouds contained in the current octree is "<<pointCloud.size()-cnt);
     return true;
 }
 
@@ -344,19 +348,23 @@ bool deletePointRecurse(Ptr<OctreeNode>& _node)
     }
 }
 
-void insertPointRecurse( Ptr<OctreeNode>& _node,  const Point3f& point, int maxDepth)
+bool insertPointRecurse( Ptr<OctreeNode>& _node,  const Point3f& point, int maxDepth)
 {
     OctreeNode& node = *_node;
-    if(!node.isPointInBound(point, node.origin, node.size))
+    bool pointInBoundFlag = node.isPointInBound(point, node.origin, node.size);
+    if(node.depth==0 && !pointInBoundFlag)
     {
         CV_Error(Error::StsBadArg, "The point is out of boundary!");
     }
+    else if (!pointInBoundFlag){
+        return false;
+    }
 
     if(node.depth == maxDepth)
     {
         node.isLeaf = true;
         node.pointList.push_back(point);
-        return;
+        return true;
     }
 
     double childSize = node.size * 0.5;
@@ -377,7 +385,7 @@ void insertPointRecurse( Ptr<OctreeNode>& _node,  const Point3f& point, int maxD
         node.children[childIndex] = new OctreeNode(node.depth + 1, childSize, childOrigin, int(childIndex));
         node.children[childIndex]->parent = _node;
     }
-    insertPointRecurse(node.children[childIndex], point, maxDepth);
+    return insertPointRecurse(node.children[childIndex], point, maxDepth);
 }
 
 // For Nearest neighbor search.
diff --git a/modules/3d/test/test_octree.cpp b/modules/3d/test/test_octree.cpp
index 45d60600f9..106d17b12e 100644
--- a/modules/3d/test/test_octree.cpp
+++ b/modules/3d/test/test_octree.cpp
@@ -14,7 +14,7 @@ protected:
     void SetUp() override
     {
         pointCloudSize = 1000;
-        maxDepth = 4;
+        maxDepth = 18;
 
         int scale;
         Point3i pmin, pmax;