From 0e47b0510666a733a6a9181dbc11ae16714905f0 Mon Sep 17 00:00:00 2001 From: Zhangjie Chen Date: Thu, 29 Feb 2024 19:02:44 +0800 Subject: [PATCH] Merge pull request #23985 from starga2er777:pcc [GSoC] Update octree methods and create frames for PCC #23985 ## PR for GSoC Point Cloud Compression [Issue for GSoC 2023](https://github.com/opencv/opencv/issues/23624) * We are **updating the Octree method create() by using OctreeKey**: Through voxelization, directly calculate the leaf nodes that the point cloud belongs to, and omit the judgment whether the point cloud is in the range when inserted. The index of the child node is calculated by bit operation. * We are also **introducing a new header file pcc.h (Point Cloud Compression) with API framework**. * We added tests for restoring point clouds from an octree. * Currently, the features related to octree creation and point cloud compression are part of the internal API, which means they are not directly accessible to users. However, our plan for the future is to **include only the 'PointCloudCompression' class in the 'opencv2/3d.hpp' header file**. This will provide an interface for utilizing the point cloud compression functionality. The previous PR of this was closed due to repo name conflicts, therefore we resubmitted in this PR. ### Pull Request Readiness Checklist See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request - [x] I agree to contribute to the project under Apache 2 License. - [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV - [x] The PR is proposed to the proper branch - [x] There is a reference to the original bug report and related work - [x] There is accuracy test, performance test and test data in opencv_extra repository, if applicable Patch to opencv_extra has the same branch name. - [x] The feature is well documented and sample code can be built with the project CMake --- modules/3d/include/opencv2/3d.hpp | 152 ++-- modules/3d/misc/python/test/test_octree.py | 31 + modules/3d/src/octree.cpp | 878 ++++++++++++--------- modules/3d/src/octree.hpp | 94 ++- modules/3d/src/pcc.h | 94 +++ modules/3d/src/precomp.hpp | 2 + modules/3d/test/test_octree.cpp | 135 ++-- 7 files changed, 912 insertions(+), 474 deletions(-) create mode 100644 modules/3d/misc/python/test/test_octree.py create mode 100644 modules/3d/src/pcc.h 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