diff --git a/modules/3d/include/opencv2/3d.hpp b/modules/3d/include/opencv2/3d.hpp index ee09dd6482..f8b284e0b2 100644 --- a/modules/3d/include/opencv2/3d.hpp +++ b/modules/3d/include/opencv2/3d.hpp @@ -2676,112 +2676,156 @@ CV_EXPORTS_W bool solvePnP( InputArray objectPoints, InputArray imagePoints, * children[7]: origin == (1, 1, 1), size == 1, furthest from child 0 */ -class CV_EXPORTS Octree { - +class CV_EXPORTS_W 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); + * @brief Creates an empty Octree with given maximum depth + * + * @param maxDepth The max depth of the Octree + * @param size bounding box size for the Octree + * @param origin Initial center coordinate + * @param withColors Whether to keep per-point colors or not + * @return resulting Octree + */ + CV_WRAP static Ptr createWithDepth(int maxDepth, double size, const Point3f& origin = { }, bool withColors = false); /** @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); + * @brief Create an Octree from the PointCloud data with the specific maxDepth + * + * @param maxDepth Max depth of the octree + * @param pointCloud point cloud data, should be 3-channel float array + * @param colors color attribute of point cloud in the same 3-channel float format + * @return resulting Octree + */ + CV_WRAP static Ptr createWithDepth(int maxDepth, InputArray pointCloud, InputArray colors = noArray()); /** @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); + * @brief Creates an empty Octree with given resolution + * + * @param resolution The size of the octree leaf node + * @param size bounding box size for the Octree + * @param origin Initial center coordinate + * @param withColors Whether to keep per-point colors or not + * @return resulting Octree + */ + CV_WRAP static Ptr createWithResolution(double resolution, double size, const Point3f& origin = { }, bool withColors = false); + + /** @overload + * @brief Create an Octree from the PointCloud data with the specific resolution + * + * @param resolution The size of the octree leaf node + * @param pointCloud point cloud data, should be 3-channel float array + * @param colors color attribute of point cloud in the same 3-channel float format + * @return resulting octree + */ + CV_WRAP static Ptr createWithResolution(double resolution, InputArray pointCloud, InputArray colors = noArray()); //! Default destructor ~Octree(); - /** @brief Insert a point data to a OctreeNode. + /** @overload + * @brief Insert a point data with color to a OctreeNode. * * @param point The point data in Point3f format. + * @param color The color attribute of point in Point3f format. * @return Returns whether the insertion is successful. */ - bool 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); + CV_WRAP bool insertPoint(const Point3f& point, const Point3f& color = { }); /** @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); + CV_WRAP bool isPointInBound(const Point3f& point) const; //! returns true if the rootnode is NULL. - bool empty() const; + CV_WRAP bool empty() const; /** @brief Reset all octree parameter. * * Clear all the nodes of the octree and initialize the parameters. */ - void clear(); + CV_WRAP 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. + * @param point The point coordinates, comparison is epsilon-based * @return return ture if the point is deleted successfully. */ - bool deletePoint(const Point3f& point); + CV_WRAP bool deletePoint(const Point3f& point); - /** @brief Radius Nearest Neighbor Search in Octree + /** @brief restore point cloud data from Octree. + * + * Restore the point cloud data from existing octree. The points in same leaf node will be seen as the same point. + * This point is the center of the leaf node. If the resolution is small, it will work as a downSampling function. + * @param restoredPointCloud The output point cloud data, can be replaced by noArray() if not needed + * @param restoredColor The color attribute of point cloud data, can be omitted if not needed + */ + CV_WRAP void getPointCloudByOctree(OutputArray restoredPointCloud, OutputArray restoredColor = noArray()); + + /** @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. + * @param points Point output. Contains searched points in 3-float format, and output vector is not in order, + * can be replaced by noArray() if not needed + * @param squareDists Dist output. Contains searched squared distance in floats, and output vector is not in order, + * can be omitted if not needed * @return the number of searched points. */ - int radiusNNSearch(const Point3f& query, float radius, std::vector &pointSet, std::vector &squareDistSet) const; + CV_WRAP int radiusNNSearch(const Point3f& query, float radius, OutputArray points, OutputArray squareDists = noArray()) const; + + /** @overload + * @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 points Point output. Contains searched points in 3-float format, and output vector is not in order, + * can be replaced by noArray() if not needed + * @param colors Color output. Contains colors corresponding to points in pointSet, can be replaced by noArray() if not needed + * @param squareDists Dist output. Contains searched squared distance in floats, and output vector is not in order, + * can be replaced by noArray() if not needed + * @return the number of searched points. + */ + CV_WRAP int radiusNNSearch(const Point3f& query, float radius, OutputArray points, OutputArray colors, OutputArray squareDists) 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. + * @param K amount of nearest neighbors to find + * @param points Point output. Contains K points in 3-float format, arranged in order of distance from near to far, + * can be replaced by noArray() if not needed + * @param squareDists Dist output. Contains K squared distance in floats, arranged in order of distance from near to far, + * can be omitted if not needed */ - void KNNSearch(const Point3f& query, const int K, std::vector &pointSet, std::vector &squareDistSet) const; + CV_WRAP void KNNSearch(const Point3f& query, const int K, OutputArray points, OutputArray squareDists = noArray()) const; + + /** @overload + * @brief K Nearest Neighbor Search in Octree. + * + * Find the K nearest neighbors to the query point. + * @param query Query point. + * @param K amount of nearest neighbors to find + * @param points Point output. Contains K points in 3-float format, arranged in order of distance from near to far, + * can be replaced by noArray() if not needed + * @param colors Color output. Contains colors corresponding to points in pointSet, can be replaced by noArray() if not needed + * @param squareDists Dist output. Contains K squared distance in floats, arranged in order of distance from near to far, + * can be replaced by noArray() if not needed + */ + CV_WRAP void KNNSearch(const Point3f& query, const int K, OutputArray points, OutputArray colors, OutputArray squareDists) const; protected: struct Impl; diff --git a/modules/3d/misc/python/test/test_octree.py b/modules/3d/misc/python/test/test_octree.py new file mode 100644 index 0000000000..db5a95353e --- /dev/null +++ b/modules/3d/misc/python/test/test_octree.py @@ -0,0 +1,31 @@ +#!/usr/bin/env python + +import os +import numpy as np +import math +import unittest +import cv2 as cv + +from tests_common import NewOpenCVTests + +class octree_test(NewOpenCVTests): + def test_octree_basic_test(self): + pointCloudSize = 1000 + resolution = 0.0001 + scale = 1 << 20 + + pointCloud = np.random.randint(-scale, scale, size=(pointCloudSize, 3)) * (10.0 / scale) + pointCloud = pointCloud.astype(np.float32) + + octree = cv.Octree_createWithResolution(resolution, pointCloud) + + restPoint = np.random.randint(-scale, scale, size=(1, 3)) * (10.0 / scale) + restPoint = [restPoint[0, 0], restPoint[0, 1], restPoint[0, 2]] + + self.assertTrue(octree.isPointInBound(restPoint)) + self.assertFalse(octree.deletePoint(restPoint)) + self.assertTrue(octree.insertPoint(restPoint)) + self.assertTrue(octree.deletePoint(restPoint)) + +if __name__ == '__main__': + NewOpenCVTests.bootstrap() diff --git a/modules/3d/src/octree.cpp b/modules/3d/src/octree.cpp index 7dfc6f47fc..04811d6183 100644 --- a/modules/3d/src/octree.cpp +++ b/modules/3d/src/octree.cpp @@ -4,32 +4,27 @@ #include "precomp.hpp" #include "octree.hpp" - -#define OCTREE_CHILD_NUM 8 +#include "opencv2/3d.hpp" namespace cv{ -// Locate the OctreeNode corresponding to the input point from the given OctreeNode. -static Ptr index(const Point3f& point, Ptr& node); +OctreeNode::OctreeNode() : + children(), + depth(0), + size(0), + origin(0,0,0), + neigh(), + parentIndex(-1) +{ } -static bool _isPointInBound(const Point3f& _point, const Point3f& _origin, double _size); -static bool 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) -{ -} +OctreeNode::OctreeNode(int _depth, double _size, const Point3f &_origin, int _parentIndex) : + children(), + depth(_depth), + size(_size), + origin(_origin), + neigh(), + parentIndex(_parentIndex) +{ } bool OctreeNode::empty() const { @@ -42,7 +37,7 @@ bool OctreeNode::empty() const } else { - for(size_t i = 0; i< OCTREE_CHILD_NUM; i++) + for(size_t i = 0; i < 8; i++) { if(!this->children[i].empty()) { @@ -53,166 +48,255 @@ bool OctreeNode::empty() const } } + bool OctreeNode::isPointInBound(const Point3f& _point) const { - return isPointInBound(_point, origin, size); -} + Point3f eps; + eps.x = std::max(std::abs(_point.x), std::abs(this->origin.x)); + eps.y = std::max(std::abs(_point.y), std::abs(this->origin.y)); + eps.z = std::max(std::abs(_point.z), std::abs(this->origin.z)); + eps *= std::numeric_limits::epsilon(); + Point3f ptEps = _point + eps; + Point3f upPt = this->origin + eps + Point3f {(float)this->size, (float)this->size, (float)this->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; - } + return (ptEps.x >= this->origin.x) && + (ptEps.y >= this->origin.y) && + (ptEps.z >= this->origin.z) && + (_point.x <= upPt.x) && + (_point.y <= upPt.y) && + (_point.z <= upPt.z); } struct Octree::Impl { public: - Impl():maxDepth(-1), size(0), origin(0,0,0) - {} + Impl() : Impl(0, 0, {0, 0, 0}, 0, false) { } - ~Impl() - {} + Impl(int _maxDepth, double _size, const Point3f& _origin, double _resolution, + bool _hasColor) : + maxDepth(_maxDepth), + size(_size), + origin(_origin), + resolution(_resolution), + hasColor(_hasColor) + { } - // The pointer to Octree root node. + ~Impl() { } + + void fill(bool useResolution, InputArray pointCloud, InputArray colorAttribute); + bool insertPoint(const Point3f& point, const Point3f &color); + + // The pointer to Octree root node Ptr rootNode = nullptr; - //! Max depth of the Octree. And depth must be greater than zero + //! Max depth of the Octree int maxDepth; - //! The size of the cube of the . + //! The size of the cube double size; - //! The origin coordinate of root node. + //! The origin coordinate of root node Point3f origin; + //! The size of the leaf node + double resolution; + //! Whether the point cloud has a color attribute + bool hasColor; }; -Octree::Octree() : p(new Impl) +Octree::Octree() : + p(makePtr()) +{ } + +Ptr Octree::createWithDepth(int maxDepth, double size, const Point3f& origin, bool withColors) { - p->maxDepth = -1; - p->size = 0; - p->origin = Point3f(0,0,0); + CV_Assert(maxDepth > 0); + CV_Assert(size > 0); + + Ptr octree = makePtr(); + octree->p = makePtr(maxDepth, size, origin, /*resolution*/ 0, withColors); + return octree; } -Octree::Octree(int _maxDepth, double _size, const Point3f& _origin ) : p(new Impl) +Ptr Octree::createWithDepth(int maxDepth, InputArray pointCloud, InputArray colors) { - p->maxDepth = _maxDepth; - p->size = _size; - p->origin = _origin; + CV_Assert(maxDepth > 0); + + Ptr octree = makePtr(); + octree->p->maxDepth = maxDepth; + octree->p->fill(/* useResolution */ false, pointCloud, colors); + return octree; } -Octree::Octree(const std::vector& _pointCloud, int _maxDepth) : p(new Impl) +Ptr Octree::createWithResolution(double resolution, double size, const Point3f& origin, bool withColors) { - CV_Assert( _maxDepth > -1 ); - this->create(_pointCloud, _maxDepth); + CV_Assert(resolution > 0); + CV_Assert(size > 0); + + Ptr octree = makePtr(); + octree->p = makePtr(/*maxDepth*/ 0, size, origin, resolution, withColors); + return octree; } -Octree::Octree(int _maxDepth) : p(new Impl) +Ptr Octree::createWithResolution(double resolution, InputArray pointCloud, InputArray colors) { - p->maxDepth = _maxDepth; - p->size = 0; - p->origin = Point3f(0,0,0); + CV_Assert(resolution > 0); + + Ptr octree = makePtr(); + octree->p->resolution = resolution; + octree->p->fill(/* useResolution */ true, pointCloud, colors); + return octree; } -Octree::~Octree(){} +Octree::~Octree() { } -bool Octree::insertPoint(const Point3f& point) +bool Octree::insertPoint(const Point3f& point, const Point3f &color) { - if(p->rootNode.empty()) + return p->insertPoint(point, color); +} + +bool Octree::Impl::insertPoint(const Point3f& point, const Point3f &color) +{ + size_t depthMask = (size_t)(1ULL << (this->maxDepth - 1)); + + if(this->rootNode.empty()) { - p->rootNode = new OctreeNode( 0, p->size, p->origin, -1); + this->rootNode = new OctreeNode( 0, this->size, this->origin, -1); } - return insertPointRecurse(p->rootNode, point, p->maxDepth); -} - - -bool Octree::create(const std::vector &pointCloud, int _maxDepth) -{ - if(_maxDepth > -1) + bool pointInBoundFlag = this->rootNode->isPointInBound(point); + if(this->rootNode->depth == 0 && !pointInBoundFlag) { - 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 origin.x) / this->resolution), + (size_t)floor((point.y - this->origin.y) / this->resolution), + (size_t)floor((point.z - this->origin.z) / this->resolution)); - 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. - int cnt=0; - for(size_t idx = 0; idx < pointCloud.size(); idx++ ) + Ptr node = this->rootNode; + while (node->depth != maxDepth) { - if(!insertPoint(pointCloud[idx])){ - cnt++; - }; + double childSize = node->size * 0.5; + + // calculate the index and the origin of child. + size_t childIndex = key.findChildIdxByMask(depthMask); + size_t xIndex = (childIndex & 1) ? 1 : 0; + size_t yIndex = (childIndex & 2) ? 1 : 0; + size_t zIndex = (childIndex & 4) ? 1 : 0; + + Point3f childOrigin = node->origin + Point3f(float(xIndex), float(yIndex), float(zIndex)) * float(childSize); + + Ptr &childPtr = node->children[childIndex]; + if (!childPtr) + { + childPtr = new OctreeNode(node->depth + 1, childSize, childOrigin, int(childIndex)); + childPtr->parent = node; + } + + node = childPtr; + depthMask = depthMask >> 1; } - CV_LOG_IF_WARNING(NULL,cnt!=0,"OverAll "<isLeaf = true; + node->pointList.push_back(point); + node->colorList.push_back(color); + return true; } -void Octree::setMaxDepth(int _maxDepth) + +static Vec6f getBoundingBox(const Mat& points) { - if(_maxDepth ) - this->p->maxDepth = _maxDepth; + const float mval = std::numeric_limits::max(); + Vec6f bb(mval, mval, mval, -mval, -mval, -mval); + + for (int i = 0; i < (int)points.total(); i++) + { + Point3f pt = points.at(i); + bb[0] = min(bb[0], pt.x); + bb[1] = min(bb[1], pt.y); + bb[2] = min(bb[2], pt.z); + bb[3] = max(bb[3], pt.x); + bb[4] = max(bb[4], pt.y); + bb[5] = max(bb[5], pt.z); + } + + return bb; } -void Octree::setSize(double _size) +void Octree::Impl::fill(bool useResolution, InputArray _points, InputArray _colors) { - this->p->size = _size; -}; + CV_CheckFalse(_points.empty(), "No points provided"); -void Octree::setOrigin(const Point3f& _origin) -{ - this->p->origin = _origin; + Mat points, colors; + int nPoints = 0, nColors = 0; + + int pointType = _points.type(); + CV_Assert(pointType == CV_32FC1 || pointType == CV_32FC3); + points = _points.getMat(); + // transform 3xN matrix to Nx3, except 3x3 + if ((_points.channels() == 1) && (_points.rows() == 3) && (_points.cols() != 3)) + { + points = points.t(); + } + // This transposition is performed on 1xN matrix so it's almost free in terms of performance + points = points.reshape(3, 1).t(); + nPoints = (int)points.total(); + + if (!_colors.empty()) + { + int colorType = _colors.type(); + CV_Assert(colorType == CV_32FC1 || colorType == CV_32FC3); + colors = _colors.getMat(); + // transform 3xN matrix to Nx3, except 3x3 + if ((_colors.channels() == 1) && (_colors.rows() == 3) && (_colors.cols() != 3)) + { + colors = colors.t(); + } + colors = colors.reshape(3, 1).t(); + nColors = (int)colors.total(); + + CV_Assert(nColors == nPoints); + this->hasColor = true; + } + + Vec6f bbox = getBoundingBox(points); + Point3f minBound(bbox[0], bbox[1], bbox[2]); + Point3f maxBound(bbox[3], bbox[4], bbox[5]); + + double maxSize = max(max(maxBound.x - minBound.x, maxBound.y - minBound.y), maxBound.z - minBound.z); + + // Extend maxSize to the closest power of 2 that exceeds it for bit operations + maxSize = double(1 << int(ceil(log2(maxSize)))); + + // to calculate maxDepth from resolution or vice versa + if (useResolution) + { + this->maxDepth = (int)ceil(log2(maxSize / this->resolution)); + } + else + { + this->resolution = (maxSize / (1 << (this->maxDepth + 1))); + } + + this->size = (1 << this->maxDepth) * this->resolution; + this->origin = Point3f(float(floor(minBound.x / this->resolution) * this->resolution), + float(floor(minBound.y / this->resolution) * this->resolution), + float(floor(minBound.z / this->resolution) * this->resolution)); + + // Insert every point in PointCloud data. + for (int idx = 0; idx < nPoints; idx++) + { + Point3f pt = points.at(idx); + Point3f insertColor = this->hasColor ? colors.at(idx) : Point3f { }; + if (!this->insertPoint(pt, insertColor)) + { + CV_Error(Error::StsBadArg, "The point is out of boundary!"); + } + } } + void Octree::clear() { - if(!p->rootNode.empty()) - { - p->rootNode.release(); - } - - p->size = 0; - p->maxDepth = -1; - p->origin = Point3f (0,0,0); // origin coordinate + p = makePtr(); } bool Octree::empty() const @@ -220,189 +304,174 @@ 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); + return p->rootNode->isPointInBound(_point); } bool Octree::deletePoint(const Point3f& point) { - Ptr node = index(point, p->rootNode); + OctreeKey key = OctreeKey((size_t)floor((point.x - this->p->origin.x) / p->resolution), + (size_t)floor((point.y - this->p->origin.y) / p->resolution), + (size_t)floor((point.z - this->p->origin.z) / p->resolution)); + size_t depthMask = (size_t)1 << (p->maxDepth - 1); - if(!node.empty()) + Ptr node = p->rootNode; + while(node) { - size_t i = 0; - while (!node->pointList.empty() && i < node->pointList.size() ) + if (node->empty()) { - 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++; - } + node = nullptr; } - - // 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()) + else if (node->isLeaf) { - Ptr parent = node.parent; - parent->children[node.parentIndex] = nullptr; - _node.release(); - - return deletePointRecurse(parent); + const float eps = 1e-9f; + bool found = std::any_of(node->pointList.begin(), node->pointList.end(), + [point, eps](const Point3f& pt) -> bool + { + return abs(point.x - pt.x) < eps && + abs(point.y - pt.y) < eps && + abs(point.z - pt.z) < eps; + }); + if (!found) + node = nullptr; + break; } else { - return true; + node = node->children[key.findChildIdxByMask(depthMask)]; + depthMask = depthMask >> 1; } } - else + + if(!node) + return false; + + const float eps = 1e-9f; + + // we've found a leaf node and delete all verts equal to given one + size_t ctr = 0; + while (!node->pointList.empty() && ctr < node->pointList.size()) { - 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 (abs(point.x - node->pointList[ctr].x) < eps && + abs(point.y - node->pointList[ctr].y) < eps && + abs(point.z - node->pointList[ctr].z) < eps) { - if(!node.children[i].empty()) - { - deleteFlag = false; - break; - } - } - - if(deleteFlag) - { - Ptr parent = node.parent; - _node.release(); - return deletePointRecurse(parent); + node->pointList.erase(node->pointList.begin() + ctr); } else { - return true; + ctr++; } } + + if (node->pointList.empty()) + { + // empty node and its empty parents should be removed + OctreeNode *parentPtr = node->parent; + int parentdIdx = node->parentIndex; + + while (parentPtr) + { + parentPtr->children[parentdIdx].release(); + + // check if all children were deleted + bool deleteFlag = true; + for (size_t i = 0; i < 8; i++) + { + if (!parentPtr->children[i].empty()) + { + deleteFlag = false; + break; + } + } + + if (deleteFlag) + { + // we're at empty node, going up + parentdIdx = parentPtr->parentIndex; + parentPtr = parentPtr->parent; + } + else + { + // reached first non-empty node, stopping + parentPtr = nullptr; + } + } + } + return true; } -bool insertPointRecurse( Ptr& _node, const Point3f& point, int maxDepth) + +void Octree::getPointCloudByOctree(OutputArray restorePointCloud, OutputArray restoreColor) { - OctreeNode& node = *_node; - bool pointInBoundFlag = node.isPointInBound(point, node.origin, node.size); - if(node.depth==0 && !pointInBoundFlag) + Ptr root = p->rootNode; + double resolution = p->resolution; + std::vector outPts, outColors; + + std::stack, size_t, size_t, size_t>> toCheck; + toCheck.push({root, 0, 0, 0}); + while (!toCheck.empty()) { - CV_Error(Error::StsBadArg, "The point is out of boundary!"); - } - else if (!pointInBoundFlag){ - return false; + auto top = toCheck.top(); + toCheck.pop(); + Ptr node = std::get<0>(top); + size_t x_key = std::get<1>(top); + size_t y_key = std::get<2>(top); + size_t z_key = std::get<3>(top); + + if (node->isLeaf) + { + outPts.emplace_back( + (float) (resolution * x_key) + (float) (resolution * 0.5) + p->origin.x, + (float) (resolution * y_key) + (float) (resolution * 0.5) + p->origin.y, + (float) (resolution * z_key) + (float) (resolution * 0.5) + p->origin.z); + if (p->hasColor) + { + Point3f avgColor { }; + for (const auto& c : node->colorList) + { + avgColor += c; + } + avgColor *= (1.f/(float)node->colorList.size()); + outColors.emplace_back(avgColor); + } + } + else + { + unsigned char x_mask = 1; + unsigned char y_mask = 2; + unsigned char z_mask = 4; + for (unsigned char i = 0; i < 8; i++) + { + size_t x_copy = x_key; + size_t y_copy = y_key; + size_t z_copy = z_key; + if (!node->children[i].empty()) + { + size_t x_offSet = !!(x_mask & i); + size_t y_offSet = !!(y_mask & i); + size_t z_offSet = !!(z_mask & i); + x_copy = (x_copy << 1) | x_offSet; + y_copy = (y_copy << 1) | y_offSet; + z_copy = (z_copy << 1) | z_offSet; + toCheck.push({node->children[i], x_copy, y_copy, z_copy}); + } + } + } } - if(node.depth == maxDepth) + if (restorePointCloud.needed()) { - node.isLeaf = true; - node.pointList.push_back(point); - return true; + Mat(outPts).copyTo(restorePointCloud); } - - 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()) + if (restoreColor.needed()) { - 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; + Mat(outColors).copyTo(restoreColor); } - return 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) { @@ -410,145 +479,216 @@ static float SquaredDistance(const Point3f& query, const Point3f& origin) return diff.dot(diff); } -static bool overlap(const OctreeNode& node, const Point3f& query, float squareRadius) +bool OctreeNode::overlap(const Point3f& query, float squareRadius) const { - float halfSize = float(node.size * 0.5); - Point3f center = node.origin + Point3f( halfSize, halfSize, halfSize ); + float halfSize = float(this->size * 0.5); + Point3f center = this->origin + Point3f( halfSize, halfSize, halfSize ); float dist = SquaredDistance(center, query); - float temp = float(node.size) * float(node.size) * 3.0f; + float temp = float(this->size) * float(this->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) + +int Octree::radiusNNSearch(const Point3f& query, float radius, OutputArray pointSet, OutputArray squareDistSet) const { - float dist; - Ptr child; + return this->radiusNNSearch(query, radius, pointSet, noArray(), squareDistSet); +} - // iterate eight children. - for(size_t i = 0; i< OCTREE_CHILD_NUM; i++) +int Octree::radiusNNSearch(const Point3f& query, float radius, OutputArray points, OutputArray colors, OutputArray squareDists) const +{ + std::vector outPoints, outColors; + std::vector outSqDists; + + if (!p->rootNode.empty()) { - 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]; + float squareRadius = radius * radius; - for(size_t j = 0; j < child->pointList.size(); j++) + std::vector> candidatePoints; + + std::stack> toCheck; + toCheck.push(p->rootNode); + + while (!toCheck.empty()) + { + Ptr node = toCheck.top(); + toCheck.pop(); + for(size_t i = 0; i < 8; i++) + { + Ptr child = node->children[i]; + if( child && child->overlap(query, squareRadius)) { - dist = SquaredDistance(child->pointList[j], query); - if(dist + dist * std::numeric_limits::epsilon() <= squareRadius ) + if(child->isLeaf) { - candidatePoint.emplace_back(dist, child->pointList[j]); + for(size_t j = 0; j < child->pointList.size(); j++) + { + Point3f pt = child->pointList[j]; + Point3f col; + if (!child->colorList.empty()) + { + col = child->colorList[j]; + } + float dist = SquaredDistance(pt, query); + if(dist + dist * std::numeric_limits::epsilon() <= squareRadius) + { + candidatePoints.emplace_back(dist, pt, col); + } + } + } + else + { + toCheck.push(child); } } } } - } -} -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()) + for (size_t i = 0; i < candidatePoints.size(); i++) { - float halfSize = float(node->children[i]->size * 0.5); + auto cp = candidatePoints[i]; + outSqDists.push_back(std::get<0>(cp)); + outPoints.push_back(std::get<1>(cp)); + outColors.push_back(std::get<2>(cp)); + } + } - center = node->children[i]->origin + Point3f(halfSize, halfSize, halfSize); + if (points.needed()) + { + Mat(outPoints).copyTo(points); + } + if (colors.needed()) + { + CV_Assert(this->p->hasColor); + Mat(outColors).copyTo(colors); + } + if (squareDists.needed()) + { + Mat(outSqDists).copyTo(squareDists); + } - dist = SquaredDistance(query, center); + return int(outPoints.size()); +} + + +void OctreeNode::KNNSearchRecurse(const Point3f& query, const int K, + float& smallestDist, std::vector>& candidatePoint) const +{ + std::vector> priorityQue; + + // Add the non-empty OctreeNode to priorityQue + for(size_t i = 0; i < 8; i++) + { + Ptr child = this->children[i]; + if(child) + { + float halfSize = float(child->size * 0.5); + + Point3f center = child->origin + Point3f(halfSize, halfSize, halfSize); + + float dist = SquaredDistance(query, center); priorityQue.emplace_back(dist, int(i)); } } - std::sort(priorityQue.rbegin(), priorityQue.rend()); - child = node->children[priorityQue.back().t]; + std::sort(priorityQue.rbegin(), priorityQue.rend(), + [](const std::pair& a, const std::pair& b) -> bool + { + return std::get<0>(a) < std::get<0>(b); + }); + Ptr child = this->children[std::get<1>(priorityQue.back())]; - while (!priorityQue.empty() && overlap(*child, query, smallestDist)) + while (!priorityQue.empty() && child->overlap(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); + child->KNNSearchRecurse(query, K, smallestDist, candidatePoint); + } + else + { + for (size_t i = 0; i < child->pointList.size(); i++) + { + float dist = SquaredDistance(child->pointList[i], query); - if ( dist + dist * std::numeric_limits::epsilon() <= smallestDist ) { - candidatePoint.emplace_back(dist, child->pointList[i]); + if ( dist + dist * std::numeric_limits::epsilon() <= smallestDist ) + { + Point3f pt = child->pointList[i]; + Point3f col { }; + if (child->colorList.empty()) + { + col = child->colorList[i]; + } + candidatePoint.emplace_back(dist, pt, col); } } - std::sort(candidatePoint.begin(), candidatePoint.end()); + std::sort(candidatePoint.begin(), candidatePoint.end(), + [](const std::tuple& a, const std::tuple& b) -> bool + { + return std::get<0>(a) < std::get<0>(b); + } + ); - if (int(candidatePoint.size()) > K) { + if (int(candidatePoint.size()) > K) + { candidatePoint.resize(K); } - if (int(candidatePoint.size()) == K) { - smallestDist = candidatePoint.back().dist; + if (int(candidatePoint.size()) == K) + { + smallestDist = std::get<0>(candidatePoint.back()); } } priorityQue.pop_back(); - // To next child. + // To next child if(!priorityQue.empty()) - child = node->children[priorityQue.back().t]; + { + child = this->children[std::get<1>(priorityQue.back())]; + } } } -void Octree::KNNSearch(const Point3f& query, const int K, std::vector& pointSet, std::vector& squareDistSet) const + +void Octree::KNNSearch(const Point3f &query, const int K, OutputArray pointSet, OutputArray 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); - } + this->KNNSearch(query, K, pointSet, noArray(), squareDistSet); } +void Octree::KNNSearch(const Point3f &query, const int K, OutputArray points, OutputArray colors, OutputArray squareDists) const +{ + std::vector outPoints, outColors; + std::vector outSqDists; + + if (!p->rootNode.empty()) + { + std::vector> candidatePoints; + float smallestDist = std::numeric_limits::max(); + + p->rootNode->KNNSearchRecurse(query, K, smallestDist, candidatePoints); + + for(size_t i = 0; i < candidatePoints.size(); i++) + { + auto cp = candidatePoints[i]; + outSqDists.push_back(std::get<0>(cp)); + outPoints.push_back(std::get<1>(cp)); + outColors.push_back(std::get<2>(cp)); + } + } + + if (points.needed()) + { + Mat(outPoints).copyTo(points); + } + if (colors.needed()) + { + CV_Assert(this->p->hasColor); + Mat(outColors).copyTo(colors); + } + if (squareDists.needed()) + { + Mat(outSqDists).copyTo(squareDists); + } +} } \ No newline at end of file diff --git a/modules/3d/src/octree.hpp b/modules/3d/src/octree.hpp index af0ae9fd54..481682cf15 100644 --- a/modules/3d/src/octree.hpp +++ b/modules/3d/src/octree.hpp @@ -13,9 +13,13 @@ #define OPENCV_3D_SRC_OCTREE_HPP #include +#include #include "opencv2/core.hpp" -namespace cv { +namespace cv +{ +// Forward declaration +class OctreeKey; /** @brief OctreeNode for Octree. @@ -41,7 +45,8 @@ within the node, which will be used for octree indexing and mapping from point c 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{ +class OctreeNode +{ public: /** @@ -63,15 +68,17 @@ public: //! 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; + bool overlap(const Point3f& query, float squareRadius) const; + + void KNNSearchRecurse(const Point3f& query, const int K, float& smallestDist, std::vector>& candidatePoint) const; + //! Contains 8 pointers to its 8 children. - std::vector< Ptr > children; + std::array, 8> 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; + OctreeNode* parent; //! 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; @@ -83,6 +90,22 @@ public: //! And the center of cube is `center = origin + Point3f(size/2, size/2, size/2)`. Point3f origin; + //! RAHTCoefficient of octree node, used for color attribute compression. + Point3f RAHTCoefficient = { }; + + /** The list of 6 adjacent neighbor node. + * index mapping: + * +z [101] + * | | [110] + * | | / + * O-------- +x [001]----{000} ----[011] + * / / | + * / [010] | + * +y [100] + * index 000, 111 are reserved + */ + std::array, 8> neigh; + /** 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. */ @@ -93,6 +116,65 @@ public: //! Contains pointers to all point cloud data in this node. std::vector pointList; + + //! color attribute of octree node. + std::vector colorList; +}; + +/** @brief Key for pointCloud, used to compute the child node index through bit operations. + +When building the octree, the point cloud data is firstly voxelized/discretized: by inserting +all the points into a voxel coordinate system. For example, when resolution is set to 0.01, a point +with coordinate Point3f(0.251,0.502,0.753) would be transformed to:(0.251/0.01,0.502/0.01,0.753/0.01) +=(25,50,75). And the OctreeKey will be (x_key:1_1001,y_key:11_0010,z_key:100_1011). Assume the Octree->depth +is 100_0000, It can quickly calculate the index of the child nodes at each layer. +layer Depth Mask x&Depth Mask y&Depth Mask z&Depth Mask Child Index(0-7) +1 100_0000 0 0 1 4 +2 10_0000 0 1 0 2 +3 1_0000 1 1 0 3 +4 1000 1 0 1 5 +5 100 0 0 0 0 +6 10 0 1 1 6 +7 1 1 0 1 5 +*/ + +class OctreeKey +{ +public: + size_t x_key; + size_t y_key; + size_t z_key; + +public: + OctreeKey() : x_key(0), y_key(0), z_key(0) { } + OctreeKey(size_t x, size_t y, size_t z) : x_key(x), y_key(y), z_key(z) { } + + /** @brief compute the child node index through bit operations. + * + * @param mask The mask of specify layer. + * @return the index of child(0-7) + */ + inline unsigned char findChildIdxByMask(size_t mask) const + { + return static_cast((!!(z_key & mask))<<2) | ((!!(y_key & mask))<<1) | (!!(x_key & mask)); + } + + /** @brief get occupancy code from node. + * + * The occupancy code type is unsigned char that represents whether the eight child nodes of the octree node exist + * If a octree node has 3 child which indexes are 0,1,7, then the occupancy code of this node is 1000_0011 + * @param node The octree node. + * @return the occupancy code(0000_0000-1111_1111) + */ + static inline unsigned char getBitPattern(OctreeNode &node) + { + unsigned char res = 0; + for (unsigned char i = 0; i < node.children.size(); i++) + { + res |= static_cast((!node.children[i].empty()) << i); + } + return res; + } }; } diff --git a/modules/3d/src/pcc.h b/modules/3d/src/pcc.h new file mode 100644 index 0000000000..778ad59fdb --- /dev/null +++ b/modules/3d/src/pcc.h @@ -0,0 +1,94 @@ +// 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. +// +// Author: Yuhang Wang +// Chengwei Ye +// Zhangjie Cheng + +#ifndef OPENCV_PCC_H +#define OPENCV_PCC_H + +#include +#include "opencv2/core.hpp" +#include "octree.hpp" +#include "opencv2/3d.hpp" + +namespace cv { + + +/** @brief class to serialize or deserialize pointcloud data + * + * A class for "encoding" pointcloud data to a + * meaningful, unevenly distributed char vector, + * so that it can be further compressed by Entropy coding. + * And the opposite "decoding" way as well. + * + * The current implementation is to represent pointcloud as Octree, + * then traverse OctreeNodes to get vector of "occupancy code". +*/ +class OctreeSerializeCoder { +private: + Octree *octree; + float resolution; +public: + + OctreeSerializeCoder(); + + OctreeSerializeCoder(float resolution); + + //! encode Pointcloud data to serialized char vector + void encode(const std::vector &pointCloud, + std::vector &serializedVector); + + //! decode Pointcloud data from serialized char vector + void decode(const std::vector &serializedVector, + std::vector &pointCloud); +}; + +/** @brief Class to reduce vectorized data size by EntropyCoding + * + * The algorithm used here is Range Coding Algorithm. + * +*/ +class EntropyCoder { +public: + //! encode char vector to bit stream + void encodeCharVectorToStream(const std::vector &inputCharVector, + std::ostream &outputStream); + + //! decode char vector from bit stream + void decodeStreamToCharVector(const std::istream &inputStream, + std::vector &outputCharVector); +}; + +/** @brief pointcloud compression class + * + * This class enables user to do compression and decompression to pointcloud, + * currently based on Octree, + * may support other method (like kd-tree, etc.) in future if necessary. + * +*/ +class PointCloudCompression{ +private: + OctreeSerializeCoder _coder; + EntropyCoder _entropyCoder; +public: + /** @brief User compress the pointcloud to stream + * + * @param pointCloud the pointcloud to compress + * @param outputStream the output compressed bit stream destination + */ + void compress(const std::vector &pointCloud, std::ostream &outputStream); + + /** @brief User decompress(recover) pointcloud from stream + * + * @param inputStream the input compressed bit stream source + * @param pointCloud the output pointcloud + */ + void decompress(std::istream &inputStream, const std::vector &pointCloud); +}; + +} + +#endif //OPENCV_PCC_H diff --git a/modules/3d/src/precomp.hpp b/modules/3d/src/precomp.hpp index ef6bf01297..579b099a70 100755 --- a/modules/3d/src/precomp.hpp +++ b/modules/3d/src/precomp.hpp @@ -67,8 +67,10 @@ #include "opencv2/3d/detail/kinfu_frame.hpp" #include +#include #include #include +#include #include #include #include diff --git a/modules/3d/test/test_octree.cpp b/modules/3d/test/test_octree.cpp index 106d17b12e..0dbdb0e120 100644 --- a/modules/3d/test/test_octree.cpp +++ b/modules/3d/test/test_octree.cpp @@ -14,64 +14,65 @@ protected: void SetUp() override { pointCloudSize = 1000; - maxDepth = 18; - + resolution = 0.0001; int scale; Point3i pmin, pmax; - scale = 1<<20; + 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++) + for(size_t 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)); + pointCloud.push_back(Point3f(_x, _y, _z)); } - // Generate Octree From PointCloud. - treeTest.create(pointcloud, maxDepth); + // Generate Octree From PointCloud + treeTest = Octree::createWithResolution(resolution, pointCloud); // 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; + //Origin point cloud data + std::vector pointCloud; -private: - int maxDepth; + //Point cloud data from octree + std::vector restorePointCloudData; + + //Color attribute of pointCloud from octree + std::vector restorePointCloudColor; + + size_t pointCloudSize; + Point3f restPoint; + Ptr treeTest; + double resolution; }; TEST_F(OctreeTest, BasicFunctionTest) { // Check if the point in Bound. - EXPECT_TRUE(treeTest.isPointInBound(restPoint)); - EXPECT_FALSE(treeTest.isPointInBound(restPoint + Point3f(20, 20, 20))); + EXPECT_TRUE(treeTest->isPointInBound(restPoint)); + EXPECT_FALSE(treeTest->isPointInBound(restPoint + Point3f(60, 60, 60))); // 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()); + EXPECT_FALSE(treeTest->deletePoint(restPoint)); + EXPECT_FALSE(treeTest->insertPoint(restPoint + Point3f(60, 60, 60))); + EXPECT_TRUE(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) @@ -79,16 +80,30 @@ TEST_F(OctreeTest, RadiusSearchTest) float radius = 2.0f; std::vector outputPoints; std::vector outputSquareDist; - EXPECT_NO_THROW(treeTest.radiusNNSearch(restPoint, radius, outputPoints, outputSquareDist)); + EXPECT_NO_THROW(treeTest->radiusNNSearch(restPoint, radius, outputPoints, outputSquareDist)); + EXPECT_EQ(outputPoints.size(),(unsigned int)5); - 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); + // The output is unsorted, so let's sort it before checking + std::map sortResults; + for (int i = 0; i < (int)outputPoints.size(); i++) + { + sortResults[outputSquareDist[i]] = outputPoints[i]; + } + + std::vector goldVals = { + {-8.1184864044189f, -0.528564453125f, 0.f}, + {-8.405818939208f, -2.991247177124f, 0.f}, + {-8.88461112976f, -1.881799697875f, 0.f}, + {-6.551313400268f, -0.708484649658f, 0.f} + }; + + auto it = sortResults.begin(); + for (int i = 0; i < (int)goldVals.size(); i++, it++) + { + Point3f p = it->second; + EXPECT_FLOAT_EQ(goldVals[i].x, p.x); + EXPECT_FLOAT_EQ(goldVals[i].y, p.y); + } } TEST_F(OctreeTest, KNNSearchTest) @@ -96,18 +111,48 @@ TEST_F(OctreeTest, KNNSearchTest) int K = 10; std::vector outputPoints; std::vector outputSquareDist; - EXPECT_NO_THROW(treeTest.KNNSearch(restPoint, K, outputPoints, 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); + // The output is unsorted, so let's sort it before checking + std::map sortResults; + for (int i = 0; i < (int)outputPoints.size(); i++) + { + sortResults[outputSquareDist[i]] = outputPoints[i]; + } + + std::vector goldVals = { + { -8.118486404418f, -0.528564453125f, 0.f }, + { -8.405818939208f, -2.991247177124f, 0.f }, + { -8.884611129760f, -1.881799697875f, 0.f }, + { -6.551313400268f, -0.708484649658f, 0.f } + }; + + auto it = sortResults.begin(); + for (int i = 0; i < (int)goldVals.size(); i++, it++) + { + Point3f p = it->second; + EXPECT_FLOAT_EQ(goldVals[i].x, p.x); + EXPECT_FLOAT_EQ(goldVals[i].y, p.y); + } } +TEST_F(OctreeTest, restoreTest) { + //restore the pointCloud data from octree. + EXPECT_NO_THROW(treeTest->getPointCloudByOctree(restorePointCloudData,restorePointCloudColor)); + + //The points in same leaf node will be seen as the same point. So if the resolution is small, + //it will work as a downSampling function. + EXPECT_LE(restorePointCloudData.size(),pointCloudSize); + + //The distance between the restore point cloud data and origin data should less than resolution. + std::vector outputPoints; + std::vector outputSquareDist; + EXPECT_NO_THROW(treeTest->getPointCloudByOctree(restorePointCloudData,restorePointCloudColor)); + EXPECT_NO_THROW(treeTest->KNNSearch(restorePointCloudData[0], 1, outputPoints, outputSquareDist)); + EXPECT_LE(abs(outputPoints[0].x - restorePointCloudData[0].x), resolution); + EXPECT_LE(abs(outputPoints[0].y - restorePointCloudData[0].y), resolution); + EXPECT_LE(abs(outputPoints[0].z - restorePointCloudData[0].z), resolution); +} } // namespace -} // opencv_test +} // opencv_test \ No newline at end of file