From 441c955933b7da5f12febf0837040803a32c667c Mon Sep 17 00:00:00 2001 From: Alessio Rosiello Date: Tue, 24 Jun 2025 17:28:06 +0200 Subject: [PATCH 1/3] structured_light: GreyCodePattern 16-bit support and performance improvements - Vastly improve performance of the GreyCodePattern structured light algorithm by leveraging built-in OpenCV functions and parallel code instead of raw loops, at the cost of a bit more memory (decoding a set of 96 5320x4600 images acquired using stereo cameras goes from taking ~100 seconds to taking ~2.5 seconds, in release mode) - Automatically gain the ability to decode 16-bit images (switching from raw loops and uchar-indexing to OpenCV functions gives this for free) Manual testing was done to ensure that the calculated results are correct, both for generation of the patterns and for decoding, in both Debug and Release mode, using real world data. --- .../structured_light/src/graycodepattern.cpp | 512 ++++++++++-------- 1 file changed, 274 insertions(+), 238 deletions(-) diff --git a/modules/structured_light/src/graycodepattern.cpp b/modules/structured_light/src/graycodepattern.cpp index f37bc3bb526..17245dfdf23 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,152 @@ 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 j = range.start; j < range.end; ++j) + { + for (size_t k = 0; k < numOfColImgs; ++k) + { + uchar flag = (((j >> k) & 1) ^ ((j >> (k + 1)) & 1)); + uchar pixel_color = flag * 255; + + size_t idx1 = 2 * numOfColImgs - 2 * k - 2; + size_t idx2 = 2 * numOfColImgs - 2 * k - 1; + + if (idx1 < numOfPatternImages) + { + pattern_[idx1].col(j).setTo(pixel_color); + } + if (idx2 < numOfPatternImages) + { + pattern_[idx2].col(j).setTo(255 - pixel_color); + } + } + } + }); + parallel_for_(Range(0, params.height), [&](const Range& range) { + for (int i = range.start; i < range.end; ++i) + { + for (size_t k = 0; k < numOfRowImgs; ++k) + { + uchar flag = (((i >> k) & 1) ^ ((i >> (k + 1)) & 1)); + uchar pixel_color = flag * 255; + + size_t base_idx = 2 * numOfColImgs; + size_t idx1 = base_idx + 2 * numOfRowImgs - 2 * k - 2; + size_t idx2 = base_idx + 2 * numOfRowImgs - 2 * k - 1; + + if (idx1 < numOfPatternImages) + { + pattern_[idx1].row(i).setTo(pixel_color); + } + if (idx2 < numOfPatternImages) + { + pattern_[idx2].row(i).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 +287,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 +297,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 +319,169 @@ 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); + } + }); + } +} + +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 From 49ecc247e39afe501f24b242130db8b6013488f9 Mon Sep 17 00:00:00 2001 From: Alessio <53220763+Atari2@users.noreply.github.com> Date: Tue, 25 Nov 2025 09:34:20 +0100 Subject: [PATCH 2/3] structured_light: Document bit-level math and clarify obscure variable names --- .../structured_light/src/graycodepattern.cpp | 40 ++++++++++++------- 1 file changed, 26 insertions(+), 14 deletions(-) diff --git a/modules/structured_light/src/graycodepattern.cpp b/modules/structured_light/src/graycodepattern.cpp index 17245dfdf23..c79d41c822e 100644 --- a/modules/structured_light/src/graycodepattern.cpp +++ b/modules/structured_light/src/graycodepattern.cpp @@ -132,46 +132,55 @@ bool GrayCodePattern_Impl::generate( OutputArrayOfArrays pattern ) pattern_[i].create(params.height, params.width, CV_8U); } parallel_for_(Range(0, params.width), [&](const Range& range) { - for (int j = range.start; j < range.end; ++j) + for (int col = range.start; col < range.end; ++col) { - for (size_t k = 0; k < numOfColImgs; ++k) + for (size_t nBits = 0; nBits < numOfColImgs; ++nBits) { - uchar flag = (((j >> k) & 1) ^ ((j >> (k + 1)) & 1)); + // 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 * k - 2; - size_t idx2 = 2 * numOfColImgs - 2 * k - 1; + size_t idx1 = 2 * numOfColImgs - 2 * nBits - 2; + size_t idx2 = 2 * numOfColImgs - 2 * nBits - 1; if (idx1 < numOfPatternImages) { - pattern_[idx1].col(j).setTo(pixel_color); + pattern_[idx1].col(col).setTo(pixel_color); } if (idx2 < numOfPatternImages) { - pattern_[idx2].col(j).setTo(255 - pixel_color); + pattern_[idx2].col(col).setTo(255 - pixel_color); } } } }); parallel_for_(Range(0, params.height), [&](const Range& range) { - for (int i = range.start; i < range.end; ++i) + for (int row = range.start; row < range.end; ++row) { - for (size_t k = 0; k < numOfRowImgs; ++k) + for (size_t nBits = 0; nBits < numOfRowImgs; ++nBits) { - uchar flag = (((i >> k) & 1) ^ ((i >> (k + 1)) & 1)); + // 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 * k - 2; - size_t idx2 = base_idx + 2 * numOfRowImgs - 2 * k - 1; + 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(i).setTo(pixel_color); + pattern_[idx1].row(row).setTo(pixel_color); } if (idx2 < numOfPatternImages) { - pattern_[idx2].row(i).setTo(255 - pixel_color); + pattern_[idx2].row(row).setTo(255 - pixel_color); } } } @@ -353,6 +362,9 @@ void GrayCodePattern_Impl::populateFastPatternImages(const std::vector& gray) { if (gray.empty()) From d1ec904ca1d90bfd18671ce1455e602a2444b7e0 Mon Sep 17 00:00:00 2001 From: Alessio <53220763+Atari2@users.noreply.github.com> Date: Tue, 25 Nov 2025 10:34:31 +0100 Subject: [PATCH 3/3] structured_light: Add tests for 16-bit features and performance improvements regression checks Created shared plane test logic into test_plane_common.hpp and updated test_plane.cpp and test_plane_16bit.cpp to use the new common template. Created some tests for 16u support and graycode pattern verification in test_graycodepattern_fast.cpp --- .../test/test_graycodepattern_fast.cpp | 396 ++++++++++++++++++ modules/structured_light/test/test_plane.cpp | 310 +------------- .../test/test_plane_16bit.cpp | 52 +++ .../test/test_plane_common.hpp | 284 +++++++++++++ 4 files changed, 734 insertions(+), 308 deletions(-) create mode 100644 modules/structured_light/test/test_graycodepattern_fast.cpp create mode 100644 modules/structured_light/test/test_plane_16bit.cpp create mode 100644 modules/structured_light/test/test_plane_common.hpp 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