vendor: OpenCV 5.0.0 snapshot at 755e50675d97db9b7d449d8bd6b09888646f6c6e
This commit is contained in:
Gitea Mirror Bot
@@ -0,0 +1,480 @@
|
||||
/*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 CV_FINAL : public GrayCodePattern
|
||||
{
|
||||
public:
|
||||
// Constructor
|
||||
explicit GrayCodePattern_Impl( const GrayCodePattern::Params ¶meters = GrayCodePattern::Params() );
|
||||
|
||||
// Destructor
|
||||
virtual ~GrayCodePattern_Impl() CV_OVERRIDE {};
|
||||
|
||||
// Generates the gray code pattern as a std::vector<Mat>
|
||||
bool generate( OutputArrayOfArrays patternImages ) CV_OVERRIDE;
|
||||
|
||||
// Decodes the gray code pattern, computing the disparity map
|
||||
bool decode( const std::vector< std::vector<Mat> >& patternImages, OutputArray disparityMap, InputArrayOfArrays blackImages = noArray(),
|
||||
InputArrayOfArrays whiteImages = noArray(), int flags = DECODE_3D_UNDERWORLD ) const CV_OVERRIDE;
|
||||
|
||||
// Returns the number of pattern images for the graycode pattern
|
||||
size_t getNumberOfPatternImages() const CV_OVERRIDE;
|
||||
|
||||
// Sets the value for black threshold
|
||||
void setBlackThreshold( size_t val ) CV_OVERRIDE;
|
||||
|
||||
// Sets the value for set the value for white threshold
|
||||
void setWhiteThreshold( size_t val ) CV_OVERRIDE;
|
||||
|
||||
// Generates the images needed for shadowMasks computation
|
||||
void getImagesForShadowMasks( InputOutputArray blackImage, InputOutputArray whiteImage ) const CV_OVERRIDE;
|
||||
|
||||
// For a (x,y) pixel of the camera returns the corresponding projector pixel
|
||||
bool getProjPixel(InputArrayOfArrays patternImages, int x, int y, CV_OUT Point &projPix) const CV_OVERRIDE;
|
||||
|
||||
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)
|
||||
size_t blackThreshold;
|
||||
|
||||
// Number between 0-255 that represents the minimum brightness difference
|
||||
// between the gray-code pattern and its inverse images
|
||||
size_t 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( const std::vector< std::vector<Mat> >& patternImages, OutputArray disparityMap,
|
||||
InputArrayOfArrays blackImages, InputArrayOfArrays whitheImages, int flags ) const
|
||||
{
|
||||
const std::vector<std::vector<Mat> >& acquired_pattern = patternImages;
|
||||
|
||||
if( flags == DECODE_3D_UNDERWORLD )
|
||||
{
|
||||
// Computing shadows mask
|
||||
std::vector<Mat> 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<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( 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<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 ) );
|
||||
|
||||
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;
|
||||
|
||||
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++ )
|
||||
{
|
||||
double white = whiteImages_[k].at<uchar>( Point( i, j ) );
|
||||
double black = blackImages_[k].at<uchar>( Point( i, j ) );
|
||||
|
||||
if( abs(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_8U, Scalar( 0 ) );
|
||||
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
|
||||
{
|
||||
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 );
|
||||
}
|
||||
|
||||
// Creates the GrayCodePattern instance
|
||||
// alias for scripting
|
||||
Ptr<GrayCodePattern> GrayCodePattern::create( int width, int height )
|
||||
{
|
||||
Params params;
|
||||
params.width = width;
|
||||
params.height = height;
|
||||
return makePtr<GrayCodePattern_Impl>( params );
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,49 @@
|
||||
/*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*/
|
||||
|
||||
#ifndef __OPENCV_PRECOMP_H__
|
||||
#define __OPENCV_PRECOMP_H__
|
||||
|
||||
#include "opencv2/structured_light.hpp"
|
||||
#include "opencv2/core/utility.hpp"
|
||||
#include "opencv2/core/private.hpp"
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,919 @@
|
||||
/*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 SinusoidalPatternProfilometry_Impl CV_FINAL : public SinusoidalPattern
|
||||
{
|
||||
public:
|
||||
// Constructor
|
||||
explicit SinusoidalPatternProfilometry_Impl( const SinusoidalPattern::Params ¶meters =
|
||||
SinusoidalPattern::Params() );
|
||||
// Destructor
|
||||
virtual ~SinusoidalPatternProfilometry_Impl() CV_OVERRIDE {};
|
||||
|
||||
// Generate sinusoidal patterns
|
||||
bool generate( OutputArrayOfArrays patternImages ) CV_OVERRIDE;
|
||||
|
||||
bool decode( const std::vector< std::vector<Mat> >& patternImages, OutputArray disparityMap,
|
||||
InputArrayOfArrays blackImages = noArray(), InputArrayOfArrays whiteImages =
|
||||
noArray(), int flags = 0 ) const CV_OVERRIDE;
|
||||
|
||||
// Compute a wrapped phase map from the sinusoidal patterns
|
||||
void computePhaseMap( InputArrayOfArrays patternImages, OutputArray wrappedPhaseMap,
|
||||
OutputArray shadowMask = noArray(), InputArray fundamental = noArray()) CV_OVERRIDE;
|
||||
// Unwrap the wrapped phase map to retrieve correspondences
|
||||
void unwrapPhaseMap( InputArray wrappedPhaseMap,
|
||||
OutputArray unwrappedPhaseMap,
|
||||
cv::Size camSize,
|
||||
InputArray shadowMask = noArray() ) CV_OVERRIDE;
|
||||
// Find correspondences between the devices
|
||||
void findProCamMatches( InputArray projUnwrappedPhaseMap, InputArray camUnwrappedPhaseMap,
|
||||
OutputArrayOfArrays matches ) CV_OVERRIDE;
|
||||
|
||||
void computeDataModulationTerm( InputArrayOfArrays patternImages,
|
||||
OutputArray dataModulationTerm,
|
||||
InputArray shadowMask ) CV_OVERRIDE;
|
||||
|
||||
private:
|
||||
// Compute The Fourier transform of a pattern. Output is complex. Taken from the DFT example in OpenCV
|
||||
void computeDft( InputArray patternImage, OutputArray FourierTransform );
|
||||
// Compute the inverse Fourier transform. Output can be complex or real
|
||||
void computeInverseDft( InputArray FourierTransform, OutputArray inverseFourierTransform,
|
||||
bool realOutput );
|
||||
// Compute the DFT magnitude which is used to find maxima in the spectrum
|
||||
void computeDftMagnitude( InputArray FourierTransform, OutputArray FourierTransformMagnitude );
|
||||
// Compute phase map from the complex signal given by non-symmetrical filtering of DFT
|
||||
void computeFtPhaseMap( InputArray inverseFourierTransform,
|
||||
InputArray shadowMask,
|
||||
OutputArray wrappedPhaseMap );
|
||||
// Swap DFT quadrants. Come from opencv example
|
||||
void swapQuadrants( InputOutputArray image, int centerX, int centerY );
|
||||
// Filter (non)-symmetrically the DFT.
|
||||
void frequencyFiltering( InputOutputArray FourierTransform, int centerX1, int centerY1,
|
||||
int halfRegionWidth, int halfRegionHeight, bool keepInsideRegion,
|
||||
int centerX2 = -1, int centerY2 = -1 );
|
||||
// Find maxima in the spectrum so that we know how it should be filtered
|
||||
bool findMaxInHalvesTransform( InputArray FourierTransformMag, Point &maxPosition1,
|
||||
Point &maxPosition2 );
|
||||
// Compute phase map from the three sinusoidal patterns
|
||||
void computePsPhaseMap( InputArrayOfArrays patternImages,
|
||||
InputArray shadowMask,
|
||||
OutputArray wrappedPhaseMap );
|
||||
|
||||
void computeFapsPhaseMap( InputArray a, InputArray b, InputArray theta1, InputArray theta2,
|
||||
InputArray shadowMask, OutputArray wrappedPhaseMap );
|
||||
// Compute a shadow mask to discard shadow regions
|
||||
void computeShadowMask( InputArrayOfArrays patternImages, OutputArray shadowMask );
|
||||
// Data modulation term is used to isolate cross markers
|
||||
|
||||
void extractMarkersLocation( InputArray dataModulationTerm,
|
||||
std::vector<Point> &markersLocation );
|
||||
|
||||
void convertToAbsolutePhaseMap( InputArrayOfArrays camPatterns,
|
||||
InputArray unwrappedProjPhaseMap,
|
||||
InputArray unwrappedCamPhaseMap,
|
||||
InputArray shadowMask,
|
||||
InputArray fundamentalMatrix );
|
||||
|
||||
Params params;
|
||||
phase_unwrapping::HistogramPhaseUnwrapping::Params unwrappingParams;
|
||||
// Class describing markers that are added to the patterns
|
||||
class Marker{
|
||||
private:
|
||||
Point center, up, right, left, down;
|
||||
public:
|
||||
Marker();
|
||||
Marker( Point c );
|
||||
void drawMarker( OutputArray pattern );
|
||||
};
|
||||
};
|
||||
// Default parameters value
|
||||
SinusoidalPattern::Params::Params()
|
||||
{
|
||||
width = 800;
|
||||
height = 600;
|
||||
nbrOfPeriods = 20;
|
||||
shiftValue = (float)(2 * CV_PI / 3);
|
||||
methodId = FAPS;
|
||||
nbrOfPixelsBetweenMarkers = 56;
|
||||
horizontal = false;
|
||||
setMarkers = false;
|
||||
}
|
||||
SinusoidalPatternProfilometry_Impl::Marker::Marker(){};
|
||||
|
||||
SinusoidalPatternProfilometry_Impl::Marker::Marker( Point c )
|
||||
{
|
||||
center = c;
|
||||
up.x = c.x;
|
||||
up.y = c.y - 1;
|
||||
left.x = c.x - 1;
|
||||
left.y = c.y;
|
||||
|
||||
down.x = c.x;
|
||||
down.y = c.y + 1;
|
||||
right.x = c.x + 1;
|
||||
right.y = c.y;
|
||||
}
|
||||
// Draw marker on a pattern
|
||||
void SinusoidalPatternProfilometry_Impl::Marker::drawMarker( OutputArray pattern )
|
||||
{
|
||||
Mat &pattern_ = *(Mat*) pattern.getObj();
|
||||
|
||||
pattern_.at<uchar>(center.x, center.y) = 255;
|
||||
pattern_.at<uchar>(up.x, up.y) = 255;
|
||||
pattern_.at<uchar>(right.x, right.y) = 255;
|
||||
pattern_.at<uchar>(left.x, left.y) = 255;
|
||||
pattern_.at<uchar>(down.x, down.y) = 255;
|
||||
}
|
||||
|
||||
SinusoidalPatternProfilometry_Impl::SinusoidalPatternProfilometry_Impl(
|
||||
const SinusoidalPattern::Params ¶meters ) : params(parameters)
|
||||
{
|
||||
|
||||
}
|
||||
// Generate sinusoidal patterns. Markers are optional
|
||||
bool SinusoidalPatternProfilometry_Impl::generate( OutputArrayOfArrays pattern )
|
||||
{
|
||||
// Three patterns are used in the reference paper.
|
||||
int nbrOfPatterns = 3;
|
||||
float meanAmpl = 127.5;
|
||||
float sinAmpl = 127.5;
|
||||
// Period in number of pixels
|
||||
int period;
|
||||
float frequency;
|
||||
// m and n are parameters described in the reference paper
|
||||
int m = params.nbrOfPixelsBetweenMarkers;
|
||||
int n;
|
||||
// Offset for the first marker of the first row.
|
||||
int firstMarkerOffset = 10;
|
||||
int mnRatio;
|
||||
int nbrOfMarkersOnOneRow;
|
||||
std::vector<Mat> &pattern_ = *(std::vector<Mat>*) pattern.getObj();
|
||||
|
||||
n = params.nbrOfPeriods / nbrOfPatterns;
|
||||
mnRatio = m / n;
|
||||
|
||||
pattern_.resize(nbrOfPatterns);
|
||||
|
||||
if( params.horizontal )
|
||||
{
|
||||
period = params.height / params.nbrOfPeriods;
|
||||
nbrOfMarkersOnOneRow = (int)floor(static_cast<float>((params.width - firstMarkerOffset) / m));
|
||||
}
|
||||
else
|
||||
{
|
||||
period = params.width / params.nbrOfPeriods;
|
||||
nbrOfMarkersOnOneRow = (int)floor(static_cast<float>((params.height - firstMarkerOffset) / m));
|
||||
}
|
||||
frequency = (float) 1 / period;
|
||||
|
||||
for( int i = 0; i < nbrOfPatterns; ++i )
|
||||
{
|
||||
pattern_[i] = Mat(params.height, params.width, CV_8UC1);
|
||||
|
||||
if( params.horizontal )
|
||||
pattern_[i] = pattern_[i].t();
|
||||
}
|
||||
// Patterns vary along one direction only so, a row Mat can be created and copied to the pattern's rows
|
||||
for( int i = 0; i < nbrOfPatterns; ++i )
|
||||
{
|
||||
Mat rowValues(1, pattern_[i].cols, CV_8UC1);
|
||||
|
||||
for( int j = 0; j < pattern_[i].cols; ++j )
|
||||
{
|
||||
rowValues.at<uchar>(0, j) = saturate_cast<uchar>(
|
||||
meanAmpl + sinAmpl * std::sin(2 * CV_PI * frequency * j + i * params.shiftValue));
|
||||
}
|
||||
|
||||
for( int j = 0; j < pattern_[i].rows; ++j )
|
||||
{
|
||||
rowValues.row(0).copyTo(pattern_[i].row(j));
|
||||
}
|
||||
}
|
||||
// Add cross markers to the patterns.
|
||||
if( params.setMarkers )
|
||||
{
|
||||
for( int i = 0; i < nbrOfPatterns; ++i )
|
||||
{
|
||||
for( int j = 0; j < n; ++j )
|
||||
{
|
||||
for( int k = 0; k < nbrOfMarkersOnOneRow; ++k )
|
||||
{
|
||||
Marker mark(Point(firstMarkerOffset + k * m + j * mnRatio,
|
||||
3 * period / 4 + j * period + i * period * n - i * period / 3));
|
||||
mark.drawMarker(pattern_[i]);
|
||||
params.markersLocation.push_back(Point2f((float)(firstMarkerOffset + k * m + j * mnRatio),
|
||||
(float) (3 * period / 4 + j * period + i * period * n - i * period / 3)));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
if( params.horizontal )
|
||||
for( int i = 0; i < nbrOfPatterns; ++i )
|
||||
{
|
||||
pattern_[i] = pattern_[i].t();
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool SinusoidalPatternProfilometry_Impl::decode(const std::vector< std::vector<Mat> >& patternImages,
|
||||
OutputArray disparityMap,
|
||||
InputArrayOfArrays blackImages,
|
||||
InputArrayOfArrays whiteImages, int flags ) const
|
||||
{
|
||||
CV_UNUSED(patternImages);
|
||||
CV_UNUSED(disparityMap);
|
||||
CV_UNUSED(blackImages);
|
||||
CV_UNUSED(whiteImages);
|
||||
CV_UNUSED(flags);
|
||||
return true;
|
||||
}
|
||||
// Most of the steps described in the paper to get the wrapped phase map take place here
|
||||
void SinusoidalPatternProfilometry_Impl::computePhaseMap( InputArrayOfArrays patternImages,
|
||||
OutputArray wrappedPhaseMap,
|
||||
OutputArray shadowMask,
|
||||
InputArray fundamental )
|
||||
{
|
||||
std::vector<Mat> &pattern_ = *(std::vector<Mat>*) patternImages.getObj();
|
||||
Mat &wrappedPhaseMap_ = *(Mat*) wrappedPhaseMap.getObj();
|
||||
int rows = pattern_[0].rows;
|
||||
int cols = pattern_[0].cols;
|
||||
int dcWidth = 5;
|
||||
int dcHeight = 5;
|
||||
int bpWidth = 21;
|
||||
int bpHeight = 21;
|
||||
// Compute wrapped phase map for FTP
|
||||
if( params.methodId == FTP )
|
||||
{
|
||||
Mat &shadowMask_ = *(Mat*) shadowMask.getObj();
|
||||
Mat dftImage, complexInverseDft;
|
||||
Mat dftMag;
|
||||
int halfWidth = cols/2;
|
||||
int halfHeight = rows/2;
|
||||
Point m1, m2;
|
||||
computeShadowMask(pattern_, shadowMask_);
|
||||
|
||||
computeDft(pattern_[0], dftImage); //compute the complex pattern DFT
|
||||
swapQuadrants(dftImage, halfWidth, halfHeight); //swap quadrants to get 0 frequency in (halfWidth, halfHeight)
|
||||
frequencyFiltering(dftImage, halfHeight, halfWidth, dcHeight, dcWidth, false); //get rid of 0 frequency
|
||||
computeDftMagnitude(dftImage, dftMag); //compute magnitude to find maxima
|
||||
findMaxInHalvesTransform(dftMag, m1, m2); //look for maxima in the magnitude. Useful information is located around maxima
|
||||
frequencyFiltering(dftImage, m2.y, m2.x, bpHeight, bpWidth, true); //keep useful information only
|
||||
swapQuadrants(dftImage,halfWidth, halfHeight); //swap quadrants again to compute inverse dft
|
||||
computeInverseDft(dftImage, complexInverseDft, false); //compute inverse dft. Result is complex since we only keep half of the spectrum
|
||||
computeFtPhaseMap(complexInverseDft, shadowMask_, wrappedPhaseMap_); //compute phaseMap from the complex image.
|
||||
}
|
||||
// Compute wrapped pahse map for PSP
|
||||
else if( params.methodId == PSP )
|
||||
{
|
||||
Mat &shadowMask_ = *(Mat*) shadowMask.getObj();
|
||||
//Mat &fundamental_ = *(Mat*) fundamental.getObj();
|
||||
CV_UNUSED(fundamental);
|
||||
Mat dmt;
|
||||
int nbrOfPatterns = static_cast<int>(pattern_.size());
|
||||
std::vector<Mat> filteredPatterns(nbrOfPatterns);
|
||||
std::vector<Mat> dftImages(nbrOfPatterns);
|
||||
std::vector<Mat> dftMags(nbrOfPatterns);
|
||||
int halfWidth = cols/2;
|
||||
int halfHeight = rows/2;
|
||||
Point m1, m2;
|
||||
|
||||
computeShadowMask(pattern_, shadowMask_);
|
||||
|
||||
//this loop symmetrically filters pattern to remove cross markers.
|
||||
for( int i = 0; i < nbrOfPatterns; ++i )
|
||||
{
|
||||
computeDft(pattern_[i], dftImages[i]);
|
||||
swapQuadrants(dftImages[i], halfWidth, halfHeight);
|
||||
frequencyFiltering(dftImages[i], halfHeight, halfWidth, dcHeight, dcWidth, false);
|
||||
computeDftMagnitude(dftImages[i], dftMags[i]);
|
||||
findMaxInHalvesTransform(dftMags[i], m1, m2);
|
||||
frequencyFiltering(dftImages[i], m1.y, m1.x, bpHeight, bpWidth, true, m2.y, m2.x);//symmetrical filtering
|
||||
swapQuadrants(dftImages[i], halfWidth, halfHeight);
|
||||
computeInverseDft(dftImages[i], filteredPatterns[i], true);
|
||||
|
||||
}
|
||||
computePsPhaseMap(filteredPatterns, shadowMask_, wrappedPhaseMap_);
|
||||
}
|
||||
else if( params.methodId == FAPS )
|
||||
{
|
||||
Mat &shadowMask_ = *(Mat*) shadowMask.getObj();
|
||||
int nbrOfPatterns = static_cast<int>(pattern_.size());
|
||||
std::vector<Mat> unwrappedFTPhaseMaps;
|
||||
std::vector<Mat> filteredPatterns(nbrOfPatterns);
|
||||
Mat dmt;
|
||||
Mat theta1, theta2, a, b;
|
||||
std::vector<Point> markersLoc;
|
||||
cv::Size camSize;
|
||||
camSize.height = pattern_[0].rows;
|
||||
camSize.width = pattern_[0].cols;
|
||||
computeShadowMask(pattern_, shadowMask_);
|
||||
|
||||
for( int i = 0; i < nbrOfPatterns; ++i )
|
||||
{
|
||||
Mat dftImage, complexInverseDft;
|
||||
Mat dftMag;
|
||||
Mat tempWrappedPhaseMap;
|
||||
Mat tempUnwrappedPhaseMap;
|
||||
int halfWidth = cols/2;
|
||||
int halfHeight = rows/2;
|
||||
Point m1, m2;
|
||||
|
||||
computeDft(pattern_[i], dftImage); //compute the complex pattern DFT
|
||||
swapQuadrants(dftImage, halfWidth, halfHeight); //swap quadrants to get 0 frequency in (halfWidth, halfHeight)
|
||||
frequencyFiltering(dftImage, halfHeight, halfWidth, dcHeight, dcWidth, false); //get rid of 0 frequency
|
||||
computeDftMagnitude(dftImage, dftMag); //compute magnitude to find maxima
|
||||
findMaxInHalvesTransform(dftMag, m1, m2); //look for maxima in the magnitude. Useful information is located around maxima
|
||||
frequencyFiltering(dftImage, m2.y, m2.x, bpHeight, bpWidth, true); //keep useful information only
|
||||
swapQuadrants(dftImage,halfWidth, halfHeight); //swap quadrants again to compute inverse dft
|
||||
computeInverseDft(dftImage, complexInverseDft, false); //compute inverse dft. Result is complex since we only keep half of the spectrum
|
||||
computeFtPhaseMap(complexInverseDft, shadowMask_, tempWrappedPhaseMap); //compute phaseMap from the complex image.
|
||||
unwrapPhaseMap(tempWrappedPhaseMap, tempUnwrappedPhaseMap, camSize, shadowMask);
|
||||
unwrappedFTPhaseMaps.push_back(tempUnwrappedPhaseMap);
|
||||
computeInverseDft(dftImage, filteredPatterns[i], true);
|
||||
}
|
||||
|
||||
theta1.create(camSize.height, camSize.width, unwrappedFTPhaseMaps[0].type());
|
||||
theta2.create(camSize.height, camSize.width, unwrappedFTPhaseMaps[0].type());
|
||||
a.create(camSize.height, camSize.width, CV_32FC1);
|
||||
b.create(camSize.height, camSize.width, CV_32FC1);
|
||||
|
||||
a = filteredPatterns[0] - filteredPatterns[1];
|
||||
b = filteredPatterns[1] - filteredPatterns[2];
|
||||
|
||||
theta1 = unwrappedFTPhaseMaps[1] - unwrappedFTPhaseMaps[0];
|
||||
theta2 = unwrappedFTPhaseMaps[2] - unwrappedFTPhaseMaps[1];
|
||||
|
||||
computeFapsPhaseMap(a, b, theta1, theta2, shadowMask_, wrappedPhaseMap_);
|
||||
}
|
||||
}
|
||||
|
||||
void SinusoidalPatternProfilometry_Impl::unwrapPhaseMap( InputArray wrappedPhaseMap,
|
||||
OutputArray unwrappedPhaseMap,
|
||||
cv::Size camSize,
|
||||
InputArray shadowMask )
|
||||
{
|
||||
int rows = params.height;
|
||||
int cols = params.width;
|
||||
unwrappingParams.width = camSize.width;
|
||||
unwrappingParams.height = camSize.height;
|
||||
|
||||
Mat &wPhaseMap = *(Mat*) wrappedPhaseMap.getObj();
|
||||
Mat &uPhaseMap = *(Mat*) unwrappedPhaseMap.getObj();
|
||||
Mat mask;
|
||||
|
||||
if( shadowMask.empty() )
|
||||
{
|
||||
mask.create(rows, cols, CV_8UC1);
|
||||
mask = Scalar::all(255);
|
||||
}
|
||||
else
|
||||
{
|
||||
Mat &temp = *(Mat*) shadowMask.getObj();
|
||||
temp.copyTo(mask);
|
||||
}
|
||||
|
||||
Ptr<phase_unwrapping::HistogramPhaseUnwrapping> phaseUnwrapping =
|
||||
phase_unwrapping::HistogramPhaseUnwrapping::create(unwrappingParams);
|
||||
|
||||
phaseUnwrapping->unwrapPhaseMap(wPhaseMap, uPhaseMap, mask);
|
||||
}
|
||||
|
||||
void SinusoidalPatternProfilometry_Impl::findProCamMatches( InputArray projUnwrappedPhaseMap,
|
||||
InputArray camUnwrappedPhaseMap,
|
||||
OutputArrayOfArrays matches )
|
||||
{
|
||||
CV_UNUSED(projUnwrappedPhaseMap);
|
||||
CV_UNUSED(camUnwrappedPhaseMap);
|
||||
CV_UNUSED(matches);
|
||||
}
|
||||
|
||||
void SinusoidalPatternProfilometry_Impl::computeDft( InputArray patternImage,
|
||||
OutputArray FourierTransform )
|
||||
{
|
||||
Mat &pattern_ = *(Mat*) patternImage.getObj();
|
||||
Mat &FourierTransform_ = *(Mat*) FourierTransform.getObj();
|
||||
Mat padded;
|
||||
int m = getOptimalDFTSize(pattern_.rows);
|
||||
int n = getOptimalDFTSize(pattern_.cols);
|
||||
copyMakeBorder(pattern_, padded, 0, m - pattern_.rows, 0, n - pattern_.cols, BORDER_CONSTANT,
|
||||
Scalar::all(0));
|
||||
Mat planes[] = {Mat_<float>(padded), Mat::zeros(padded.size(), CV_32F)};
|
||||
merge(planes, 2, FourierTransform_);
|
||||
dft(FourierTransform_, FourierTransform_);
|
||||
}
|
||||
|
||||
void SinusoidalPatternProfilometry_Impl::computeInverseDft( InputArray FourierTransform,
|
||||
OutputArray inverseFourierTransform,
|
||||
bool realOutput )
|
||||
{
|
||||
Mat &FourierTransform_ = *(Mat*) FourierTransform.getObj();
|
||||
Mat &inverseFourierTransform_ = *(Mat*) inverseFourierTransform.getObj();
|
||||
if( realOutput )
|
||||
idft(FourierTransform_, inverseFourierTransform_, DFT_SCALE | DFT_REAL_OUTPUT);
|
||||
else
|
||||
idft(FourierTransform_, inverseFourierTransform_, DFT_SCALE);
|
||||
}
|
||||
|
||||
void SinusoidalPatternProfilometry_Impl::computeDftMagnitude( InputArray FourierTransform,
|
||||
OutputArray FourierTransformMagnitude )
|
||||
{
|
||||
Mat &FourierTransform_ = *(Mat*) FourierTransform.getObj();
|
||||
Mat &FourierTransformMagnitude_ = *(Mat*) FourierTransformMagnitude.getObj();
|
||||
Mat planes[2];
|
||||
split(FourierTransform_, planes);
|
||||
magnitude(planes[0], planes[1], planes[0]);
|
||||
FourierTransformMagnitude_ = planes[0];
|
||||
FourierTransformMagnitude_ += Scalar::all(1);
|
||||
log(FourierTransformMagnitude_, FourierTransformMagnitude_);
|
||||
FourierTransformMagnitude_ = FourierTransformMagnitude_(
|
||||
Rect(0, 0, FourierTransformMagnitude_.cols & -2, FourierTransformMagnitude_.rows & - 2));
|
||||
normalize(FourierTransformMagnitude_, FourierTransformMagnitude_, 0, 1, NORM_MINMAX);
|
||||
}
|
||||
|
||||
void SinusoidalPatternProfilometry_Impl::computeFtPhaseMap( InputArray inverseFourierTransform,
|
||||
InputArray shadowMask,
|
||||
OutputArray wrappedPhaseMap )
|
||||
{
|
||||
|
||||
Mat &inverseFourierTransform_ = *(Mat*) inverseFourierTransform.getObj();
|
||||
Mat &wrappedPhaseMap_ = *(Mat*) wrappedPhaseMap.getObj();
|
||||
Mat &shadowMask_ = *(Mat*) shadowMask.getObj();
|
||||
Mat planes[2];
|
||||
|
||||
int rows = inverseFourierTransform_.rows;
|
||||
int cols = inverseFourierTransform_.cols;
|
||||
|
||||
if( wrappedPhaseMap_.empty () )
|
||||
wrappedPhaseMap_.create(rows, cols, CV_32FC1);
|
||||
|
||||
split(inverseFourierTransform_, planes);
|
||||
|
||||
for( int i = 0; i < rows; ++i )
|
||||
{
|
||||
for( int j = 0; j < cols; ++j )
|
||||
{
|
||||
if( shadowMask_.at<uchar>(i, j) != 0 )
|
||||
{
|
||||
float im = planes[1].at<float>(i, j);
|
||||
float re = planes[0].at<float>(i, j);
|
||||
wrappedPhaseMap_.at<float>(i, j) = atan2(re, im);
|
||||
}
|
||||
else
|
||||
{
|
||||
wrappedPhaseMap_.at<float>(i, j) = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
void SinusoidalPatternProfilometry_Impl::swapQuadrants( InputOutputArray image,
|
||||
int centerX, int centerY )
|
||||
{
|
||||
Mat &image_ = *(Mat*) image.getObj();
|
||||
Mat q0(image_, Rect(0, 0, centerX, centerY));
|
||||
Mat q1(image_, Rect(centerX, 0, centerX, centerY));
|
||||
Mat q2(image_, Rect(0, centerY, centerX, centerY));
|
||||
Mat q3(image_, Rect(centerX, centerY, centerX, centerY));
|
||||
Mat tmp;
|
||||
|
||||
q0.copyTo(tmp);
|
||||
q3.copyTo(q0);
|
||||
tmp.copyTo(q3);
|
||||
|
||||
q1.copyTo(tmp);
|
||||
q2.copyTo(q1);
|
||||
tmp.copyTo(q2);
|
||||
}
|
||||
|
||||
void SinusoidalPatternProfilometry_Impl::frequencyFiltering( InputOutputArray FourierTransform,
|
||||
int centerX1, int centerY1,
|
||||
int halfRegionWidth, int halfRegionHeight,
|
||||
bool keepInsideRegion, int centerX2,
|
||||
int centerY2 )
|
||||
{
|
||||
Mat &FourierTransform_ = *(Mat*) FourierTransform.getObj();
|
||||
int rows = FourierTransform_.rows;
|
||||
int cols = FourierTransform_.cols;
|
||||
int type = FourierTransform_.type();
|
||||
if( keepInsideRegion )
|
||||
{
|
||||
Mat maskedTransform(rows, cols, type);
|
||||
maskedTransform = Scalar::all(0);
|
||||
Mat roi1 = FourierTransform_(
|
||||
Rect(centerY1 - halfRegionHeight, centerX1 - halfRegionWidth,
|
||||
2 * halfRegionHeight, 2 * halfRegionWidth));
|
||||
Mat dstRoi1 = maskedTransform(
|
||||
Rect(centerY1 - halfRegionHeight, centerX1 - halfRegionWidth,
|
||||
2 * halfRegionHeight, 2 * halfRegionWidth));
|
||||
roi1.copyTo(dstRoi1);
|
||||
|
||||
if( centerY2 != -1 || centerX2 != -1 )
|
||||
{
|
||||
Mat roi2 = FourierTransform_(
|
||||
Rect(centerY2 - halfRegionHeight, centerX2 - halfRegionWidth,
|
||||
2 * halfRegionHeight, 2 * halfRegionWidth));
|
||||
Mat dstRoi2 = maskedTransform(
|
||||
Rect(centerY2 - halfRegionHeight, centerX2 - halfRegionWidth,
|
||||
2 * halfRegionHeight, 2 * halfRegionWidth));
|
||||
roi2.copyTo(dstRoi2);
|
||||
}
|
||||
FourierTransform_ = maskedTransform;
|
||||
}
|
||||
else
|
||||
{
|
||||
Mat roi(2 * halfRegionHeight, 2 * halfRegionWidth, type);
|
||||
roi = Scalar::all(0);
|
||||
|
||||
Mat dstRoi1 = FourierTransform_(
|
||||
Rect(centerY1 - halfRegionHeight, centerX1 - halfRegionWidth,
|
||||
2 * halfRegionHeight, 2 * halfRegionWidth));
|
||||
roi.copyTo(dstRoi1);
|
||||
|
||||
if( centerY2 != -1 || centerX2 != -1 )
|
||||
{
|
||||
Mat dstRoi2 = FourierTransform_(
|
||||
Rect(centerY2 - halfRegionHeight, centerX2 - halfRegionWidth,
|
||||
2 * halfRegionHeight, 2 * halfRegionWidth));
|
||||
roi.copyTo(dstRoi2);
|
||||
}
|
||||
}
|
||||
}
|
||||
bool SinusoidalPatternProfilometry_Impl::findMaxInHalvesTransform( InputArray FourierTransformMag,
|
||||
Point &maxPosition1,
|
||||
Point &maxPosition2 )
|
||||
{
|
||||
Mat &FourierTransformMag_ = *(Mat*) FourierTransformMag.getObj();
|
||||
|
||||
int centerX = FourierTransformMag_.cols / 2;
|
||||
int centerY = FourierTransformMag_.rows / 2;
|
||||
Mat h0, h1;
|
||||
double maxV1 = -1;
|
||||
double maxV2 = -1;
|
||||
int margin = 5;
|
||||
|
||||
if( params.horizontal )
|
||||
{
|
||||
h0 = FourierTransformMag_(Rect(0, 0, FourierTransformMag_.cols, centerY - margin));
|
||||
h1 = FourierTransformMag_(
|
||||
Rect(0, centerY + margin, FourierTransformMag_.cols, centerY - margin));
|
||||
}
|
||||
else
|
||||
{
|
||||
h0 = FourierTransformMag_(Rect(0, 0, centerX - margin, FourierTransformMag_.rows));
|
||||
h1 = FourierTransformMag_(
|
||||
Rect(centerX + margin, 0, centerX - margin, FourierTransformMag_.rows));
|
||||
}
|
||||
|
||||
minMaxLoc(h0, NULL, &maxV1, NULL, &maxPosition1);
|
||||
minMaxLoc(h1, NULL, &maxV2, NULL, &maxPosition2);
|
||||
|
||||
if( params.horizontal )
|
||||
{
|
||||
maxPosition2.y = maxPosition2.y + centerY + margin;
|
||||
}
|
||||
else
|
||||
{
|
||||
maxPosition2.x = maxPosition2.x + centerX + margin;
|
||||
}
|
||||
|
||||
if( maxV1 == -1 || maxV2 == -1 )
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void SinusoidalPatternProfilometry_Impl::computePsPhaseMap( InputArrayOfArrays patternImages,
|
||||
InputArray shadowMask,
|
||||
OutputArray wrappedPhaseMap )
|
||||
{
|
||||
std::vector<Mat> &pattern_ = *(std::vector<Mat>*) patternImages.getObj();
|
||||
Mat &wrappedPhaseMap_ = *(Mat*) wrappedPhaseMap.getObj();
|
||||
Mat &shadowMask_ = *(Mat*) shadowMask.getObj();
|
||||
|
||||
int rows = pattern_[0].rows;
|
||||
int cols = pattern_[0].cols;
|
||||
|
||||
float i1 = 0;
|
||||
float i2 = 0;
|
||||
float i3 = 0;
|
||||
|
||||
if( wrappedPhaseMap_.empty() )
|
||||
wrappedPhaseMap_.create(rows, cols, CV_32FC1);
|
||||
|
||||
for( int i = 0; i < rows; ++i )
|
||||
{
|
||||
for( int j = 0; j < cols; ++j )
|
||||
{
|
||||
if( shadowMask_.at<uchar>(i, j) != 0 )
|
||||
{
|
||||
if( pattern_[0].type() == CV_8UC1 )
|
||||
{
|
||||
i1 = pattern_[0].at<uchar>(i, j);
|
||||
i2 = pattern_[1].at<uchar>(i, j);
|
||||
i3 = pattern_[2].at<uchar>(i, j);
|
||||
}
|
||||
else if( pattern_[0].type() == CV_32FC1 )
|
||||
{
|
||||
i1 = pattern_[0].at<float>(i, j);
|
||||
i2 = pattern_[1].at<float>(i, j);
|
||||
i3 = pattern_[2].at<float>(i, j);
|
||||
}
|
||||
float num = (1- std::cos(params.shiftValue)) * (i3 - i2);
|
||||
float den = std::sin(params.shiftValue) * (2 * i1 - i2 - i3);
|
||||
wrappedPhaseMap_.at<float>(i,j) = atan2(num, den);
|
||||
}
|
||||
else
|
||||
{
|
||||
wrappedPhaseMap_.at<float>(i,j) = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void SinusoidalPatternProfilometry_Impl::computeFapsPhaseMap( InputArray a,
|
||||
InputArray b,
|
||||
InputArray theta1,
|
||||
InputArray theta2,
|
||||
InputArray shadowMask,
|
||||
OutputArray wrappedPhaseMap )
|
||||
{
|
||||
Mat &a_ = *(Mat*) a.getObj();
|
||||
Mat &b_ = *(Mat*) b.getObj();
|
||||
Mat &theta1_ = *(Mat*) theta1.getObj();
|
||||
Mat &theta2_ = *(Mat*) theta2.getObj();
|
||||
Mat &wrappedPhaseMap_ = *(Mat*) wrappedPhaseMap.getObj();
|
||||
Mat &shadowMask_ = *(Mat*) shadowMask.getObj();
|
||||
|
||||
int rows = a_.rows;
|
||||
int cols = a_.cols;
|
||||
|
||||
if( wrappedPhaseMap_.empty() )
|
||||
wrappedPhaseMap_.create(rows, cols, CV_32FC1);
|
||||
|
||||
for( int i = 0; i < rows; ++i )
|
||||
{
|
||||
for( int j = 0; j < cols; ++j )
|
||||
{
|
||||
if( shadowMask_.at<uchar>(i, j ) != 0 )
|
||||
{
|
||||
float num = (1 - std::cos(theta2_.at<float>(i, j))) * a_.at<float>(i, j) +
|
||||
(1 - std::cos(theta1_.at<float>(i, j))) * b_.at<float>(i, j);
|
||||
|
||||
float den = std::sin(theta1_.at<float>(i, j)) * b_.at<float>(i, j) -
|
||||
std::sin(theta2_.at<float>(i, j)) * a_.at<float>(i, j);
|
||||
|
||||
wrappedPhaseMap_.at<float>(i, j) = atan2(num, den);
|
||||
}
|
||||
else
|
||||
{
|
||||
wrappedPhaseMap_.at<float>(i, j) = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//compute shadow mask from three patterns. Valid pixels are lit at least by one pattern
|
||||
void SinusoidalPatternProfilometry_Impl::computeShadowMask( InputArrayOfArrays patternImages,
|
||||
OutputArray shadowMask )
|
||||
{
|
||||
std::vector<Mat> &patternImages_ = *(std::vector<Mat>*) patternImages.getObj();
|
||||
Mat &shadowMask_ = *(Mat*) shadowMask.getObj();
|
||||
Mat mean;
|
||||
int rows = patternImages_[0].rows;
|
||||
int cols = patternImages_[0].cols;
|
||||
float i1, i2, i3;
|
||||
|
||||
mean.create(rows, cols, CV_32FC1);
|
||||
|
||||
for( int i = 0; i < rows; ++i )
|
||||
{
|
||||
for( int j = 0; j < cols; ++j )
|
||||
{
|
||||
i1 = (float) patternImages_[0].at<uchar>(i, j);
|
||||
i2 = (float) patternImages_[1].at<uchar>(i, j);
|
||||
i3 = (float) patternImages_[2].at<uchar>(i, j);
|
||||
mean.at<float>(i, j) = (i1 + i2 + i3) / 3;
|
||||
}
|
||||
}
|
||||
mean.convertTo(mean, CV_8UC1);
|
||||
threshold(mean, shadowMask_, 10, 255, 0);
|
||||
|
||||
}
|
||||
// Compute the data modulation term according to the formula given in the reference paper
|
||||
void SinusoidalPatternProfilometry_Impl::computeDataModulationTerm( InputArrayOfArrays patternImages,
|
||||
OutputArray dataModulationTerm,
|
||||
InputArray shadowMask )
|
||||
{
|
||||
std::vector<Mat> &patternImages_ = *(std::vector<Mat>*) patternImages.getObj();
|
||||
Mat &dataModulationTerm_ = *(Mat*) dataModulationTerm.getObj();
|
||||
Mat &shadowMask_ = *(Mat*) shadowMask.getObj();
|
||||
int rows = patternImages_[0].rows;
|
||||
int cols = patternImages_[0].cols;
|
||||
float num = 0;
|
||||
float den = 0;
|
||||
float i1 = 0;
|
||||
float i2 = 0;
|
||||
float i3 = 0;
|
||||
|
||||
int iOffset, jOffset;
|
||||
Mat dmt(rows, cols, CV_32FC1);
|
||||
Mat threshedDmt;
|
||||
|
||||
if( dataModulationTerm_.empty() )
|
||||
{
|
||||
dataModulationTerm_.create(rows, cols, CV_8UC1);
|
||||
}
|
||||
if( shadowMask_.empty() )
|
||||
{
|
||||
shadowMask_.create(rows, cols, CV_8U);
|
||||
shadowMask_ = Scalar::all(255);
|
||||
}
|
||||
for( int i = 0; i < rows; ++i )
|
||||
{
|
||||
for( int j = 0; j < cols; ++j )
|
||||
{
|
||||
if( shadowMask_.at<uchar>(i, j) != 0 ){
|
||||
if( i - 2 == - 2 )
|
||||
{
|
||||
iOffset = 0;
|
||||
}
|
||||
else if( i - 2 == - 1 )
|
||||
{
|
||||
iOffset = -1;
|
||||
}
|
||||
else if( i - 2 + 4 == rows + 1 )
|
||||
{
|
||||
iOffset = -3;
|
||||
}
|
||||
else
|
||||
{
|
||||
iOffset = -2;
|
||||
}
|
||||
if( j - 2 == -2 )
|
||||
{
|
||||
jOffset = 0;
|
||||
}
|
||||
else if( j - 2 == -1 )
|
||||
{
|
||||
jOffset = -1;
|
||||
}
|
||||
else if( j - 2 + 4 == cols + 1 )
|
||||
{
|
||||
jOffset = -3;
|
||||
}
|
||||
else
|
||||
{
|
||||
jOffset = -2;
|
||||
}
|
||||
Mat roi = shadowMask_(Rect(j + jOffset, i + iOffset, 4, 4));
|
||||
Scalar nbrOfValidPixels = sum(roi);
|
||||
if( nbrOfValidPixels[0] < 14*255 )
|
||||
{
|
||||
dmt.at<float>(i, j) = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
i1 = patternImages_[0].at<uchar>(i, j);
|
||||
i2 = patternImages_[1].at<uchar>(i, j);
|
||||
i3 = patternImages_[2].at<uchar>(i, j);
|
||||
|
||||
num = sqrt(3 * ( i1 - i3 ) * ( i1 - i3 ) + ( 2 * i2 - i1 - i3 ) * ( 2 * i2 - i1 - i3 ));
|
||||
den = i1 + i2 + i3;
|
||||
dmt.at<float>(i, j) = 1 - num / den;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
dmt.at<float>(i, j) = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
Mat kernel(3, 3, CV_32F);
|
||||
kernel.at<float>(0, 0) = 1.f/16.f;
|
||||
kernel.at<float>(1, 0) = 2.f/16.f;
|
||||
kernel.at<float>(2, 0) = 1.f/16.f;
|
||||
|
||||
kernel.at<float>(0, 1) = 2.f/16.f;
|
||||
kernel.at<float>(1, 1) = 4.f/16.f;
|
||||
kernel.at<float>(2, 1) = 2.f/16.f;
|
||||
|
||||
kernel.at<float>(0, 2) = 1.f/16.f;
|
||||
kernel.at<float>(1, 2) = 2.f/16.f;
|
||||
kernel.at<float>(2, 2) = 1.f/16.f;
|
||||
|
||||
Point anchor = Point(-1, -1);
|
||||
double delta = 0;
|
||||
int ddepth = -1;
|
||||
|
||||
filter2D(dmt, dmt, ddepth, kernel, anchor, delta, BORDER_DEFAULT);
|
||||
|
||||
threshold(dmt, threshedDmt, 0.4, 1, THRESH_BINARY);
|
||||
threshedDmt.convertTo(dataModulationTerm_, CV_8UC1, 255, 0);
|
||||
}
|
||||
|
||||
//Extract marker location on the DMT. Duplicates are removed
|
||||
void SinusoidalPatternProfilometry_Impl::extractMarkersLocation( InputArray dataModulationTerm,
|
||||
std::vector<Point> &markersLocation )
|
||||
{
|
||||
Mat &dmt = *(Mat*) dataModulationTerm.getObj();
|
||||
int rows = dmt.rows;
|
||||
int cols = dmt.cols;
|
||||
int halfRegionSize = 6;
|
||||
|
||||
for( int i = 0; i < rows; ++i )
|
||||
{
|
||||
for( int j = 0; j < cols; ++j )
|
||||
{
|
||||
if( dmt.at<uchar>(i,j) != 0 )
|
||||
{
|
||||
bool addToVector = true;
|
||||
for(int k = 0; k < (int)markersLocation.size(); ++k)
|
||||
{
|
||||
if( markersLocation[k].x - halfRegionSize < i &&
|
||||
markersLocation[k].x + halfRegionSize > i &&
|
||||
markersLocation[k].y - halfRegionSize < j &&
|
||||
markersLocation[k].y + halfRegionSize > j ){
|
||||
addToVector = false;
|
||||
}
|
||||
}
|
||||
if(addToVector)
|
||||
{
|
||||
Point temp(i,j);
|
||||
markersLocation.push_back(temp);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
void SinusoidalPatternProfilometry_Impl::convertToAbsolutePhaseMap( InputArrayOfArrays camPatterns,
|
||||
InputArray unwrappedProjPhaseMap,
|
||||
InputArray unwrappedCamPhaseMap,
|
||||
InputArray shadowMask,
|
||||
InputArray fundamentalMatrix )
|
||||
{
|
||||
std::vector<Mat> &camPatterns_ = *(std::vector<Mat>*) camPatterns.getObj();
|
||||
CV_UNUSED(unwrappedCamPhaseMap);
|
||||
CV_UNUSED(unwrappedProjPhaseMap);
|
||||
|
||||
Mat &fundamental = *(Mat*) fundamentalMatrix.getObj();
|
||||
|
||||
Mat camDmt;
|
||||
|
||||
std::vector<Point> markersLocation;
|
||||
|
||||
computeDataModulationTerm(camPatterns_, camDmt, shadowMask);
|
||||
|
||||
std::vector<Vec3f> epilines;
|
||||
computeCorrespondEpilines(params.markersLocation, 2, fundamental, epilines);
|
||||
|
||||
}
|
||||
Ptr<SinusoidalPattern> SinusoidalPattern::create( Ptr<SinusoidalPattern::Params> params )
|
||||
{
|
||||
return makePtr<SinusoidalPatternProfilometry_Impl>(*params);
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user