From f5ef071c117817b0e98b2bf509407f0c7a60efd7 Mon Sep 17 00:00:00 2001 From: Pat O'Keefe Date: Mon, 24 Nov 2014 16:22:01 -0500 Subject: [PATCH] Add RGBD registration function that registers depth to an external camera. --- modules/rgbd/include/opencv2/rgbd.hpp | 25 ++ modules/rgbd/src/depth_registration.cpp | 381 ++++++++++++++++++++++++ modules/rgbd/test/test_registration.cpp | 160 ++++++++++ 3 files changed, 566 insertions(+) create mode 100644 modules/rgbd/src/depth_registration.cpp create mode 100644 modules/rgbd/test/test_registration.cpp diff --git a/modules/rgbd/include/opencv2/rgbd.hpp b/modules/rgbd/include/opencv2/rgbd.hpp index 62947ae21..173239311 100644 --- a/modules/rgbd/include/opencv2/rgbd.hpp +++ b/modules/rgbd/include/opencv2/rgbd.hpp @@ -231,6 +231,31 @@ namespace rgbd mutable void* depth_cleaner_impl_; }; + + /** 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 + 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 diff --git a/modules/rgbd/src/depth_registration.cpp b/modules/rgbd/src/depth_registration.cpp new file mode 100644 index 000000000..f6bcf7d74 --- /dev/null +++ b/modules/rgbd/src/depth_registration.cpp @@ -0,0 +1,381 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. +// Copyright (C) 2009, Willow Garage Inc., all rights reserved. +// Copyright (C) 2014, OpenCV Foundation, all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ + + +#include "precomp.hpp" + + +namespace cv +{ +namespace rgbd +{ + + /////////////////////////////////////////////////////////////////////////////////// + + // Our three input types have a different value for a depth pixel with no depth + template + inline DepthDepth + noDepthSentinelValue() + { + return 0; + } + + template<> + inline float + noDepthSentinelValue() + { + return std::numeric_limits::quiet_NaN(); + } + + template<> + inline double + noDepthSentinelValue() + { + return std::numeric_limits::quiet_NaN(); + } + +/////////////////////////////////////////////////////////////////////////////////// + + // Testing for depth pixels with no depth isn't straightforward for NaN values. We + // need to specialize the equality check for floats and doubles. + template + inline bool + isEqualToNoDepthSentinelValue(const DepthDepth &value) + { + return value == noDepthSentinelValue(); + } + + template<> + inline bool + isEqualToNoDepthSentinelValue(const float &value) + { + return cvIsNaN(value) != 0; + } + + template<> + inline bool + isEqualToNoDepthSentinelValue(const double &value) + { + return cvIsNaN(value) != 0; + } + + /////////////////////////////////////////////////////////////////////////////////// + + + // When using the unsigned short representation, we'd like to round the values to the nearest + // integer value. The float/double representations don't need to be rounded + template + inline DepthDepth + floatToInputDepth(const float &value) + { + return (DepthDepth)value; + } + + template<> + inline unsigned short + floatToInputDepth(const float &value) + { + return (unsigned short)(value+0.5); + } + + /////////////////////////////////////////////////////////////////////////////////// + + + /** Computes a registered depth image from an unregistered image. + * + * @param unregisteredDepth the input depth data + * @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 rbtRgb2Depth the rigid body transform between the cameras. + * @param outputImagePlaneSize the image plane dimensions of the external camera (width, height) + * @param depthDilation whether or not the depth is dilated to avoid holes and occlusion errors + * @param inputDepthToMetersScale the scale needed to transform the input depth units to meters + * @param registeredDepth the result of transforming the depth into the external camera + */ + template + void + performRegistration(const Mat_ &unregisteredDepth, + const Matx33f &unregisteredCameraMatrix, + const Matx33f ®isteredCameraMatrix, + const Mat_ ®isteredDistCoeffs, + const Matx44f &rbtRgb2Depth, + const Size outputImagePlaneSize, + const bool depthDilation, + const float inputDepthToMetersScale, + Mat ®isteredDepth) + { + + // Create output Mat of the correct type, filled with an initial value indicating no depth + registeredDepth = Mat_(outputImagePlaneSize, noDepthSentinelValue()); + + // Figure out whether we'll have to apply a distortion + bool hasDistortion = (countNonZero(registeredDistCoeffs) > 0); + + // A point (i,j,1) will have to be converted to 3d first, by multiplying it by K.inv() + // It will then be transformed by rbtRgb2Depth. + // Finally, it will be projected into the external camera via registeredCameraMatrix and + // its distortion coefficients. If there is no distortion in the external camera, we + // can linearly chain all three operations together. + + Matx44f K = Matx44f::zeros(); + for(unsigned char j = 0; j < 3; ++j) + for(unsigned char i = 0; i < 3; ++i) + K(j, i) = unregisteredCameraMatrix(j, i); + K(3, 3) = 1; + + Matx44f initialProjection; + if (hasDistortion) + { + // The projection into the external camera will be done separately with distortion + initialProjection = rbtRgb2Depth * K.inv(); + } + else + { + // No distortion, so all operations can be chained + initialProjection = Matx44f::zeros(); + for(unsigned char j = 0; j < 3; ++j) + for(unsigned char i = 0; i < 3; ++i) + initialProjection(j, i) = registeredCameraMatrix(j, i); + initialProjection(3, 3) = 1; + + initialProjection = initialProjection * rbtRgb2Depth * K.inv(); + } + + // Apply the initial projection to the input depth + Mat_ transformedCloud; + { + Mat_ point_tmp(outputImagePlaneSize); + + for(int j = 0; j < point_tmp.rows; ++j) + { + const DepthDepth *unregisteredDepthPtr = unregisteredDepth[j]; + + Point3f *point = point_tmp[j]; + for(int i = 0; i < point_tmp.cols; ++i, ++unregisteredDepthPtr, ++point) + { + float rescaled_depth = float(*unregisteredDepthPtr) * inputDepthToMetersScale; + + // If the DepthDepth is of type unsigned short, zero is a sentinel value to indicate + // no depth. CV_32F and CV_64F should already have NaN for no depth values. + if (rescaled_depth == 0) + { + rescaled_depth = std::numeric_limits::quiet_NaN(); + } + + point->x = i * rescaled_depth; + point->y = j * rescaled_depth; + point->z = rescaled_depth; + } + } + + perspectiveTransform(point_tmp, transformedCloud, initialProjection); + } + + std::vector transformedAndProjectedPoints(transformedCloud.cols); + const float metersToInputUnitsScale = 1/inputDepthToMetersScale; + const Rect registeredDepthBounds(Point(), outputImagePlaneSize); + + for( int y = 0; y < transformedCloud.rows; y++ ) + { + if (hasDistortion) + { + + // Project an entire row of points with distortion. + // Doing this for the entire image at once would require more memory. + projectPoints(transformedCloud.row(y), + Vec3f(0,0,0), + Vec3f(0,0,0), + registeredCameraMatrix, + registeredDistCoeffs, + transformedAndProjectedPoints); + + } + else + { + + // With no distortion, we just have to dehomogenize the point since all major transforms + // already happened with initialProjection. + Point2f *point2d = &transformedAndProjectedPoints[0]; + const Point2f *point2d_end = point2d + transformedAndProjectedPoints.size(); + const Point3f *point3d = transformedCloud[y]; + for( ; point2d < point2d_end; ++point2d, ++point3d ) + { + point2d->x = point3d->x / point3d->z; + point2d->y = point3d->y / point3d->z; + } + + } + + const Point2f *outputProjectedPoint = &transformedAndProjectedPoints[0]; + const Point3f *p = transformedCloud[y], *p_end = p + transformedCloud.cols; + + + for( ; p < p_end; ++outputProjectedPoint, ++p ) + { + + + + // Skip this one if there isn't a valid depth + const Point2f projectedPixelFloatLocation = *outputProjectedPoint; + if (cvIsNaN(projectedPixelFloatLocation.x)) + continue; + + //Get integer pixel location + const Point2i projectedPixelLocation = projectedPixelFloatLocation; + + // Ensure that the projected point is actually contained in our output image + if (!registeredDepthBounds.contains(projectedPixelLocation)) + continue; + + // Go back to our original scale, since that's what our output will be + // The templated function is to ensure that integer values are rounded to the nearest integer + const DepthDepth cloudDepth = floatToInputDepth(p->z*metersToInputUnitsScale); + + DepthDepth& outputDepth = registeredDepth.at(projectedPixelLocation.y, projectedPixelLocation.x); + + + // Occlusion check + if ( isEqualToNoDepthSentinelValue(outputDepth) || (outputDepth > cloudDepth) ) + outputDepth = cloudDepth; + + + // If desired, dilate this point to avoid holes in the final image + if (depthDilation) + { + + // Choosing to dilate in a 2x2 region, where the original projected location is in the bottom right of this + // region. This is what's done on PrimeSense devices, but a more accurate scheme could be used. + const Point2i dilatedProjectedLocations[3] = {Point2i(projectedPixelLocation.x - 1, projectedPixelLocation.y ), + Point2i(projectedPixelLocation.x , projectedPixelLocation.y - 1), + Point2i(projectedPixelLocation.x - 1, projectedPixelLocation.y - 1)}; + + for (int i = 0; i < 3; i++) { + + const Point2i& dilatedCoordinates = dilatedProjectedLocations[i]; + + if (!registeredDepthBounds.contains(dilatedCoordinates)) + continue; + + DepthDepth& outputDilatedDepth = registeredDepth.at(dilatedCoordinates.y, dilatedCoordinates.x); + + // Occlusion check + if ( isEqualToNoDepthSentinelValue(outputDilatedDepth) || (outputDilatedDepth > cloudDepth) ) + outputDilatedDepth = cloudDepth; + + } + + } // depthDilation + + } // iterate cols + } // iterate rows + + } + + + + void + registerDepth(InputArray unregisteredCameraMatrix, InputArray registeredCameraMatrix,InputArray registeredDistCoeffs, + InputArray Rt, InputArray unregisteredDepth, const Size& outputImagePlaneSize, + OutputArray registeredDepth, bool depthDilation) + { + + CV_Assert(unregisteredCameraMatrix.depth() == CV_64F || unregisteredCameraMatrix.depth() == CV_32F); + + CV_Assert(registeredCameraMatrix.depth() == CV_64F || registeredCameraMatrix.depth() == CV_32F); + + CV_Assert(registeredDistCoeffs.empty() || registeredDistCoeffs.depth() == CV_64F || registeredDistCoeffs.depth() == CV_32F); + + CV_Assert(Rt.depth() == CV_64F || Rt.depth() == CV_32F); + + CV_Assert(unregisteredDepth.cols() > 0 && unregisteredDepth.rows() > 0 && + (unregisteredDepth.depth() == CV_32F || unregisteredDepth.depth() == CV_64F || unregisteredDepth.depth() == CV_16U)); + + CV_Assert(outputImagePlaneSize.height > 0 && outputImagePlaneSize.width > 0); + + // Implicitly checking dimensions of the InputArrays + Matx33f _unregisteredCameraMatrix = unregisteredCameraMatrix.getMat(); + Matx33f _registeredCameraMatrix = registeredCameraMatrix.getMat(); + Mat_ _registeredDistCoeffs = registeredDistCoeffs.getMat(); + Matx44f _rbtRgb2Depth = Rt.getMat(); + + + Mat ®isteredDepthMat = registeredDepth.getMatRef(); + + switch (unregisteredDepth.depth()) + { + case CV_16U: + { + performRegistration(unregisteredDepth.getMat(), _unregisteredCameraMatrix, + _registeredCameraMatrix, _registeredDistCoeffs, + _rbtRgb2Depth, outputImagePlaneSize, depthDilation, + .001f, registeredDepthMat); + break; + } + case CV_32F: + { + performRegistration(unregisteredDepth.getMat(), _unregisteredCameraMatrix, + _registeredCameraMatrix, _registeredDistCoeffs, + _rbtRgb2Depth, outputImagePlaneSize, depthDilation, + 1.0f, registeredDepthMat); + break; + } + case CV_64F: + { + performRegistration(unregisteredDepth.getMat(), _unregisteredCameraMatrix, + _registeredCameraMatrix, _registeredDistCoeffs, + _rbtRgb2Depth, outputImagePlaneSize, depthDilation, + 1.0f, registeredDepthMat); + break; + } + default: + { + CV_Error(Error::StsUnsupportedFormat, "Input depth must be unsigned short, float, or double."); + } + } + + + } + +} /* namespace rgbd */ +} /* namespace cv */ diff --git a/modules/rgbd/test/test_registration.cpp b/modules/rgbd/test/test_registration.cpp new file mode 100644 index 000000000..6c12b79a4 --- /dev/null +++ b/modules/rgbd/test/test_registration.cpp @@ -0,0 +1,160 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. +// Copyright (C) 2009, Willow Garage Inc., all rights reserved. +// Copyright (C) 2014, OpenCV Foundation, all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ + + +#include "test_precomp.hpp" + + +namespace cv +{ +namespace rgbd +{ + +class CV_RgbdDepthRegistrationTest: public cvtest::BaseTest +{ +public: + CV_RgbdDepthRegistrationTest() + { + } + ~CV_RgbdDepthRegistrationTest() + { + } +protected: + void + run(int) + { + + // Test all three input types for no-op registrations (where a depth image is registered to itself) + + int code = noOpRandomRegistrationTest(100, 2500); + if( code != cvtest::TS::OK ) + { + ts->set_failed_test_info(code); + return; + } + + code = noOpRandomRegistrationTest(0.1f, 2.5f); + if( code != cvtest::TS::OK ) + { + ts->set_failed_test_info(code); + return; + } + + code = noOpRandomRegistrationTest(0.1, 2.5); + if( code != cvtest::TS::OK ) + { + ts->set_failed_test_info(code); + return; + } + + + // Test sentinel value handling, occlusion, and dilation + { + + // K from a VGA Kinect + Mat K = (Mat_(3, 3) << 525., 0., 319.5, 0., 525., 239.5, 0., 0., 1.); + + int width = 640, height = 480; + + // All elements are zero except for first two along the diagonal + Mat_ vgaDepth(height, width, (unsigned short)0); + vgaDepth(0,0) = 1001; + vgaDepth(1,1) = 1000; + + Mat_ registeredDepth; + registerDepth(K, K, Mat(), Matx44f::eye(), vgaDepth, Size(width, height), registeredDepth, true); + + // We expect the closer depth of 1000 to occlude the more distant depth and occupy the + // upper four left pixels in the depth image because of dilation + Mat_ expectedResult(height, width, (unsigned short)0); + expectedResult(0,0) = 1000; + expectedResult(0,1) = 1000; + expectedResult(1,0) = 1000; + expectedResult(1,1) = 1000; + + int cmpResult = cvtest::cmpEps2( ts, registeredDepth, expectedResult, 0, true, "Dilation and occlusion"); + + if( cmpResult != cvtest::TS::OK ) + { + ts->set_failed_test_info(cmpResult); + return; + } + + } + + ts->set_failed_test_info(cvtest::TS::OK); + + } +private: + + template + int noOpRandomRegistrationTest(DepthDepth minDepth, DepthDepth maxDepth) + { + + // K from a VGA Kinect + Mat K = (Mat_(3, 3) << 525., 0., 319.5, 0., 525., 239.5, 0., 0., 1.); + + // Create a random depth image + RNG rng; + Mat_ randomVGADepth(480, 640); + rng.fill(randomVGADepth, RNG::UNIFORM, minDepth, maxDepth); + + Mat registeredDepth; + registerDepth(K, K, Mat(), Matx44f::eye(), randomVGADepth, Size(640, 480), registeredDepth); + + // See if registeredDepth == depth + return cvtest::cmpEps2( ts, registeredDepth, randomVGADepth, 1e-5, true, "No-op registration"); + + } + +}; + + +} +} + +TEST(Rgbd_DepthRegistration, compute) +{ + cv::rgbd::CV_RgbdDepthRegistrationTest test; + test.safe_run(); +}