diff --git a/modules/structured_light/src/graycodepattern.cpp b/modules/structured_light/src/graycodepattern.cpp index f37bc3bb526..c79d41c822e 100644 --- a/modules/structured_light/src/graycodepattern.cpp +++ b/modules/structured_light/src/graycodepattern.cpp @@ -87,11 +87,11 @@ class CV_EXPORTS_W GrayCodePattern_Impl CV_FINAL : public GrayCodePattern // The number of column images of the pattern size_t numOfColImgs; - // Number between 0-255 that represents the minimum brightness difference + // Number between 0-255 or 0-65535 that represents the minimum brightness difference // between the fully illuminated (white) and the non - illuminated images (black) size_t blackThreshold; - // Number between 0-255 that represents the minimum brightness difference + // Number between 0-255 or 0-65535 that represents the minimum brightness difference // between the gray-code pattern and its inverse images size_t whiteThreshold; @@ -102,8 +102,8 @@ class CV_EXPORTS_W GrayCodePattern_Impl CV_FINAL : public GrayCodePattern void computeShadowMasks( InputArrayOfArrays blackImages, InputArrayOfArrays whiteImages, OutputArrayOfArrays shadowMasks ) const; - // Converts a gray code sequence (~ binary number) to a decimal number - int grayToDec( const std::vector& gray ) const; + bool getProjPixelFast(const std::vector& fastColImages, const std::vector& fastRowImages, int x, int y, Point& projPix) const; + void populateFastPatternImages(const std::vector>& acquiredPatterns, std::vector>& fastPatternColImages, std::vector>& fastPatternRowImages) const; }; /* @@ -125,184 +125,161 @@ GrayCodePattern_Impl::GrayCodePattern_Impl( const GrayCodePattern::Params ¶m bool GrayCodePattern_Impl::generate( OutputArrayOfArrays pattern ) { - std::vector& pattern_ = *( std::vector* ) pattern.getObj(); - pattern_.resize( numOfPatternImages ); - - for( size_t i = 0; i < numOfPatternImages; i++ ) - { - pattern_[i] = Mat( params.height, params.width, CV_8U ); - } - - uchar flag = 0; - - for( int j = 0; j < params.width; j++ ) // rows loop - { - int rem = 0, num = j, prevRem = j % 2; - - for( size_t k = 0; k < numOfColImgs; k++ ) // images loop + std::vector& pattern_ = *(std::vector*)pattern.getObj(); + pattern_.resize(numOfPatternImages); + for (size_t i = 0; i < numOfPatternImages; i++) { - num = num / 2; - rem = num % 2; - - if( ( rem == 0 && prevRem == 1 ) || ( rem == 1 && prevRem == 0) ) - { - flag = 1; - } - else - { - flag = 0; - } - - for( int i = 0; i < params.height; i++ ) // rows loop - { - - uchar pixel_color = ( uchar ) flag * 255; - - pattern_[2 * numOfColImgs - 2 * k - 2].at( i, j ) = pixel_color; - if( pixel_color > 0 ) - pixel_color = ( uchar ) 0; - else - pixel_color = ( uchar ) 255; - pattern_[2 * numOfColImgs - 2 * k - 1].at( i, j ) = pixel_color; // inverse - } - - prevRem = rem; + pattern_[i].create(params.height, params.width, CV_8U); } - } - - for( int i = 0; i < params.height; i++ ) // rows loop - { - int rem = 0, num = i, prevRem = i % 2; - - for( size_t k = 0; k < numOfRowImgs; k++ ) - { - num = num / 2; - rem = num % 2; - - if( (rem == 0 && prevRem == 1) || (rem == 1 && prevRem == 0) ) - { - flag = 1; - } - else - { - flag = 0; - } - - for( int j = 0; j < params.width; j++ ) - { - - uchar pixel_color = ( uchar ) flag * 255; - pattern_[2 * numOfRowImgs - 2 * k + 2 * numOfColImgs - 2].at( i, j ) = pixel_color; - - if( pixel_color > 0 ) - pixel_color = ( uchar ) 0; - else - pixel_color = ( uchar ) 255; - - pattern_[2 * numOfRowImgs - 2 * k + 2 * numOfColImgs - 1].at( i, j ) = pixel_color; - } - - prevRem = rem; - } - } + parallel_for_(Range(0, params.width), [&](const Range& range) { + for (int col = range.start; col < range.end; ++col) + { + for (size_t nBits = 0; nBits < numOfColImgs; ++nBits) + { + // Gray-code column bit-plane: XOR adjacent binary bits so just one stripe + // toggles between consecutive frames without rebuilding the Gray number. + // It is a more efficient alternative to the decodePixel + greyToDec approach. + // Here we get the column bit-plane for the n-th and (n+1)-th binary bits and XOR them + // following the Gray-code definition (x_n = b_n XOR b_(n+1)) + uchar flag = (((col >> nBits) & 1) ^ ((col >> (nBits + 1)) & 1)); + uchar pixel_color = flag * 255; + + size_t idx1 = 2 * numOfColImgs - 2 * nBits - 2; + size_t idx2 = 2 * numOfColImgs - 2 * nBits - 1; + + if (idx1 < numOfPatternImages) + { + pattern_[idx1].col(col).setTo(pixel_color); + } + if (idx2 < numOfPatternImages) + { + pattern_[idx2].col(col).setTo(255 - pixel_color); + } + } + } + }); + parallel_for_(Range(0, params.height), [&](const Range& range) { + for (int row = range.start; row < range.end; ++row) + { + for (size_t nBits = 0; nBits < numOfRowImgs; ++nBits) + { + // Same Gray extraction for rows so vertical patterns flip a single stripe per exposure + // It is a more efficient alternative to the decodePixel + greyToDec approach. + // Here we get the row bit-plane for the n-th and (n+1)-th binary bits and XOR them + // following the Gray-code definition (x_n = b_n XOR b_(n+1)) + uchar flag = (((row >> nBits) & 1) ^ ((row >> (nBits + 1)) & 1)); + uchar pixel_color = flag * 255; + + size_t base_idx = 2 * numOfColImgs; + size_t idx1 = base_idx + 2 * numOfRowImgs - 2 * nBits - 2; + size_t idx2 = base_idx + 2 * numOfRowImgs - 2 * nBits - 1; + + if (idx1 < numOfPatternImages) + { + pattern_[idx1].row(row).setTo(pixel_color); + } + if (idx2 < numOfPatternImages) + { + pattern_[idx2].row(row).setTo(255 - pixel_color); + } + } + } + }); - return true; + return true; } -bool GrayCodePattern_Impl::decode( const std::vector< std::vector >& patternImages, OutputArray disparityMap, - InputArrayOfArrays blackImages, InputArrayOfArrays whitheImages, int flags ) const -{ - const std::vector >& acquired_pattern = patternImages; - - if( flags == DECODE_3D_UNDERWORLD ) - { - // Computing shadows mask - std::vector shadowMasks; - computeShadowMasks( blackImages, whitheImages, shadowMasks ); - - int cam_width = acquired_pattern[0][0].cols; - int cam_height = acquired_pattern[0][0].rows; - Point projPixel; - - // Storage for the pixels of the two cams that correspond to the same pixel of the projector - std::vector > > camsPixels; - camsPixels.resize( acquired_pattern.size() ); +bool GrayCodePattern_Impl::decode(const std::vector< std::vector >& patternImages, OutputArray disparityMap, InputArrayOfArrays blackImages, InputArrayOfArrays whiteImages, int flags) const +{ + const std::vector>& acquired_pattern = patternImages; - // TODO: parallelize for (k and j) - for( size_t k = 0; k < acquired_pattern.size(); k++ ) + if (flags == DECODE_3D_UNDERWORLD) { - camsPixels[k].resize( params.height * params.width ); - for( int i = 0; i < cam_width; i++ ) - { - for( int j = 0; j < cam_height; j++ ) - { - //if the pixel is not shadowed, reconstruct - if( shadowMasks[k].at( j, i ) ) - { - //for a (x,y) pixel of the camera returns the corresponding projector pixel by calculating the decimal number - bool error = getProjPixel( acquired_pattern[k], i, j, projPixel ); - - if( error ) - { - continue; + // Shadow mask computation remains the same + std::vector shadowMasks; + computeShadowMasks(blackImages, whiteImages, shadowMasks); + + int num_cameras = static_cast(acquired_pattern.size()); + + int cam_width = acquired_pattern[0][0].cols; + int cam_height = acquired_pattern[0][0].rows; + int proj_width = params.width; + int proj_height = params.height; + + std::vector> fastPatternColImages; + std::vector> fastPatternRowImages; + populateFastPatternImages(acquired_pattern, fastPatternColImages, fastPatternRowImages); + + std::vector projectorCoordinateMap(num_cameras); + parallel_for_(Range(0, num_cameras), [&](const Range& range) { + for (int k = range.start; k < range.end; k++) { + projectorCoordinateMap[k] = Mat(cam_height, cam_width, CV_32SC2, Vec2i(-1, -1)); + Point projPixel; + const auto& fastCol = fastPatternColImages[k]; + const auto& fastRow = fastPatternRowImages[k]; + for (int j = 0; j < cam_height; j++) { + for (int i = 0; i < cam_width; i++) { + if (shadowMasks[k].at(j, i)) { + if (!getProjPixelFast(fastCol, fastRow, i, j, projPixel)) { + projectorCoordinateMap[k].at(j, i) = Vec2i(projPixel.x, projPixel.y); + } + } + } + } } + }); - camsPixels[k][projPixel.x * params.height + projPixel.y].push_back( Point( i, j ) ); - } + std::vector sumX(num_cameras); + std::vector counts(num_cameras); + for (int k = 0; k < num_cameras; ++k) { + sumX[k] = Mat::zeros(proj_height, proj_width, CV_64F); + counts[k] = Mat::zeros(proj_height, proj_width, CV_32S); } - } - } - std::vector cam1Pixs, cam2Pixs; + parallel_for_(Range(0, num_cameras), [&](const Range& range) { + for (int k = range.start; k < range.end; ++k) { + for (int j = 0; j < cam_height; ++j) { + for (int i = 0; i < cam_width; ++i) { + const Vec2i& projPt = projectorCoordinateMap[k].at(j, i); + if (projPt[0] != -1) { + sumX[k].at(projPt[1], projPt[0]) += i; + counts[k].at(projPt[1], projPt[0]) += 1; + } + } + } + } + }); - Mat& disparityMap_ = *( Mat* ) disparityMap.getObj(); - disparityMap_ = Mat( cam_height, cam_width, CV_64F, double( 0 ) ); + Mat counts_64F[2], avgX[2]; + counts[0].convertTo(counts_64F[0], CV_64F); + counts[1].convertTo(counts_64F[1], CV_64F); + cv::divide(sumX[0], counts_64F[0], avgX[0]); + cv::divide(sumX[1], counts_64F[1], avgX[1]); - for( int i = 0; i < params.width; i++ ) - { - for( int j = 0; j < params.height; j++ ) - { - cam1Pixs = camsPixels[0][i * params.height + j]; - cam2Pixs = camsPixels[1][i * params.height + j]; + // Handle invalid pixels (e.g. not seen by one of the cameras) by setting them to zero. + cv::Mat invalidPixels = (counts[0] == 0) | (counts[1] == 0); - if( cam1Pixs.size() == 0 || cam2Pixs.size() == 0 ) - continue; + Mat projectorDisparity = avgX[1] - avgX[0]; - Point p1; - Point p2; + Mat& disparityMap_ = *(Mat*)disparityMap.getObj(); + Mat map_x(cam_height, cam_width, CV_32F); + Mat map_y(cam_height, cam_width, CV_32F); - double sump1x = 0; - double sump2x = 0; + std::vector projMapChannels; + cv::split(projectorCoordinateMap[0], projMapChannels); + projMapChannels[0].convertTo(map_x, CV_32F); // Projector x-coordinates + projMapChannels[1].convertTo(map_y, CV_32F); // Projector y-coordinates - for( int c1 = 0; c1 < (int) cam1Pixs.size(); c1++ ) - { - p1 = cam1Pixs[c1]; - sump1x += p1.x; - } - for( int c2 = 0; c2 < (int) cam2Pixs.size(); c2++ ) - { - p2 = cam2Pixs[c2]; - sump2x += p2.x; - } + projectorDisparity.setTo(Scalar(0.0), invalidPixels); - sump2x /= cam2Pixs.size(); - sump1x /= cam1Pixs.size(); - for( int c1 = 0; c1 < (int) cam1Pixs.size(); c1++ ) - { - p1 = cam1Pixs[c1]; - disparityMap_.at( p1.y, p1.x ) = ( double ) (sump2x - sump1x); - } + cv::remap(projectorDisparity, disparityMap_, map_x, map_y, + INTER_NEAREST, BORDER_CONSTANT, Scalar(0)); - sump2x = 0; - sump1x = 0; - } + return true; } - return true; - } // end if flags - - return false; + return false; } // Computes the required number of pattern images @@ -319,9 +296,9 @@ size_t GrayCodePattern_Impl::getNumberOfPatternImages() const return numOfPatternImages; } -// Computes the shadows occlusion where we cannot reconstruct the model + // Computes the shadows occlusion where we cannot reconstruct the model void GrayCodePattern_Impl::computeShadowMasks( InputArrayOfArrays blackImages, InputArrayOfArrays whiteImages, - OutputArrayOfArrays shadowMasks ) const + OutputArrayOfArrays shadowMasks ) const { std::vector& whiteImages_ = *( std::vector* ) whiteImages.getObj(); std::vector& blackImages_ = *( std::vector* ) blackImages.getObj(); @@ -329,31 +306,16 @@ void GrayCodePattern_Impl::computeShadowMasks( InputArrayOfArrays blackImages, I shadowMasks_.resize( whiteImages_.size() ); - int cam_width = whiteImages_[0].cols; - int cam_height = whiteImages_[0].rows; - - // TODO: parallelize for - for( int k = 0; k < (int) shadowMasks_.size(); k++ ) + parallel_for_(Range(0, (int) whiteImages_.size()), [&](const Range& range) { - shadowMasks_[k] = Mat( cam_height, cam_width, CV_8U ); - for( int i = 0; i < cam_width; i++ ) + for( int k = range.start; k < range.end; k++ ) { - for( int j = 0; j < cam_height; j++ ) - { - double white = whiteImages_[k].at( Point( i, j ) ); - double black = blackImages_[k].at( Point( i, j ) ); - - if( abs(white - black) > blackThreshold ) - { - shadowMasks_[k].at( Point( i, j ) ) = ( uchar ) 1; - } - else - { - shadowMasks_[k].at( Point( i, j ) ) = ( uchar ) 0; - } - } + cv::Mat diffImage; + cv::absdiff(whiteImages_[k], blackImages_[k], diffImage); + cv::compare(diffImage, static_cast(blackThreshold), shadowMasks_[k], cv::CMP_GT); + shadowMasks_[k] /= 255; } - } + }); } // Generates the images needed for shadowMasks computation @@ -366,86 +328,172 @@ void GrayCodePattern_Impl::getImagesForShadowMasks( InputOutputArray blackImage, whiteImage_ = Mat( params.height, params.width, CV_8U, Scalar( 255 ) ); } -// For a (x,y) pixel of the camera returns the corresponding projector's pixel -bool GrayCodePattern_Impl::getProjPixel( InputArrayOfArrays patternImages, int x, int y, Point &projPix ) const +void GrayCodePattern_Impl::populateFastPatternImages(const std::vector>& acquiredPatterns, std::vector>& fastPatternColImages, std::vector>& fastPatternRowImages) const { + fastPatternColImages.resize(acquiredPatterns.size()); + fastPatternRowImages.resize(acquiredPatterns.size()); + + for (size_t i = 0; i < acquiredPatterns.size(); i++) { + const auto& _patternImages = acquiredPatterns[i]; + auto& fastColImagesN = fastPatternColImages[i]; + auto& fastRowImagesN = fastPatternRowImages[i]; + fastColImagesN.resize(numOfColImgs); + fastRowImagesN.resize(numOfRowImgs); + parallel_for_(Range(0, (int)numOfColImgs), [&](const Range& range) { + for (int count = range.start; count < range.end; count++) { + cv::Mat diffImage; + cv::subtract(_patternImages[count * 2], _patternImages[count * 2 + 1], diffImage, cv::noArray(), CV_32S); + cv::Mat invalidMask = abs(diffImage) < static_cast(whiteThreshold); + cv::compare(diffImage, cv::Scalar(0), diffImage, cv::CMP_GT); + diffImage.setTo(1, invalidMask); + fastColImagesN[count] = std::move(diffImage); + } + }); + + parallel_for_(Range(0, (int)numOfRowImgs), [&](const Range& range) { + for (int count = range.start; count < range.end; count++) { + cv::Mat diffImage; + cv::subtract(_patternImages[count * 2 + numOfColImgs * 2], _patternImages[count * 2 + numOfColImgs * 2 + 1], diffImage, cv::noArray(), CV_32S); + cv::Mat invalidMask = abs(diffImage) < static_cast(whiteThreshold); + cv::compare(diffImage, cv::Scalar(0), diffImage, cv::CMP_GT); + diffImage.setTo(1, invalidMask); + fastRowImagesN[count] = std::move(diffImage); + } + }); + } +} + +// NOTE: these 2 functions (grayToDec and decodePixel) are kept to keep backward compatibility with the existing API +// since they are used in the getProjPixel function. However, the new faster method getProjPixelFast is preferred for performance +// when decoding full patterns instead of single pixels. +static int grayToDec(const std::vector& gray) +{ + if (gray.empty()) + { + return 0; + } + + int dec = 0; + uchar prev_binary_bit = 0; + for (size_t i = 0; i < gray.size(); ++i) + { + uchar current_binary_bit = prev_binary_bit ^ gray[i]; + dec = (dec << 1) | current_binary_bit; + prev_binary_bit = current_binary_bit; + } + return dec; +} + +template +static bool decodePixel(int x, int y, size_t numOfColImgs, size_t numOfRowImgs, std::vector& patternImagesVec, size_t whiteThreshold, std::vector& grayRow, std::vector& grayCol) { + bool error = false; + + for (size_t k = 0; k < numOfColImgs; ++k) + { + T val1 = patternImagesVec[k * 2].at(y, x); + T val2 = patternImagesVec[k * 2 + 1].at(y, x); + + if (std::abs(static_cast(val1) - static_cast(val2)) < whiteThreshold) + { + error = true; + } + grayCol[k] = (val1 > val2); + } + + size_t base_idx = 2 * numOfColImgs; + for (size_t k = 0; k < numOfRowImgs; ++k) + { + T val1 = patternImagesVec[base_idx + k * 2].at(y, x); + T val2 = patternImagesVec[base_idx + k * 2 + 1].at(y, x); + + if (std::abs(static_cast(val1) - static_cast(val2)) < whiteThreshold) + { + error = true; + } + grayRow[k] = (val1 > val2); + } + return error; +} + +bool GrayCodePattern_Impl::getProjPixel(InputArrayOfArrays patternImages, int x, int y, Point& projPix) const { - std::vector& _patternImages = *( std::vector* ) patternImages.getObj(); - std::vector grayCol; - std::vector grayRow; + std::vector& patternImagesVec = *(std::vector*)patternImages.getObj(); - bool error = false; - int xDec, yDec; + std::vector grayCol(numOfColImgs); + std::vector grayRow(numOfRowImgs); + + bool error = false; + int depth = patternImagesVec[0].depth(); + + switch (depth) + { + case CV_8U: + error = decodePixel(x, y, numOfColImgs, numOfRowImgs, patternImagesVec, whiteThreshold, grayRow, grayCol); + break; + case CV_16U: + error = decodePixel(x, y, numOfColImgs, numOfRowImgs, patternImagesVec, whiteThreshold, grayRow, grayCol); + break; + default: + CV_Error(Error::StsUnsupportedFormat, "Image depth not supported, only CV_8U and CV_16U"); + } + + int xDec = grayToDec(grayCol); + int yDec = grayToDec(grayRow); + + if (yDec >= params.height || xDec >= params.width) + { + error = true; + } + + projPix.x = xDec; + projPix.y = yDec; + + return error; +} + + +// For a (x,y) pixel of the camera returns the corresponding projector's pixel +bool GrayCodePattern_Impl::getProjPixelFast( const std::vector& fastColImages, const std::vector& fastRowImages, int x, int y, Point &projPix) const +{ + int xDec = 0, yDec = 0; + ushort binBit = 0; + Point pattPoint = Point(x, y); // process column images - for( size_t count = 0; count < numOfColImgs; count++ ) + for(const auto& fastColImage : fastColImages) { - // get pixel intensity for regular pattern projection and its inverse - double val1 = _patternImages[count * 2].at( Point( x, y ) ); - double val2 = _patternImages[count * 2 + 1].at( Point( x, y ) ); + uchar grayBit = fastColImage.at(pattPoint); // check if the intensity difference between the values of the normal and its inverse projection image is in a valid range - if( abs(val1 - val2) < whiteThreshold ) - error = true; + if( grayBit == 1) return true; // determine if projection pixel is on or off - if( val1 > val2 ) - grayCol.push_back( 1 ); - else - grayCol.push_back( 0 ); + binBit = binBit ^ (grayBit == 255); + xDec = (xDec << 1) | binBit; } - xDec = grayToDec( grayCol ); + binBit = 0; // reset binary bit for row images // process row images - for( size_t count = 0; count < numOfRowImgs; count++ ) + for(const auto& fastRowImage : fastRowImages) { - // get pixel intensity for regular pattern projection and its inverse - double val1 = _patternImages[count * 2 + numOfColImgs * 2].at( Point( x, y ) ); - double val2 = _patternImages[count * 2 + numOfColImgs * 2 + 1].at( Point( x, y ) ); - + uchar grayBit = fastRowImage.at(pattPoint); // check if the intensity difference between the values of the normal and its inverse projection image is in a valid range - if( abs(val1 - val2) < whiteThreshold ) - error = true; + if( grayBit == 1 ) return true; // determine if projection pixel is on or off - if( val1 > val2 ) - grayRow.push_back( 1 ); - else - grayRow.push_back( 0 ); + binBit = binBit ^ (grayBit == 255); + yDec = (yDec << 1) | binBit; } - yDec = grayToDec( grayRow ); - if( (yDec >= params.height || xDec >= params.width) ) { - error = true; + return true; } projPix.x = xDec; projPix.y = yDec; - return error; -} - -// Converts a gray code sequence (~ binary number) to a decimal number -int GrayCodePattern_Impl::grayToDec( const std::vector& gray ) const -{ - int dec = 0; - - uchar tmp = gray[0]; - - if( tmp ) - dec += ( int ) pow( ( float ) 2, int( gray.size() - 1 ) ); - - for( int i = 1; i < (int) gray.size(); i++ ) - { - // XOR operation - tmp = tmp ^ gray[i]; - if( tmp ) - dec += (int) pow( ( float ) 2, int( gray.size() - i - 1 ) ); - } - - return dec; + return false; } // Sets the value for black threshold diff --git a/modules/structured_light/test/test_graycodepattern_fast.cpp b/modules/structured_light/test/test_graycodepattern_fast.cpp new file mode 100644 index 00000000000..773aa12fa27 --- /dev/null +++ b/modules/structured_light/test/test_graycodepattern_fast.cpp @@ -0,0 +1,396 @@ +/*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) 2015, 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" + +#include + +namespace opencv_test { namespace { + +using namespace cv; +using namespace cv::structured_light; + +static size_t requiredBits(int value) +{ + return value > 1 ? static_cast(std::ceil(std::log(static_cast(value)) / std::log(2.0))) : 1; +} + +static void circularShiftCols(const Mat& src, Mat& dst, int shift) +{ + CV_Assert(src.channels() == 1); + if (src.empty()) + { + dst.release(); + return; + } + + const int width = src.cols; + if (width == 0) + { + dst.release(); + return; + } + + shift %= width; + if (shift < 0) + shift += width; + + if (shift == 0) + { + src.copyTo(dst); + return; + } + + dst.create(src.size(), src.type()); + src.colRange(0, width - shift).copyTo(dst.colRange(shift, width)); + src.colRange(width - shift, width).copyTo(dst.colRange(0, shift)); +} + +static void applyLowContrastColumns(std::vector& images, int colStart, int colEnd, int value) +{ + if (images.empty()) + return; + + const int width = images[0].cols; + colStart = std::max(0, std::min(colStart, width)); + colEnd = std::max(0, std::min(colEnd, width)); + if (colStart >= colEnd) + return; + + for (Mat& img : images) + { + img.colRange(colStart, colEnd).setTo(Scalar(value)); + } +} + +static void buildSyntheticCapture(const Ptr& graycode, + int columnShift, + int lowContrastStart, + int lowContrastEnd, + std::vector>& captured, + std::vector& blackImages, + std::vector& whiteImages) +{ + std::vector basePattern; + ASSERT_TRUE(graycode->generate(basePattern)); + + const size_t numImages = basePattern.size(); + captured.assign(2, std::vector(numImages)); + for (size_t idx = 0; idx < numImages; ++idx) + { + basePattern[idx].copyTo(captured[0][idx]); + circularShiftCols(basePattern[idx], captured[1][idx], columnShift); + } + + applyLowContrastColumns(captured[0], lowContrastStart, lowContrastEnd, 128); + applyLowContrastColumns(captured[1], lowContrastStart, lowContrastEnd, 128); + + const int height = basePattern[0].rows; + const int width = basePattern[0].cols; + blackImages.assign(2, Mat(height, width, basePattern[0].type(), Scalar(0))); + whiteImages.assign(2, Mat(height, width, basePattern[0].type(), Scalar(255))); +} + +static void convertTo16U(std::vector>& captured, + std::vector& blackImages, + std::vector& whiteImages) +{ + const double scale = 257.0; // 255 * 257 = 65535 + for (std::vector& cameraImages : captured) + { + for (Mat& img : cameraImages) + { + Mat tmp; + img.convertTo(tmp, CV_16U, scale); + img = tmp; + } + } + for (Mat& img : blackImages) + { + Mat tmp; + img.convertTo(tmp, CV_16U, scale); + img = tmp; + } + for (Mat& img : whiteImages) + { + Mat tmp; + img.convertTo(tmp, CV_16U, scale); + img = tmp; + } +} + +static Mat computeLegacyDisparity(const Ptr& graycode, + const std::vector>& captured) +{ + CV_Assert(captured.size() == 2); + CV_Assert(!captured[0].empty()); + + const int projWidth = captured[0][0].cols; + const int projHeight = captured[0][0].rows; + + std::vector sumX(2), counts(2); + for (int k = 0; k < 2; ++k) + { + sumX[k] = Mat::zeros(projHeight, projWidth, CV_64F); + counts[k] = Mat::zeros(projHeight, projWidth, CV_32S); + } + + Mat projectorCoordinateMap(projHeight, projWidth, CV_32SC2, Vec2i(-1, -1)); + + for (int cam = 0; cam < 2; ++cam) + { + for (int y = 0; y < projHeight; ++y) + { + for (int x = 0; x < projWidth; ++x) + { + Point projPix; + bool error = graycode->getProjPixel(captured[cam], x, y, projPix); + if (!error && projPix.x >= 0 && projPix.x < projWidth && projPix.y >= 0 && projPix.y < projHeight) + { + sumX[cam].at(projPix.y, projPix.x) += x; + counts[cam].at(projPix.y, projPix.x) += 1; + if (cam == 0) + { + projectorCoordinateMap.at(y, x) = Vec2i(projPix.x, projPix.y); + } + } + } + } + } + + Mat avgX[2]; + for (int cam = 0; cam < 2; ++cam) + { + Mat cnt64; + counts[cam].convertTo(cnt64, CV_64F); + Mat zeroMask = cnt64 == 0; + cnt64.setTo(1.0, zeroMask); + cv::divide(sumX[cam], cnt64, avgX[cam]); + avgX[cam].setTo(0.0, zeroMask); + } + + Mat projectorDisparity = avgX[1] - avgX[0]; + Mat invalidMask = (counts[0] == 0) | (counts[1] == 0); + projectorDisparity.setTo(0.0, invalidMask); + + Mat expected(projHeight, projWidth, CV_64F, Scalar(0)); + for (int y = 0; y < projHeight; ++y) + { + for (int x = 0; x < projWidth; ++x) + { + const Vec2i proj = projectorCoordinateMap.at(y, x); + if (proj[0] >= 0) + { + expected.at(y, x) = projectorDisparity.at(proj[1], proj[0]); + } + } + } + + return expected; +} + +/****************************************************************************************\ +* Pattern generation test * +\****************************************************************************************/ +class CV_GrayPatternEncodingTest : public cvtest::BaseTest +{ +public: + void run(int) CV_OVERRIDE + { + GrayCodePattern::Params params; + params.width = 8; + params.height = 6; + Ptr graycode = GrayCodePattern::create(params); + + std::vector pattern; + ASSERT_TRUE(graycode->generate(pattern)); + + const size_t numColImgs = requiredBits(params.width); + const size_t numRowImgs = requiredBits(params.height); + EXPECT_EQ(pattern.size(), 2 * (numColImgs + numRowImgs)); + + Mat diff; + for (size_t bit = 0; bit < numColImgs; ++bit) + { + size_t idx1 = 2 * numColImgs - 2 * bit - 2; + size_t idx2 = idx1 + 1; + for (int col = 0; col < params.width; ++col) + { + uchar expected = static_cast((((col >> bit) & 1) ^ ((col >> (bit + 1)) & 1)) * 255); + cv::compare(pattern[idx1].col(col), Scalar(expected), diff, CMP_NE); + EXPECT_EQ(0, countNonZero(diff)); + cv::compare(pattern[idx2].col(col), Scalar(255 - expected), diff, CMP_NE); + EXPECT_EQ(0, countNonZero(diff)); + } + } + + const size_t baseIdx = 2 * numColImgs; + for (size_t bit = 0; bit < numRowImgs; ++bit) + { + size_t idx1 = baseIdx + 2 * numRowImgs - 2 * bit - 2; + size_t idx2 = idx1 + 1; + for (int row = 0; row < params.height; ++row) + { + uchar expected = static_cast((((row >> bit) & 1) ^ ((row >> (bit + 1)) & 1)) * 255); + cv::compare(pattern[idx1].row(row), Scalar(expected), diff, CMP_NE); + EXPECT_EQ(0, countNonZero(diff)); + cv::compare(pattern[idx2].row(row), Scalar(255 - expected), diff, CMP_NE); + EXPECT_EQ(0, countNonZero(diff)); + } + } + } +}; + +/****************************************************************************************\ +* Fast decode parity (8-bit) test * +\****************************************************************************************/ +class CV_GrayPatternFastDecode8uTest : public cvtest::BaseTest +{ +public: + void run(int) CV_OVERRIDE + { + GrayCodePattern::Params params; + params.width = 16; + params.height = 8; + Ptr graycode = GrayCodePattern::create(params); + + std::vector> captured; + std::vector blackImages, whiteImages; + buildSyntheticCapture(graycode, 3, 2, 5, captured, blackImages, whiteImages); + + Mat disparity; + ASSERT_TRUE(graycode->decode(captured, disparity, blackImages, whiteImages, DECODE_3D_UNDERWORLD)); + ASSERT_FALSE(disparity.empty()); + + Mat expected = computeLegacyDisparity(graycode, captured); + EXPECT_EQ(disparity.type(), CV_64F); + EXPECT_EQ(expected.type(), CV_64F); + EXPECT_EQ(disparity.size(), expected.size()); + double err = cv::norm(disparity, expected, NORM_INF); + EXPECT_LT(err, 1e-9); + } +}; + +/****************************************************************************************\ +* Fast decode parity (16-bit) test * +\****************************************************************************************/ +class CV_GrayPatternFastDecode16uTest : public cvtest::BaseTest +{ +public: + void run(int) CV_OVERRIDE + { + GrayCodePattern::Params params; + params.width = 16; + params.height = 8; + Ptr graycode = GrayCodePattern::create(params); + + std::vector> captured; + std::vector blackImages, whiteImages; + buildSyntheticCapture(graycode, 4, 1, 4, captured, blackImages, whiteImages); + convertTo16U(captured, blackImages, whiteImages); + + Mat disparity; + ASSERT_TRUE(graycode->decode(captured, disparity, blackImages, whiteImages, DECODE_3D_UNDERWORLD)); + ASSERT_FALSE(disparity.empty()); + + Mat expected = computeLegacyDisparity(graycode, captured); + double err = cv::norm(disparity, expected, NORM_INF); + EXPECT_LT(err, 1e-9); + } +}; + +/****************************************************************************************\ +* Test registration * +\****************************************************************************************/ + +TEST(GrayCodePattern, generates_gray_bitplanes) +{ + CV_GrayPatternEncodingTest test; + test.safe_run(); +} + +TEST(GrayCodePattern, fast_decode_matches_legacy_8u) +{ + CV_GrayPatternFastDecode8uTest test; + test.safe_run(); +} + +TEST(GrayCodePattern, fast_decode_matches_legacy_16u) +{ + CV_GrayPatternFastDecode16uTest test; + test.safe_run(); +} + +TEST(GrayCodePattern, shadow_mask_respects_16u_thresholds) +{ + GrayCodePattern::Params params; + params.width = 16; + params.height = 8; + Ptr graycode = GrayCodePattern::create(params); + + std::vector> captured; + std::vector unusedBlack, unusedWhite; + buildSyntheticCapture(graycode, 2, 0, 0, captured, unusedBlack, unusedWhite); + + std::vector blackImages(2), whiteImages(2); + for (int cam = 0; cam < 2; ++cam) + { + blackImages[cam] = Mat(params.height, params.width, CV_8U, Scalar(100)); + whiteImages[cam] = Mat(params.height, params.width, CV_8U, Scalar(140)); // 40 levels above black + } + + convertTo16U(captured, blackImages, whiteImages); + + graycode->setWhiteThreshold(static_cast(10 * 257)); + + graycode->setBlackThreshold(static_cast(50 * 257)); + Mat disparityMasked; + ASSERT_TRUE(graycode->decode(captured, disparityMasked, blackImages, whiteImages, DECODE_3D_UNDERWORLD)); + EXPECT_EQ(CV_64F, disparityMasked.type()); + EXPECT_EQ(0, countNonZero(disparityMasked)) << "Scaled threshold should mark all pixels as shadowed"; + + graycode->setBlackThreshold(50); // forgetting the scale makes the mask pass + Mat disparityUnmasked; + ASSERT_TRUE(graycode->decode(captured, disparityUnmasked, blackImages, whiteImages, DECODE_3D_UNDERWORLD)); + EXPECT_GT(countNonZero(disparityUnmasked), 0); +} + +}} // namespace diff --git a/modules/structured_light/test/test_plane.cpp b/modules/structured_light/test/test_plane.cpp index 8b757886670..82cc85ad46f 100644 --- a/modules/structured_light/test/test_plane.cpp +++ b/modules/structured_light/test/test_plane.cpp @@ -39,319 +39,13 @@ // //M*/ -#include "test_precomp.hpp" +#include "test_plane_common.hpp" namespace opencv_test { namespace { -const string STRUCTURED_LIGHT_DIR = "structured_light"; -const string FOLDER_DATA = "data"; - -/****************************************************************************************\ -* Plane test * - \****************************************************************************************/ -class CV_PlaneTest : public cvtest::BaseTest -{ - public: - CV_PlaneTest(); - ~CV_PlaneTest(); - - ////////////////////////////////////////////////////////////////////////////////////////////////// - // From rgbd module: since I needed the distance method of plane class, I copied the class from rgb module - // it will be made a pull request to make Plane class public - - /** Structure defining a plane. The notations are from the second paper */ - class PlaneBase - { - public: - PlaneBase(const Vec3f & m, const Vec3f &n_in, int index) : - index_(index), - n_(n_in), - m_sum_(Vec3f(0, 0, 0)), - m_(m), - Q_(Matx33f::zeros()), - mse_(0), - K_(0) - { - UpdateD(); - } - - virtual ~PlaneBase() - { - } - - /** Compute the distance to the plane. This will be implemented by the children to take into account different - * sensor models - * @param p_j - * @return - */ - virtual - float - distance(const Vec3f& p_j) const = 0; - - /** The d coefficient in the plane equation ax+by+cz+d = 0 - * @return - */ - inline float d() const - { - return d_; - } - - /** The normal to the plane - * @return the normal to the plane - */ - const Vec3f & - n() const - { - return n_; - } - - /** Update the different coefficients of the plane, based on the new statistics - */ - void UpdateParameters() - { - if( empty() ) - return; - m_ = m_sum_ / K_; - // Compute C - Matx33f C = Q_ - m_sum_ * m_.t(); - - // Compute n - SVD svd(C); - n_ = Vec3f(svd.vt.at(2, 0), svd.vt.at(2, 1), svd.vt.at(2, 2)); - mse_ = svd.w.at(2) / K_; - - UpdateD(); - } - - /** Update the different sum of point and sum of point*point.t() - */ - void UpdateStatistics(const Vec3f & point, const Matx33f & Q_local) - { - m_sum_ += point; - Q_ += Q_local; - ++K_; - } - - inline size_t empty() const - { - return K_ == 0; - } - - inline int K() const - { - return K_; - } - /** The index of the plane */ - int index_; - protected: - /** The 4th coefficient in the plane equation ax+by+cz+d = 0 */ - float d_; - /** Normal of the plane */ - Vec3f n_; - private: - inline void UpdateD() - { - // Hessian form (d = nc . p_plane (centroid here) + p) - //d = -1 * n.dot (xyz_centroid);//d =-axP+byP+czP - d_ = -m_.dot(n_); - } - /** The sum of the points */ - Vec3f m_sum_; - /** The mean of the points */ - Vec3f m_; - /** The sum of pi * pi^\top */ - Matx33f Q_; - /** The different matrices we need to update */ - Matx33f C_; - float mse_; - /** the number of points that form the plane */ - int K_; - }; - - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - - /** Basic planar child, with no sensor error model - */ - class Plane : public PlaneBase - { - public: - Plane(const Vec3f & m, const Vec3f &n_in, int index) : - PlaneBase(m, n_in, index) - { - } - - /** The computed distance is perfect in that case - * @param p_j the point to compute its distance to - * @return - */ - float distance(const Vec3f& p_j) const - { - return std::abs(float(p_j.dot(n_) + d_)); - } - }; - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - - protected: - void run( int ); - -}; - -CV_PlaneTest::CV_PlaneTest(){} - -CV_PlaneTest::~CV_PlaneTest(){} - -void CV_PlaneTest::run( int ) -{ - string folder = cvtest::TS::ptr()->get_data_path() + "/" + STRUCTURED_LIGHT_DIR + "/" + FOLDER_DATA + "/"; - structured_light::GrayCodePattern::Params params; - params.width = 1280; - params.height = 800; - // Set up GraycodePattern with params - Ptr graycode = structured_light::GrayCodePattern::create( params ); - size_t numberOfPatternImages = graycode->getNumberOfPatternImages(); - - - FileStorage fs( folder + "calibrationParameters.yml", FileStorage::READ ); - if( !fs.isOpened() ) - { - ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_TEST_DATA ); - } - - FileStorage fs2( folder + "gt_plane.yml", FileStorage::READ ); - if( !fs.isOpened() ) - { - ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_TEST_DATA ); - } - - // Loading ground truth plane parameters - Vec4f plane_coefficients; - Vec3f m; - fs2["plane_coefficients"] >> plane_coefficients; - fs2["m"] >> m; - - // Loading calibration parameters - Mat cam1intrinsics, cam1distCoeffs, cam2intrinsics, cam2distCoeffs, R, T; - - fs["cam1_intrinsics"] >> cam1intrinsics; - fs["cam2_intrinsics"] >> cam2intrinsics; - fs["cam1_distorsion"] >> cam1distCoeffs; - fs["cam2_distorsion"] >> cam2distCoeffs; - fs["R"] >> R; - fs["T"] >> T; - - // Loading white and black images - vector blackImages; - vector whiteImages; - - blackImages.resize( 2 ); - whiteImages.resize( 2 ); - - whiteImages[0] = imread( folder + "pattern_cam1_im43.jpg", 0 ); - whiteImages[1] = imread( folder + "pattern_cam2_im43.jpg", 0 ); - blackImages[0] = imread( folder + "pattern_cam1_im44.jpg", 0 ); - blackImages[1] = imread( folder + "pattern_cam2_im44.jpg", 0 ); - - Size imagesSize = whiteImages[0].size(); - - if( ( !cam1intrinsics.data ) || ( !cam2intrinsics.data ) || ( !cam1distCoeffs.data ) || ( !cam2distCoeffs.data ) || ( !R.data ) - || ( !T.data ) || ( !whiteImages[0].data ) || ( !whiteImages[1].data ) || ( !blackImages[0].data ) - || ( !blackImages[1].data ) ) - { - ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_TEST_DATA ); - } - - // Computing stereo rectify parameters - Mat R1, R2, P1, P2, Q; - Rect validRoi[2]; - stereoRectify( cam1intrinsics, cam1distCoeffs, cam2intrinsics, cam2distCoeffs, imagesSize, R, T, R1, R2, P1, P2, Q, 0, - -1, imagesSize, &validRoi[0], &validRoi[1] ); - - Mat map1x, map1y, map2x, map2y; - initUndistortRectifyMap( cam1intrinsics, cam1distCoeffs, R1, P1, imagesSize, CV_32FC1, map1x, map1y ); - initUndistortRectifyMap( cam2intrinsics, cam2distCoeffs, R2, P2, imagesSize, CV_32FC1, map2x, map2y ); - - vector > captured_pattern; - captured_pattern.resize( 2 ); - captured_pattern[0].resize( numberOfPatternImages ); - captured_pattern[1].resize( numberOfPatternImages ); - - // Loading and rectifying pattern images - for( size_t i = 0; i < numberOfPatternImages; i++ ) - { - std::ostringstream name1; - name1 << "pattern_cam1_im" << i + 1 << ".jpg"; - captured_pattern[0][i] = imread( folder + name1.str(), 0 ); - std::ostringstream name2; - name2 << "pattern_cam2_im" << i + 1 << ".jpg"; - captured_pattern[1][i] = imread( folder + name2.str(), 0 ); - - if( (!captured_pattern[0][i].data) || (!captured_pattern[1][i].data) ) - { - ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_TEST_DATA ); - } - - remap( captured_pattern[0][i], captured_pattern[0][i], map2x, map2y, INTER_NEAREST, BORDER_CONSTANT, Scalar() ); - remap( captured_pattern[1][i], captured_pattern[1][i], map1x, map1y, INTER_NEAREST, BORDER_CONSTANT, Scalar() ); - } - - // Rectifying white and black images - remap( whiteImages[0], whiteImages[0], map2x, map2y, INTER_NEAREST, BORDER_CONSTANT, Scalar() ); - remap( whiteImages[1], whiteImages[1], map1x, map1y, INTER_NEAREST, BORDER_CONSTANT, Scalar() ); - - remap( blackImages[0], blackImages[0], map2x, map2y, INTER_NEAREST, BORDER_CONSTANT, Scalar() ); - remap( blackImages[1], blackImages[1], map1x, map1y, INTER_NEAREST, BORDER_CONSTANT, Scalar() ); - - // Setting up threshold parameters to reconstruct only the plane in foreground - graycode->setBlackThreshold( 55 ); - graycode->setWhiteThreshold( 10 ); - - // Computing the disparity map - Mat disparityMap; - bool decoded = graycode->decode( captured_pattern, disparityMap, blackImages, whiteImages, - structured_light::DECODE_3D_UNDERWORLD ); - EXPECT_TRUE( decoded ); - - // Computing the point cloud - Mat pointcloud; - disparityMap.convertTo( disparityMap, CV_32FC1 ); - reprojectImageTo3D( disparityMap, pointcloud, Q, true, -1 ); - // from mm (unit of calibration) to m - pointcloud = pointcloud / 1000; - - // Setting up plane with ground truth plane values - Vec3f normal( plane_coefficients.val[0], plane_coefficients.val[1], plane_coefficients.val[2] ); - Ptr plane = Ptr( new Plane( m, normal, 0 ) ); - - // Computing the distance of every point of the pointcloud from ground truth plane - float sum_d = 0; - int cont = 0; - for( int i = 0; i < disparityMap.rows; i++ ) - { - for( int j = 0; j < disparityMap.cols; j++ ) - { - float value = disparityMap.at( i, j ); - if( value != 0 ) - { - Vec3f point = pointcloud.at( i, j ); - sum_d += plane->distance( point ); - cont++; - } - } - } - - sum_d /= cont; - - // test pass if the mean of points distance from ground truth plane is lower than 3 mm - EXPECT_LE( sum_d, 0.003 ); -} - -/****************************************************************************************\ -* Test registration * - \****************************************************************************************/ - TEST( GrayCodePattern, plane_reconstruction ) { - CV_PlaneTest test; + CV_PlaneTest test; test.safe_run(); } diff --git a/modules/structured_light/test/test_plane_16bit.cpp b/modules/structured_light/test/test_plane_16bit.cpp new file mode 100644 index 00000000000..27dc9d2707f --- /dev/null +++ b/modules/structured_light/test/test_plane_16bit.cpp @@ -0,0 +1,52 @@ +/*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) 2015, 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_plane_common.hpp" + +namespace opencv_test { namespace { + +TEST( GrayCodePattern, plane_reconstruction_16bit ) +{ + CV_PlaneTest test; + test.safe_run(); +} + +}} // namespace diff --git a/modules/structured_light/test/test_plane_common.hpp b/modules/structured_light/test/test_plane_common.hpp new file mode 100644 index 00000000000..11ecf87ea27 --- /dev/null +++ b/modules/structured_light/test/test_plane_common.hpp @@ -0,0 +1,284 @@ +// 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_TEST_PLANE_COMMON_HPP__ +#define __OPENCV_TEST_PLANE_COMMON_HPP__ + +#include "test_precomp.hpp" + +#include +#include + +namespace opencv_test { namespace { + +const string STRUCTURED_LIGHT_DIR = "structured_light"; +const string FOLDER_DATA = "data"; + +class PlaneBase +{ + public: + PlaneBase(const Vec3f & m, const Vec3f &n_in, int index) : + index_(index), + n_(n_in), + m_sum_(Vec3f(0, 0, 0)), + m_(m), + Q_(Matx33f::zeros()), + mse_(0), + K_(0) + { + UpdateD(); + } + + virtual ~PlaneBase() + { + } + + virtual float distance(const Vec3f& p_j) const = 0; + + inline float d() const + { + return d_; + } + + const Vec3f & + n() const + { + return n_; + } + + void UpdateParameters() + { + if( empty() ) + return; + m_ = m_sum_ / K_; + Matx33f C = Q_ - m_sum_ * m_.t(); + SVD svd(C); + n_ = Vec3f(svd.vt.at(2, 0), svd.vt.at(2, 1), svd.vt.at(2, 2)); + mse_ = svd.w.at(2) / K_; + UpdateD(); + } + + void UpdateStatistics(const Vec3f & point, const Matx33f & Q_local) + { + m_sum_ += point; + Q_ += Q_local; + ++K_; + } + + inline size_t empty() const + { + return K_ == 0; + } + + inline int K() const + { + return K_; + } + int index_; + protected: + float d_; + Vec3f n_; + private: + inline void UpdateD() + { + d_ = -m_.dot(n_); + } + Vec3f m_sum_; + Vec3f m_; + Matx33f Q_; + Matx33f C_; + float mse_; + int K_; +}; + +class Plane : public PlaneBase +{ + public: + Plane(const Vec3f & m, const Vec3f &n_in, int index) : + PlaneBase(m, n_in, index) + { + } + + float distance(const Vec3f& p_j) const CV_OVERRIDE + { + return std::abs(float(p_j.dot(n_) + d_)); + } +}; + +template +class CV_PlaneTest : public cvtest::BaseTest +{ + public: + CV_PlaneTest() = default; + ~CV_PlaneTest() CV_OVERRIDE = default; + + protected: + void run( int ) CV_OVERRIDE + { + string folder = cvtest::TS::ptr()->get_data_path() + "/" + STRUCTURED_LIGHT_DIR + "/" + FOLDER_DATA + "/"; + structured_light::GrayCodePattern::Params params; + params.width = 1280; + params.height = 800; + Ptr graycode = structured_light::GrayCodePattern::create( params ); + size_t numberOfPatternImages = graycode->getNumberOfPatternImages(); + + FileStorage fs( folder + "calibrationParameters.yml", FileStorage::READ ); + if( !fs.isOpened() ) + { + ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_TEST_DATA ); + return; + } + + FileStorage fs2( folder + "gt_plane.yml", FileStorage::READ ); + if( !fs2.isOpened() ) + { + ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_TEST_DATA ); + return; + } + + Vec4f plane_coefficients; + Vec3f m; + fs2["plane_coefficients"] >> plane_coefficients; + fs2["m"] >> m; + + Mat cam1intrinsics, cam1distCoeffs, cam2intrinsics, cam2distCoeffs, R, T; + + fs["cam1_intrinsics"] >> cam1intrinsics; + fs["cam2_intrinsics"] >> cam2intrinsics; + fs["cam1_distorsion"] >> cam1distCoeffs; + fs["cam2_distorsion"] >> cam2distCoeffs; + fs["R"] >> R; + fs["T"] >> T; + + vector blackImages(2); + vector whiteImages(2); + + whiteImages[0] = imread( folder + "pattern_cam1_im43.jpg", IMREAD_GRAYSCALE ); + whiteImages[1] = imread( folder + "pattern_cam2_im43.jpg", IMREAD_GRAYSCALE ); + blackImages[0] = imread( folder + "pattern_cam1_im44.jpg", IMREAD_GRAYSCALE ); + blackImages[1] = imread( folder + "pattern_cam2_im44.jpg", IMREAD_GRAYSCALE ); + + if( Use16Bit ) + { + for( int i = 0; i < 2; i++ ) + { + whiteImages[i].convertTo( whiteImages[i], CV_16U, 257.0 ); + blackImages[i].convertTo( blackImages[i], CV_16U, 257.0 ); + } + } + + Size imagesSize = whiteImages[0].size(); + + if( ( !cam1intrinsics.data ) || ( !cam2intrinsics.data ) || ( !cam1distCoeffs.data ) || ( !cam2distCoeffs.data ) || ( !R.data ) + || ( !T.data ) || ( !whiteImages[0].data ) || ( !whiteImages[1].data ) || ( !blackImages[0].data ) + || ( !blackImages[1].data ) ) + { + ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_TEST_DATA ); + return; + } + + Mat R1, R2, P1, P2, Q; + Rect validRoi[2]; + stereoRectify( cam1intrinsics, cam1distCoeffs, cam2intrinsics, cam2distCoeffs, imagesSize, R, T, R1, R2, P1, P2, Q, 0, + -1, imagesSize, &validRoi[0], &validRoi[1] ); + + Mat map1x, map1y, map2x, map2y; + initUndistortRectifyMap( cam1intrinsics, cam1distCoeffs, R1, P1, imagesSize, CV_32FC1, map1x, map1y ); + initUndistortRectifyMap( cam2intrinsics, cam2distCoeffs, R2, P2, imagesSize, CV_32FC1, map2x, map2y ); + + vector > captured_pattern( 2, std::vector( numberOfPatternImages ) ); + + for( size_t i = 0; i < numberOfPatternImages; i++ ) + { + std::ostringstream name1; + name1 << "pattern_cam1_im" << i + 1 << ".jpg"; + captured_pattern[0][i] = imread( folder + name1.str(), IMREAD_GRAYSCALE ); + std::ostringstream name2; + name2 << "pattern_cam2_im" << i + 1 << ".jpg"; + captured_pattern[1][i] = imread( folder + name2.str(), IMREAD_GRAYSCALE ); + + if( Use16Bit ) + { + captured_pattern[0][i].convertTo( captured_pattern[0][i], CV_16U, 257.0 ); + captured_pattern[1][i].convertTo( captured_pattern[1][i], CV_16U, 257.0 ); + } + + if( (!captured_pattern[0][i].data) || (!captured_pattern[1][i].data) ) + { + ts->set_failed_test_info( cvtest::TS::FAIL_INVALID_TEST_DATA ); + return; + } + + remap( captured_pattern[0][i], captured_pattern[0][i], map2x, map2y, INTER_NEAREST, BORDER_CONSTANT, Scalar() ); + remap( captured_pattern[1][i], captured_pattern[1][i], map1x, map1y, INTER_NEAREST, BORDER_CONSTANT, Scalar() ); + } + + remap( whiteImages[0], whiteImages[0], map2x, map2y, INTER_NEAREST, BORDER_CONSTANT, Scalar() ); + remap( whiteImages[1], whiteImages[1], map1x, map1y, INTER_NEAREST, BORDER_CONSTANT, Scalar() ); + + remap( blackImages[0], blackImages[0], map2x, map2y, INTER_NEAREST, BORDER_CONSTANT, Scalar() ); + remap( blackImages[1], blackImages[1], map1x, map1y, INTER_NEAREST, BORDER_CONSTANT, Scalar() ); + + const double scale = Use16Bit ? 257.0 : 1.0; + const size_t blackThreshold = static_cast(55 * scale); + const size_t whiteThreshold = static_cast(10 * scale); + graycode->setBlackThreshold( blackThreshold ); + graycode->setWhiteThreshold( whiteThreshold ); + + for( int cam = 0; cam < 2; ++cam ) + { + Mat diffImage, mask; + absdiff( whiteImages[cam], blackImages[cam], diffImage ); + cv::compare( diffImage, Scalar( static_cast(blackThreshold) ), mask, CMP_GT ); + EXPECT_GT( countNonZero( mask ), 0 ) << "Shadow mask empty for camera " << cam; + } + + Mat disparityMap; + bool decoded = graycode->decode( captured_pattern, disparityMap, blackImages, whiteImages, + structured_light::DECODE_3D_UNDERWORLD ); + ASSERT_TRUE( decoded ); + ASSERT_FALSE( disparityMap.empty() ); + + double minVal = 0, maxVal = 0; + minMaxLoc( disparityMap, &minVal, &maxVal ); + EXPECT_LT( minVal, maxVal ); + + Mat pointcloud; + disparityMap.convertTo( disparityMap, CV_32FC1 ); + reprojectImageTo3D( disparityMap, pointcloud, Q, true, -1 ); + pointcloud = pointcloud / 1000; + + Vec3f normal( plane_coefficients.val[0], plane_coefficients.val[1], plane_coefficients.val[2] ); + Ptr plane = Ptr( new Plane( m, normal, 0 ) ); + + float sum_d = 0; + int cont = 0; + for( int i = 0; i < disparityMap.rows; i++ ) + { + for( int j = 0; j < disparityMap.cols; j++ ) + { + float disparityValue = disparityMap.at( i, j ); + if( disparityValue <= std::numeric_limits::epsilon() ) + continue; + + Vec3f point = pointcloud.at( i, j ); + + if( std::isfinite( point[0] ) && std::isfinite( point[1] ) && std::isfinite( point[2] ) && + std::abs( point[2] ) <= 10.0f ) + { + sum_d += plane->distance( point ); + cont++; + } + } + } + + ASSERT_GT( cont, 0 ); + float mean_distance = sum_d / cont; + + EXPECT_LE( mean_distance, 0.003f ); + } +}; + +}} // namespace + +#endif // __OPENCV_TEST_PLANE_COMMON_HPP__ \ No newline at end of file