Merge pull request #19684 from zihaomu:octree_in_3d

modules/3d/include/opencv2/3d.hpp

@@ -2374,6 +2374,151 @@ void undistortPoints(InputArray src, OutputArray dst,
                      InputArray R = noArray(), InputArray P = noArray(),
                      TermCriteria criteria=TermCriteria(TermCriteria::MAX_ITER, 5, 0.01));
 
+
+/** @brief Octree for 3D vision.
+ *
+ * In 3D vision filed, the Octree is used to process and accelerate the pointcloud data. The class Octree represents
+ * the Octree data structure. Each Octree will have a fixed depth. The depth of Octree refers to the distance from
+ * the root node to the leaf node.All OctreeNodes will not exceed this depth.Increasing the depth will increase
+ * the amount of calculation exponentially. And the small number of depth refers low resolution of Octree.
+ * Each node contains 8 children, which are used to divide the space cube into eight parts. Each octree node represents
+ * a cube. And these eight children will have a fixed order, the order is described as follows:
+ *
+ * For illustration, assume,
+ *
+ * rootNode: origin == (0, 0, 0), size == 2
+ *
+ * Then,
+ *
+ * children[0]: origin == (0, 0, 0), size == 1
+ *
+ * children[1]: origin == (1, 0, 0), size == 1, along X-axis next to child 0
+ *
+ * children[2]: origin == (0, 1, 0), size == 1, along Y-axis next to child 0
+ *
+ * children[3]: origin == (1, 1, 0), size == 1, in X-Y plane
+ *
+ * children[4]: origin == (0, 0, 1), size == 1, along Z-axis next to child 0
+ *
+ * children[5]: origin == (1, 0, 1), size == 1, in X-Z plane
+ *
+ * children[6]: origin == (0, 1, 1), size == 1, in Y-Z plane
+ *
+ * children[7]: origin == (1, 1, 1), size == 1, furthest from child 0
+ */
+
+class CV_EXPORTS Octree {
+
+public:
+
+    //! Default constructor.
+    Octree();
+
+    /** @overload
+    * @brief Create an empty Octree and set the maximum depth.
+    *
+    * @param maxDepth The max depth of the Octree. The maxDepth > -1.
+    */
+    explicit Octree(int maxDepth);
+
+    /** @overload
+    * @brief Create an Octree from the PointCloud data with the specific max depth.
+    *
+    * @param pointCloud Point cloud data.
+    * @param maxDepth The max depth of the Octree.
+    */
+    Octree(const std::vector<Point3f> &pointCloud, int maxDepth);
+
+    /** @overload
+    * @brief Create an empty Octree.
+    *
+    * @param maxDepth Max depth.
+    * @param size Initial Cube size.
+    * @param origin Initial center coordinate.
+    */
+    Octree(int maxDepth, double size, const Point3f& origin);
+
+    //! Default destructor
+    ~Octree();
+
+    /** @brief Insert a point data to a OctreeNode.
+    *
+    * @param point The point data in Point3f format.
+    */
+    void insertPoint(const Point3f& point);
+
+    /** @brief Read point cloud data and create OctreeNode.
+    *
+    * This function is only called when the octree is being created.
+    * @param pointCloud PointCloud data.
+    * @param maxDepth The max depth of the Octree.
+    * @return Returns whether the creation is successful.
+    */
+    bool create(const std::vector<Point3f> &pointCloud, int maxDepth = -1);
+
+    /** @brief Determine whether the point is within the space range of the specific cube.
+     *
+     * @param point The point coordinates.
+     * @return If point is in bound, return ture. Otherwise, false.
+     */
+    bool isPointInBound(const Point3f& point) const;
+
+    //! Set MaxDepth for Octree.
+    void setMaxDepth(int maxDepth);
+
+    //! Set Box Size for Octree.
+    void setSize(double size);
+
+    //! Set Origin coordinates for Octree.
+    void setOrigin(const Point3f& origin);
+
+    //! returns true if the rootnode is NULL.
+    bool empty() const;
+
+    /** @brief Reset all octree parameter.
+    *
+    *  Clear all the nodes of the octree and initialize the parameters.
+    */
+    void clear();
+
+    /** @brief Delete a given point from the Octree.
+    *
+    * Delete the corresponding element from the pointList in the corresponding leaf node. If the leaf node
+    * does not contain other points after deletion, this node will be deleted. In the same way,
+    * its parent node may also be deleted if its last child is deleted.
+    * @param point The point coordinates.
+    * @return return ture if the point is deleted successfully.
+    */
+    bool deletePoint(const Point3f& point);
+
+    /** @brief Radius Nearest Neighbor Search in Octree
+    *
+    * Search all points that are less than or equal to radius.
+    * And return the number of searched points.
+    * @param query Query point.
+    * @param radius Retrieved radius value.
+    * @param pointSet Point output. Contains searched points, and output vector is not in order.
+    * @param squareDistSet Dist output. Contains searched squared distance, and output vector is not in order.
+    * @return the number of searched points.
+    */
+    int radiusNNSearch(const Point3f& query, float radius, std::vector<Point3f> &pointSet, std::vector<float> &squareDistSet) const;
+
+    /** @brief K Nearest Neighbor Search in Octree.
+    *
+    * Find the K nearest neighbors to the query point.
+    * @param query Query point.
+    * @param K
+    * @param pointSet Point output. Contains K points, arranged in order of distance from near to far.
+    * @param squareDistSet Dist output. Contains K squared distance, arranged in order of distance from near to far.
+    */
+    void KNNSearch(const Point3f& query, const int K, std::vector<Point3f> &pointSet, std::vector<float> &squareDistSet) const;
+
+protected:
+    struct Impl;
+    Ptr<Impl> p;
+
+};
+
 //! @} _3d
 } //end namespace cv
 

modules/3d/src/octree.cpp

@@ -0,0 +1,546 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html
+
+#include "precomp.hpp"
+#include "octree.hpp"
+
+#define OCTREE_CHILD_NUM 8
+
+namespace cv{
+
+// Locate the OctreeNode corresponding to the input point from the given OctreeNode.
+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);
+bool deletePointRecurse( Ptr<OctreeNode>& node);
+
+// For Nearest neighbor search.
+template<typename T> struct PQueueElem; // Priority queue
+static void radiusNNSearchRecurse(const Ptr<OctreeNode>& node, const Point3f& query, float squareRadius,
+                           std::vector<PQueueElem<Point3f> >& candidatePoint);
+static void KNNSearchRecurse(const Ptr<OctreeNode>& node, const Point3f& query, const int K,
+                      float& smallestDist, std::vector<PQueueElem<Point3f> >& candidatePoint);
+
+OctreeNode::OctreeNode():children(OCTREE_CHILD_NUM, nullptr), depth(0), size(0), origin(0,0,0), parentIndex(-1)
+{
+}
+
+OctreeNode::OctreeNode(int _depth, double _size, const Point3f& _origin, int _parentIndex):children(OCTREE_CHILD_NUM), depth(_depth), size(_size), origin(_origin), parentIndex(_parentIndex)
+{
+}
+
+bool OctreeNode::empty() const
+{
+    if(this->isLeaf)
+    {
+        if(this->pointList.empty())
+            return true;
+        else
+            return false;
+    }
+    else
+    {
+        for(size_t i = 0; i< OCTREE_CHILD_NUM; i++)
+        {
+            if(!this->children[i].empty())
+            {
+                return false;
+            }
+        }
+        return true;
+    }
+}
+
+bool OctreeNode::isPointInBound(const Point3f& _point) const
+{
+    return isPointInBound(_point, origin, size);
+}
+
+bool OctreeNode::isPointInBound(const Point3f& _point, const Point3f& _origin, double _size) const
+{
+    return _isPointInBound(_point, _origin, _size);
+}
+
+bool _isPointInBound(const Point3f& _point, const Point3f& _origin, double _size)
+{
+    float epsX = std::numeric_limits<float>::epsilon() * std::max(std::abs(_point.x), std::abs(_origin.x));
+    float epsY = std::numeric_limits<float>::epsilon() * std::max(std::abs(_point.y), std::abs(_origin.y));
+    float epsZ = std::numeric_limits<float>::epsilon() * std::max(std::abs(_point.z), std::abs(_origin.z));
+
+    if((_point.x + epsX >= _origin.x && _point.y + epsY >= _origin.y && _point.z + epsZ >= _origin.z) &&
+       (_point.x <= _origin.x + _size + epsX && _point.y <= _origin.y + _size + epsY && _point.z <= _origin.z + _size + epsZ))
+    {
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+struct Octree::Impl
+{
+public:
+    Impl():maxDepth(-1), size(0), origin(0,0,0)
+    {}
+
+    ~Impl()
+    {}
+
+    // The pointer to Octree root node.
+    Ptr <OctreeNode> rootNode = nullptr;
+    //! Max depth of the Octree. And depth must be greater than zero
+    int maxDepth;
+    //! The size of the cube of the .
+    double size;
+    //! The origin coordinate of root node.
+    Point3f origin;
+};
+
+Octree::Octree() : p(new Impl)
+{
+    p->maxDepth = -1;
+    p->size = 0;
+    p->origin = Point3f(0,0,0);
+}
+
+Octree::Octree(int _maxDepth, double _size, const Point3f& _origin ) : p(new Impl)
+{
+    p->maxDepth = _maxDepth;
+    p->size = _size;
+    p->origin = _origin;
+}
+
+Octree::Octree(const std::vector<Point3f>& _pointCloud, int _maxDepth) : p(new Impl)
+{
+    CV_Assert( _maxDepth > -1 );
+    this->create(_pointCloud, _maxDepth);
+}
+
+Octree::Octree(int _maxDepth) : p(new Impl)
+{
+    p->maxDepth = _maxDepth;
+    p->size = 0;
+    p->origin = Point3f(0,0,0);
+}
+
+Octree::~Octree(){}
+
+void 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);
+}
+
+
+bool Octree::create(const std::vector<Point3f> &pointCloud, int _maxDepth)
+{
+    if(_maxDepth > -1)
+    {
+        p->maxDepth = _maxDepth;
+    }
+
+    CV_Assert( p->maxDepth > -1 && !pointCloud.empty());
+
+    if(pointCloud.empty())
+        return false;
+
+    Point3f maxBound(pointCloud[0]);
+    Point3f minBound(pointCloud[0]);
+    Point3f center, temp;
+
+    // Find center coordinate of PointCloud data.
+    for(size_t idx = 0; idx <pointCloud.size(); idx++)
+    {
+        maxBound.x = max(pointCloud[idx].x, maxBound.x);
+        maxBound.y = max(pointCloud[idx].y, maxBound.y);
+        maxBound.z = max(pointCloud[idx].z, maxBound.z);
+
+        minBound.x = min(pointCloud[idx].x, minBound.x);
+        minBound.y = min(pointCloud[idx].y, minBound.y);
+        minBound.z = min(pointCloud[idx].z, minBound.z);
+    }
+
+    center = (maxBound + minBound) * 0.5f;
+
+    temp = center - minBound;
+    float halfSize = std::max(temp.x, std::max(temp.y, temp.z));
+    this->p->origin = center - Point3f(halfSize, halfSize, halfSize);
+    this->p->size = 2 * halfSize;
+
+    // Insert every point in PointCloud data.
+    for(size_t idx = 0; idx < pointCloud.size(); idx++ )
+    {
+        insertPoint(pointCloud[idx]);
+    }
+
+    return true;
+}
+
+void Octree::setMaxDepth(int _maxDepth)
+{
+    if(_maxDepth )
+        this->p->maxDepth = _maxDepth;
+}
+
+void Octree::setSize(double _size)
+{
+    this->p->size = _size;
+};
+
+void Octree::setOrigin(const Point3f& _origin)
+{
+    this->p->origin = _origin;
+}
+
+void Octree::clear()
+{
+    if(!p->rootNode.empty())
+    {
+        p->rootNode.release();
+    }
+
+    p->size = 0;
+    p->maxDepth = -1;
+    p->origin = Point3f (0,0,0); // origin coordinate
+}
+
+bool Octree::empty() const
+{
+    return p->rootNode.empty();
+}
+
+Ptr<OctreeNode> index(const Point3f& point, Ptr<OctreeNode>& _node)
+{
+    OctreeNode &node = *_node;
+
+    if(node.empty())
+    {
+        return Ptr<OctreeNode>();
+    }
+
+    if(node.isLeaf)
+    {
+        for(size_t i = 0; i < node.pointList.size(); i++ )
+        {
+            if((point.x == node.pointList[i].x) &&
+               (point.y == node.pointList[i].y) &&
+               (point.z == node.pointList[i].z)
+                    )
+            {
+                return _node;
+            }
+        }
+        return Ptr<OctreeNode>();
+    }
+
+    if(node.isPointInBound(point))
+    {
+        double childSize = node.size * 0.5;
+
+        float epsX = std::numeric_limits<float>::epsilon() * std::max(std::abs(point.x), std::abs(node.origin.x));
+        float epsY = std::numeric_limits<float>::epsilon() * std::max(std::abs(point.y), std::abs(node.origin.y));
+        float epsZ = std::numeric_limits<float>::epsilon() * std::max(std::abs(point.z), std::abs(node.origin.z));
+
+        size_t xIndex = point.x <= node.origin.x + float(childSize) + epsX ? 0 : 1;
+        size_t yIndex = point.y <= node.origin.y + float(childSize) + epsY ? 0 : 1;
+        size_t zIndex = point.z <= node.origin.z + float(childSize) + epsZ ? 0 : 1;
+        size_t childIndex = xIndex + yIndex * 2 + zIndex * 4;
+
+        if(!node.children[childIndex].empty())
+        {
+            return index(point, node.children[childIndex]);
+        }
+    }
+    return Ptr<OctreeNode>();
+}
+
+bool Octree::isPointInBound(const Point3f& _point) const
+{
+    return _isPointInBound(_point, p->origin, p->size);
+}
+
+bool Octree::deletePoint(const Point3f& point)
+{
+    Ptr<OctreeNode> node = index(point, p->rootNode);
+
+    if(!node.empty())
+    {
+        size_t i = 0;
+        while (!node->pointList.empty() && i < node->pointList.size() )
+        {
+            if((point.x == node->pointList[i].x) &&
+               (point.y == node->pointList[i].y) &&
+               (point.z == node->pointList[i].z)
+                    )
+            {
+                node->pointList.erase(node->pointList.begin() + i);
+            } else{
+                i++;
+            }
+        }
+
+        // If it is the last point cloud in the OctreeNode, recursively delete the node.
+        return deletePointRecurse(node);
+    }
+    else
+    {
+        return false;
+    }
+}
+
+bool deletePointRecurse(Ptr<OctreeNode>& _node)
+{
+    OctreeNode& node = *_node;
+
+    if(_node.empty())
+        return false;
+
+    if(node.isLeaf)
+    {
+        if( !node.pointList.empty())
+        {
+            Ptr<OctreeNode> parent = node.parent;
+            parent->children[node.parentIndex] = nullptr;
+            _node.release();
+
+            return deletePointRecurse(parent);
+        }
+        else
+        {
+            return true;
+        }
+    }
+    else
+    {
+        bool deleteFlag = true;
+
+        // Only all children was deleted, can we delete the tree node.
+        for(size_t i = 0; i< OCTREE_CHILD_NUM; i++)
+        {
+            if(!node.children[i].empty())
+            {
+                deleteFlag = false;
+                break;
+            }
+        }
+
+        if(deleteFlag)
+        {
+            Ptr<OctreeNode> parent = node.parent;
+            _node.release();
+            return deletePointRecurse(parent);
+        }
+        else
+        {
+            return true;
+        }
+    }
+}
+
+void insertPointRecurse( Ptr<OctreeNode>& _node,  const Point3f& point, int maxDepth)
+{
+    OctreeNode& node = *_node;
+    if(!node.isPointInBound(point, node.origin, node.size))
+    {
+        CV_Error(Error::StsBadArg, "The point is out of boundary!");
+    }
+
+    if(node.depth == maxDepth)
+    {
+        node.isLeaf = true;
+        node.pointList.push_back(point);
+        return;
+    }
+
+    double childSize = node.size * 0.5;
+
+    float epsX = std::numeric_limits<float>::epsilon() * std::max(std::abs(point.x), std::abs(node.origin.x));
+    float epsY = std::numeric_limits<float>::epsilon() * std::max(std::abs(point.y), std::abs(node.origin.y));
+    float epsZ = std::numeric_limits<float>::epsilon() * std::max(std::abs(point.z), std::abs(node.origin.z));
+
+    size_t xIndex = point.x <= node.origin.x + float(childSize) + epsX ? 0 : 1;
+    size_t yIndex = point.y <= node.origin.y + float(childSize) + epsY ? 0 : 1;
+    size_t zIndex = point.z <= node.origin.z + float(childSize) + epsZ ? 0 : 1;
+
+    size_t childIndex = xIndex + yIndex * 2 + zIndex * 4;
+
+    if(node.children[childIndex].empty())
+    {
+        Point3f childOrigin = node.origin + Point3f(xIndex * float(childSize), yIndex * float(childSize), zIndex * float(childSize));
+        node.children[childIndex] = new OctreeNode(node.depth + 1, childSize, childOrigin, int(childIndex));
+        node.children[childIndex]->parent = _node;
+    }
+    insertPointRecurse(node.children[childIndex], point, maxDepth);
+}
+
+// For Nearest neighbor search.
+template<typename T>
+struct PQueueElem
+{
+    PQueueElem() : dist(0), t(0) {}
+    PQueueElem(float _dist, T _t) : dist(_dist), t(_t) {}
+    float dist;
+    T t;
+
+    bool
+    operator<(const PQueueElem<T> p1) const
+    {
+        return (this->dist < p1.dist);
+    }
+};
+
+static float SquaredDistance(const Point3f& query, const Point3f& origin)
+{
+    Point3f diff = query - origin;
+    return diff.dot(diff);
+}
+
+static bool overlap(const OctreeNode& node, const Point3f& query, float squareRadius)
+{
+    float halfSize = float(node.size * 0.5);
+    Point3f center = node.origin + Point3f( halfSize, halfSize, halfSize );
+
+    float dist = SquaredDistance(center, query);
+    float temp = float(node.size) * float(node.size) * 3.0f;
+
+    return ( dist + dist * std::numeric_limits<float>::epsilon() ) <= float(temp * 0.25f + squareRadius + sqrt(temp * squareRadius)) ;
+}
+
+void radiusNNSearchRecurse(const Ptr<OctreeNode>& node, const Point3f& query, float squareRadius,
+                           std::vector<PQueueElem<Point3f> >& candidatePoint)
+{
+    float dist;
+    Ptr<OctreeNode> child;
+
+    // iterate eight children.
+    for(size_t i = 0; i< OCTREE_CHILD_NUM; i++)
+    {
+        if( !node->children[i].empty() && overlap(*node->children[i], query, squareRadius))
+        {
+            if(!node->children[i]->isLeaf)
+            {
+                // Reach the branch node.
+                radiusNNSearchRecurse(node->children[i], query, squareRadius, candidatePoint);
+            }
+            else
+            {
+                // Reach the leaf node.
+                child = node->children[i];
+
+                for(size_t j = 0; j < child->pointList.size(); j++)
+                {
+                    dist = SquaredDistance(child->pointList[j], query);
+                    if(dist + dist * std::numeric_limits<float>::epsilon() <= squareRadius )
+                    {
+                        candidatePoint.emplace_back(dist, child->pointList[j]);
+                    }
+                }
+            }
+        }
+    }
+}
+
+int Octree::radiusNNSearch(const Point3f& query, float radius,
+                           std::vector<Point3f>& pointSet, std::vector<float>& squareDistSet) const
+{
+    if(p->rootNode.empty())
+        return 0;
+    float squareRadius = radius * radius;
+
+    PQueueElem<Point3f> elem;
+    std::vector<PQueueElem<Point3f> > candidatePoint;
+
+    radiusNNSearchRecurse(p->rootNode, query, squareRadius, candidatePoint);
+
+    for(size_t i = 0; i < candidatePoint.size(); i++)
+    {
+        pointSet.push_back(candidatePoint[i].t);
+        squareDistSet.push_back(candidatePoint[i].dist);
+    }
+    return int(pointSet.size());
+}
+
+void KNNSearchRecurse(const Ptr<OctreeNode>& node, const Point3f& query, const int K,
+                      float& smallestDist, std::vector<PQueueElem<Point3f> >& candidatePoint)
+{
+    std::vector<PQueueElem<int> > priorityQue;
+    Ptr<OctreeNode> child;
+    float dist = 0;
+    Point3f center; // the OctreeNode Center
+
+    // Add the non-empty OctreeNode to priorityQue.
+    for(size_t i = 0; i < OCTREE_CHILD_NUM; i++)
+    {
+        if(!node->children[i].empty())
+        {
+            float halfSize = float(node->children[i]->size * 0.5);
+
+            center = node->children[i]->origin + Point3f(halfSize, halfSize, halfSize);
+
+            dist = SquaredDistance(query, center);
+            priorityQue.emplace_back(dist, int(i));
+        }
+    }
+
+    std::sort(priorityQue.rbegin(), priorityQue.rend());
+    child = node->children[priorityQue.back().t];
+
+    while (!priorityQue.empty() && overlap(*child, query, smallestDist))
+    {
+        if (!child->isLeaf)
+        {
+            KNNSearchRecurse(child, query, K, smallestDist, candidatePoint);
+        } else {
+            for (size_t i = 0; i < child->pointList.size(); i++) {
+                dist = SquaredDistance(child->pointList[i], query);
+
+                if ( dist + dist * std::numeric_limits<float>::epsilon() <= smallestDist ) {
+                    candidatePoint.emplace_back(dist, child->pointList[i]);
+                }
+            }
+
+            std::sort(candidatePoint.begin(), candidatePoint.end());
+
+            if (int(candidatePoint.size()) > K) {
+                candidatePoint.resize(K);
+            }
+
+            if (int(candidatePoint.size()) == K) {
+                smallestDist = candidatePoint.back().dist;
+            }
+        }
+
+        priorityQue.pop_back();
+
+        // To next child.
+        if(!priorityQue.empty())
+            child = node->children[priorityQue.back().t];
+    }
+}
+
+void Octree::KNNSearch(const Point3f& query, const int K, std::vector<Point3f>& pointSet, std::vector<float>& squareDistSet) const
+{
+    if(p->rootNode.empty())
+        return;
+
+    PQueueElem<Ptr<Point3f> > elem;
+    std::vector<PQueueElem<Point3f> > candidatePoint;
+    float smallestDist = std::numeric_limits<float>::max();
+
+    KNNSearchRecurse(p->rootNode, query, K, smallestDist, candidatePoint);
+
+    for(size_t i = 0; i < candidatePoint.size(); i++)
+    {
+        pointSet.push_back(candidatePoint[i].t);
+        squareDistSet.push_back(candidatePoint[i].dist);
+    }
+}
+
+}

modules/3d/src/octree.hpp

@@ -0,0 +1,99 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+//
+// Copyright (C) 2021, Huawei Technologies Co., Ltd. All rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Author: Zihao Mu <zihaomu6@gmail.com>
+//         Liangqian Kong <chargerKong@126.com>
+//         Longbu Wang <riskiest@gmail.com>
+
+#ifndef OPENCV_3D_SRC_OCTREE_HPP
+#define OPENCV_3D_SRC_OCTREE_HPP
+
+#include <vector>
+#include "opencv2/core.hpp"
+
+namespace cv {
+
+/** @brief OctreeNode for Octree.
+
+The class OctreeNode represents the node of the octree. Each node contains 8 children, which are used to divide the
+space cube into eight parts. Each octree node represents a cube.
+And these eight children will have a fixed order, the order is described as follows:
+
+For illustration, assume,
+    rootNode: origin == (0, 0, 0), size == 2
+ Then,
+    children[0]: origin == (0, 0, 0), size == 1
+    children[1]: origin == (1, 0, 0), size == 1, along X-axis next to child 0
+    children[2]: origin == (0, 1, 0), size == 1, along Y-axis next to child 0
+    children[3]: origin == (1, 1, 0), size == 1, in X-Y plane
+    children[4]: origin == (0, 0, 1), size == 1, along Z-axis next to child 0
+    children[5]: origin == (1, 0, 1), size == 1, in X-Z plane
+    children[6]: origin == (0, 1, 1), size == 1, in Y-Z plane
+    children[7]: origin == (1, 1, 1), size == 1, furthest from child 0
+
+There are two kinds of nodes in an octree, intermediate nodes and leaf nodes, which are distinguished by isLeaf.
+Intermediate nodes are used to contain leaf nodes, and leaf nodes will contain pointers to all pointcloud data
+within the node, which will be used for octree indexing and mapping from point clouds to octree. Note that,
+in an octree, each leaf node contains at least one point cloud data. Similarly, every intermediate OctreeNode
+contains at least one non-empty child pointer, except for the root node.
+*/
+class OctreeNode{
+public:
+
+    /**
+    * There are multiple constructors to create OctreeNode.
+    * */
+    OctreeNode();
+
+    /** @overload
+    *
+    * @param _depth The depth of the current node. The depth of the root node is 0, and the leaf node is equal
+    * to the depth of Octree.
+    * @param _size The length of the OctreeNode. In space, every OctreeNode represents a cube.
+    * @param _origin The absolute coordinates of the center of the cube.
+    * @param _parentIndex The serial number of the child of the current node in the parent node,
+    * the range is (-1~7). Among them, only the root node's _parentIndex is -1.
+    */
+    OctreeNode(int _depth, double _size, const Point3f& _origin, int _parentIndex);
+
+    //! returns true if the rootNode is NULL.
+    bool empty() const;
+
+    bool isPointInBound(const Point3f& _point, const Point3f& _origin, double _size) const;
+
+    bool isPointInBound(const Point3f& _point) const;
+
+    //! Contains 8 pointers to its 8 children.
+    std::vector< Ptr<OctreeNode> > children;
+
+    //! Point to the parent node of the current node. The root node has no parent node and the value is NULL.
+    Ptr<OctreeNode> parent = nullptr;
+
+    //! The depth of the current node. The depth of the root node is 0, and the leaf node is equal to the depth of Octree.
+    int depth;
+
+    //! The length of the OctreeNode. In space, every OctreeNode represents a cube.
+    double size;
+
+    //! Absolute coordinates of the smallest point of the cube.
+    //! And the center of cube is `center = origin + Point3f(size/2, size/2, size/2)`.
+    Point3f origin;
+
+    /**  The serial number of the child of the current node in the parent node,
+    * the range is (-1~7). Among them, only the root node's _parentIndex is -1.
+    */
+    int parentIndex;
+
+    //! If the OctreeNode is LeafNode.
+    bool isLeaf = false;
+
+    //! Contains pointers to all point cloud data in this node.
+    std::vector<Point3f> pointList;
+};
+
+}
+#endif //OPENCV_3D_SRC_OCTREE_HPP

modules/3d/test/test_octree.cpp

@@ -0,0 +1,113 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+
+#include "test_precomp.hpp"
+
+namespace opencv_test { namespace {
+
+using namespace cv;
+
+class OctreeTest: public testing::Test
+{
+protected:
+    void SetUp() override
+    {
+        pointCloudSize = 1000;
+        maxDepth = 4;
+
+        int scale;
+        Point3i pmin, pmax;
+        scale = 1<<20;
+        pmin = Point3i(-scale, -scale, -scale);
+        pmax = Point3i(scale, scale, scale);
+
+        RNG& rng_Point = theRNG(); // set random seed for fixing output 3D point.
+
+        // Generate 3D PointCloud
+        for(int i = 0; i < pointCloudSize; i++)
+        {
+            float _x = 10 * (float)rng_Point.uniform(pmin.x, pmax.x)/scale;
+            float _y = 10 * (float)rng_Point.uniform(pmin.y, pmax.y)/scale;
+            float _z = 10 * (float)rng_Point.uniform(pmin.z, pmax.z)/scale;
+            pointcloud.push_back(Point3f(_x, _y, _z));
+        }
+
+        // Generate Octree From PointCloud.
+        treeTest.create(pointcloud, maxDepth);
+
+        // Randomly generate another 3D point.
+        float _x = 10 * (float)rng_Point.uniform(pmin.x, pmax.x)/scale;
+        float _y = 10 * (float)rng_Point.uniform(pmin.y, pmax.y)/scale;
+        float _z = 10 * (float)rng_Point.uniform(pmin.z, pmax.z)/scale;
+        restPoint = Point3f(_x, _y, _z);
+
+    }
+
+public:
+    std::vector<Point3f> pointcloud;
+    int pointCloudSize;
+    Point3f restPoint;
+    Octree treeTest;
+
+private:
+    int maxDepth;
+};
+
+TEST_F(OctreeTest, BasicFunctionTest)
+{
+    // Check if the point in Bound.
+    EXPECT_TRUE(treeTest.isPointInBound(restPoint));
+    EXPECT_FALSE(treeTest.isPointInBound(restPoint + Point3f(20, 20, 20)));
+
+    // insert, delete Test.
+    EXPECT_FALSE(treeTest.deletePoint(restPoint));
+
+    EXPECT_THROW(treeTest.insertPoint(restPoint + Point3f(20, 20, 20)), cv::Exception);
+    EXPECT_NO_THROW(treeTest.insertPoint(restPoint));
+
+    EXPECT_TRUE(treeTest.deletePoint(restPoint));
+
+    EXPECT_FALSE(treeTest.empty());
+    EXPECT_NO_THROW(treeTest.clear());
+    EXPECT_TRUE(treeTest.empty());
+
+}
+
+TEST_F(OctreeTest, RadiusSearchTest)
+{
+    float radius = 2.0f;
+    std::vector<Point3f> outputPoints;
+    std::vector<float> outputSquareDist;
+    EXPECT_NO_THROW(treeTest.radiusNNSearch(restPoint, radius, outputPoints, outputSquareDist));
+
+    EXPECT_FLOAT_EQ(outputPoints[0].x, -8.88461112976f);
+    EXPECT_FLOAT_EQ(outputPoints[0].y, -1.881799697875f);
+    EXPECT_FLOAT_EQ(outputPoints[1].x, -8.405818939208f);
+    EXPECT_FLOAT_EQ(outputPoints[1].y, -2.991247177124f);
+    EXPECT_FLOAT_EQ(outputPoints[2].x, -8.1184864044189f);
+    EXPECT_FLOAT_EQ(outputPoints[2].y, -0.528564453125f);
+    EXPECT_FLOAT_EQ(outputPoints[3].x, -6.551313400268f);
+    EXPECT_FLOAT_EQ(outputPoints[3].y, -0.708484649658f);
+}
+
+TEST_F(OctreeTest, KNNSearchTest)
+{
+    int K = 10;
+    std::vector<Point3f> outputPoints;
+    std::vector<float> outputSquareDist;
+    EXPECT_NO_THROW(treeTest.KNNSearch(restPoint, K, outputPoints, outputSquareDist));
+
+    EXPECT_FLOAT_EQ(outputPoints[0].x, -8.118486404418f);
+    EXPECT_FLOAT_EQ(outputPoints[0].y, -0.528564453125f);
+    EXPECT_FLOAT_EQ(outputPoints[1].x, -8.405818939208f);
+    EXPECT_FLOAT_EQ(outputPoints[1].y, -2.991247177124f);
+    EXPECT_FLOAT_EQ(outputPoints[2].x, -8.88461112976f);
+    EXPECT_FLOAT_EQ(outputPoints[2].y, -1.881799697875f);
+    EXPECT_FLOAT_EQ(outputPoints[3].x, -6.551313400268f);
+    EXPECT_FLOAT_EQ(outputPoints[3].y, -0.708484649658f);
+}
+
+
+} // namespace
+} // opencv_test