mirror of
https://github.com/opencv/opencv_contrib.git
synced 2025-10-21 14:41:58 +08:00
Added structured_light module
This commit is contained in:
RobertaRavanelli
475
modules/structured_light/src/graycodepattern.cpp
Normal file
475
modules/structured_light/src/graycodepattern.cpp
Normal file
@@ -0,0 +1,475 @@
|
||||
/*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 "precomp.hpp"
|
||||
|
||||
namespace cv {
|
||||
namespace structured_light {
|
||||
class CV_EXPORTS_W GrayCodePattern_Impl : public GrayCodePattern
|
||||
{
|
||||
public:
|
||||
// Constructor
|
||||
explicit GrayCodePattern_Impl( const GrayCodePattern::Params ¶meters = GrayCodePattern::Params() );
|
||||
|
||||
// Destructor
|
||||
virtual ~GrayCodePattern_Impl(){};
|
||||
|
||||
// Generates the gray code pattern as a std::vector<Mat>
|
||||
bool generate( OutputArrayOfArrays patternImages );
|
||||
|
||||
// Decodes the gray code pattern, computing the disparity map
|
||||
bool decode( InputArrayOfArrays patternImages, OutputArray disparityMap, InputArrayOfArrays blackImages = noArray(),
|
||||
InputArrayOfArrays whiteImages = noArray(), int flags = DECODE_3D_UNDERWORLD ) const;
|
||||
|
||||
// Returns the number of pattern images for the graycode pattern
|
||||
size_t getNumberOfPatternImages() const;
|
||||
|
||||
// Sets the value for black threshold
|
||||
void setBlackThreshold( size_t val );
|
||||
|
||||
// Sets the value for set the value for white threshold
|
||||
void setWhiteThreshold( size_t val );
|
||||
|
||||
// Generates the images needed for shadowMasks computation
|
||||
void getImagesForShadowMasks( InputOutputArray blackImage, InputOutputArray whiteImage ) const;
|
||||
|
||||
// For a (x,y) pixel of the camera returns the corresponding projector pixel
|
||||
bool getProjPixel(InputArrayOfArrays patternImages, int x, int y, Point &projPix) const;
|
||||
|
||||
private:
|
||||
// Parameters
|
||||
Params params;
|
||||
|
||||
// The number of images of the pattern
|
||||
size_t numOfPatternImages;
|
||||
|
||||
// The number of row images of the pattern
|
||||
size_t numOfRowImgs;
|
||||
|
||||
// The number of column images of the pattern
|
||||
size_t numOfColImgs;
|
||||
|
||||
// Number between 0-255 that represents the minimum brightness difference
|
||||
// between the fully illuminated (white) and the non - illuminated images (black)
|
||||
int blackThreshold;
|
||||
|
||||
// Number between 0-255 that represents the minimum brightness difference
|
||||
// between the gray-code pattern and its inverse images
|
||||
int whiteThreshold;
|
||||
|
||||
// Computes the required number of pattern images, allocating the pattern vector
|
||||
void computeNumberOfPatternImages();
|
||||
|
||||
// Computes the shadows occlusion where we cannot reconstruct the model
|
||||
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<uchar>& gray ) const;
|
||||
};
|
||||
|
||||
/*
|
||||
* GrayCodePattern
|
||||
*/
|
||||
GrayCodePattern::Params::Params()
|
||||
{
|
||||
width = 1024;
|
||||
height = 768;
|
||||
}
|
||||
|
||||
GrayCodePattern_Impl::GrayCodePattern_Impl( const GrayCodePattern::Params ¶meters ) :
|
||||
params( parameters )
|
||||
{
|
||||
computeNumberOfPatternImages();
|
||||
blackThreshold = 40; // 3D_underworld default value
|
||||
whiteThreshold = 5; // 3D_underworld default value
|
||||
}
|
||||
|
||||
bool GrayCodePattern_Impl::generate( OutputArrayOfArrays pattern )
|
||||
{
|
||||
std::vector<Mat>& pattern_ = *( std::vector<Mat>* ) 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
|
||||
{
|
||||
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<uchar>( i, j ) = pixel_color;
|
||||
if( pixel_color > 0 )
|
||||
pixel_color = ( uchar ) 0;
|
||||
else
|
||||
pixel_color = ( uchar ) 255;
|
||||
pattern_[2 * numOfColImgs - 2 * k - 1].at<uchar>( i, j ) = pixel_color; // inverse
|
||||
}
|
||||
|
||||
prevRem = rem;
|
||||
}
|
||||
}
|
||||
|
||||
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<uchar>( 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<uchar>( i, j ) = pixel_color;
|
||||
}
|
||||
|
||||
prevRem = rem;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool GrayCodePattern_Impl::decode( InputArrayOfArrays patternImages, OutputArray disparityMap,
|
||||
InputArrayOfArrays blackImages, InputArrayOfArrays whitheImages, int flags ) const
|
||||
{
|
||||
std::vector<std::vector<Mat> >& acquired_pattern = *( std::vector<std::vector<Mat> >* ) patternImages.getObj();
|
||||
|
||||
if( flags == DECODE_3D_UNDERWORLD )
|
||||
{
|
||||
// Computing shadows mask
|
||||
std::vector<Mat> shadowMasks;
|
||||
computeShadowMasks( blackImages, whitheImages, shadowMasks );
|
||||
|
||||
size_t cam_width = acquired_pattern[0][0].cols;
|
||||
size_t 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<std::vector<std::vector<Point> > > camsPixels;
|
||||
camsPixels.resize( acquired_pattern.size() );
|
||||
|
||||
// TODO: parallelize for (k and j)
|
||||
for( size_t k = 0; k < acquired_pattern.size(); k++ )
|
||||
{
|
||||
camsPixels[k].resize( params.height * params.width );
|
||||
for( size_t i = 0; i < cam_width; i++ )
|
||||
{
|
||||
for( size_t j = 0; j < cam_height; j++ )
|
||||
{
|
||||
//if the pixel is not shadowed, reconstruct
|
||||
if( shadowMasks[k].at<uchar>( 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;
|
||||
}
|
||||
|
||||
camsPixels[k][projPixel.x * params.height + projPixel.y].push_back( Point( i, j ) );
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<Point> cam1Pixs, cam2Pixs;
|
||||
|
||||
Mat& disparityMap_ = *( Mat* ) disparityMap.getObj();
|
||||
disparityMap_ = Mat( cam_height, cam_width, CV_64F, double( 0 ) );
|
||||
|
||||
double number_of_pixels_cam1 = 0;
|
||||
double number_of_pixels_cam2 = 0;
|
||||
|
||||
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];
|
||||
|
||||
if( cam1Pixs.size() == 0 || cam2Pixs.size() == 0 )
|
||||
continue;
|
||||
|
||||
Point p1;
|
||||
Point p2;
|
||||
|
||||
double sump1x = 0;
|
||||
double sump2x = 0;
|
||||
|
||||
number_of_pixels_cam1 += cam1Pixs.size();
|
||||
number_of_pixels_cam2 += cam2Pixs.size();
|
||||
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;
|
||||
}
|
||||
|
||||
sump2x /= cam2Pixs.size();
|
||||
sump1x /= cam1Pixs.size();
|
||||
for( int c1 = 0; c1 < (int) cam1Pixs.size(); c1++ )
|
||||
{
|
||||
p1 = cam1Pixs[c1];
|
||||
disparityMap_.at<double>( p1.y, p1.x ) = ( double ) (sump2x - sump1x);
|
||||
}
|
||||
|
||||
sump2x = 0;
|
||||
sump1x = 0;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
} // end if flags
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
// Computes the required number of pattern images
|
||||
void GrayCodePattern_Impl::computeNumberOfPatternImages()
|
||||
{
|
||||
numOfColImgs = ( size_t ) ceil( log( double( params.width ) ) / log( 2.0 ) );
|
||||
numOfRowImgs = ( size_t ) ceil( log( double( params.height ) ) / log( 2.0 ) );
|
||||
numOfPatternImages = 2 * numOfColImgs + 2 * numOfRowImgs;
|
||||
}
|
||||
|
||||
// Returns the number of pattern images to project / decode
|
||||
size_t GrayCodePattern_Impl::getNumberOfPatternImages() const
|
||||
{
|
||||
return numOfPatternImages;
|
||||
}
|
||||
|
||||
// Computes the shadows occlusion where we cannot reconstruct the model
|
||||
void GrayCodePattern_Impl::computeShadowMasks( InputArrayOfArrays blackImages, InputArrayOfArrays whiteImages,
|
||||
OutputArrayOfArrays shadowMasks ) const
|
||||
{
|
||||
std::vector<Mat>& whiteImages_ = *( std::vector<Mat>* ) whiteImages.getObj();
|
||||
std::vector<Mat>& blackImages_ = *( std::vector<Mat>* ) blackImages.getObj();
|
||||
std::vector<Mat>& shadowMasks_ = *( std::vector<Mat>* ) shadowMasks.getObj();
|
||||
|
||||
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++ )
|
||||
{
|
||||
shadowMasks_[k] = Mat( cam_height, cam_width, CV_8U );
|
||||
for( int i = 0; i < cam_width; i++ )
|
||||
{
|
||||
for( int j = 0; j < cam_height; j++ )
|
||||
{
|
||||
uchar white = whiteImages_[k].at<uchar>( Point( i, j ) );
|
||||
uchar black = blackImages_[k].at<uchar>( Point( i, j ) );
|
||||
|
||||
if( white - black > blackThreshold )
|
||||
{
|
||||
shadowMasks_[k].at<uchar>( Point( i, j ) ) = ( uchar ) 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
shadowMasks_[k].at<uchar>( Point( i, j ) ) = ( uchar ) 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Generates the images needed for shadowMasks computation
|
||||
void GrayCodePattern_Impl::getImagesForShadowMasks( InputOutputArray blackImage, InputOutputArray whiteImage ) const
|
||||
{
|
||||
Mat& blackImage_ = *( Mat* ) blackImage.getObj();
|
||||
Mat& whiteImage_ = *( Mat* ) whiteImage.getObj();
|
||||
|
||||
blackImage_ = Mat( params.height, params.width, CV_8UC3, Scalar( 0, 0, 0 ) );
|
||||
whiteImage_ = Mat( params.height, params.width, CV_8UC3, Scalar( 255, 255, 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
|
||||
{
|
||||
std::vector<Mat>& _patternImages = *( std::vector<Mat>* ) patternImages.getObj();
|
||||
std::vector<uchar> grayCol;
|
||||
std::vector<uchar> grayRow;
|
||||
|
||||
bool error = false;
|
||||
int xDec, yDec;
|
||||
|
||||
// process column images
|
||||
for( size_t count = 0; count < numOfColImgs; count++ )
|
||||
{
|
||||
// get pixel intensity for regular pattern projection and its inverse
|
||||
double val1 = _patternImages[count * 2].at<uchar>( Point( x, y ) );
|
||||
double val2 = _patternImages[count * 2 + 1].at<uchar>( Point( x, y ) );
|
||||
|
||||
// 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;
|
||||
|
||||
// determine if projection pixel is on or off
|
||||
if( val1 > val2 )
|
||||
grayCol.push_back( 1 );
|
||||
else
|
||||
grayCol.push_back( 0 );
|
||||
}
|
||||
|
||||
xDec = grayToDec( grayCol );
|
||||
|
||||
// process row images
|
||||
for( size_t count = 0; count < numOfRowImgs; count++ )
|
||||
{
|
||||
// get pixel intensity for regular pattern projection and its inverse
|
||||
double val1 = _patternImages[count * 2 + numOfColImgs * 2].at<uchar>( Point( x, y ) );
|
||||
double val2 = _patternImages[count * 2 + numOfColImgs * 2 + 1].at<uchar>( Point( x, y ) );
|
||||
|
||||
// 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;
|
||||
|
||||
// determine if projection pixel is on or off
|
||||
if( val1 > val2 )
|
||||
grayRow.push_back( 1 );
|
||||
else
|
||||
grayRow.push_back( 0 );
|
||||
}
|
||||
|
||||
yDec = grayToDec( grayRow );
|
||||
|
||||
if( (yDec >= params.height || xDec >= params.width) )
|
||||
{
|
||||
error = 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<uchar>& 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;
|
||||
}
|
||||
|
||||
// Sets the value for black threshold
|
||||
void GrayCodePattern_Impl::setBlackThreshold( size_t val )
|
||||
{
|
||||
blackThreshold = val;
|
||||
}
|
||||
|
||||
// Sets the value for white threshold
|
||||
void GrayCodePattern_Impl::setWhiteThreshold( size_t val )
|
||||
{
|
||||
whiteThreshold = val;
|
||||
}
|
||||
|
||||
// Creates the GrayCodePattern instance
|
||||
Ptr<GrayCodePattern> GrayCodePattern::create( const GrayCodePattern::Params& params )
|
||||
{
|
||||
return makePtr<GrayCodePattern_Impl>( params );
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user