/*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<typename DepthDepth> inline DepthDepth noDepthSentinelValue() { return 0; } template<> inline float noDepthSentinelValue<float>() { return std::numeric_limits<float>::quiet_NaN(); } template<> inline double noDepthSentinelValue<double>() { return std::numeric_limits<double>::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<typename DepthDepth> inline bool isEqualToNoDepthSentinelValue(const DepthDepth &value) { return value == noDepthSentinelValue<DepthDepth>(); } template<> inline bool isEqualToNoDepthSentinelValue<float>(const float &value) { return cvIsNaN(value) != 0; } template<> inline bool isEqualToNoDepthSentinelValue<double>(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<typename DepthDepth> inline DepthDepth floatToInputDepth(const float &value) { return (DepthDepth)value; } template<> inline unsigned short floatToInputDepth<unsigned short>(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<typename DepthDepth> void performRegistration(const Mat_<DepthDepth> &unregisteredDepth, const Matx33f &unregisteredCameraMatrix, const Matx33f ®isteredCameraMatrix, const Mat_<float> ®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_<DepthDepth>(outputImagePlaneSize, noDepthSentinelValue<DepthDepth>()); // 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_<Point3f> transformedCloud; { Mat_<Point3f> 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<float>::quiet_NaN(); } point->x = i * rescaled_depth; point->y = j * rescaled_depth; point->z = rescaled_depth; } } perspectiveTransform(point_tmp, transformedCloud, initialProjection); } std::vector<Point2f> 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<DepthDepth>(p->z*metersToInputUnitsScale); DepthDepth& outputDepth = registeredDepth.at<DepthDepth>(projectedPixelLocation.y, projectedPixelLocation.x); // Occlusion check if ( isEqualToNoDepthSentinelValue<DepthDepth>(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<DepthDepth>(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_<float> _registeredDistCoeffs = registeredDistCoeffs.getMat(); Matx44f _rbtRgb2Depth = Rt.getMat(); Mat ®isteredDepthMat = registeredDepth.getMatRef(); switch (unregisteredDepth.depth()) { case CV_16U: { performRegistration<unsigned short>(unregisteredDepth.getMat(), _unregisteredCameraMatrix, _registeredCameraMatrix, _registeredDistCoeffs, _rbtRgb2Depth, outputImagePlaneSize, depthDilation, .001f, registeredDepthMat); break; } case CV_32F: { performRegistration<float>(unregisteredDepth.getMat(), _unregisteredCameraMatrix, _registeredCameraMatrix, _registeredDistCoeffs, _rbtRgb2Depth, outputImagePlaneSize, depthDilation, 1.0f, registeredDepthMat); break; } case CV_64F: { performRegistration<double>(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 */