diff --git a/modules/rgbd/include/opencv2/rgbd.hpp b/modules/rgbd/include/opencv2/rgbd.hpp index a7d1127d9..8f7434425 100755 --- a/modules/rgbd/include/opencv2/rgbd.hpp +++ b/modules/rgbd/include/opencv2/rgbd.hpp @@ -379,6 +379,25 @@ namespace rgbd { } + /** Constructor + * @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, 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. + */ + RgbdPlane(int method, int block_size, + int min_size, double threshold, double sensor_error_a = 0, + double sensor_error_b = 0, double sensor_error_c = 0); + + ~RgbdPlane(); + + CV_WRAP static Ptr create(int method, int block_size, int min_size, double threshold, + double sensor_error_a = 0, double sensor_error_b = 0, + double sensor_error_c = 0); + /** 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 diff --git a/modules/rgbd/misc/python/test/test_rgbd.py b/modules/rgbd/misc/python/test/test_rgbd.py new file mode 100644 index 000000000..1c8e295a1 --- /dev/null +++ b/modules/rgbd/misc/python/test/test_rgbd.py @@ -0,0 +1,45 @@ +#!/usr/bin/env python + +# Python 2/3 compatibility +from __future__ import print_function + +import os, numpy + +import cv2 as cv + +from tests_common import NewOpenCVTests + +class rgbd_test(NewOpenCVTests): + + def test_computeRgbdPlane(self): + + depth_image = self.get_sample('/cv/rgbd/depth.png', cv.IMREAD_ANYDEPTH) + if depth_image is None: + raise unittest.SkipTest("Missing files with test data") + + K = numpy.array([[525, 0, 320.5], [0, 525, 240.5], [0, 0, 1]]) + points3d = cv.rgbd.depthTo3d(depth_image, K) + normals_computer = normals_computer = cv.rgbd.RgbdNormals_create(480, 640, 5, K) + normals = normals_computer.apply(points3d) + rgbd_plane = cv.rgbd.RgbdPlane_create(cv.rgbd.RgbdPlane_RGBD_PLANE_METHOD_DEFAULT, 40, 1600, 0.01, 0, 0, 0) + _, planes_coeff = rgbd_plane.apply(points3d, normals) + + planes_coeff_expected = \ + numpy.asarray([[[-0.02447728, -0.8678335 , -0.49625182, 4.02800846]], + [[-0.05055107, -0.86144137, -0.50533485, 3.95456314]], + [[-0.03294908, -0.86964548, -0.49257591, 3.97052431]], + [[-0.02886586, -0.87153459, -0.48948362, 7.77550507]], + [[-0.04455929, -0.87659335, -0.47916424, 3.93200684]], + [[-0.21514639, 0.18835169, -0.95824611, 7.59479475]], + [[-0.01006953, -0.86679155, -0.49856904, 4.01355648]], + [[-0.00876531, -0.87571168, -0.48275498, 3.96768975]], + [[-0.06395926, -0.86951321, -0.48975089, 4.08618736]], + [[-0.01403128, -0.87593341, -0.48222789, 7.74559402]], + [[-0.01143177, -0.87495202, -0.4840748 , 7.75355816]]], + dtype=numpy.float32) + + eps = 0.05 + self.assertLessEqual(cv.norm(planes_coeff, planes_coeff_expected, cv.NORM_L2), eps) + +if __name__ == '__main__': + NewOpenCVTests.bootstrap() diff --git a/modules/rgbd/src/odometry.cpp b/modules/rgbd/src/odometry.cpp index 454e9643b..5283a124b 100755 --- a/modules/rgbd/src/odometry.cpp +++ b/modules/rgbd/src/odometry.cpp @@ -1009,6 +1009,28 @@ Ptr DepthCleaner::create(int depth_in, int window_size_in, int met return makePtr(depth_in, window_size_in, method_in); } +RgbdPlane::RgbdPlane(int method, int block_size, + int min_size, double threshold, double sensor_error_a, + double sensor_error_b, double sensor_error_c) : + method_(method), + block_size_(block_size), + min_size_(min_size), + threshold_(threshold), + sensor_error_a_(sensor_error_a), + sensor_error_b_(sensor_error_b), + sensor_error_c_(sensor_error_c) +{} + +Ptr RgbdPlane::create(int method, int block_size, int min_size, double threshold, + double sensor_error_a, double sensor_error_b, + double sensor_error_c ) { + return makePtr(method, block_size, min_size, threshold, + sensor_error_a, sensor_error_b, sensor_error_c); +} + +RgbdPlane::~RgbdPlane() +{} + RgbdFrame::RgbdFrame() : ID(-1) {}