/*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 : public SinusoidalPattern
{
public:
// Constructor
explicit SinusoidalPatternProfilometry_Impl( const SinusoidalPattern::Params ¶meters =
SinusoidalPattern::Params() );
// Destructor
virtual ~SinusoidalPatternProfilometry_Impl(){};
// Generate sinusoidal patterns
bool generate( OutputArrayOfArrays patternImages );
bool decode( InputArrayOfArrays patternImages, OutputArray disparityMap,
InputArrayOfArrays blackImages = noArray(), InputArrayOfArrays whiteImages =
noArray(), int flags = 0 ) const;
// Compute a wrapped phase map from the sinusoidal patterns
void computePhaseMap( InputArrayOfArrays patternImages, OutputArray wrappedPhaseMap,
OutputArray shadowMask = noArray(), InputArray fundamental = noArray());
// Unwrap the wrapped phase map to retrieve correspondences
void unwrapPhaseMap( InputArray wrappedPhaseMap,
OutputArray unwrappedPhaseMap,
cv::Size camSize,
InputArray shadowMask = noArray() );
// Find correspondences between the devices
void findProCamMatches( InputArray projUnwrappedPhaseMap, InputArray camUnwrappedPhaseMap,
OutputArrayOfArrays matches );
void computeDataModulationTerm( InputArrayOfArrays patternImages,
OutputArray dataModulationTerm,
InputArray shadowMask );
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((params.width - firstMarkerOffset) / m);
}
else
{
period = params.width / params.nbrOfPeriods;
nbrOfMarkersOnOneRow = (int)floor((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 * 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( InputArrayOfArrays patternImages,
OutputArray disparityMap,
InputArrayOfArrays blackImages,
InputArrayOfArrays whiteImages, int flags ) const
{
(void) patternImages;
(void) disparityMap;
(void) blackImages;
(void) whiteImages;
(void) 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();
(void) 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 )
{
(void) projUnwrappedPhaseMap;
(void) camUnwrappedPhaseMap;
(void) 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- cos(params.shiftValue)) * (i3 - i2);
float den = 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 - cos(theta2_.at<float>(i, j))) * a_.at<float>(i, j) +
(1 - cos(theta1_.at<float>(i, j))) * b_.at<float>(i, j);
float den = sin(theta1_.at<float>(i, j)) * b_.at<float>(i, j) -
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();
(void) unwrappedCamPhaseMap;
(void) 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( const SinusoidalPattern::Params ¶ms )
{
return makePtr<SinusoidalPatternProfilometry_Impl>(params);
}
}
}