Merge pull request #132 from pokeefe:add-rgbd-registration-function
Vadim Pisarevsky
9 years ago
commit
ae00a82dfc
3 changed files with 566 additions and 0 deletions
@ -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<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 */ |
||||
@ -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<unsigned short>(100, 2500); |
||||
if( code != cvtest::TS::OK ) |
||||
{ |
||||
ts->set_failed_test_info(code); |
||||
return; |
||||
} |
||||
|
||||
code = noOpRandomRegistrationTest<float>(0.1f, 2.5f); |
||||
if( code != cvtest::TS::OK ) |
||||
{ |
||||
ts->set_failed_test_info(code); |
||||
return; |
||||
} |
||||
|
||||
code = noOpRandomRegistrationTest<double>(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_<float>(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_<unsigned short> vgaDepth(height, width, (unsigned short)0); |
||||
vgaDepth(0,0) = 1001; |
||||
vgaDepth(1,1) = 1000; |
||||
|
||||
Mat_<unsigned short> 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_<unsigned short> 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 <class DepthDepth> |
||||
int noOpRandomRegistrationTest(DepthDepth minDepth, DepthDepth maxDepth) |
||||
{ |
||||
|
||||
// K from a VGA Kinect
|
||||
Mat K = (Mat_<float>(3, 3) << 525., 0., 319.5, 0., 525., 239.5, 0., 0., 1.); |
||||
|
||||
// Create a random depth image
|
||||
RNG rng; |
||||
Mat_<DepthDepth> 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(); |
||||
} |
||||
Loading…
Reference in new issue