From 5c0ac371639c594ffefb0a365ffce4a001303df9 Mon Sep 17 00:00:00 2001 From: zihaomu Date: Tue, 8 Jun 2021 11:19:01 +0800 Subject: [PATCH] add Octree to 3d module of next branch. --- modules/3d/include/opencv2/3d.hpp | 145 ++++++++ modules/3d/src/octree.cpp | 546 ++++++++++++++++++++++++++++++ modules/3d/src/octree.hpp | 99 ++++++ modules/3d/test/test_octree.cpp | 113 +++++++ 4 files changed, 903 insertions(+) create mode 100644 modules/3d/src/octree.cpp create mode 100644 modules/3d/src/octree.hpp create mode 100644 modules/3d/test/test_octree.cpp diff --git a/modules/3d/include/opencv2/3d.hpp b/modules/3d/include/opencv2/3d.hpp index c5f0b8e94b..2d5f1c6c2c 100644 --- a/modules/3d/include/opencv2/3d.hpp +++ b/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 &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 &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 &pointSet, std::vector &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 &pointSet, std::vector &squareDistSet) const; + +protected: + struct Impl; + Ptr p; + +}; + //! @} _3d } //end namespace cv diff --git a/modules/3d/src/octree.cpp b/modules/3d/src/octree.cpp new file mode 100644 index 0000000000..be284a38a2 --- /dev/null +++ b/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 index(const Point3f& point, Ptr& node); + +static bool _isPointInBound(const Point3f& _point, const Point3f& _origin, double _size); +static void insertPointRecurse( Ptr& node, const Point3f& point, int maxDepth); +bool deletePointRecurse( Ptr& node); + +// For Nearest neighbor search. +template struct PQueueElem; // Priority queue +static void radiusNNSearchRecurse(const Ptr& node, const Point3f& query, float squareRadius, + std::vector >& candidatePoint); +static void KNNSearchRecurse(const Ptr& node, const Point3f& query, const int K, + float& smallestDist, std::vector >& 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::epsilon() * std::max(std::abs(_point.x), std::abs(_origin.x)); + float epsY = std::numeric_limits::epsilon() * std::max(std::abs(_point.y), std::abs(_origin.y)); + float epsZ = std::numeric_limits::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 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& _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 &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 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 index(const Point3f& point, Ptr& _node) +{ + OctreeNode &node = *_node; + + if(node.empty()) + { + return Ptr(); + } + + 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(); + } + + if(node.isPointInBound(point)) + { + double childSize = node.size * 0.5; + + float epsX = std::numeric_limits::epsilon() * std::max(std::abs(point.x), std::abs(node.origin.x)); + float epsY = std::numeric_limits::epsilon() * std::max(std::abs(point.y), std::abs(node.origin.y)); + float epsZ = std::numeric_limits::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(); +} + +bool Octree::isPointInBound(const Point3f& _point) const +{ + return _isPointInBound(_point, p->origin, p->size); +} + +bool Octree::deletePoint(const Point3f& point) +{ + Ptr 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& _node) +{ + OctreeNode& node = *_node; + + if(_node.empty()) + return false; + + if(node.isLeaf) + { + if( !node.pointList.empty()) + { + Ptr 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 parent = node.parent; + _node.release(); + return deletePointRecurse(parent); + } + else + { + return true; + } + } +} + +void insertPointRecurse( Ptr& _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::epsilon() * std::max(std::abs(point.x), std::abs(node.origin.x)); + float epsY = std::numeric_limits::epsilon() * std::max(std::abs(point.y), std::abs(node.origin.y)); + float epsZ = std::numeric_limits::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 +struct PQueueElem +{ + PQueueElem() : dist(0), t(0) {} + PQueueElem(float _dist, T _t) : dist(_dist), t(_t) {} + float dist; + T t; + + bool + operator<(const PQueueElem 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::epsilon() ) <= float(temp * 0.25f + squareRadius + sqrt(temp * squareRadius)) ; +} + +void radiusNNSearchRecurse(const Ptr& node, const Point3f& query, float squareRadius, + std::vector >& candidatePoint) +{ + float dist; + Ptr 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::epsilon() <= squareRadius ) + { + candidatePoint.emplace_back(dist, child->pointList[j]); + } + } + } + } + } +} + +int Octree::radiusNNSearch(const Point3f& query, float radius, + std::vector& pointSet, std::vector& squareDistSet) const +{ + if(p->rootNode.empty()) + return 0; + float squareRadius = radius * radius; + + PQueueElem elem; + std::vector > 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& node, const Point3f& query, const int K, + float& smallestDist, std::vector >& candidatePoint) +{ + std::vector > priorityQue; + Ptr 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::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& pointSet, std::vector& squareDistSet) const +{ + if(p->rootNode.empty()) + return; + + PQueueElem > elem; + std::vector > candidatePoint; + float smallestDist = std::numeric_limits::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); + } +} + +} \ No newline at end of file diff --git a/modules/3d/src/octree.hpp b/modules/3d/src/octree.hpp new file mode 100644 index 0000000000..af0ae9fd54 --- /dev/null +++ b/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 +// Liangqian Kong +// Longbu Wang + +#ifndef OPENCV_3D_SRC_OCTREE_HPP +#define OPENCV_3D_SRC_OCTREE_HPP + +#include +#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 > children; + + //! Point to the parent node of the current node. The root node has no parent node and the value is NULL. + Ptr 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 pointList; +}; + +} +#endif //OPENCV_3D_SRC_OCTREE_HPP \ No newline at end of file diff --git a/modules/3d/test/test_octree.cpp b/modules/3d/test/test_octree.cpp new file mode 100644 index 0000000000..45d60600f9 --- /dev/null +++ b/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 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 outputPoints; + std::vector 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 outputPoints; + std::vector 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