mirror of
https://github.com/opencv/opencv.git
synced 2025-06-07 01:13:28 +08:00
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
This commit is contained in:
Zhangjie Chen
GitHub
parent
92b940792a
commit
0e47b05106
@ -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<Octree> 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<Point3f> &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<Octree> 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<Octree> 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<Octree> 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<Point3f> &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<Point3f> &pointSet, std::vector<float> &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<Point3f> &pointSet, std::vector<float> &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;
|
||||
|
||||
31
modules/3d/misc/python/test/test_octree.py
Normal file
31
modules/3d/misc/python/test/test_octree.py
Normal file
@ -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()
|
||||
File diff suppressed because it is too large
Load Diff
@ -13,9 +13,13 @@
|
||||
#define OPENCV_3D_SRC_OCTREE_HPP
|
||||
|
||||
#include <vector>
|
||||
#include <array>
|
||||
#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<std::tuple<float, Point3f, Point3f>>& candidatePoint) const;
|
||||
|
||||
//! Contains 8 pointers to its 8 children.
|
||||
std::vector< Ptr<OctreeNode> > children;
|
||||
std::array<Ptr<OctreeNode>, 8> children;
|
||||
|
||||
//! Point to the parent node of the current node. The root node has no parent node and the value is NULL.
|
||||
Ptr<OctreeNode> parent = nullptr;
|
||||
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<Ptr<OctreeNode>, 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<Point3f> pointList;
|
||||
|
||||
//! color attribute of octree node.
|
||||
std::vector<Point3f> 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<unsigned char>((!!(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<unsigned char>((!node.children[i].empty()) << i);
|
||||
}
|
||||
return res;
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
94
modules/3d/src/pcc.h
Normal file
94
modules/3d/src/pcc.h
Normal file
@ -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 <yuhangwang0012@gmail.com>
|
||||
// Chengwei Ye <broweigg@gmail.com>
|
||||
// Zhangjie Cheng <zhangjiec01@gmail.com>
|
||||
|
||||
#ifndef OPENCV_PCC_H
|
||||
#define OPENCV_PCC_H
|
||||
|
||||
#include <vector>
|
||||
#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<Point3f> &pointCloud,
|
||||
std::vector<unsigned char> &serializedVector);
|
||||
|
||||
//! decode Pointcloud data from serialized char vector
|
||||
void decode(const std::vector<unsigned char> &serializedVector,
|
||||
std::vector<Point3f> &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<unsigned char> &inputCharVector,
|
||||
std::ostream &outputStream);
|
||||
|
||||
//! decode char vector from bit stream
|
||||
void decodeStreamToCharVector(const std::istream &inputStream,
|
||||
std::vector<unsigned char> &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<Point3f> &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<Point3f> &pointCloud);
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif //OPENCV_PCC_H
|
||||
@ -67,8 +67,10 @@
|
||||
#include "opencv2/3d/detail/kinfu_frame.hpp"
|
||||
|
||||
#include <atomic>
|
||||
#include <algorithm>
|
||||
#include <functional>
|
||||
#include <limits>
|
||||
#include <stack>
|
||||
#include <vector>
|
||||
#include <list>
|
||||
#include <set>
|
||||
|
||||
@ -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<Point3f> pointcloud;
|
||||
int pointCloudSize;
|
||||
Point3f restPoint;
|
||||
Octree treeTest;
|
||||
//Origin point cloud data
|
||||
std::vector<Point3f> pointCloud;
|
||||
|
||||
private:
|
||||
int maxDepth;
|
||||
//Point cloud data from octree
|
||||
std::vector<Point3f> restorePointCloudData;
|
||||
|
||||
//Color attribute of pointCloud from octree
|
||||
std::vector<Point3f> restorePointCloudColor;
|
||||
|
||||
size_t pointCloudSize;
|
||||
Point3f restPoint;
|
||||
Ptr<Octree> 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<Point3f> outputPoints;
|
||||
std::vector<float> 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<float, Point3f> sortResults;
|
||||
for (int i = 0; i < (int)outputPoints.size(); i++)
|
||||
{
|
||||
sortResults[outputSquareDist[i]] = outputPoints[i];
|
||||
}
|
||||
|
||||
std::vector<Point3f> 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<Point3f> outputPoints;
|
||||
std::vector<float> 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<float, Point3f> sortResults;
|
||||
for (int i = 0; i < (int)outputPoints.size(); i++)
|
||||
{
|
||||
sortResults[outputSquareDist[i]] = outputPoints[i];
|
||||
}
|
||||
|
||||
std::vector<Point3f> 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<Point3f> outputPoints;
|
||||
std::vector<float> 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
|
||||
Loading…
Reference in New Issue
Block a user