mirror of
https://github.com/opencv/opencv.git
synced 2026-07-29 23:33:05 +04:00
Merge pull request #20013 from savuor:rgbd_to_3d
Moving RGBD parts to 3d * files moved from rgbd module in contrib repo * header paths fixed * perf file added * lapack compilation fixed * Rodrigues fixed in tests * rgbd namespace removed * headers fixed * initial: rgbd files moved to 3d module * rgbd updated from latest contrib master; less file duplication * "std::" for sin(), cos(), etc. * KinFu family -> back to contrib * paths & namespaces * removed duplicates, file version updated * namespace kinfu removed from 3d module * forgot to move test_colored_kinfu.cpp to contrib * tests fixed: Params removed * kinfu namespace removed * it works without objc bindings * include headers fixed * tests: data paths fixed * headers moved to/from public API * Intr -> Matx33f in public API * from kinfu_frame.hpp to utils.hpp * submap: Intr -> Matx33f, HashTSDFVolume -> Volume; no extra headers * no RgbdFrame class, no Mat fields & arg -> InputArray & pImpl * get/setPyramidAt() instead of lots of methods * Mat -> InputArray, TMat * prepareFrameCache: refactored * FastICPOdometry: +truncate threshold, +depthFactor; Mat/UMat choose * Mat/UMat choose * minor stuff related to headers * (un)signed int warnings; compilation minor issues * minors: submap: pyramids -> OdometryFrame; tests fix; FastICP minor; CV_EXPORTS_W for kinfu_frame.hpp * FastICPOdometry: caching, rgbCameraMatrix * OdometryFrame: pyramid%s% -> pyramids[] * drop: rgbCameraMatrix from FastICP, RGB cache mode, makeColoredFrameFrom depth and all color-functions it calls * makeFrameFromDepth, buildPyramidPointsNormals -> from public to internal utils.hpp * minors * FastICPOdometry: caching updated, init fields * OdometryFrameImpl<UMat> fixed * matrix building fixed; minors * returning linemode back to contrib * params.pose is Mat now * precomp headers reorganized * minor fixes, header paths, extra header removed * minors: intrinsics -> utils.hpp; whitespaces; empty namespace; warning fixed * moving declarations from/to headers * internal headers reorganized (once again) * fix include * extra var fix * fix include, fix (un)singed warning * calibration.cpp: reverting back * headers fix * workaround to fix bindings * temporary removed wrappers * VolumeType -> VolumeParams * (temporarily) removing wrappers for Volume and VolumeParams * pyopencv_linemod -> contrib * try to fix test_rgbd.py * headers fixed * fixing wrappers for rgbd * fixing docs * fixing rgbdPlane * RgbdNormals wrapped * wrap Volume and VolumeParams, VolumeType from enum to int * DepthCleaner wrapped * header folder "rgbd" -> "3d" * fixing header path * VolumeParams referenced by Ptr to support Python wrappers * render...() fixed * Ptr<VolumeParams> fixed * makeVolume(... resolution -> [X, Y, Z]) * fixing static declaration * try to fix ios objc bindings * OdometryFrame::release...() removed * fix for Odometry algos not supporting UMats: prepareFrameCache<>() * preparePyramidMask(): fix to compile with TMat = UMat * fixing debug guards * removing references back; adding makeOdometryFrame() instead * fixing OpenCL ICP hanging (some threads exit before reaching the barrier -> the rest threads hang) * try to fix objc wrapper warnings; rerun builders * VolumeType -> VolumeKind * try to fix OCL bug * prints removed * indentation fixed * headers fixed * license fix * WillowGarage licence notion removed, since it's in OpenCV's COPYRIGHT already * KinFu license notion shortened * debugging code removed * include guards fixed * KinFu license left in contrib module * isValidDepth() moved to private header * indentation fix * indentation fix in src files * RgbdNormals rewritten to pImpl * minor * DepthCleaner removed due to low code quality, no depthScale provided, no depth images found to be successfully filtered; can be replaced by bilateral filtering * minors, indentation * no "private" in public headers * depthTo3d test moved from separate file * Normals: setDepth() is useless, removing it * RgbdPlane => findPlanes() * rescaleDepth(): minor * warpFrame: minor * minor TODO * all Odometries (except base abstract class) rewritten to pImpl * FastICPOdometry now supports maxRotation and maxTranslation * minor * Odometry's children: now checks are done in setters * get rid of protected members in Odometry class * get/set cameraMatrix, transformType, maxRot/Trans, iters, minGradients -> OdometryImpl * cameraMatrix: from double to float * matrix exponentiation: Eigen -> dual quaternions * Odometry evaluation fixed to reuse existing code * "small" macro fixed by undef * pixNorm is calculated on CPU only now (and then uploads on GPU) * test registration: no cvtest classes * test RgbdNormals and findPlanes(): no cvtest classes * test_rgbd.py: minor fix * tests for Odometry: no cvtest classes; UMat tests; logging fixed * more CV_OVERRIDE to overriden functions * fixing nondependent names to dependent * more to prev commit * forgotten fixes: overriden functions, (non)dependent names * FastICPOdometry: fix UMat support when OpenCL is off * try to fix compilation: missing namespaces * Odometry: static const-mimicking functions to internal constants * forgotten change to prev commit * more forgotten fixes * do not expose "submap.hpp" by default * in-class enums: give names, CamelCase, int=>enums; minors * namespaces, underscores, String * std::map is used by pose graph, adding it * compute()'s signature fixed, computeImpl()'s too * RgbdNormals: Mat -> InputArray * depth.hpp: Mat -> InputArray * cameraMatrix: Matx33f -> InputArray + default value + checks * "details" headers are not visible by default * TSDF tests: rearranging checks * cameraMatrix: no (realistic) default value * renderPointsNormals*(): no wrappers for them * debug: assert on empty frame in TSDF tests * debugging code for TSDF GPU * debug from integrate to raycast * no (non-zero) default camera matrix anymore * drop debugging code (does not help) * try to fix TSDF GPU: constant -> global const ptr
This commit is contained in:
Rostislav Vasilikhin
committed by
GitHub
GitHub
parent
6b199bd1bb
commit
bae9cef0b5
@@ -8,6 +8,9 @@
|
||||
#include "opencv2/core.hpp"
|
||||
#include "opencv2/core/types_c.h"
|
||||
|
||||
#include "opencv2/3d/depth.hpp"
|
||||
#include "opencv2/3d/volume.hpp"
|
||||
|
||||
/**
|
||||
@defgroup _3d 3D vision functionality
|
||||
|
||||
|
||||
@@ -0,0 +1,503 @@
|
||||
// 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
|
||||
};
|
||||
|
||||
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 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,
|
||||
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;
|
||||
CV_WRAP virtual void setMethod(RgbdNormals::RgbdNormalsMethod val) = 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
|
||||
*/
|
||||
CV_EXPORTS_W void depthTo3dSparse(InputArray depth, InputArray in_K, InputArray in_points, OutputArray points3d);
|
||||
|
||||
/** Converts a depth image to an organized set of 3d points.
|
||||
* 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. They are of depth the same as `depth` if it is CV_32F or CV_64F, and the
|
||||
* depth of `K` if `depth` is of depth CV_U
|
||||
* @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);
|
||||
|
||||
/** Warp the image: compute 3d points from the depth, transform them using given transformation,
|
||||
* then project color point cloud to an image plane.
|
||||
* This function can be used to visualize results of the Odometry algorithm.
|
||||
* @param image The image (of CV_8UC1 or CV_8UC3 type)
|
||||
* @param depth The depth (of type used in depthTo3d fuction)
|
||||
* @param mask The mask of used pixels (of CV_8UC1), it can be empty
|
||||
* @param Rt The transformation that will be applied to the 3d points computed from the depth
|
||||
* @param cameraMatrix Camera matrix
|
||||
* @param distCoeff Distortion coefficients
|
||||
* @param warpedImage The warped image.
|
||||
* @param warpedDepth The warped depth.
|
||||
* @param warpedMask The warped mask.
|
||||
*/
|
||||
CV_EXPORTS_W void warpFrame(InputArray image, InputArray depth, InputArray mask, InputArray Rt, InputArray cameraMatrix, InputArray distCoeff,
|
||||
OutputArray warpedImage, OutputArray warpedDepth = 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
|
||||
* @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);
|
||||
|
||||
/** Object that contains a frame data that is possibly needed for the Odometry.
|
||||
* It's used for the efficiency (to pass precomputed/cached data of the frame that participates
|
||||
* in the Odometry processing several times).
|
||||
*/
|
||||
struct CV_EXPORTS_W OdometryFrame
|
||||
{
|
||||
public:
|
||||
/** These constants are used to set a type of cache which has to be prepared depending on the frame role:
|
||||
* srcFrame or dstFrame (see compute method of the Odometry class). For the srcFrame and dstFrame different cache data may be required,
|
||||
* some part of a cache may be common for both frame roles.
|
||||
* @param CACHE_SRC The cache data for the srcFrame will be prepared.
|
||||
* @param CACHE_DST The cache data for the dstFrame will be prepared.
|
||||
* @param CACHE_ALL The cache data for both srcFrame and dstFrame roles will be computed.
|
||||
* @param CACHE_DEPTH The frame will be generated from depth image
|
||||
* @param CACHE_PTS The frame will be built from point cloud
|
||||
*/
|
||||
enum OdometryFrameCacheType
|
||||
{
|
||||
CACHE_SRC = 1, CACHE_DST = 2, CACHE_ALL = CACHE_SRC + CACHE_DST
|
||||
};
|
||||
|
||||
/** Indicates what pyramid is to access using get/setPyramid... methods:
|
||||
* @param PYR_IMAGE The pyramid of RGB images
|
||||
* @param PYR_DEPTH The pyramid of depth images
|
||||
* @param PYR_MASK The pyramid of masks
|
||||
* @param PYR_CLOUD The pyramid of point clouds, produced from the pyramid of depths
|
||||
* @param PYR_DIX The pyramid of dI/dx derivative images
|
||||
* @param PYR_DIY The pyramid of dI/dy derivative images
|
||||
* @param PYR_TEXMASK The pyramid of textured masks
|
||||
* @param PYR_NORM The pyramid of normals
|
||||
* @param PYR_NORMMASK The pyramid of normals masks
|
||||
*/
|
||||
enum OdometryFramePyramidType
|
||||
{
|
||||
PYR_IMAGE = 0, PYR_DEPTH = 1, PYR_MASK = 2, PYR_CLOUD = 3, PYR_DIX = 4, PYR_DIY = 5, PYR_TEXMASK = 6, PYR_NORM = 7, PYR_NORMMASK = 8,
|
||||
N_PYRAMIDS
|
||||
};
|
||||
|
||||
OdometryFrame() : ID(-1) { }
|
||||
virtual ~OdometryFrame() { }
|
||||
|
||||
CV_WRAP static Ptr<OdometryFrame> create(InputArray image = noArray(), InputArray depth = noArray(),
|
||||
InputArray mask = noArray(), InputArray normals = noArray(), int ID = -1);
|
||||
|
||||
CV_WRAP virtual void setImage(InputArray image) = 0;
|
||||
CV_WRAP virtual void getImage(OutputArray image) = 0;
|
||||
CV_WRAP virtual void setDepth(InputArray depth) = 0;
|
||||
CV_WRAP virtual void getDepth(OutputArray depth) = 0;
|
||||
CV_WRAP virtual void setMask(InputArray mask) = 0;
|
||||
CV_WRAP virtual void getMask(OutputArray mask) = 0;
|
||||
CV_WRAP virtual void setNormals(InputArray normals) = 0;
|
||||
CV_WRAP virtual void getNormals(OutputArray normals) = 0;
|
||||
|
||||
CV_WRAP virtual void setPyramidLevels(size_t nLevels) = 0;
|
||||
CV_WRAP virtual size_t getPyramidLevels(OdometryFrame::OdometryFramePyramidType pyrType) = 0;
|
||||
|
||||
CV_WRAP virtual void setPyramidAt(InputArray pyrImage, OdometryFrame::OdometryFramePyramidType pyrType, size_t level) = 0;
|
||||
CV_WRAP virtual void getPyramidAt(OutputArray pyrImage, OdometryFrame::OdometryFramePyramidType pyrType, size_t level) = 0;
|
||||
|
||||
CV_PROP int ID;
|
||||
};
|
||||
|
||||
|
||||
/** Base class for computation of odometry.
|
||||
*/
|
||||
class CV_EXPORTS_W Odometry
|
||||
{
|
||||
public:
|
||||
|
||||
/** A class of transformation*/
|
||||
enum OdometryTransformType
|
||||
{
|
||||
ROTATION = 1, TRANSLATION = 2, RIGID_BODY_MOTION = 4
|
||||
};
|
||||
|
||||
virtual ~Odometry() { }
|
||||
|
||||
/** Create a new Odometry object based on its name. Currently supported names are:
|
||||
* "RgbdOdometry", "ICPOdometry", "RgbdICPOdometry", "FastICPOdometry".
|
||||
* @param odometryType algorithm's name
|
||||
*/
|
||||
CV_WRAP static Ptr<Odometry> createFromName(const std::string& odometryType);
|
||||
|
||||
/** Method to compute a transformation from the source frame to the destination one.
|
||||
* Some odometry algorithms do not used some data of frames (eg. ICP does not use images).
|
||||
* In such case corresponding arguments can be set as empty Mat.
|
||||
* The method returns true if all internal computations were possible (e.g. there were enough correspondences,
|
||||
* system of equations has a solution, etc) and resulting transformation satisfies some test if it's provided
|
||||
* by the Odometry inheritor implementation (e.g. thresholds for maximum translation and rotation).
|
||||
* @param srcImage Image data of the source frame (CV_8UC1)
|
||||
* @param srcDepth Depth data of the source frame (CV_32FC1, in meters)
|
||||
* @param srcMask Mask that sets which pixels have to be used from the source frame (CV_8UC1)
|
||||
* @param dstImage Image data of the destination frame (CV_8UC1)
|
||||
* @param dstDepth Depth data of the destination frame (CV_32FC1, in meters)
|
||||
* @param dstMask Mask that sets which pixels have to be used from the destination frame (CV_8UC1)
|
||||
* @param Rt Resulting transformation from the source frame to the destination one (rigid body motion):
|
||||
dst_p = Rt * src_p, where dst_p is a homogeneous point in the destination frame and src_p is
|
||||
homogeneous point in the source frame,
|
||||
Rt is 4x4 matrix of CV_64FC1 type.
|
||||
* @param initRt Initial transformation from the source frame to the destination one (optional)
|
||||
*/
|
||||
CV_WRAP virtual bool compute(InputArray srcImage, InputArray srcDepth, InputArray srcMask, InputArray dstImage, InputArray dstDepth,
|
||||
InputArray dstMask, OutputArray Rt, InputArray initRt = noArray()) const = 0;
|
||||
|
||||
/** One more method to compute a transformation from the source frame to the destination one.
|
||||
* It is designed to save on computing the frame data (image pyramids, normals, etc.).
|
||||
*/
|
||||
CV_WRAP_AS(compute2) virtual bool compute(const Ptr<OdometryFrame>& srcFrame, const Ptr<OdometryFrame>& dstFrame,
|
||||
OutputArray Rt, InputArray initRt = noArray()) const = 0;
|
||||
|
||||
/** Prepare a cache for the frame. The function checks the precomputed/passed data (throws the error if this data
|
||||
* does not satisfy) and computes all remaining cache data needed for the frame. Returned size is a resolution
|
||||
* of the prepared frame.
|
||||
* @param frame The odometry which will process the frame.
|
||||
* @param cacheType The cache type: CACHE_SRC, CACHE_DST or CACHE_ALL.
|
||||
*/
|
||||
CV_WRAP virtual Size prepareFrameCache(Ptr<OdometryFrame> frame, OdometryFrame::OdometryFrameCacheType cacheType) const = 0;
|
||||
|
||||
/** Create odometry frame for current Odometry implementation
|
||||
* @param image Image data of the frame (CV_8UC1)
|
||||
* @param depth Depth data of the frame (CV_32FC1, in meters)
|
||||
* @param mask Mask that sets which pixels have to be used from the frame (CV_8UC1)
|
||||
*/
|
||||
CV_WRAP virtual Ptr<OdometryFrame> makeOdometryFrame(InputArray image, InputArray depth, InputArray mask) const = 0;
|
||||
|
||||
CV_WRAP virtual void getCameraMatrix(OutputArray val) const = 0;
|
||||
CV_WRAP virtual void setCameraMatrix(InputArray val) = 0;
|
||||
CV_WRAP virtual Odometry::OdometryTransformType getTransformType() const = 0;
|
||||
CV_WRAP virtual void setTransformType(Odometry::OdometryTransformType val) = 0;
|
||||
CV_WRAP virtual void getIterationCounts(OutputArray val) const = 0;
|
||||
CV_WRAP virtual void setIterationCounts(InputArray val) = 0;
|
||||
/** For each pyramid level the pixels will be filtered out if they have gradient magnitude less than minGradientMagnitudes[level].
|
||||
* Makes sense for RGB-based algorithms only.
|
||||
*/
|
||||
CV_WRAP virtual void getMinGradientMagnitudes(OutputArray val) const = 0;
|
||||
CV_WRAP virtual void setMinGradientMagnitudes(InputArray val) = 0;
|
||||
|
||||
/** Get max allowed translation in meters, default is 0.15 meters
|
||||
Found delta transform is considered successful only if the translation is in given limits. */
|
||||
CV_WRAP virtual double getMaxTranslation() const = 0;
|
||||
/** Set max allowed translation in meters.
|
||||
* Found delta transform is considered successful only if the translation is in given limits. */
|
||||
CV_WRAP virtual void setMaxTranslation(double val) = 0;
|
||||
/** Get max allowed rotation in degrees, default is 15 degrees.
|
||||
* Found delta transform is considered successful only if the rotation is in given limits. */
|
||||
CV_WRAP virtual double getMaxRotation() const = 0;
|
||||
/** Set max allowed rotation in degrees.
|
||||
* Found delta transform is considered successful only if the rotation is in given limits. */
|
||||
CV_WRAP virtual void setMaxRotation(double val) = 0;
|
||||
};
|
||||
|
||||
/** Odometry based on the paper "Real-Time Visual Odometry from Dense RGB-D Images",
|
||||
* F. Steinbucker, J. Strum, D. Cremers, ICCV, 2011.
|
||||
*/
|
||||
class CV_EXPORTS_W RgbdOdometry: public Odometry
|
||||
{
|
||||
public:
|
||||
RgbdOdometry() { }
|
||||
|
||||
/** Creates RgbdOdometry object
|
||||
* @param cameraMatrix Camera matrix
|
||||
* @param minDepth Pixels with depth less than minDepth will not be used (in meters)
|
||||
* @param maxDepth Pixels with depth larger than maxDepth will not be used (in meters)
|
||||
* @param maxDepthDiff Correspondences between pixels of two given frames will be filtered out
|
||||
* if their depth difference is larger than maxDepthDiff (in meters)
|
||||
* @param iterCounts Count of iterations on each pyramid level, defaults are [7, 7, 7, 10]
|
||||
* @param minGradientMagnitudes For each pyramid level the pixels will be filtered out, default is 10 per each level
|
||||
* if they have gradient magnitude less than minGradientMagnitudes[level].
|
||||
* @param maxPointsPart The method uses a random pixels subset of size frameWidth x frameHeight x pointsPart (from 0 to 1)
|
||||
* @param transformType Class of transformation
|
||||
*/
|
||||
CV_WRAP static Ptr<RgbdOdometry> create(InputArray cameraMatrix = noArray(),
|
||||
float minDepth = 0.f,
|
||||
float maxDepth = 4.f,
|
||||
float maxDepthDiff = 0.07f,
|
||||
InputArray iterCounts = noArray(),
|
||||
InputArray minGradientMagnitudes = noArray(),
|
||||
float maxPointsPart = 0.07f,
|
||||
Odometry::OdometryTransformType transformType = Odometry::RIGID_BODY_MOTION);
|
||||
|
||||
CV_WRAP virtual double getMinDepth() const = 0;
|
||||
CV_WRAP virtual void setMinDepth(double val) = 0;
|
||||
CV_WRAP virtual double getMaxDepth() const = 0;
|
||||
CV_WRAP virtual void setMaxDepth(double val) = 0;
|
||||
CV_WRAP virtual double getMaxDepthDiff() const = 0;
|
||||
CV_WRAP virtual void setMaxDepthDiff(double val) = 0;
|
||||
CV_WRAP virtual double getMaxPointsPart() const = 0;
|
||||
CV_WRAP virtual void setMaxPointsPart(double val) = 0;
|
||||
|
||||
};
|
||||
|
||||
/** Odometry based on the paper "KinectFusion: Real-Time Dense Surface Mapping and Tracking",
|
||||
* Richard A. Newcombe, Andrew Fitzgibbon, at al, SIGGRAPH, 2011.
|
||||
*/
|
||||
class CV_EXPORTS_W ICPOdometry: public Odometry
|
||||
{
|
||||
public:
|
||||
ICPOdometry() { }
|
||||
|
||||
/** Creates new ICPOdometry object
|
||||
* @param cameraMatrix Camera matrix
|
||||
* @param minDepth Pixels with depth less than minDepth will not be used (in meters)
|
||||
* @param maxDepth Pixels with depth larger than maxDepth will not be used (in meters)
|
||||
* @param maxDepthDiff Correspondences between pixels of two given frames will be filtered out
|
||||
* if their depth difference is larger than maxDepthDiff (in meters)
|
||||
* @param maxPointsPart The method uses a random pixels subset of size frameWidth x frameHeight x pointsPart (from 0 to 1)
|
||||
* @param iterCounts Count of iterations on each pyramid level, defaults are [7, 7, 7, 10]
|
||||
* @param transformType Class of trasformation
|
||||
*/
|
||||
CV_WRAP static Ptr<ICPOdometry> create(InputArray cameraMatrix = noArray(),
|
||||
float minDepth = 0.f,
|
||||
float maxDepth = 4.f,
|
||||
float maxDepthDiff = 0.07f,
|
||||
float maxPointsPart = 0.07f,
|
||||
InputArray iterCounts = noArray(),
|
||||
Odometry::OdometryTransformType transformType = Odometry::RIGID_BODY_MOTION);
|
||||
|
||||
CV_WRAP virtual double getMinDepth() const = 0;
|
||||
CV_WRAP virtual void setMinDepth(double val) = 0;
|
||||
CV_WRAP virtual double getMaxDepth() const = 0;
|
||||
CV_WRAP virtual void setMaxDepth(double val) = 0;
|
||||
CV_WRAP virtual double getMaxDepthDiff() const = 0;
|
||||
CV_WRAP virtual void setMaxDepthDiff(double val) = 0;
|
||||
CV_WRAP virtual double getMaxPointsPart() const = 0;
|
||||
CV_WRAP virtual void setMaxPointsPart(double val) = 0;
|
||||
CV_WRAP virtual Ptr<RgbdNormals> getNormalsComputer() const = 0;
|
||||
};
|
||||
|
||||
/** Odometry that merges RgbdOdometry and ICPOdometry by minimize sum of their energy functions.
|
||||
*/
|
||||
class CV_EXPORTS_W RgbdICPOdometry: public Odometry
|
||||
{
|
||||
public:
|
||||
RgbdICPOdometry() { }
|
||||
|
||||
/** Creates RgbdICPOdometry object
|
||||
* @param cameraMatrix Camera matrix
|
||||
* @param minDepth Pixels with depth less than minDepth will not be used (in meters)
|
||||
* @param maxDepth Pixels with depth larger than maxDepth will not be used (in meters)
|
||||
* @param maxDepthDiff Correspondences between pixels of two given frames will be filtered out
|
||||
* if their depth difference is larger than maxDepthDiff (in meters)
|
||||
* @param maxPointsPart The method uses a random pixels subset of size frameWidth x frameHeight x pointsPart (from 0 to 1)
|
||||
* @param iterCounts Count of iterations on each pyramid level, defaults are [7, 7, 7, 10]
|
||||
* @param minGradientMagnitudes For each pyramid level the pixels will be filtered out
|
||||
* if they have gradient magnitude less than minGradientMagnitudes[level], default is 10 per each level
|
||||
* @param transformType Class of trasformation
|
||||
*/
|
||||
CV_WRAP static Ptr<RgbdICPOdometry> create(InputArray cameraMatrix = noArray(),
|
||||
float minDepth = 0.f,
|
||||
float maxDepth = 4.f,
|
||||
float maxDepthDiff = 0.07f,
|
||||
float maxPointsPart = 0.07f,
|
||||
InputArray iterCounts = noArray(),
|
||||
InputArray minGradientMagnitudes = noArray(),
|
||||
Odometry::OdometryTransformType transformType = Odometry::RIGID_BODY_MOTION);
|
||||
|
||||
CV_WRAP virtual double getMinDepth() const = 0;
|
||||
CV_WRAP virtual void setMinDepth(double val) = 0;
|
||||
CV_WRAP virtual double getMaxDepth() const = 0;
|
||||
CV_WRAP virtual void setMaxDepth(double val) = 0;
|
||||
CV_WRAP virtual double getMaxDepthDiff() const = 0;
|
||||
CV_WRAP virtual void setMaxDepthDiff(double val) = 0;
|
||||
CV_WRAP virtual double getMaxPointsPart() const = 0;
|
||||
CV_WRAP virtual void setMaxPointsPart(double val) = 0;
|
||||
|
||||
CV_WRAP virtual Ptr<RgbdNormals> getNormalsComputer() const = 0;
|
||||
};
|
||||
|
||||
/** A faster version of ICPOdometry which is used in KinectFusion implementation
|
||||
* Partial list of differences:
|
||||
* - UMats are processed using OpenCL
|
||||
* - CPU version is written in universal intrinsics
|
||||
* - Filters points by angle
|
||||
* - Interpolates points and normals
|
||||
* - Doesn't use masks or min/max depth filtering
|
||||
* - Doesn't use random subsets of points
|
||||
* - Supports only Rt transform type
|
||||
* - Initial transform is not supported and always ignored
|
||||
* - Supports only 4-float vectors as input type
|
||||
*/
|
||||
class CV_EXPORTS_W FastICPOdometry: public Odometry
|
||||
{
|
||||
public:
|
||||
FastICPOdometry() { }
|
||||
|
||||
/** Creates FastICPOdometry object
|
||||
* @param cameraMatrix Camera matrix
|
||||
* @param maxDistDiff Correspondences between pixels of two given frames will be filtered out
|
||||
* if their depth difference is larger than maxDepthDiff (in meters)
|
||||
* @param angleThreshold Correspondence will be filtered out
|
||||
* if an angle between their normals is bigger than threshold
|
||||
* @param sigmaDepth Depth sigma in meters for bilateral smooth
|
||||
* @param sigmaSpatial Spatial sigma in pixels for bilateral smooth
|
||||
* @param kernelSize Kernel size in pixels for bilateral smooth
|
||||
* @param iterCounts Count of iterations on each pyramid level, defaults are [7, 7, 7, 10]
|
||||
* @param depthFactor pre-scale per 1 meter for input values
|
||||
* @param truncateThreshold Threshold for depth truncation in meters
|
||||
* All depth values beyond this threshold will be set to zero
|
||||
*/
|
||||
CV_WRAP static Ptr<FastICPOdometry> create(InputArray cameraMatrix = noArray(),
|
||||
float maxDistDiff = 0.07f,
|
||||
float angleThreshold = (float)(30. * CV_PI / 180.),
|
||||
float sigmaDepth = 0.04f,
|
||||
float sigmaSpatial = 4.5f,
|
||||
int kernelSize = 7,
|
||||
InputArray iterCounts = noArray(),
|
||||
float depthFactor = 1.f,
|
||||
float truncateThreshold = 0.f);
|
||||
|
||||
CV_WRAP virtual double getMaxDistDiff() const = 0;
|
||||
CV_WRAP virtual void setMaxDistDiff(float val) = 0;
|
||||
CV_WRAP virtual float getAngleThreshold() const = 0;
|
||||
CV_WRAP virtual void setAngleThreshold(float f) = 0;
|
||||
CV_WRAP virtual float getSigmaDepth() const = 0;
|
||||
CV_WRAP virtual void setSigmaDepth(float f) = 0;
|
||||
CV_WRAP virtual float getSigmaSpatial() const = 0;
|
||||
CV_WRAP virtual void setSigmaSpatial(float f) = 0;
|
||||
CV_WRAP virtual int getKernelSize() const = 0;
|
||||
CV_WRAP virtual void setKernelSize(int f) = 0;
|
||||
CV_WRAP virtual float getDepthFactor() const = 0;
|
||||
CV_WRAP virtual void setDepthFactor(float depthFactor) = 0;
|
||||
CV_WRAP virtual float getTruncateThreshold() const = 0;
|
||||
CV_WRAP virtual void setTruncateThreshold(float truncateThreshold) = 0;
|
||||
|
||||
};
|
||||
|
||||
// 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,60 @@
|
||||
// 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_POSE_GRAPH_HPP
|
||||
#define OPENCV_3D_DETAIL_POSE_GRAPH_HPP
|
||||
|
||||
#include "opencv2/core/affine.hpp"
|
||||
#include "opencv2/core/quaternion.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 size_t getNumEdges() const = 0;
|
||||
|
||||
// checks if graph is connected and each edge connects exactly 2 nodes
|
||||
virtual bool isValid() const = 0;
|
||||
|
||||
// Termination criteria are max number of iterations and min relative energy change to current energy
|
||||
// Returns number of iterations elapsed or -1 if max number of iterations was reached or failed to optimize
|
||||
virtual int optimize(const cv::TermCriteria& tc = cv::TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 100, 1e-6)) = 0;
|
||||
|
||||
// calculate cost function based on current nodes parameters
|
||||
virtual double calcEnergy() const = 0;
|
||||
};
|
||||
|
||||
} // namespace detail
|
||||
} // namespace cv
|
||||
|
||||
#endif // include guard
|
||||
@@ -0,0 +1,515 @@
|
||||
// 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/3d/detail/pose_graph.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)
|
||||
{
|
||||
Matx44f accPose = estimatedPose.matrix * weight + _pose.matrix * _weight;
|
||||
weight += _weight;
|
||||
accPose /= float(weight);
|
||||
estimatedPose = Affine3f(accPose);
|
||||
}
|
||||
};
|
||||
typedef std::map<int, PoseConstraint> Constraints;
|
||||
|
||||
Submap(int _id, const VolumeParams& volumeParams, const cv::Affine3f& _pose = cv::Affine3f::Identity(),
|
||||
int _startFrameId = 0)
|
||||
: id(_id), pose(_pose), cameraPose(Affine3f::Identity()), startFrameId(_startFrameId)
|
||||
{
|
||||
VolumeParams vp = volumeParams;
|
||||
vp.kind = VolumeParams::VolumeKind::HASHTSDF;
|
||||
Ptr<VolumeParams> pvp = makePtr<VolumeParams>(vp);
|
||||
volume = makeVolume(pvp);
|
||||
}
|
||||
virtual ~Submap() = default;
|
||||
|
||||
virtual void integrate(InputArray _depth, float depthFactor, const cv::Matx33f& intrinsics, const int currframeId);
|
||||
virtual void raycast(const cv::Affine3f& cameraPose, const cv::Matx33f& intrinsics, cv::Size frameSize,
|
||||
OutputArray points = noArray(), OutputArray normals = noArray());
|
||||
|
||||
virtual int getTotalAllocatedBlocks() const { return int(volume->getTotalVolumeUnits()); };
|
||||
virtual int getVisibleBlocks(int currFrameId) const
|
||||
{
|
||||
return volume->getVisibleBlocks(currFrameId, FRAME_VISIBILITY_THRESHOLD);
|
||||
}
|
||||
|
||||
float calcVisibilityRatio(int currFrameId) const
|
||||
{
|
||||
int allocate_blocks = getTotalAllocatedBlocks();
|
||||
int visible_blocks = getVisibleBlocks(currFrameId);
|
||||
return float(visible_blocks) / float(allocate_blocks);
|
||||
}
|
||||
|
||||
//! 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)
|
||||
Ptr<OdometryFrame> frame;
|
||||
|
||||
std::shared_ptr<Volume> volume;
|
||||
};
|
||||
|
||||
template<typename MatType>
|
||||
|
||||
void Submap<MatType>::integrate(InputArray _depth, float depthFactor, const cv::Matx33f& intrinsics,
|
||||
const int currFrameId)
|
||||
{
|
||||
CV_Assert(currFrameId >= startFrameId);
|
||||
volume->integrate(_depth, depthFactor, cameraPose.matrix, intrinsics, currFrameId);
|
||||
}
|
||||
|
||||
template<typename MatType>
|
||||
void Submap<MatType>::raycast(const cv::Affine3f& _cameraPose, const cv::Matx33f& intrinsics, cv::Size frameSize,
|
||||
OutputArray points, OutputArray normals)
|
||||
{
|
||||
if (!points.needed() && !normals.needed())
|
||||
{
|
||||
MatType pts, nrm;
|
||||
frame->getPyramidAt(pts, OdometryFrame::PYR_CLOUD, 0);
|
||||
frame->getPyramidAt(nrm, OdometryFrame::PYR_NORM, 0);
|
||||
volume->raycast(_cameraPose.matrix, intrinsics, frameSize, pts, nrm);
|
||||
frame->setPyramidAt(pts, OdometryFrame::PYR_CLOUD, 0);
|
||||
frame->setPyramidAt(nrm, OdometryFrame::PYR_NORM, 0);
|
||||
}
|
||||
else
|
||||
{
|
||||
volume->raycast(_cameraPose.matrix, intrinsics, frameSize, points, normals);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @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;
|
||||
|
||||
SubmapManager(const VolumeParams& _volumeParams) : volumeParams(_volumeParams) {}
|
||||
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, Ptr<OdometryFrame> _frame);
|
||||
|
||||
Ptr<detail::PoseGraph> MapToPoseGraph();
|
||||
void PoseGraphToMap(const Ptr<detail::PoseGraph>& updatedPoseGraph);
|
||||
|
||||
VolumeParams volumeParams;
|
||||
|
||||
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, volumeParams, 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);
|
||||
|
||||
if (ratio < 0.2f)
|
||||
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;
|
||||
Matx44f inlierConstraint;
|
||||
for (int i = 0; i < int(weights.size()); i++)
|
||||
{
|
||||
if (weights[i] > INLIER_WEIGHT_THRESH)
|
||||
{
|
||||
localInliers++;
|
||||
if (i == int(weights.size() - 1))
|
||||
inlierConstraint += prevConstraint.matrix;
|
||||
else
|
||||
inlierConstraint += fromSubmapData.constraints[i].matrix;
|
||||
}
|
||||
}
|
||||
inlierConstraint /= float(max(localInliers, 1));
|
||||
inlierPose = Affine3f(inlierConstraint);
|
||||
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>::updateMap(int _frameId, Ptr<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,128 @@
|
||||
// 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 "opencv2/core/affine.hpp"
|
||||
|
||||
namespace cv
|
||||
{
|
||||
class CV_EXPORTS_W Volume
|
||||
{
|
||||
public:
|
||||
Volume(float _voxelSize, Matx44f _pose, float _raycastStepFactor) :
|
||||
voxelSize(_voxelSize),
|
||||
voxelSizeInv(1.0f / voxelSize),
|
||||
pose(_pose),
|
||||
raycastStepFactor(_raycastStepFactor)
|
||||
{ }
|
||||
|
||||
virtual ~Volume(){};
|
||||
|
||||
CV_WRAP
|
||||
virtual void integrate(InputArray _depth, float depthFactor, const Matx44f& cameraPose,
|
||||
const Matx33f& intrinsics, const int frameId = 0) = 0;
|
||||
virtual void integrate(InputArray _depth, InputArray _rgb, float depthFactor,
|
||||
const Matx44f& cameraPose, const Matx33f& intrinsics,
|
||||
const Matx33f& rgb_intrinsics, const int frameId = 0) = 0;
|
||||
CV_WRAP
|
||||
virtual void raycast(const Matx44f& cameraPose, const Matx33f& intrinsics,
|
||||
const Size& frameSize, OutputArray points, OutputArray normals) const = 0;
|
||||
virtual void raycast(const Matx44f& cameraPose, const Matx33f& intrinsics, const Size& frameSize,
|
||||
OutputArray points, OutputArray normals, OutputArray colors) const = 0;
|
||||
CV_WRAP
|
||||
virtual void fetchNormals(InputArray points, OutputArray _normals) const = 0;
|
||||
CV_WRAP
|
||||
virtual void fetchPointsNormals(OutputArray points, OutputArray normals) const = 0;
|
||||
CV_WRAP
|
||||
virtual void reset() = 0;
|
||||
|
||||
// Works for hash-based volumes only, otherwise returns 1
|
||||
virtual int getVisibleBlocks(int /*currFrameId*/, int /*frameThreshold*/) const { return 1; }
|
||||
virtual size_t getTotalVolumeUnits() const { return 1; }
|
||||
|
||||
public:
|
||||
const float voxelSize;
|
||||
const float voxelSizeInv;
|
||||
const Affine3f pose;
|
||||
const float raycastStepFactor;
|
||||
};
|
||||
|
||||
struct CV_EXPORTS_W VolumeParams
|
||||
{
|
||||
enum VolumeKind
|
||||
{
|
||||
TSDF = 0,
|
||||
HASHTSDF = 1,
|
||||
COLOREDTSDF = 2
|
||||
};
|
||||
|
||||
/** @brief Kind of Volume
|
||||
Values can be TSDF (single volume) or HASHTSDF (hashtable of volume units)
|
||||
*/
|
||||
CV_PROP_RW int kind;
|
||||
|
||||
/** @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
|
||||
*/
|
||||
CV_PROP_RW int resolutionX;
|
||||
CV_PROP_RW int resolutionY;
|
||||
CV_PROP_RW int resolutionZ;
|
||||
|
||||
/** @brief Resolution of volumeUnit in voxel space
|
||||
Number of voxels in each dimension for volumeUnit
|
||||
Applicable only for hashTSDF.
|
||||
*/
|
||||
CV_PROP_RW int unitResolution = {0};
|
||||
|
||||
/** @brief Initial pose of the volume in meters, should be 4x4 float or double matrix */
|
||||
CV_PROP_RW Mat pose;
|
||||
|
||||
/** @brief Length of voxels in meters */
|
||||
CV_PROP_RW float voxelSize;
|
||||
|
||||
/** @brief TSDF truncation distance
|
||||
Distances greater than value from surface will be truncated to 1.0
|
||||
*/
|
||||
CV_PROP_RW float tsdfTruncDist;
|
||||
|
||||
/** @brief 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_PROP_RW int maxWeight;
|
||||
|
||||
/** @brief Threshold for depth truncation in meters
|
||||
Truncates the depth greater than threshold to 0
|
||||
*/
|
||||
CV_PROP_RW float depthTruncThreshold;
|
||||
|
||||
/** @brief Length of single raycast step
|
||||
Describes the percentage of voxel length that is skipped per march
|
||||
*/
|
||||
CV_PROP_RW float raycastStepFactor;
|
||||
|
||||
/** @brief Default set of parameters that provide higher quality reconstruction
|
||||
at the cost of slow performance.
|
||||
*/
|
||||
CV_WRAP static Ptr<VolumeParams> defaultParams(int _volumeType);
|
||||
|
||||
/** @brief Coarse set of parameters that provides relatively higher performance
|
||||
at the cost of reconstrution quality.
|
||||
*/
|
||||
CV_WRAP static Ptr<VolumeParams> coarseParams(int _volumeType);
|
||||
};
|
||||
|
||||
|
||||
CV_EXPORTS_W Ptr<Volume> makeVolume(const Ptr<VolumeParams>& _volumeParams);
|
||||
CV_EXPORTS_W Ptr<Volume> makeVolume(int _volumeType, float _voxelSize, Matx44f _pose,
|
||||
float _raycastStepFactor, float _truncDist, int _maxWeight, float _truncateThreshold,
|
||||
int _resolutionX, int _resolutionY, int _resolutionZ);
|
||||
|
||||
} // namespace cv
|
||||
|
||||
#endif // include guard
|
||||
Reference in New Issue
Block a user