mirror of
https://github.com/opencv/opencv.git
synced 2026-07-29 23:33:05 +04:00
Merge pull request #29224 from asmorkalov:as/ptcloud2
Dedicated pointcloud module #29224 OpenCV contrib: https://github.com/opencv/opencv_contrib/pull/4134 ### 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 - [ ] The PR is proposed to the proper branch - [ ] There is a reference to the original bug report and related work - [ ] There is accuracy test, performance test and test data in opencv_extra repository, if applicable Patch to opencv_extra has the same branch name. - [ ] The feature is well documented and sample code can be built with the project CMake
This commit is contained in:
Alexander Smorkalov
committed by
GitHub
GitHub
parent
6a1a2754c8
commit
fc3803c67b
@@ -0,0 +1,415 @@
|
||||
// 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
|
||||
|
||||
#ifndef POINT_CLOUD_MODULE_HPP
|
||||
#define POINT_CLOUD_MODULE_HPP
|
||||
|
||||
#include "opencv2/core.hpp"
|
||||
#include "opencv2/ptcloud/depth.hpp"
|
||||
#include "opencv2/ptcloud/volume.hpp"
|
||||
#include "opencv2/ptcloud/odometry.hpp"
|
||||
#include "opencv2/ptcloud/odometry_frame.hpp"
|
||||
#include "opencv2/ptcloud/odometry_settings.hpp"
|
||||
|
||||
/**
|
||||
@defgroup ptcloud Point Clound Processing
|
||||
*/
|
||||
|
||||
//! @addtogroup ptcloud
|
||||
//! @{
|
||||
|
||||
namespace cv {
|
||||
|
||||
/** @brief Loads a point cloud from a file.
|
||||
*
|
||||
* The function loads point cloud from the specified file and returns it.
|
||||
* If the cloud cannot be read, throws an error.
|
||||
* Vertex coordinates, normals and colors are returned as they are saved in the file
|
||||
* even if these arrays have different sizes and their elements do not correspond to each other
|
||||
* (which is typical for OBJ files for example)
|
||||
*
|
||||
* Currently, the following file formats are supported:
|
||||
* - [Wavefront obj file *.obj](https://en.wikipedia.org/wiki/Wavefront_.obj_file)
|
||||
* - [Polygon File Format *.ply](https://en.wikipedia.org/wiki/PLY_(file_format))
|
||||
*
|
||||
* @param filename Name of the file
|
||||
* @param vertices vertex coordinates, each value contains 3 floats
|
||||
* @param normals per-vertex normals, each value contains 3 floats
|
||||
* @param rgb per-vertex colors, each value contains 3 floats
|
||||
*/
|
||||
CV_EXPORTS_W void loadPointCloud(const String &filename, OutputArray vertices, OutputArray normals = noArray(), OutputArray rgb = noArray());
|
||||
|
||||
/** @brief Saves a point cloud to a specified file.
|
||||
*
|
||||
* The function saves point cloud to the specified file.
|
||||
* File format is chosen based on the filename extension.
|
||||
*
|
||||
* @param filename Name of the file
|
||||
* @param vertices vertex coordinates, each value contains 3 floats
|
||||
* @param normals per-vertex normals, each value contains 3 floats
|
||||
* @param rgb per-vertex colors, each value contains 3 floats
|
||||
*/
|
||||
CV_EXPORTS_W void savePointCloud(const String &filename, InputArray vertices, InputArray normals = noArray(), InputArray rgb = noArray());
|
||||
|
||||
/** @brief Loads a mesh from a file.
|
||||
*
|
||||
* The function loads mesh from the specified file and returns it.
|
||||
* If the mesh cannot be read, throws an error
|
||||
* Vertex attributes (i.e. space and texture coodinates, normals and colors) are returned in same-sized
|
||||
* arrays with corresponding elements having the same indices.
|
||||
* This means that if a face uses a vertex with a normal or a texture coordinate with different indices
|
||||
* (which is typical for OBJ files for example), this vertex will be duplicated for each face it uses.
|
||||
*
|
||||
* Currently, the following file formats are supported:
|
||||
* - [Wavefront obj file *.obj](https://en.wikipedia.org/wiki/Wavefront_.obj_file) (ONLY TRIANGULATED FACES)
|
||||
* - [Polygon File Format *.ply](https://en.wikipedia.org/wiki/PLY_(file_format))
|
||||
* @param filename Name of the file
|
||||
* @param vertices vertex coordinates, each value contains 3 floats
|
||||
* @param indices per-face list of vertices, each value is a vector of ints
|
||||
* @param normals per-vertex normals, each value contains 3 floats
|
||||
* @param colors per-vertex colors, each value contains 3 floats
|
||||
* @param texCoords per-vertex texture coordinates, each value contains 2 or 3 floats
|
||||
*/
|
||||
CV_EXPORTS_W void loadMesh(const String &filename, OutputArray vertices, OutputArrayOfArrays indices,
|
||||
OutputArray normals = noArray(), OutputArray colors = noArray(),
|
||||
OutputArray texCoords = noArray());
|
||||
|
||||
/** @brief Saves a mesh to a specified file.
|
||||
*
|
||||
* The function saves mesh to the specified file.
|
||||
* File format is chosen based on the filename extension.
|
||||
*
|
||||
* @param filename Name of the file.
|
||||
* @param vertices vertex coordinates, each value contains 3 floats
|
||||
* @param indices per-face list of vertices, each value is a vector of ints
|
||||
* @param normals per-vertex normals, each value contains 3 floats
|
||||
* @param colors per-vertex colors, each value contains 3 floats
|
||||
* @param texCoords per-vertex texture coordinates, each value contains 2 or 3 floats
|
||||
*/
|
||||
CV_EXPORTS_W void saveMesh(const String &filename, InputArray vertices, InputArrayOfArrays indices,
|
||||
InputArray normals = noArray(), InputArray colors = noArray(), InputArray texCoords = noArray());
|
||||
|
||||
|
||||
//! Triangle fill settings
|
||||
enum TriangleShadingType
|
||||
{
|
||||
RASTERIZE_SHADING_WHITE = 0, //!< a white color is used for the whole triangle
|
||||
RASTERIZE_SHADING_FLAT = 1, //!< a color of 1st vertex of each triangle is used
|
||||
RASTERIZE_SHADING_SHADED = 2 //!< a color is interpolated between 3 vertices with perspective correction
|
||||
};
|
||||
|
||||
//! Face culling settings: what faces are drawn after face culling
|
||||
enum TriangleCullingMode
|
||||
{
|
||||
RASTERIZE_CULLING_NONE = 0, //!< all faces are drawn, no culling is actually performed
|
||||
RASTERIZE_CULLING_CW = 1, //!< triangles which vertices are given in clockwork order are drawn
|
||||
RASTERIZE_CULLING_CCW = 2 //!< triangles which vertices are given in counterclockwork order are drawn
|
||||
};
|
||||
|
||||
//! GL compatibility settings
|
||||
enum TriangleGlCompatibleMode
|
||||
{
|
||||
RASTERIZE_COMPAT_DISABLED = 0, //!< Color and depth have their natural values and converted to internal formats if needed
|
||||
RASTERIZE_COMPAT_INVDEPTH = 1 //!< Color is natural, Depth is transformed from [-zNear; -zFar] to [0; 1]
|
||||
//!< by the following formula: \f$ \frac{z_{far} \left(z + z_{near}\right)}{z \left(z_{far} - z_{near}\right)} \f$ \n
|
||||
//!< In this mode the input/output depthBuf is considered to be in this format,
|
||||
//!< therefore it's faster since there're no conversions performed
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Structure to keep settings for rasterization
|
||||
*/
|
||||
struct CV_EXPORTS_W_SIMPLE TriangleRasterizeSettings
|
||||
{
|
||||
TriangleRasterizeSettings();
|
||||
|
||||
CV_WRAP TriangleRasterizeSettings& setShadingType(TriangleShadingType st) { shadingType = st; return *this; }
|
||||
CV_WRAP TriangleRasterizeSettings& setCullingMode(TriangleCullingMode cm) { cullingMode = cm; return *this; }
|
||||
CV_WRAP TriangleRasterizeSettings& setGlCompatibleMode(TriangleGlCompatibleMode gm) { glCompatibleMode = gm; return *this; }
|
||||
|
||||
TriangleShadingType shadingType;
|
||||
TriangleCullingMode cullingMode;
|
||||
TriangleGlCompatibleMode glCompatibleMode;
|
||||
};
|
||||
|
||||
|
||||
/** @brief Renders a set of triangles on a depth and color image
|
||||
*
|
||||
* Triangles can be drawn white (1.0, 1.0, 1.0), flat-shaded or with a color interpolation between vertices.
|
||||
* In flat-shaded mode the 1st vertex color of each triangle is used to fill the whole triangle.
|
||||
*
|
||||
* The world2cam is an inverted camera pose matrix in fact. It transforms vertices from world to
|
||||
* camera coordinate system.
|
||||
*
|
||||
* The camera coordinate system emulates the OpenGL's coordinate system having coordinate origin in a screen center,
|
||||
* X axis pointing right, Y axis pointing up and Z axis pointing towards the viewer
|
||||
* except that image is vertically flipped after the render.
|
||||
* This means that all visible objects are placed in z-negative area, or exactly in -zNear > z > -zFar since
|
||||
* zNear and zFar are positive.
|
||||
* For example, at fovY = PI/2 the point (0, 1, -1) will be projected to (width/2, 0) screen point,
|
||||
* (1, 0, -1) to (width/2 + height/2, height/2). Increasing fovY makes projection smaller and vice versa.
|
||||
*
|
||||
* The function does not create or clear output images before the rendering. This means that it can be used
|
||||
* for drawing over an existing image or for rendering a model into a 3D scene using pre-filled Z-buffer.
|
||||
*
|
||||
* Empty scene results in a depth buffer filled by the maximum value since every pixel is infinitely far from the camera.
|
||||
* Therefore, before rendering anything from scratch the depthBuf should be filled by zFar values (or by ones in INVDEPTH mode).
|
||||
*
|
||||
* There are special versions of this function named triangleRasterizeDepth and triangleRasterizeColor
|
||||
* for cases if a user needs a color image or a depth image alone; they may run slightly faster.
|
||||
*
|
||||
* @param vertices vertices coordinates array. Should contain values of CV_32FC3 type or a compatible one (e.g. cv::Vec3f, etc.)
|
||||
* @param indices triangle vertices index array, 3 per triangle. Each index indicates a vertex in a vertices array.
|
||||
* Should contain CV_32SC3 values or compatible
|
||||
* @param colors per-vertex colors of CV_32FC3 type or compatible. Can be empty or the same size as vertices array.
|
||||
* If the values are out of [0; 1] range, the result correctness is not guaranteed
|
||||
* @param colorBuf an array representing the final rendered image. Should containt CV_32FC3 values and be the same size as depthBuf.
|
||||
* Not cleared before rendering, i.e. the content is reused as there is some pre-rendered scene.
|
||||
* @param depthBuf an array of floats containing resulting Z buffer. Should contain float values and be the same size as colorBuf.
|
||||
* Not cleared before rendering, i.e. the content is reused as there is some pre-rendered scene.
|
||||
* Empty scene corresponds to all values set to zFar (or to 1.0 in INVDEPTH mode)
|
||||
* @param world2cam a 4x3 or 4x4 float or double matrix containing inverted (sic!) camera pose
|
||||
* @param fovY field of view in vertical direction, given in radians
|
||||
* @param zNear minimum Z value to render, everything closer is clipped
|
||||
* @param zFar maximum Z value to render, everything farther is clipped
|
||||
* @param settings see TriangleRasterizeSettings. By default the smooth shading is on,
|
||||
* with CW culling and with disabled GL compatibility
|
||||
*/
|
||||
CV_EXPORTS_W void triangleRasterize(InputArray vertices, InputArray indices, InputArray colors,
|
||||
InputOutputArray colorBuf, InputOutputArray depthBuf,
|
||||
InputArray world2cam, double fovY, double zNear, double zFar,
|
||||
const TriangleRasterizeSettings& settings = TriangleRasterizeSettings());
|
||||
|
||||
/** @brief Overloaded version of triangleRasterize() with depth-only rendering
|
||||
*
|
||||
* @param vertices vertices coordinates array. Should contain values of CV_32FC3 type or a compatible one (e.g. cv::Vec3f, etc.)
|
||||
* @param indices triangle vertices index array, 3 per triangle. Each index indicates a vertex in a vertices array.
|
||||
* Should contain CV_32SC3 values or compatible
|
||||
* @param depthBuf an array of floats containing resulting Z buffer. Should contain float values and be the same size as colorBuf.
|
||||
* Not cleared before rendering, i.e. the content is reused as there is some pre-rendered scene.
|
||||
* Empty scene corresponds to all values set to zFar (or to 1.0 in INVDEPTH mode)
|
||||
* @param world2cam a 4x3 or 4x4 float or double matrix containing inverted (sic!) camera pose
|
||||
* @param fovY field of view in vertical direction, given in radians
|
||||
* @param zNear minimum Z value to render, everything closer is clipped
|
||||
* @param zFar maximum Z value to render, everything farther is clipped
|
||||
* @param settings see TriangleRasterizeSettings. By default the smooth shading is on,
|
||||
* with CW culling and with disabled GL compatibility
|
||||
*/
|
||||
CV_EXPORTS_W void triangleRasterizeDepth(InputArray vertices, InputArray indices, InputOutputArray depthBuf,
|
||||
InputArray world2cam, double fovY, double zNear, double zFar,
|
||||
const TriangleRasterizeSettings& settings = TriangleRasterizeSettings());
|
||||
|
||||
/** @brief Overloaded version of triangleRasterize() with color-only rendering
|
||||
*
|
||||
* @param vertices vertices coordinates array. Should contain values of CV_32FC3 type or a compatible one (e.g. cv::Vec3f, etc.)
|
||||
* @param indices triangle vertices index array, 3 per triangle. Each index indicates a vertex in a vertices array.
|
||||
* Should contain CV_32SC3 values or compatible
|
||||
* @param colors per-vertex colors of CV_32FC3 type or compatible. Can be empty or the same size as vertices array.
|
||||
* If the values are out of [0; 1] range, the result correctness is not guaranteed
|
||||
* @param colorBuf an array representing the final rendered image. Should containt CV_32FC3 values and be the same size as depthBuf.
|
||||
* Not cleared before rendering, i.e. the content is reused as there is some pre-rendered scene.
|
||||
* @param world2cam a 4x3 or 4x4 float or double matrix containing inverted (sic!) camera pose
|
||||
* @param fovY field of view in vertical direction, given in radians
|
||||
* @param zNear minimum Z value to render, everything closer is clipped
|
||||
* @param zFar maximum Z value to render, everything farther is clipped
|
||||
* @param settings see TriangleRasterizeSettings. By default the smooth shading is on,
|
||||
* with CW culling and with disabled GL compatibility
|
||||
*/
|
||||
CV_EXPORTS_W void triangleRasterizeColor(InputArray vertices, InputArray indices, InputArray colors, InputOutputArray colorBuf,
|
||||
InputArray world2cam, double fovY, double zNear, double zFar,
|
||||
const TriangleRasterizeSettings& settings = TriangleRasterizeSettings());
|
||||
|
||||
/** @brief Octree for 3D vision.
|
||||
*
|
||||
* In 3D vision filed, the Octree is used to process and accelerate the pointcloud data. The class Octree represents
|
||||
* the Octree data structure. Each Octree will have a fixed depth. The depth of Octree refers to the distance from
|
||||
* the root node to the leaf node.All OctreeNodes will not exceed this depth.Increasing the depth will increase
|
||||
* the amount of calculation exponentially. And the small number of depth refers low resolution of Octree.
|
||||
* Each node contains 8 children, which are used to divide the space cube into eight parts. Each octree node represents
|
||||
* a cube. And these eight children will have a fixed order, the order is described as follows:
|
||||
*
|
||||
* For illustration, assume,
|
||||
*
|
||||
* rootNode: origin == (0, 0, 0), size == 2
|
||||
*
|
||||
* Then,
|
||||
*
|
||||
* children[0]: origin == (0, 0, 0), size == 1
|
||||
*
|
||||
* children[1]: origin == (1, 0, 0), size == 1, along X-axis next to child 0
|
||||
*
|
||||
* children[2]: origin == (0, 1, 0), size == 1, along Y-axis next to child 0
|
||||
*
|
||||
* children[3]: origin == (1, 1, 0), size == 1, in X-Y plane
|
||||
*
|
||||
* children[4]: origin == (0, 0, 1), size == 1, along Z-axis next to child 0
|
||||
*
|
||||
* children[5]: origin == (1, 0, 1), size == 1, in X-Z plane
|
||||
*
|
||||
* children[6]: origin == (0, 1, 1), size == 1, in Y-Z plane
|
||||
*
|
||||
* children[7]: origin == (1, 1, 1), size == 1, furthest from child 0
|
||||
*/
|
||||
|
||||
class CV_EXPORTS_W Octree
|
||||
{
|
||||
public:
|
||||
//! Default constructor.
|
||||
Octree();
|
||||
|
||||
/** @overload
|
||||
* @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 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 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();
|
||||
|
||||
/** @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.
|
||||
*/
|
||||
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.
|
||||
*/
|
||||
CV_WRAP bool isPointInBound(const Point3f& point) const;
|
||||
|
||||
//! returns true if the rootnode is NULL.
|
||||
CV_WRAP bool empty() const;
|
||||
|
||||
/** @brief Reset all octree parameter.
|
||||
*
|
||||
* Clear all the nodes of the octree and initialize the parameters.
|
||||
*/
|
||||
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, comparison is epsilon-based
|
||||
* @return return ture if the point is deleted successfully.
|
||||
*/
|
||||
CV_WRAP bool deletePoint(const Point3f& point);
|
||||
|
||||
/** @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 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.
|
||||
*/
|
||||
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 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
|
||||
*/
|
||||
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;
|
||||
Ptr<Impl> p;
|
||||
};
|
||||
|
||||
} // namespace cv
|
||||
|
||||
//! @} ptcloud
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,181 @@
|
||||
// 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
|
||||
|
||||
#ifndef OPENCV_3D_DEPTH_HPP
|
||||
#define OPENCV_3D_DEPTH_HPP
|
||||
|
||||
#include <opencv2/core.hpp>
|
||||
#include <limits>
|
||||
|
||||
namespace cv
|
||||
{
|
||||
//! @addtogroup rgbd
|
||||
//! @{
|
||||
|
||||
/** Object that can compute the normals in an image.
|
||||
* It is an object as it can cache data for speed efficiency
|
||||
* The implemented methods are either:
|
||||
* - FALS (the fastest) and SRI from
|
||||
* ``Fast and Accurate Computation of Surface Normals from Range Images``
|
||||
* by H. Badino, D. Huber, Y. Park and T. Kanade
|
||||
* - the normals with bilateral filtering on a depth image from
|
||||
* ``Gradient Response Maps for Real-Time Detection of Texture-Less Objects``
|
||||
* by S. Hinterstoisser, C. Cagniart, S. Ilic, P. Sturm, N. Navab, P. Fua, and V. Lepetit
|
||||
*/
|
||||
class CV_EXPORTS_W RgbdNormals
|
||||
{
|
||||
public:
|
||||
enum RgbdNormalsMethod
|
||||
{
|
||||
RGBD_NORMALS_METHOD_FALS = 0,
|
||||
RGBD_NORMALS_METHOD_LINEMOD = 1,
|
||||
RGBD_NORMALS_METHOD_SRI = 2,
|
||||
RGBD_NORMALS_METHOD_CROSS_PRODUCT = 3
|
||||
};
|
||||
|
||||
RgbdNormals() { }
|
||||
virtual ~RgbdNormals() { }
|
||||
|
||||
/** Creates new RgbdNormals object
|
||||
* @param rows the number of rows of the depth image normals will be computed on
|
||||
* @param cols the number of cols of the depth image normals will be computed on
|
||||
* @param depth the depth of the normals (only CV_32F or CV_64F)
|
||||
* @param K the calibration matrix to use
|
||||
* @param window_size the window size to compute the normals: can only be 1,3,5 or 7
|
||||
* @param diff_threshold threshold in depth difference, used in LINEMOD algirithm
|
||||
* @param method one of the methods to use: RGBD_NORMALS_METHOD_SRI, RGBD_NORMALS_METHOD_FALS
|
||||
*/
|
||||
CV_WRAP static Ptr<RgbdNormals> create(int rows = 0, int cols = 0, int depth = 0, InputArray K = noArray(), int window_size = 5,
|
||||
float diff_threshold = 50.f,
|
||||
RgbdNormals::RgbdNormalsMethod method = RgbdNormals::RgbdNormalsMethod::RGBD_NORMALS_METHOD_FALS);
|
||||
|
||||
/** Given a set of 3d points in a depth image, compute the normals at each point.
|
||||
* @param points a rows x cols x 3 matrix of CV_32F/CV64F or a rows x cols x 1 CV_U16S
|
||||
* @param normals a rows x cols x 3 matrix
|
||||
*/
|
||||
CV_WRAP virtual void apply(InputArray points, OutputArray normals) const = 0;
|
||||
|
||||
/** Prepares cached data required for calculation
|
||||
* If not called by user, called automatically at first calculation
|
||||
*/
|
||||
CV_WRAP virtual void cache() const = 0;
|
||||
|
||||
CV_WRAP virtual int getRows() const = 0;
|
||||
CV_WRAP virtual void setRows(int val) = 0;
|
||||
CV_WRAP virtual int getCols() const = 0;
|
||||
CV_WRAP virtual void setCols(int val) = 0;
|
||||
CV_WRAP virtual int getWindowSize() const = 0;
|
||||
CV_WRAP virtual void setWindowSize(int val) = 0;
|
||||
CV_WRAP virtual int getDepth() const = 0;
|
||||
CV_WRAP virtual void getK(OutputArray val) const = 0;
|
||||
CV_WRAP virtual void setK(InputArray val) = 0;
|
||||
CV_WRAP virtual RgbdNormals::RgbdNormalsMethod getMethod() const = 0;
|
||||
};
|
||||
|
||||
|
||||
/** Registers depth data to an external camera
|
||||
* Registration is performed by creating a depth cloud, transforming the cloud by
|
||||
* the rigid body transformation between the cameras, and then projecting the
|
||||
* transformed points into the RGB camera.
|
||||
*
|
||||
* uv_rgb = K_rgb * [R | t] * z * inv(K_ir) * uv_ir
|
||||
*
|
||||
* Currently does not check for negative depth values.
|
||||
*
|
||||
* @param unregisteredCameraMatrix the camera matrix of the depth camera
|
||||
* @param registeredCameraMatrix the camera matrix of the external camera
|
||||
* @param registeredDistCoeffs the distortion coefficients of the external camera
|
||||
* @param Rt the rigid body transform between the cameras. Transforms points from depth camera frame to external camera frame.
|
||||
* @param unregisteredDepth the input depth data
|
||||
* @param outputImagePlaneSize the image plane dimensions of the external camera (width, height)
|
||||
* @param registeredDepth the result of transforming the depth into the external camera
|
||||
* @param depthDilation whether or not the depth is dilated to avoid holes and occlusion errors (optional)
|
||||
*/
|
||||
CV_EXPORTS_W void registerDepth(InputArray unregisteredCameraMatrix, InputArray registeredCameraMatrix, InputArray registeredDistCoeffs,
|
||||
InputArray Rt, InputArray unregisteredDepth, const Size& outputImagePlaneSize,
|
||||
OutputArray registeredDepth, bool depthDilation=false);
|
||||
|
||||
/**
|
||||
* @param depth the depth image
|
||||
* @param in_K
|
||||
* @param in_points the list of xy coordinates
|
||||
* @param points3d the resulting 3d points (point is represented by 4 chanels value [x, y, z, 0])
|
||||
*/
|
||||
CV_EXPORTS_W void depthTo3dSparse(InputArray depth, InputArray in_K, InputArray in_points, OutputArray points3d);
|
||||
|
||||
/** Converts a depth image to 3d points. If the mask is empty then the resulting array has the same dimensions as `depth`,
|
||||
* otherwise it is 1d vector containing mask-enabled values only.
|
||||
* The coordinate system is x pointing left, y down and z away from the camera
|
||||
* @param depth the depth image (if given as short int CV_U, it is assumed to be the depth in millimeters
|
||||
* (as done with the Microsoft Kinect), otherwise, if given as CV_32F or CV_64F, it is assumed in meters)
|
||||
* @param K The calibration matrix
|
||||
* @param points3d the resulting 3d points (point is represented by 4 channels value [x, y, z, 0]). They are of the same depth as `depth` if it is CV_32F or CV_64F, and the
|
||||
* depth of `K` if `depth` is of depth CV_16U or CV_16S
|
||||
* @param mask the mask of the points to consider (can be empty)
|
||||
*/
|
||||
CV_EXPORTS_W void depthTo3d(InputArray depth, InputArray K, OutputArray points3d, InputArray mask = noArray());
|
||||
|
||||
/** If the input image is of type CV_16UC1 (like the Kinect one), the image is converted to floats, divided
|
||||
* by depth_factor to get a depth in meters, and the values 0 are converted to std::numeric_limits<float>::quiet_NaN()
|
||||
* Otherwise, the image is simply converted to floats
|
||||
* @param in the depth image (if given as short int CV_U, it is assumed to be the depth in millimeters
|
||||
* (as done with the Microsoft Kinect), it is assumed in meters)
|
||||
* @param type the desired output depth (CV_32F or CV_64F)
|
||||
* @param out The rescaled float depth image
|
||||
* @param depth_factor (optional) factor by which depth is converted to distance (by default = 1000.0 for Kinect sensor)
|
||||
*/
|
||||
CV_EXPORTS_W void rescaleDepth(InputArray in, int type, OutputArray out, double depth_factor = 1000.0);
|
||||
|
||||
/** Warps depth or RGB-D image by reprojecting it in 3d, applying Rt transformation
|
||||
* and then projecting it back onto the image plane.
|
||||
* This function can be used to visualize the results of the Odometry algorithm.
|
||||
* @param depth Depth data, should be 1-channel CV_16U, CV_16S, CV_32F or CV_64F
|
||||
* @param image RGB image (optional), should be 1-, 3- or 4-channel CV_8U
|
||||
* @param mask Mask of used pixels (optional), should be CV_8UC1, CV_8SC1 or CV_BoolC1
|
||||
* @param Rt Rotation+translation matrix (3x4 or 4x4) to be applied to depth points
|
||||
* @param cameraMatrix Camera intrinsics matrix (3x3)
|
||||
* @param warpedDepth The warped depth data (optional)
|
||||
* @param warpedImage The warped RGB image (optional)
|
||||
* @param warpedMask The mask of valid pixels in warped image (optional)
|
||||
*/
|
||||
CV_EXPORTS_W void warpFrame(InputArray depth, InputArray image, InputArray mask, InputArray Rt, InputArray cameraMatrix,
|
||||
OutputArray warpedDepth = noArray(), OutputArray warpedImage = noArray(), OutputArray warpedMask = noArray());
|
||||
|
||||
enum RgbdPlaneMethod
|
||||
{
|
||||
RGBD_PLANE_METHOD_DEFAULT
|
||||
};
|
||||
|
||||
/** Find the planes in a depth image
|
||||
* @param points3d the 3d points organized like the depth image: rows x cols with 3 channels
|
||||
* @param normals the normals for every point in the depth image; optional, can be empty
|
||||
* @param mask An image where each pixel is labeled with the plane it belongs to
|
||||
* and 255 if it does not belong to any plane
|
||||
* @param plane_coefficients the coefficients of the corresponding planes (a,b,c,d) such that ax+by+cz+d=0, norm(a,b,c)=1
|
||||
* and c < 0 (so that the normal points towards the camera)
|
||||
* @param block_size The size of the blocks to look at for a stable MSE
|
||||
* @param min_size The minimum size of a cluster to be considered a plane
|
||||
* @param threshold The maximum distance of a point from a plane to belong to it (in meters)
|
||||
* @param sensor_error_a coefficient of the sensor error. 0 by default, use 0.0075 for a Kinect
|
||||
* @param sensor_error_b coefficient of the sensor error. 0 by default
|
||||
* @param sensor_error_c coefficient of the sensor error. 0 by default
|
||||
* @param method The method to use to compute the planes.
|
||||
*/
|
||||
CV_EXPORTS_W void findPlanes(InputArray points3d, InputArray normals, OutputArray mask, OutputArray plane_coefficients,
|
||||
int block_size = 40, int min_size = 40*40, double threshold = 0.01,
|
||||
double sensor_error_a = 0, double sensor_error_b = 0,
|
||||
double sensor_error_c = 0,
|
||||
RgbdPlaneMethod method = RGBD_PLANE_METHOD_DEFAULT);
|
||||
|
||||
|
||||
|
||||
// TODO Depth interpolation
|
||||
// Curvature
|
||||
// Get rescaleDepth return dubles if asked for
|
||||
|
||||
//! @}
|
||||
|
||||
} /* namespace cv */
|
||||
|
||||
#endif // include guard
|
||||
@@ -0,0 +1,19 @@
|
||||
// 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
|
||||
|
||||
#ifndef OPENCV_3D_DETAIL_KINFU_FRAME_HPP
|
||||
#define OPENCV_3D_DETAIL_KINFU_FRAME_HPP
|
||||
|
||||
#include <opencv2/core/affine.hpp>
|
||||
|
||||
namespace cv {
|
||||
namespace detail {
|
||||
|
||||
CV_EXPORTS void renderPointsNormals(InputArray _points, InputArray _normals, OutputArray image, cv::Vec3f lightLoc);
|
||||
CV_EXPORTS void renderPointsNormalsColors(InputArray _points, InputArray _normals, InputArray _colors, OutputArray image);
|
||||
|
||||
} // namespace detail
|
||||
} // namespace cv
|
||||
|
||||
#endif // include guard
|
||||
@@ -0,0 +1,77 @@
|
||||
// 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
|
||||
|
||||
#ifndef POINTCLOUD_POSE_GRAPTH_HPP
|
||||
#define POINTCLOUD_POSE_GRAPTH_HPP
|
||||
|
||||
#include "opencv2/core/affine.hpp"
|
||||
#include "opencv2/core/quaternion.hpp"
|
||||
#include "opencv2/geometry/3d.hpp"
|
||||
#include "opencv2/geometry/detail/optimizer.hpp"
|
||||
|
||||
namespace cv
|
||||
{
|
||||
namespace detail
|
||||
{
|
||||
|
||||
// ATTENTION! This class is used internally in Large KinFu.
|
||||
// It has been pushed to publicly available headers for tests only.
|
||||
// Source compatibility of this API is not guaranteed in the future.
|
||||
|
||||
// This class provides tools to solve so-called pose graph problem often arisen in SLAM problems
|
||||
// The pose graph format, cost function and optimization techniques
|
||||
// repeat the ones used in Ceres 3D Pose Graph Optimization:
|
||||
// http://ceres-solver.org/nnls_tutorial.html#other-examples, pose_graph_3d.cc bullet
|
||||
class CV_EXPORTS_W PoseGraph
|
||||
{
|
||||
public:
|
||||
static Ptr<PoseGraph> create();
|
||||
virtual ~PoseGraph();
|
||||
|
||||
// Node may have any id >= 0
|
||||
virtual void addNode(size_t _nodeId, const Affine3d& _pose, bool fixed) = 0;
|
||||
virtual bool isNodeExist(size_t nodeId) const = 0;
|
||||
virtual bool setNodeFixed(size_t nodeId, bool fixed) = 0;
|
||||
virtual bool isNodeFixed(size_t nodeId) const = 0;
|
||||
virtual Affine3d getNodePose(size_t nodeId) const = 0;
|
||||
virtual std::vector<size_t> getNodesIds() const = 0;
|
||||
virtual size_t getNumNodes() const = 0;
|
||||
|
||||
// Edges have consequent indices starting from 0
|
||||
virtual void addEdge(size_t _sourceNodeId, size_t _targetNodeId, const Affine3f& _transformation,
|
||||
const Matx66f& _information = Matx66f::eye()) = 0;
|
||||
virtual size_t getEdgeStart(size_t i) const = 0;
|
||||
virtual size_t getEdgeEnd(size_t i) const = 0;
|
||||
virtual Affine3d getEdgePose(size_t i) const = 0;
|
||||
virtual Matx66f getEdgeInfo(size_t i) const = 0;
|
||||
virtual size_t getNumEdges() const = 0;
|
||||
|
||||
// checks if graph is connected and each edge connects exactly 2 nodes
|
||||
virtual bool isValid() const = 0;
|
||||
|
||||
// Calculates an initial pose estimate using the Minimum Spanning Tree (Prim's MST) algorithm.
|
||||
// The result serves as a starting point for further optimization.
|
||||
// Edge weights are calculated as:
|
||||
// weight = translationNorm + lambda * rotationAngle
|
||||
// The default lambda value (0.485) was empirically chosen based on its impact on optimizer performance,
|
||||
// but can/should be tuned for different datasets.
|
||||
virtual void initializePosesWithMST(double lambda = 0.485) = 0;
|
||||
|
||||
// creates an optimizer with user-defined settings and returns a pointer on it
|
||||
virtual Ptr<cv::detail::LevMarqBase> createOptimizer(const LevMarq::Settings& settings) = 0;
|
||||
// creates an optimizer with default settings and returns a pointer on it
|
||||
virtual Ptr<cv::detail::LevMarqBase> createOptimizer() = 0;
|
||||
|
||||
// Creates an optimizer (with default settings) if it wasn't created before and runs it
|
||||
// Returns number of iterations elapsed or -1 if failed to optimize
|
||||
virtual LevMarq::Report optimize() = 0;
|
||||
|
||||
// calculate cost function based on current nodes parameters
|
||||
virtual double calcEnergy() const = 0;
|
||||
};
|
||||
|
||||
} // namespace detail
|
||||
} // namespace cv
|
||||
|
||||
#endif // include guard
|
||||
@@ -0,0 +1,558 @@
|
||||
// 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
|
||||
|
||||
#ifndef OPENCV_3D_DETAIL_SUBMAP_HPP
|
||||
#define OPENCV_3D_DETAIL_SUBMAP_HPP
|
||||
|
||||
#include <opencv2/core.hpp>
|
||||
#include <opencv2/core/affine.hpp>
|
||||
#include "opencv2/geometry/detail/optimizer.hpp"
|
||||
|
||||
//TODO: remove it when it is rewritten to robust pose graph
|
||||
#include "opencv2/core/dualquaternion.hpp"
|
||||
|
||||
#include <type_traits>
|
||||
#include <vector>
|
||||
#include <map>
|
||||
#include <unordered_map>
|
||||
|
||||
|
||||
namespace cv
|
||||
{
|
||||
namespace detail
|
||||
{
|
||||
template<typename MatType>
|
||||
class Submap
|
||||
{
|
||||
public:
|
||||
struct PoseConstraint
|
||||
{
|
||||
Affine3f estimatedPose;
|
||||
int weight;
|
||||
|
||||
PoseConstraint() : weight(0){};
|
||||
|
||||
void accumulatePose(const Affine3f& _pose, int _weight = 1)
|
||||
{
|
||||
DualQuatf accPose = DualQuatf::createFromAffine3(estimatedPose) * float(weight) + DualQuatf::createFromAffine3(_pose) * float(_weight);
|
||||
weight += _weight;
|
||||
accPose = accPose / float(weight);
|
||||
estimatedPose = accPose.toAffine3();
|
||||
}
|
||||
};
|
||||
typedef std::map<int, PoseConstraint> Constraints;
|
||||
|
||||
Submap(int _id, const VolumeSettings& settings, const cv::Affine3f& _pose = cv::Affine3f::Identity(),
|
||||
int _startFrameId = 0)
|
||||
: id(_id), pose(_pose), cameraPose(Affine3f::Identity()), startFrameId(_startFrameId),
|
||||
volume(VolumeType::HashTSDF, settings)
|
||||
{ }
|
||||
virtual ~Submap() = default;
|
||||
|
||||
virtual void integrate(InputArray _depth, const int currframeId);
|
||||
virtual void raycast(const cv::Affine3f& cameraPose, cv::Size frameSize, cv::Matx33f K,
|
||||
OutputArray points = noArray(), OutputArray normals = noArray());
|
||||
|
||||
virtual int getTotalAllocatedBlocks() const { return int(volume.getTotalVolumeUnits()); };
|
||||
virtual int getVisibleBlocks(int currFrameId) const
|
||||
{
|
||||
CV_Assert(currFrameId >= startFrameId);
|
||||
//return volume.getVisibleBlocks(currFrameId, FRAME_VISIBILITY_THRESHOLD);
|
||||
return volume.getVisibleBlocks();
|
||||
|
||||
}
|
||||
|
||||
float calcVisibilityRatio(int currFrameId) const
|
||||
{
|
||||
int allocate_blocks = getTotalAllocatedBlocks();
|
||||
int visible_blocks = getVisibleBlocks(currFrameId);
|
||||
return float(visible_blocks) / float(allocate_blocks);
|
||||
}
|
||||
|
||||
// Adding new Edge for Loop Closure Detection. Return true or false to indicate whether adding success.
|
||||
bool addEdgeToSubmap(const int tarSubmapID, const Affine3f& tarPose);
|
||||
|
||||
//! TODO: Possibly useless
|
||||
virtual void setStartFrameId(int _startFrameId) { startFrameId = _startFrameId; };
|
||||
virtual void setStopFrameId(int _stopFrameId) { stopFrameId = _stopFrameId; };
|
||||
|
||||
void composeCameraPose(const cv::Affine3f& _relativePose) { cameraPose = cameraPose * _relativePose; }
|
||||
PoseConstraint& getConstraint(const int _id)
|
||||
{
|
||||
//! Creates constraints if doesn't exist yet
|
||||
return constraints[_id];
|
||||
}
|
||||
|
||||
public:
|
||||
const int id;
|
||||
cv::Affine3f pose;
|
||||
cv::Affine3f cameraPose;
|
||||
Constraints constraints;
|
||||
|
||||
int startFrameId;
|
||||
int stopFrameId;
|
||||
//! TODO: Should we support submaps for regular volumes?
|
||||
static constexpr int FRAME_VISIBILITY_THRESHOLD = 5;
|
||||
|
||||
//! TODO: Add support for GPU arrays (UMat)
|
||||
OdometryFrame frame;
|
||||
OdometryFrame renderFrame;
|
||||
|
||||
Volume volume;
|
||||
};
|
||||
|
||||
template<typename MatType>
|
||||
|
||||
void Submap<MatType>::integrate(InputArray _depth, const int currFrameId)
|
||||
{
|
||||
CV_Assert(currFrameId >= startFrameId);
|
||||
volume.integrate(_depth, cameraPose.matrix);
|
||||
}
|
||||
|
||||
template<typename MatType>
|
||||
void Submap<MatType>::raycast(const cv::Affine3f& _cameraPose, cv::Size frameSize, cv::Matx33f K,
|
||||
OutputArray points, OutputArray normals)
|
||||
{
|
||||
if (!points.needed() && !normals.needed())
|
||||
{
|
||||
MatType pts, nrm;
|
||||
//TODO: get depth instead of pts from raycast
|
||||
volume.raycast(_cameraPose.matrix, frameSize.height, frameSize.width, K, pts, nrm);
|
||||
|
||||
std::vector<MatType> pch(3);
|
||||
split(pts, pch);
|
||||
|
||||
renderFrame = frame;
|
||||
|
||||
frame = OdometryFrame(pch[2]);
|
||||
}
|
||||
else
|
||||
{
|
||||
volume.raycast(_cameraPose.matrix, frameSize.height, frameSize.width, K, points, normals);
|
||||
}
|
||||
}
|
||||
|
||||
template<typename MatType>
|
||||
bool Submap<MatType>::addEdgeToSubmap(const int tarSubmapID, const Affine3f& tarPose)
|
||||
{
|
||||
auto iter = constraints.find(tarSubmapID);
|
||||
|
||||
// if there is NO edge of currSubmap to tarSubmap.
|
||||
if(iter == constraints.end())
|
||||
{
|
||||
// Frome pose to tarPose transformation
|
||||
Affine3f estimatePose = tarPose * pose.inv();
|
||||
|
||||
// Create new Edge.
|
||||
PoseConstraint& preConstrain = getConstraint(tarSubmapID);
|
||||
preConstrain.accumulatePose(estimatePose, 1);
|
||||
|
||||
return true;
|
||||
} else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief: Manages all the created submaps for a particular scene
|
||||
*/
|
||||
template<typename MatType>
|
||||
class SubmapManager
|
||||
{
|
||||
public:
|
||||
enum class Type
|
||||
{
|
||||
NEW = 0,
|
||||
CURRENT = 1,
|
||||
RELOCALISATION = 2,
|
||||
LOOP_CLOSURE = 3,
|
||||
LOST = 4
|
||||
};
|
||||
|
||||
struct ActiveSubmapData
|
||||
{
|
||||
Type type;
|
||||
std::vector<Affine3f> constraints;
|
||||
int trackingAttempts;
|
||||
};
|
||||
|
||||
typedef Submap<MatType> SubmapT;
|
||||
typedef std::map<int, Ptr<SubmapT>> IdToSubmapPtr;
|
||||
typedef std::unordered_map<int, ActiveSubmapData> IdToActiveSubmaps;
|
||||
|
||||
explicit SubmapManager(const VolumeSettings& _volumeSettings) : volumeSettings(_volumeSettings) {}
|
||||
virtual ~SubmapManager() = default;
|
||||
|
||||
void reset() { submapList.clear(); };
|
||||
|
||||
bool shouldCreateSubmap(int frameId);
|
||||
bool shouldChangeCurrSubmap(int _frameId, int toSubmapId);
|
||||
|
||||
//! Adds a new submap/volume into the current list of managed/Active submaps
|
||||
int createNewSubmap(bool isCurrentActiveMap, const int currFrameId = 0, const Affine3f& pose = cv::Affine3f::Identity());
|
||||
|
||||
void removeSubmap(int _id);
|
||||
size_t numOfSubmaps(void) const { return submapList.size(); };
|
||||
size_t numOfActiveSubmaps(void) const { return activeSubmaps.size(); };
|
||||
|
||||
Ptr<SubmapT> getSubmap(int _id) const;
|
||||
Ptr<SubmapT> getCurrentSubmap(void) const;
|
||||
|
||||
int estimateConstraint(int fromSubmapId, int toSubmapId, int& inliers, Affine3f& inlierPose);
|
||||
bool updateMap(int frameId, const OdometryFrame& frame);
|
||||
|
||||
bool addEdgeToCurrentSubmap(const int currentSubmapID, const int tarSubmapID);
|
||||
|
||||
Ptr<detail::PoseGraph> MapToPoseGraph();
|
||||
void PoseGraphToMap(const Ptr<detail::PoseGraph>& updatedPoseGraph);
|
||||
|
||||
VolumeSettings volumeSettings;
|
||||
|
||||
std::vector<Ptr<SubmapT>> submapList;
|
||||
IdToActiveSubmaps activeSubmaps;
|
||||
|
||||
Ptr<detail::PoseGraph> poseGraph;
|
||||
};
|
||||
|
||||
template<typename MatType>
|
||||
int SubmapManager<MatType>::createNewSubmap(bool isCurrentMap, int currFrameId, const Affine3f& pose)
|
||||
{
|
||||
int newId = int(submapList.size());
|
||||
|
||||
Ptr<SubmapT> newSubmap = cv::makePtr<SubmapT>(newId, volumeSettings, pose, currFrameId);
|
||||
submapList.push_back(newSubmap);
|
||||
|
||||
ActiveSubmapData newSubmapData;
|
||||
newSubmapData.trackingAttempts = 0;
|
||||
newSubmapData.type = isCurrentMap ? Type::CURRENT : Type::NEW;
|
||||
activeSubmaps[newId] = newSubmapData;
|
||||
|
||||
return newId;
|
||||
}
|
||||
|
||||
template<typename MatType>
|
||||
Ptr<Submap<MatType>> SubmapManager<MatType>::getSubmap(int _id) const
|
||||
{
|
||||
CV_Assert(submapList.size() > 0);
|
||||
CV_Assert(_id >= 0 && _id < int(submapList.size()));
|
||||
return submapList.at(_id);
|
||||
}
|
||||
|
||||
template<typename MatType>
|
||||
Ptr<Submap<MatType>> SubmapManager<MatType>::getCurrentSubmap(void) const
|
||||
{
|
||||
for (const auto& it : activeSubmaps)
|
||||
{
|
||||
if (it.second.type == Type::CURRENT)
|
||||
return getSubmap(it.first);
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
template<typename MatType>
|
||||
bool SubmapManager<MatType>::shouldCreateSubmap(int currFrameId)
|
||||
{
|
||||
int currSubmapId = -1;
|
||||
for (const auto& it : activeSubmaps)
|
||||
{
|
||||
auto submapData = it.second;
|
||||
// No more than 1 new submap at a time!
|
||||
if (submapData.type == Type::NEW)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if (submapData.type == Type::CURRENT)
|
||||
{
|
||||
currSubmapId = it.first;
|
||||
}
|
||||
}
|
||||
//! TODO: This shouldn't be happening? since there should always be one active current submap
|
||||
if (currSubmapId < 0)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
Ptr<SubmapT> currSubmap = getSubmap(currSubmapId);
|
||||
float ratio = currSubmap->calcVisibilityRatio(currFrameId);
|
||||
|
||||
//TODO: fix this when a new pose graph is ready
|
||||
// if (ratio < 0.2f)
|
||||
if (ratio < 0.5f)
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
template<typename MatType>
|
||||
int SubmapManager<MatType>::estimateConstraint(int fromSubmapId, int toSubmapId, int& inliers, Affine3f& inlierPose)
|
||||
{
|
||||
static constexpr int MAX_ITER = 10;
|
||||
static constexpr float CONVERGE_WEIGHT_THRESHOLD = 0.01f;
|
||||
static constexpr float INLIER_WEIGHT_THRESH = 0.8f;
|
||||
static constexpr int MIN_INLIERS = 10;
|
||||
static constexpr int MAX_TRACKING_ATTEMPTS = 25;
|
||||
|
||||
//! thresh = HUBER_THRESH
|
||||
auto huberWeight = [](float residual, float thresh = 0.1f) -> float {
|
||||
float rAbs = abs(residual);
|
||||
if (rAbs < thresh)
|
||||
return 1.0;
|
||||
float numerator = sqrt(2 * thresh * rAbs - thresh * thresh);
|
||||
return numerator / rAbs;
|
||||
};
|
||||
|
||||
Ptr<SubmapT> fromSubmap = getSubmap(fromSubmapId);
|
||||
Ptr<SubmapT> toSubmap = getSubmap(toSubmapId);
|
||||
ActiveSubmapData& fromSubmapData = activeSubmaps.at(fromSubmapId);
|
||||
|
||||
Affine3f TcameraToFromSubmap = fromSubmap->cameraPose;
|
||||
Affine3f TcameraToToSubmap = toSubmap->cameraPose;
|
||||
|
||||
// FromSubmap -> ToSubmap transform
|
||||
Affine3f candidateConstraint = TcameraToToSubmap * TcameraToFromSubmap.inv();
|
||||
fromSubmapData.trackingAttempts++;
|
||||
fromSubmapData.constraints.push_back(candidateConstraint);
|
||||
|
||||
std::vector<float> weights(fromSubmapData.constraints.size() + 1, 1.0f);
|
||||
|
||||
Affine3f prevConstraint = fromSubmap->getConstraint(toSubmap->id).estimatedPose;
|
||||
int prevWeight = fromSubmap->getConstraint(toSubmap->id).weight;
|
||||
|
||||
// Iterative reweighted least squares with huber threshold to find the inliers in the past observations
|
||||
Vec6f meanConstraint;
|
||||
float sumWeight = 0.0f;
|
||||
for (int i = 0; i < MAX_ITER; i++)
|
||||
{
|
||||
Vec6f constraintVec;
|
||||
for (int j = 0; j < int(weights.size() - 1); j++)
|
||||
{
|
||||
Affine3f currObservation = fromSubmapData.constraints[j];
|
||||
cv::vconcat(currObservation.rvec(), currObservation.translation(), constraintVec);
|
||||
meanConstraint += weights[j] * constraintVec;
|
||||
sumWeight += weights[j];
|
||||
}
|
||||
// Heavier weight given to the estimatedPose
|
||||
cv::vconcat(prevConstraint.rvec(), prevConstraint.translation(), constraintVec);
|
||||
meanConstraint += weights.back() * prevWeight * constraintVec;
|
||||
sumWeight += prevWeight;
|
||||
meanConstraint /= float(sumWeight);
|
||||
|
||||
float residual = 0.0f;
|
||||
float diff = 0.0f;
|
||||
for (int j = 0; j < int(weights.size()); j++)
|
||||
{
|
||||
int w;
|
||||
if (j == int(weights.size() - 1))
|
||||
{
|
||||
cv::vconcat(prevConstraint.rvec(), prevConstraint.translation(), constraintVec);
|
||||
w = prevWeight;
|
||||
}
|
||||
else
|
||||
{
|
||||
Affine3f currObservation = fromSubmapData.constraints[j];
|
||||
cv::vconcat(currObservation.rvec(), currObservation.translation(), constraintVec);
|
||||
w = 1;
|
||||
}
|
||||
|
||||
cv::Vec6f residualVec = (constraintVec - meanConstraint);
|
||||
residual = float(norm(residualVec));
|
||||
float newWeight = huberWeight(residual);
|
||||
diff += w * abs(newWeight - weights[j]);
|
||||
weights[j] = newWeight;
|
||||
}
|
||||
|
||||
if (diff / (prevWeight + weights.size() - 1) < CONVERGE_WEIGHT_THRESHOLD)
|
||||
break;
|
||||
}
|
||||
|
||||
int localInliers = 0;
|
||||
DualQuatf inlierConstraint;
|
||||
for (int i = 0; i < int(weights.size()); i++)
|
||||
{
|
||||
if (weights[i] > INLIER_WEIGHT_THRESH)
|
||||
{
|
||||
localInliers++;
|
||||
if (i == int(weights.size() - 1))
|
||||
inlierConstraint += DualQuatf::createFromMat(prevConstraint.matrix);
|
||||
else
|
||||
inlierConstraint += DualQuatf::createFromMat(fromSubmapData.constraints[i].matrix);
|
||||
}
|
||||
}
|
||||
inlierConstraint = inlierConstraint * 1.0f/float(max(localInliers, 1));
|
||||
inlierPose = inlierConstraint.toAffine3();
|
||||
inliers = localInliers;
|
||||
|
||||
if (inliers >= MIN_INLIERS)
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
if(fromSubmapData.trackingAttempts - inliers > (MAX_TRACKING_ATTEMPTS - MIN_INLIERS))
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
template<typename MatType>
|
||||
bool SubmapManager<MatType>::shouldChangeCurrSubmap(int _frameId, int toSubmapId)
|
||||
{
|
||||
auto toSubmap = getSubmap(toSubmapId);
|
||||
auto toSubmapData = activeSubmaps.at(toSubmapId);
|
||||
auto currActiveSubmap = getCurrentSubmap();
|
||||
|
||||
int blocksInNewMap = toSubmap->getTotalAllocatedBlocks();
|
||||
float newRatio = toSubmap->calcVisibilityRatio(_frameId);
|
||||
|
||||
float currRatio = currActiveSubmap->calcVisibilityRatio(_frameId);
|
||||
|
||||
//! TODO: Check for a specific threshold?
|
||||
if (blocksInNewMap <= 0)
|
||||
return false;
|
||||
if ((newRatio > currRatio) && (toSubmapData.type == Type::NEW))
|
||||
return true;
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
template<typename MatType>
|
||||
bool SubmapManager<MatType>::addEdgeToCurrentSubmap(const int currentSubmapID, const int tarSubmapID)
|
||||
{
|
||||
Ptr<SubmapT> currentSubmap = getSubmap(currentSubmapID);
|
||||
Ptr<SubmapT> tarSubmap = getSubmap(tarSubmapID);
|
||||
|
||||
return currentSubmap->addEdgeToSubmap(tarSubmapID, tarSubmap->pose);
|
||||
}
|
||||
|
||||
template<typename MatType>
|
||||
bool SubmapManager<MatType>::updateMap(int _frameId, const OdometryFrame& _frame)
|
||||
{
|
||||
bool mapUpdated = false;
|
||||
int changedCurrentMapId = -1;
|
||||
|
||||
const int currSubmapId = getCurrentSubmap()->id;
|
||||
|
||||
for (auto& it : activeSubmaps)
|
||||
{
|
||||
int submapId = it.first;
|
||||
auto& submapData = it.second;
|
||||
if (submapData.type == Type::NEW || submapData.type == Type::LOOP_CLOSURE)
|
||||
{
|
||||
// Check with previous estimate
|
||||
int inliers;
|
||||
Affine3f inlierPose;
|
||||
int constraintUpdate = estimateConstraint(submapId, currSubmapId, inliers, inlierPose);
|
||||
if (constraintUpdate == 1)
|
||||
{
|
||||
typename SubmapT::PoseConstraint& submapConstraint = getSubmap(submapId)->getConstraint(currSubmapId);
|
||||
submapConstraint.accumulatePose(inlierPose, inliers);
|
||||
submapData.constraints.clear();
|
||||
submapData.trackingAttempts = 0;
|
||||
|
||||
if (shouldChangeCurrSubmap(_frameId, submapId))
|
||||
{
|
||||
changedCurrentMapId = submapId;
|
||||
}
|
||||
mapUpdated = true;
|
||||
}
|
||||
else if(constraintUpdate == -1)
|
||||
{
|
||||
submapData.type = Type::LOST;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<int> createNewConstraintsList;
|
||||
for (auto& it : activeSubmaps)
|
||||
{
|
||||
int submapId = it.first;
|
||||
auto& submapData = it.second;
|
||||
|
||||
if (submapId == changedCurrentMapId)
|
||||
{
|
||||
submapData.type = Type::CURRENT;
|
||||
}
|
||||
if ((submapData.type == Type::CURRENT) && (changedCurrentMapId >= 0) && (submapId != changedCurrentMapId))
|
||||
{
|
||||
submapData.type = Type::LOST;
|
||||
createNewConstraintsList.push_back(submapId);
|
||||
}
|
||||
if ((submapData.type == Type::NEW || submapData.type == Type::LOOP_CLOSURE) && (changedCurrentMapId >= 0))
|
||||
{
|
||||
//! TODO: Add a new type called NEW_LOST?
|
||||
submapData.type = Type::LOST;
|
||||
createNewConstraintsList.push_back(submapId);
|
||||
}
|
||||
}
|
||||
|
||||
for (typename IdToActiveSubmaps::iterator it = activeSubmaps.begin(); it != activeSubmaps.end();)
|
||||
{
|
||||
auto& submapData = it->second;
|
||||
if (submapData.type == Type::LOST)
|
||||
it = activeSubmaps.erase(it);
|
||||
else
|
||||
it++;
|
||||
}
|
||||
|
||||
for (std::vector<int>::const_iterator it = createNewConstraintsList.begin(); it != createNewConstraintsList.end(); ++it)
|
||||
{
|
||||
int dataId = *it;
|
||||
ActiveSubmapData newSubmapData;
|
||||
newSubmapData.trackingAttempts = 0;
|
||||
newSubmapData.type = Type::LOOP_CLOSURE;
|
||||
activeSubmaps[dataId] = newSubmapData;
|
||||
}
|
||||
|
||||
if (shouldCreateSubmap(_frameId))
|
||||
{
|
||||
Ptr<SubmapT> currActiveSubmap = getCurrentSubmap();
|
||||
Affine3f newSubmapPose = currActiveSubmap->pose * currActiveSubmap->cameraPose;
|
||||
int submapId = createNewSubmap(false, _frameId, newSubmapPose);
|
||||
auto newSubmap = getSubmap(submapId);
|
||||
newSubmap->frame = _frame;
|
||||
}
|
||||
|
||||
return mapUpdated;
|
||||
}
|
||||
|
||||
template<typename MatType>
|
||||
Ptr<detail::PoseGraph> SubmapManager<MatType>::MapToPoseGraph()
|
||||
{
|
||||
Ptr<detail::PoseGraph> localPoseGraph = detail::PoseGraph::create();
|
||||
|
||||
for(const auto& currSubmap : submapList)
|
||||
{
|
||||
const typename SubmapT::Constraints& constraintList = currSubmap->constraints;
|
||||
for(const auto& currConstraintPair : constraintList)
|
||||
{
|
||||
// TODO: Handle case with duplicate constraints A -> B and B -> A
|
||||
/* Matx66f informationMatrix = Matx66f::eye() * (currConstraintPair.second.weight/10); */
|
||||
Matx66f informationMatrix = Matx66f::eye();
|
||||
localPoseGraph->addEdge(currSubmap->id, currConstraintPair.first, currConstraintPair.second.estimatedPose, informationMatrix);
|
||||
}
|
||||
}
|
||||
|
||||
for(const auto& currSubmap : submapList)
|
||||
{
|
||||
localPoseGraph->addNode(currSubmap->id, currSubmap->pose, (currSubmap->id == 0));
|
||||
}
|
||||
|
||||
return localPoseGraph;
|
||||
}
|
||||
|
||||
template <typename MatType>
|
||||
void SubmapManager<MatType>::PoseGraphToMap(const Ptr<detail::PoseGraph>& updatedPoseGraph)
|
||||
{
|
||||
for(const auto& currSubmap : submapList)
|
||||
{
|
||||
Affine3d pose = updatedPoseGraph->getNodePose(currSubmap->id);
|
||||
if(!updatedPoseGraph->isNodeFixed(currSubmap->id))
|
||||
currSubmap->pose = pose;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace detail
|
||||
} // namespace cv
|
||||
|
||||
#endif // include guard
|
||||
@@ -0,0 +1,112 @@
|
||||
// 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
|
||||
|
||||
#ifndef OPENCV_3D_ODOMETRY_HPP
|
||||
#define OPENCV_3D_ODOMETRY_HPP
|
||||
|
||||
#include <opencv2/core.hpp>
|
||||
|
||||
#include "odometry_frame.hpp"
|
||||
#include "odometry_settings.hpp"
|
||||
|
||||
namespace cv
|
||||
{
|
||||
|
||||
/** These constants are used to set a type of data which odometry will use
|
||||
* @param DEPTH only depth data
|
||||
* @param RGB only rgb image
|
||||
* @param RGB_DEPTH depth and rgb data simultaneously
|
||||
*/
|
||||
enum class OdometryType
|
||||
{
|
||||
DEPTH = 0,
|
||||
RGB = 1,
|
||||
RGB_DEPTH = 2
|
||||
};
|
||||
|
||||
/** These constants are used to set the speed and accuracy of odometry
|
||||
* @param COMMON accurate but not so fast
|
||||
* @param FAST less accurate but faster
|
||||
*/
|
||||
enum class OdometryAlgoType
|
||||
{
|
||||
COMMON = 0,
|
||||
FAST = 1
|
||||
};
|
||||
|
||||
class CV_EXPORTS_W Odometry
|
||||
{
|
||||
public:
|
||||
CV_WRAP Odometry();
|
||||
CV_WRAP explicit Odometry(OdometryType otype);
|
||||
CV_WRAP Odometry(OdometryType otype, const OdometrySettings& settings, OdometryAlgoType algtype);
|
||||
~Odometry();
|
||||
|
||||
/** Prepare frame for odometry calculation
|
||||
* @param frame odometry prepare this frame as src frame and dst frame simultaneously
|
||||
*/
|
||||
CV_WRAP void prepareFrame(OdometryFrame& frame) const;
|
||||
|
||||
/** Prepare frame for odometry calculation
|
||||
* @param srcFrame frame will be prepared as src frame ("original" image)
|
||||
* @param dstFrame frame will be prepared as dsr frame ("rotated" image)
|
||||
*/
|
||||
CV_WRAP void prepareFrames(OdometryFrame& srcFrame, OdometryFrame& dstFrame) const;
|
||||
|
||||
/** Compute Rigid Transformation between two frames so that Rt * src = dst
|
||||
* Both frames, source and destination, should have been prepared by calling prepareFrame() first
|
||||
*
|
||||
* @param srcFrame src frame ("original" image)
|
||||
* @param dstFrame dst frame ("rotated" image)
|
||||
* @param Rt Rigid transformation, which will be calculated, in form:
|
||||
* { R_11 R_12 R_13 t_1
|
||||
* R_21 R_22 R_23 t_2
|
||||
* R_31 R_32 R_33 t_3
|
||||
* 0 0 0 1 }
|
||||
* @return true on success, false if failed to find the transformation
|
||||
*/
|
||||
CV_WRAP bool compute(const OdometryFrame& srcFrame, const OdometryFrame& dstFrame, OutputArray Rt) const;
|
||||
/**
|
||||
* @brief Compute Rigid Transformation between two frames so that Rt * src = dst
|
||||
*
|
||||
* @param srcDepth source depth ("original" image)
|
||||
* @param dstDepth destination depth ("rotated" image)
|
||||
* @param Rt Rigid transformation, which will be calculated, in form:
|
||||
* { R_11 R_12 R_13 t_1
|
||||
* R_21 R_22 R_23 t_2
|
||||
* R_31 R_32 R_33 t_3
|
||||
* 0 0 0 1 }
|
||||
* @return true on success, false if failed to find the transformation
|
||||
*/
|
||||
CV_WRAP bool compute(InputArray srcDepth, InputArray dstDepth, OutputArray Rt) const;
|
||||
/**
|
||||
* @brief Compute Rigid Transformation between two frames so that Rt * src = dst
|
||||
*
|
||||
* @param srcDepth source depth ("original" image)
|
||||
* @param srcRGB source RGB
|
||||
* @param dstDepth destination depth ("rotated" image)
|
||||
* @param dstRGB destination RGB
|
||||
* @param Rt Rigid transformation, which will be calculated, in form:
|
||||
* { R_11 R_12 R_13 t_1
|
||||
* R_21 R_22 R_23 t_2
|
||||
* R_31 R_32 R_33 t_3
|
||||
* 0 0 0 1 }
|
||||
* @return true on success, false if failed to find the transformation
|
||||
*/
|
||||
CV_WRAP bool compute(InputArray srcDepth, InputArray srcRGB, InputArray dstDepth, InputArray dstRGB, OutputArray Rt) const;
|
||||
|
||||
/**
|
||||
* @brief Get the normals computer object used for normals calculation (if presented).
|
||||
* The normals computer is generated at first need during prepareFrame when normals are required for the ICP algorithm
|
||||
* but not presented by a user. Re-generated each time the related settings change or a new frame arrives with the different size.
|
||||
*/
|
||||
CV_WRAP Ptr<RgbdNormals> getNormalsComputer() const;
|
||||
|
||||
class Impl;
|
||||
private:
|
||||
Ptr<Impl> impl;
|
||||
};
|
||||
|
||||
}
|
||||
#endif //OPENCV_3D_ODOMETRY_HPP
|
||||
@@ -0,0 +1,108 @@
|
||||
// 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
|
||||
|
||||
#ifndef OPENCV_3D_ODOMETRY_FRAME_HPP
|
||||
#define OPENCV_3D_ODOMETRY_FRAME_HPP
|
||||
|
||||
#include <opencv2/core.hpp>
|
||||
|
||||
namespace cv
|
||||
{
|
||||
/** Indicates what pyramid is to access using getPyramidAt() method:
|
||||
*/
|
||||
|
||||
enum OdometryFramePyramidType
|
||||
{
|
||||
PYR_IMAGE = 0, //!< The pyramid of grayscale images
|
||||
PYR_DEPTH = 1, //!< The pyramid of depth images
|
||||
PYR_MASK = 2, //!< The pyramid of masks
|
||||
PYR_CLOUD = 3, //!< The pyramid of point clouds, produced from the pyramid of depths
|
||||
PYR_DIX = 4, //!< The pyramid of dI/dx derivative images
|
||||
PYR_DIY = 5, //!< The pyramid of dI/dy derivative images
|
||||
PYR_TEXMASK = 6, //!< The pyramid of "textured" masks (i.e. additional masks for normals or grayscale images)
|
||||
PYR_NORM = 7, //!< The pyramid of normals
|
||||
PYR_NORMMASK = 8, //!< The pyramid of normals masks
|
||||
N_PYRAMIDS
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief An object that keeps per-frame data for Odometry algorithms from user-provided images to algorithm-specific precalculated data.
|
||||
* When not empty, it contains a depth image, a mask of valid pixels and a set of pyramids generated from that data.
|
||||
* A BGR/Gray image and normals are optional.
|
||||
* OdometryFrame is made to be used together with Odometry class to reuse precalculated data between Rt data calculations.
|
||||
* A correct way to do that is to call Odometry::prepareFrames() on prev and next frames and then pass them to Odometry::compute() method.
|
||||
*/
|
||||
class CV_EXPORTS_W_SIMPLE OdometryFrame
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Construct a new OdometryFrame object. All non-empty images should have the same size.
|
||||
*
|
||||
* @param depth A depth image, should be CV_8UC1
|
||||
* @param image An BGR or grayscale image (or noArray() if it's not required for used ICP algorithm).
|
||||
* Should be CV_8UC3 or CV_8C4 if it's BGR image or CV_8UC1 if it's grayscale. If it's BGR then it's converted to grayscale
|
||||
* image automatically.
|
||||
* @param mask A user-provided mask of valid pixels, should be CV_8UC1
|
||||
* @param normals A user-provided normals to the depth surface, should be CV_32FC4
|
||||
*/
|
||||
CV_WRAP explicit OdometryFrame(InputArray depth = noArray(), InputArray image = noArray(), InputArray mask = noArray(), InputArray normals = noArray());
|
||||
~OdometryFrame() {};
|
||||
|
||||
/**
|
||||
* @brief Get the original user-provided BGR/Gray image
|
||||
*
|
||||
* @param image Output image
|
||||
*/
|
||||
CV_WRAP void getImage(OutputArray image) const;
|
||||
/**
|
||||
* @brief Get the gray image generated from the user-provided BGR/Gray image
|
||||
*
|
||||
* @param image Output image
|
||||
*/
|
||||
CV_WRAP void getGrayImage(OutputArray image) const;
|
||||
/**
|
||||
* @brief Get the original user-provided depth image
|
||||
*
|
||||
* @param depth Output image
|
||||
*/
|
||||
CV_WRAP void getDepth(OutputArray depth) const;
|
||||
/**
|
||||
* @brief Get the depth image generated from the user-provided one after conversion, rescale or filtering for ICP algorithm needs
|
||||
*
|
||||
* @param depth Output image
|
||||
*/
|
||||
CV_WRAP void getProcessedDepth(OutputArray depth) const;
|
||||
/**
|
||||
* @brief Get the valid pixels mask generated for the ICP calculations intersected with the user-provided mask
|
||||
*
|
||||
* @param mask Output image
|
||||
*/
|
||||
CV_WRAP void getMask(OutputArray mask) const;
|
||||
/**
|
||||
* @brief Get the normals image either generated for the ICP calculations or user-provided
|
||||
*
|
||||
* @param normals Output image
|
||||
*/
|
||||
CV_WRAP void getNormals(OutputArray normals) const;
|
||||
|
||||
/**
|
||||
* @brief Get the amount of levels in pyramids (all of them if not empty should have the same number of levels)
|
||||
* or 0 if no pyramids were prepared yet
|
||||
*/
|
||||
CV_WRAP int getPyramidLevels() const;
|
||||
/**
|
||||
* @brief Get the image generated for the ICP calculations from one of the pyramids specified by pyrType. Returns empty image if
|
||||
* the pyramid is empty or there's no such pyramid level
|
||||
*
|
||||
* @param img Output image
|
||||
* @param pyrType Type of pyramid
|
||||
* @param level Level in the pyramid
|
||||
*/
|
||||
CV_WRAP void getPyramidAt(OutputArray img, OdometryFramePyramidType pyrType, size_t level) const;
|
||||
|
||||
class Impl;
|
||||
Ptr<Impl> impl;
|
||||
};
|
||||
}
|
||||
#endif // !OPENCV_3D_ODOMETRY_FRAME_HPP
|
||||
@@ -0,0 +1,65 @@
|
||||
// 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
|
||||
|
||||
#ifndef OPENCV_3D_ODOMETRY_SETTINGS_HPP
|
||||
#define OPENCV_3D_ODOMETRY_SETTINGS_HPP
|
||||
|
||||
#include <opencv2/core.hpp>
|
||||
|
||||
namespace cv
|
||||
{
|
||||
|
||||
class CV_EXPORTS_W_SIMPLE OdometrySettings
|
||||
{
|
||||
public:
|
||||
CV_WRAP OdometrySettings();
|
||||
OdometrySettings(const OdometrySettings&);
|
||||
OdometrySettings& operator=(const OdometrySettings&);
|
||||
~OdometrySettings() {};
|
||||
CV_WRAP void setCameraMatrix(InputArray val);
|
||||
CV_WRAP void getCameraMatrix(OutputArray val) const;
|
||||
CV_WRAP void setIterCounts(InputArray val);
|
||||
CV_WRAP void getIterCounts(OutputArray val) const;
|
||||
|
||||
CV_WRAP void setMinDepth(float val);
|
||||
CV_WRAP float getMinDepth() const;
|
||||
CV_WRAP void setMaxDepth(float val);
|
||||
CV_WRAP float getMaxDepth() const;
|
||||
CV_WRAP void setMaxDepthDiff(float val);
|
||||
CV_WRAP float getMaxDepthDiff() const;
|
||||
CV_WRAP void setMaxPointsPart(float val);
|
||||
CV_WRAP float getMaxPointsPart() const;
|
||||
|
||||
CV_WRAP void setSobelSize(int val);
|
||||
CV_WRAP int getSobelSize() const;
|
||||
CV_WRAP void setSobelScale(double val);
|
||||
CV_WRAP double getSobelScale() const;
|
||||
|
||||
CV_WRAP void setNormalWinSize(int val);
|
||||
CV_WRAP int getNormalWinSize() const;
|
||||
CV_WRAP void setNormalDiffThreshold(float val);
|
||||
CV_WRAP float getNormalDiffThreshold() const;
|
||||
CV_WRAP void setNormalMethod(RgbdNormals::RgbdNormalsMethod nm);
|
||||
CV_WRAP RgbdNormals::RgbdNormalsMethod getNormalMethod() const;
|
||||
|
||||
CV_WRAP void setAngleThreshold(float val);
|
||||
CV_WRAP float getAngleThreshold() const;
|
||||
CV_WRAP void setMaxTranslation(float val);
|
||||
CV_WRAP float getMaxTranslation() const;
|
||||
CV_WRAP void setMaxRotation(float val);
|
||||
CV_WRAP float getMaxRotation() const;
|
||||
|
||||
CV_WRAP void setMinGradientMagnitude(float val);
|
||||
CV_WRAP float getMinGradientMagnitude() const;
|
||||
CV_WRAP void setMinGradientMagnitudes(InputArray val);
|
||||
CV_WRAP void getMinGradientMagnitudes(OutputArray val) const;
|
||||
|
||||
class Impl;
|
||||
|
||||
private:
|
||||
Ptr<Impl> impl;
|
||||
};
|
||||
|
||||
}
|
||||
#endif //OPENCV_3D_ODOMETRY_SETTINGS_HPP
|
||||
@@ -0,0 +1,172 @@
|
||||
// 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
|
||||
|
||||
#ifndef OPENCV_3D_VOLUME_HPP
|
||||
#define OPENCV_3D_VOLUME_HPP
|
||||
|
||||
#include "volume_settings.hpp"
|
||||
|
||||
#include "opencv2/ptcloud/odometry_frame.hpp"
|
||||
#include "opencv2/core/affine.hpp"
|
||||
|
||||
namespace cv
|
||||
{
|
||||
|
||||
class CV_EXPORTS_W Volume
|
||||
{
|
||||
public:
|
||||
/** @brief Constructor of custom volume.
|
||||
* @param vtype the volume type [TSDF, HashTSDF, ColorTSDF].
|
||||
* @param settings the custom settings for volume.
|
||||
*/
|
||||
CV_WRAP explicit Volume(VolumeType vtype = VolumeType::TSDF,
|
||||
const VolumeSettings& settings = VolumeSettings(VolumeType::TSDF));
|
||||
~Volume();
|
||||
|
||||
/** @brief Integrates the input data to the volume.
|
||||
|
||||
Camera intrinsics are taken from volume settings structure.
|
||||
|
||||
* @param frame the object from which to take depth and image data.
|
||||
For color TSDF a depth data should be registered with color data, i.e. have the same intrinsics & camera pose.
|
||||
This can be done using function registerDepth() from 3d module.
|
||||
* @param pose the pose of camera in global coordinates.
|
||||
*/
|
||||
CV_WRAP_AS(integrateFrame) void integrate(const OdometryFrame& frame, InputArray pose);
|
||||
|
||||
/** @brief Integrates the input data to the volume.
|
||||
|
||||
Camera intrinsics are taken from volume settings structure.
|
||||
|
||||
* @param depth the depth image.
|
||||
* @param pose the pose of camera in global coordinates.
|
||||
*/
|
||||
CV_WRAP_AS(integrate) void integrate(InputArray depth, InputArray pose);
|
||||
|
||||
/** @brief Integrates the input data to the volume.
|
||||
|
||||
Camera intrinsics are taken from volume settings structure.
|
||||
|
||||
* @param depth the depth image.
|
||||
* @param image the color image (only for ColorTSDF).
|
||||
For color TSDF a depth data should be registered with color data, i.e. have the same intrinsics & camera pose.
|
||||
This can be done using function registerDepth() from 3d module.
|
||||
* @param pose the pose of camera in global coordinates.
|
||||
*/
|
||||
CV_WRAP_AS(integrateColor) void integrate(InputArray depth, InputArray image, InputArray pose);
|
||||
|
||||
// Python bindings do not process noArray() default argument correctly, so the raycast() method is repeated several times
|
||||
|
||||
/** @brief Renders the volume contents into an image. The resulting points and normals are in camera's coordinate system.
|
||||
|
||||
Rendered image size and camera intrinsics are taken from volume settings structure.
|
||||
|
||||
* @param cameraPose the pose of camera in global coordinates.
|
||||
* @param points image to store rendered points.
|
||||
* @param normals image to store rendered normals corresponding to points.
|
||||
*/
|
||||
CV_WRAP void raycast(InputArray cameraPose, OutputArray points, OutputArray normals) const;
|
||||
|
||||
/** @brief Renders the volume contents into an image. The resulting points and normals are in camera's coordinate system.
|
||||
|
||||
Rendered image size and camera intrinsics are taken from volume settings structure.
|
||||
|
||||
* @param cameraPose the pose of camera in global coordinates.
|
||||
* @param points image to store rendered points.
|
||||
* @param normals image to store rendered normals corresponding to points.
|
||||
* @param colors image to store rendered colors corresponding to points (only for ColorTSDF).
|
||||
*/
|
||||
CV_WRAP_AS(raycastColor) void raycast(InputArray cameraPose, OutputArray points, OutputArray normals, OutputArray colors) const;
|
||||
|
||||
/** @brief Renders the volume contents into an image. The resulting points and normals are in camera's coordinate system.
|
||||
|
||||
Rendered image size and camera intrinsics are taken from volume settings structure.
|
||||
|
||||
* @param cameraPose the pose of camera in global coordinates.
|
||||
* @param height the height of result image
|
||||
* @param width the width of result image
|
||||
* @param K camera raycast intrinsics
|
||||
* @param points image to store rendered points.
|
||||
* @param normals image to store rendered normals corresponding to points.
|
||||
*/
|
||||
CV_WRAP_AS(raycastEx) void raycast(InputArray cameraPose, int height, int width, InputArray K, OutputArray points, OutputArray normals) const;
|
||||
|
||||
|
||||
/** @brief Renders the volume contents into an image. The resulting points and normals are in camera's coordinate system.
|
||||
|
||||
Rendered image size and camera intrinsics are taken from volume settings structure.
|
||||
|
||||
* @param cameraPose the pose of camera in global coordinates.
|
||||
* @param height the height of result image
|
||||
* @param width the width of result image
|
||||
* @param K camera raycast intrinsics
|
||||
* @param points image to store rendered points.
|
||||
* @param normals image to store rendered normals corresponding to points.
|
||||
* @param colors image to store rendered colors corresponding to points (only for ColorTSDF).
|
||||
*/
|
||||
CV_WRAP_AS(raycastExColor) void raycast(InputArray cameraPose, int height, int width, InputArray K, OutputArray points, OutputArray normals, OutputArray colors) const;
|
||||
|
||||
/** @brief Extract the all data from volume.
|
||||
* @param points the input exist point.
|
||||
* @param normals the storage of normals (corresponding to input points) in the image.
|
||||
*/
|
||||
CV_WRAP void fetchNormals(InputArray points, OutputArray normals) const;
|
||||
/** @brief Extract the all data from volume.
|
||||
* @param points the storage of all points.
|
||||
* @param normals the storage of all normals, corresponding to points.
|
||||
*/
|
||||
CV_WRAP void fetchPointsNormals(OutputArray points, OutputArray normals) const;
|
||||
/** @brief Extract the all data from volume.
|
||||
* @param points the storage of all points.
|
||||
* @param normals the storage of all normals, corresponding to points.
|
||||
* @param colors the storage of all colors, corresponding to points (only for ColorTSDF).
|
||||
*/
|
||||
CV_WRAP void fetchPointsNormalsColors(OutputArray points, OutputArray normals, OutputArray colors) const;
|
||||
|
||||
/** @brief clear all data in volume.
|
||||
*/
|
||||
CV_WRAP void reset();
|
||||
|
||||
//TODO: remove this
|
||||
/** @brief return visible blocks in volume.
|
||||
*/
|
||||
CV_WRAP int getVisibleBlocks() const;
|
||||
|
||||
/** @brief return number of volume units in volume.
|
||||
*/
|
||||
CV_WRAP size_t getTotalVolumeUnits() const;
|
||||
|
||||
enum BoundingBoxPrecision
|
||||
{
|
||||
VOLUME_UNIT = 0,
|
||||
VOXEL = 1
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Gets bounding box in volume coordinates with given precision:
|
||||
* VOLUME_UNIT - up to volume unit
|
||||
* VOXEL - up to voxel (currently not supported)
|
||||
* @param bb 6-float 1d array containing (min_x, min_y, min_z, max_x, max_y, max_z) in volume coordinates
|
||||
* @param precision bounding box calculation precision
|
||||
*/
|
||||
CV_WRAP void getBoundingBox(OutputArray bb, int precision) const;
|
||||
|
||||
/**
|
||||
* @brief Enables or disables new volume unit allocation during integration.
|
||||
* Makes sense for HashTSDF only.
|
||||
*/
|
||||
CV_WRAP void setEnableGrowth(bool v);
|
||||
/**
|
||||
* @brief Returns if new volume units are allocated during integration or not.
|
||||
* Makes sense for HashTSDF only.
|
||||
*/
|
||||
CV_WRAP bool getEnableGrowth() const;
|
||||
|
||||
class Impl;
|
||||
private:
|
||||
Ptr<Impl> impl;
|
||||
};
|
||||
|
||||
} // namespace cv
|
||||
#endif // include guard
|
||||
@@ -0,0 +1,208 @@
|
||||
// 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
|
||||
|
||||
|
||||
#ifndef OPENCV_3D_VOLUME_SETTINGS_HPP
|
||||
#define OPENCV_3D_VOLUME_SETTINGS_HPP
|
||||
|
||||
#include <opencv2/core.hpp>
|
||||
#include <opencv2/ptcloud/volume.hpp>
|
||||
|
||||
namespace cv
|
||||
{
|
||||
|
||||
enum class VolumeType
|
||||
{
|
||||
TSDF = 0,
|
||||
HashTSDF = 1,
|
||||
ColorTSDF = 2
|
||||
};
|
||||
|
||||
|
||||
class CV_EXPORTS_W_SIMPLE VolumeSettings
|
||||
{
|
||||
public:
|
||||
/** @brief Constructor of settings for custom Volume type.
|
||||
* @param volumeType volume type.
|
||||
*/
|
||||
CV_WRAP explicit VolumeSettings(VolumeType volumeType = VolumeType::TSDF);
|
||||
|
||||
VolumeSettings(const VolumeSettings& vs);
|
||||
VolumeSettings& operator=(const VolumeSettings&);
|
||||
~VolumeSettings();
|
||||
|
||||
/** @brief Sets the width of the image for integration.
|
||||
* @param val input value.
|
||||
*/
|
||||
CV_WRAP void setIntegrateWidth(int val);
|
||||
|
||||
/** @brief Returns the width of the image for integration.
|
||||
*/
|
||||
CV_WRAP int getIntegrateWidth() const;
|
||||
|
||||
/** @brief Sets the height of the image for integration.
|
||||
* @param val input value.
|
||||
*/
|
||||
CV_WRAP void setIntegrateHeight(int val);
|
||||
|
||||
/** @brief Returns the height of the image for integration.
|
||||
*/
|
||||
CV_WRAP int getIntegrateHeight() const;
|
||||
|
||||
/** @brief Sets the width of the raycasted image, used when user does not provide it at raycast() call.
|
||||
* @param val input value.
|
||||
*/
|
||||
CV_WRAP void setRaycastWidth(int val);
|
||||
|
||||
/** @brief Returns the width of the raycasted image, used when user does not provide it at raycast() call.
|
||||
*/
|
||||
CV_WRAP int getRaycastWidth() const;
|
||||
|
||||
/** @brief Sets the height of the raycasted image, used when user does not provide it at raycast() call.
|
||||
* @param val input value.
|
||||
*/
|
||||
CV_WRAP void setRaycastHeight(int val);
|
||||
|
||||
/** @brief Returns the height of the raycasted image, used when user does not provide it at raycast() call.
|
||||
*/
|
||||
CV_WRAP int getRaycastHeight() const;
|
||||
|
||||
/** @brief Sets depth factor, witch is the number for depth scaling.
|
||||
* @param val input value.
|
||||
*/
|
||||
CV_WRAP void setDepthFactor(float val);
|
||||
|
||||
/** @brief Returns depth factor, witch is the number for depth scaling.
|
||||
*/
|
||||
CV_WRAP float getDepthFactor() const;
|
||||
|
||||
/** @brief Sets the size of voxel.
|
||||
* @param val input value.
|
||||
*/
|
||||
CV_WRAP void setVoxelSize(float val);
|
||||
|
||||
/** @brief Returns the size of voxel.
|
||||
*/
|
||||
CV_WRAP float getVoxelSize() const;
|
||||
|
||||
/** @brief Sets TSDF truncation distance. Distances greater than value from surface will be truncated to 1.0.
|
||||
* @param val input value.
|
||||
*/
|
||||
CV_WRAP void setTsdfTruncateDistance(float val);
|
||||
|
||||
/** @brief Returns TSDF truncation distance. Distances greater than value from surface will be truncated to 1.0.
|
||||
*/
|
||||
CV_WRAP float getTsdfTruncateDistance() const;
|
||||
|
||||
/** @brief Sets threshold for depth truncation in meters. Truncates the depth greater than threshold to 0.
|
||||
* @param val input value.
|
||||
*/
|
||||
CV_WRAP void setMaxDepth(float val);
|
||||
|
||||
/** @brief Returns threshold for depth truncation in meters. Truncates the depth greater than threshold to 0.
|
||||
*/
|
||||
CV_WRAP float getMaxDepth() const;
|
||||
|
||||
/** @brief Sets max number of frames to integrate per voxel.
|
||||
Represents the max number of frames over which a running average of the TSDF is calculated for a voxel.
|
||||
* @param val input value.
|
||||
*/
|
||||
CV_WRAP void setMaxWeight(int val);
|
||||
|
||||
/** @brief Returns max number of frames to integrate per voxel.
|
||||
Represents the max number of frames over which a running average of the TSDF is calculated for a voxel.
|
||||
*/
|
||||
CV_WRAP int getMaxWeight() const;
|
||||
|
||||
/** @brief Sets length of single raycast step.
|
||||
Describes the percentage of voxel length that is skipped per march.
|
||||
* @param val input value.
|
||||
*/
|
||||
CV_WRAP void setRaycastStepFactor(float val);
|
||||
|
||||
/** @brief Returns length of single raycast step.
|
||||
Describes the percentage of voxel length that is skipped per march.
|
||||
*/
|
||||
CV_WRAP float getRaycastStepFactor() const;
|
||||
|
||||
/** @brief Sets volume pose.
|
||||
* @param val input value.
|
||||
*/
|
||||
CV_WRAP void setVolumePose(InputArray val);
|
||||
|
||||
/** @brief Sets volume pose.
|
||||
* @param val output value.
|
||||
*/
|
||||
CV_WRAP void getVolumePose(OutputArray val) const;
|
||||
|
||||
/** @brief Resolution of voxel space.
|
||||
Number of voxels in each dimension.
|
||||
Applicable only for TSDF Volume.
|
||||
HashTSDF volume only supports equal resolution in all three dimensions.
|
||||
* @param val input value.
|
||||
*/
|
||||
CV_WRAP void setVolumeResolution(InputArray val);
|
||||
|
||||
/** @brief Resolution of voxel space.
|
||||
Number of voxels in each dimension.
|
||||
Applicable only for TSDF Volume.
|
||||
HashTSDF volume only supports equal resolution in all three dimensions.
|
||||
* @param val output value.
|
||||
*/
|
||||
CV_WRAP void getVolumeResolution(OutputArray val) const;
|
||||
|
||||
/** @brief Returns 3 integers representing strides by x, y and z dimension.
|
||||
Can be used to iterate over raw volume unit data.
|
||||
* @param val output value.
|
||||
*/
|
||||
CV_WRAP void getVolumeStrides(OutputArray val) const;
|
||||
|
||||
/** @brief Sets intrinsics of camera for integrations.
|
||||
* Format of input:
|
||||
* [ fx 0 cx ]
|
||||
* [ 0 fy cy ]
|
||||
* [ 0 0 1 ]
|
||||
* where fx and fy are focus points of Ox and Oy axises, and cx and cy are central points of Ox and Oy axises.
|
||||
* @param val input value.
|
||||
*/
|
||||
CV_WRAP void setCameraIntegrateIntrinsics(InputArray val);
|
||||
|
||||
/** @brief Returns intrinsics of camera for integrations.
|
||||
* Format of output:
|
||||
* [ fx 0 cx ]
|
||||
* [ 0 fy cy ]
|
||||
* [ 0 0 1 ]
|
||||
* where fx and fy are focus points of Ox and Oy axises, and cx and cy are central points of Ox and Oy axises.
|
||||
* @param val output value.
|
||||
*/
|
||||
CV_WRAP void getCameraIntegrateIntrinsics(OutputArray val) const;
|
||||
|
||||
/** @brief Sets camera intrinsics for raycast image which, used when user does not provide them at raycast() call.
|
||||
* Format of input:
|
||||
* [ fx 0 cx ]
|
||||
* [ 0 fy cy ]
|
||||
* [ 0 0 1 ]
|
||||
* where fx and fy are focus points of Ox and Oy axises, and cx and cy are central points of Ox and Oy axises.
|
||||
* @param val input value.
|
||||
*/
|
||||
CV_WRAP void setCameraRaycastIntrinsics(InputArray val);
|
||||
|
||||
/** @brief Returns camera intrinsics for raycast image, used when user does not provide them at raycast() call.
|
||||
* Format of output:
|
||||
* [ fx 0 cx ]
|
||||
* [ 0 fy cy ]
|
||||
* [ 0 0 1 ]
|
||||
* where fx and fy are focus points of Ox and Oy axises, and cx and cy are central points of Ox and Oy axises.
|
||||
* @param val output value.
|
||||
*/
|
||||
CV_WRAP void getCameraRaycastIntrinsics(OutputArray val) const;
|
||||
|
||||
class Impl;
|
||||
private:
|
||||
Ptr<Impl> impl;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif // !OPENCV_3D_VOLUME_SETTINGS_HPP
|
||||
Reference in New Issue
Block a user