Rev 192 | Rev 207 | Go to most recent revision | Blame | Compare with Previous | Last modification | View Log | RSS feed
//
// Two Frequency Phase Shifting using the Heterodyne Principle
//
// This implementation follows "Reich, Ritter, Thesing, White light heterodyne principle for 3D-measurement", SPIE (1997)
// Different from the paper, it uses only two different frequencies.
//
// The number of periods in the primary frequency can be chosen freely, but small changes can have a considerable impact on quality.
// The number of phase shifts can be chosen freely (min. 3), and higher values reduce the effects of image noise. They also allow us to filter bad points based on energy at non-primary frequencies.
//
#include "AlgorithmPhaseShiftTwoFreq.h"
#include <math.h>
#include "cvtools.h"
#include "algorithmtools.h"
static unsigned int nStepsPrimary = 16; // number of shifts/steps in primary
static unsigned int nStepsSecondary = 8; // number of shifts/steps in secondary
static float nPeriodsPrimary = 40; // primary period
AlgorithmPhaseShiftTwoFreq::AlgorithmPhaseShiftTwoFreq(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){
// Set N
N = 2+nStepsPrimary+nStepsSecondary;
// Determine the secondary (wider) period to fulfill the heterodyne condition
float nPeriodsSecondary = nPeriodsPrimary + 1;
// all on pattern
cv::Mat allOn(1, screenCols, CV_8UC3, cv::Scalar::all(255));
patterns.push_back(allOn);
// all off pattern
cv::Mat allOff(1, screenCols, CV_8UC3, cv::Scalar::all(0));
patterns.push_back(allOff);
// Primary encoding patterns
for(unsigned int i=0; i<nStepsPrimary; i++){
float phase = 2.0*CV_PI/nStepsPrimary * i;
float pitch = screenCols/nPeriodsPrimary;
cv::Mat patternI(1,1,CV_8U);
patternI = computePhaseVector(screenCols, phase, pitch);
patterns.push_back(patternI.t());
}
// Secondary encoding patterns
for(unsigned int i=0; i<nStepsSecondary; i++){
float phase = 2.0*CV_PI/nStepsSecondary * i;
float pitch = screenCols/nPeriodsSecondary;
cv::Mat patternI(1,1,CV_8U);
patternI = computePhaseVector(screenCols, phase, pitch);
patterns.push_back(patternI.t());
}
}
cv::Mat AlgorithmPhaseShiftTwoFreq::getEncodingPattern(unsigned int depth){
return patterns[depth];
}
void AlgorithmPhaseShiftTwoFreq::get3DPoints(SMCalibrationParameters calibration, const std::vector<cv::Mat>& frames0, const std::vector<cv::Mat>& frames1, std::vector<cv::Point3f>& Q, std::vector<cv::Vec3b>& color){
assert(frames0.size() == N);
assert(frames1.size() == N);
int frameRows = frames0[0].rows;
int frameCols = frames0[0].cols;
// Rectifying homographies (rotation+projections)
cv::Size frameSize(frameCols, frameRows);
cv::Mat R, T;
// stereoRectify segfaults unless R is double precision
cv::Mat(calibration.R1).convertTo(R, CV_64F);
cv::Mat(calibration.T1).convertTo(T, CV_64F);
cv::Mat R0, R1, P0, P1, QRect;
cv::stereoRectify(calibration.K0, calibration.k0, calibration.K1, calibration.k1, frameSize, R, T, R0, R1, P0, P1, QRect, 0);
// Interpolation maps (lens distortion and rectification)
cv::Mat map0X, map0Y, map1X, map1Y;
cv::initUndistortRectifyMap(calibration.K0, calibration.k0, R0, P0, frameSize, CV_32F, map0X, map0Y);
cv::initUndistortRectifyMap(calibration.K1, calibration.k1, R1, P1, frameSize, CV_32F, map1X, map1Y);
int frameRectRows = map0X.rows;
int frameRectCols = map0X.cols;
// Gray-scale and remap
std::vector<cv::Mat> frames0Rect(N);
std::vector<cv::Mat> frames1Rect(N);
for(unsigned int i=0; i<N; i++){
cv::Mat temp;
cv::cvtColor(frames0[i], temp, CV_BayerBG2GRAY);
cv::remap(temp, frames0Rect[i], map0X, map0Y, CV_INTER_LINEAR);
cv::cvtColor(frames1[i], temp, CV_BayerBG2GRAY);
cv::remap(temp, frames1Rect[i], map1X, map1Y, CV_INTER_LINEAR);
}
// Decode camera0
std::vector<cv::Mat> frames0Primary(frames0Rect.begin()+2, frames0Rect.begin()+2+nStepsPrimary);
std::vector<cv::Mat> frames0Secondary(frames0Rect.begin()+2+nStepsPrimary, frames0Rect.end());
std::vector<cv::Mat> F0Primary = getDFTComponents(frames0Primary);
cv::Mat up0Primary;
cv::phase(F0Primary[2], -F0Primary[3], up0Primary);
std::vector<cv::Mat> F0Secondary = getDFTComponents(frames0Secondary);
cv::Mat up0Secondary;
cv::phase(F0Secondary[2], -F0Secondary[3], up0Secondary);
cv::Mat up0Equivalent = up0Secondary - up0Primary;
up0Equivalent = cvtools::modulo(up0Equivalent, 2.0*CV_PI);
cv::Mat up0 = unwrapWithCue(up0Primary, up0Equivalent, nPeriodsPrimary);
up0 *= screenCols/(2.0*CV_PI);
cv::Mat amplitude0;
cv::magnitude(F0Primary[2], -F0Primary[3], amplitude0);
// Collected signal energy at higher frequencies
cv::Mat energy0Primary(frameRectRows, frameRectCols, CV_32F, cv::Scalar(0.0));
for(unsigned int i=0; i<nStepsPrimary-1; i++){
cv::Mat magnitude;
cv::magnitude(F0Primary[i*2 + 2], F0Primary[i*2 + 3], magnitude);
cv::add(energy0Primary, magnitude, energy0Primary, cv::noArray(), CV_32F);
}
cv::Mat energy0Secondary(frameRectRows, frameRectCols, CV_32F, cv::Scalar(0.0));
for(unsigned int i=0; i<nStepsSecondary-1; i++){
cv::Mat magnitude;
cv::magnitude(F0Secondary[i*2 + 2], F0Secondary[i*2 + 3], magnitude);
cv::add(energy0Secondary, magnitude, energy0Secondary, cv::noArray(), CV_32F);
}
#ifdef QT_DEBUG
cvtools::writeMat(up0Primary, "up0Primary.mat", "up0Primary");
cvtools::writeMat(up0Secondary, "up0Secondary.mat", "up0Secondary");
cvtools::writeMat(up0Equivalent, "up0Equivalent.mat", "up0Equivalent");
cvtools::writeMat(up0, "up0.mat", "up0");
cvtools::writeMat(amplitude0, "amplitude0.mat", "amplitude0");
cvtools::writeMat(energy0Primary, "energy0Primary.mat", "energy0Primary");
cvtools::writeMat(energy0Secondary, "energy0Secondary.mat", "energy0Secondary");
#endif
// Decode camera1
std::vector<cv::Mat> frames1Primary(frames1Rect.begin()+2, frames1Rect.begin()+2+nStepsPrimary);
std::vector<cv::Mat> frames1Secondary(frames1Rect.begin()+2+nStepsPrimary, frames1Rect.end());
std::vector<cv::Mat> F1Primary = getDFTComponents(frames1Primary);
cv::Mat up1Primary;
cv::phase(F1Primary[2], -F1Primary[3], up1Primary);
std::vector<cv::Mat> F1Secondary = getDFTComponents(frames1Secondary);
cv::Mat up1Secondary;
cv::phase(F1Secondary[2], -F1Secondary[3], up1Secondary);
cv::Mat up1Equivalent = up1Secondary - up1Primary;
up1Equivalent = cvtools::modulo(up1Equivalent, 2.0*CV_PI);
cv::Mat up1 = unwrapWithCue(up1Primary, up1Equivalent, nPeriodsPrimary);
up1 *= screenCols/(2.0*CV_PI);
cv::Mat amplitude1;
cv::magnitude(F1Primary[2], -F1Primary[3], amplitude1);
// Collected signal energy at higher frequencies
cv::Mat energy1Primary(frameRectRows, frameRectCols, CV_32F, cv::Scalar(0.0));
for(unsigned int i=0; i<nStepsPrimary-1; i++){
cv::Mat magnitude;
cv::magnitude(F1Primary[i*2 + 2], F1Primary[i*2 + 3], magnitude);
cv::add(energy1Primary, magnitude, energy1Primary, cv::noArray(), CV_32F);
}
cv::Mat energy1Secondary(frameRectRows, frameRectCols, CV_32F, cv::Scalar(0.0));
for(unsigned int i=0; i<nStepsSecondary-1; i++){
cv::Mat magnitude;
cv::magnitude(F1Secondary[i*2 + 2], F1Secondary[i*2 + 3], magnitude);
cv::add(energy1Secondary, magnitude, energy1Secondary, cv::noArray(), CV_32F);
}
#ifdef QT_DEBUG
cvtools::writeMat(up1, "up1.mat", "up1");
#endif
// color debayer and remap
cv::Mat color0, color1;
cv::cvtColor(frames0[0], color0, CV_BayerBG2RGB);
cv::remap(color0, color0, map0X, map0Y, CV_INTER_LINEAR);
cv::cvtColor(frames1[0], color1, CV_BayerBG2RGB);
cv::remap(color1, color1, map1X, map1Y, CV_INTER_LINEAR);
#ifdef QT_DEBUG
cvtools::writeMat(color0, "color0.mat", "color0");
cvtools::writeMat(color1, "color1.mat", "color1");
#endif
// Occlusion masks
cv::Mat occlusion0, occlusion1;
cv::subtract(frames0Rect[0], frames0Rect[1], occlusion0);
occlusion0 = (occlusion0 > 25) & (occlusion0 < 250);
cv::subtract(frames1Rect[0], frames1Rect[1], occlusion1);
occlusion1 = (occlusion1 > 25) & (occlusion1 < 250);
// Threshold on energy at primary frequency
occlusion0 = occlusion0 & (amplitude0 > 5.0*nStepsPrimary);
occlusion1 = occlusion1 & (amplitude1 > 5.0*nStepsPrimary);
// Threshold on energy ratios
occlusion0 = occlusion0 & (amplitude0 > 0.85*energy0Primary);
occlusion0 = occlusion0 & (amplitude0 > 0.85*energy0Secondary);
occlusion1 = occlusion1 & (amplitude1 > 0.85*energy1Primary);
occlusion1 = occlusion1 & (amplitude1 > 0.85*energy1Secondary);
// // Erode occlusion masks
// cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5,5));
// cv::erode(occlusion0, occlusion0, strel);
// cv::erode(occlusion1, occlusion1, strel);
// Threshold on gradient of phase
cv::Mat edges0;
cv::Sobel(up0, edges0, -1, 1, 1, 5);
occlusion0 = occlusion0 & (abs(edges0) < 10);
cv::Mat edges1;
cv::Sobel(up1, edges1, -1, 1, 1, 5);
occlusion1 = occlusion1 & (abs(edges1) < 10);
#ifdef QT_DEBUG
cvtools::writeMat(edges0, "edges0.mat", "edges0");
cvtools::writeMat(edges1, "edges1.mat", "edges1");
cvtools::writeMat(occlusion0, "occlusion0.mat", "occlusion0");
cvtools::writeMat(occlusion1, "occlusion1.mat", "occlusion1");
#endif
// Match phase maps
// camera0 against camera1
std::vector<cv::Vec2f> q0, q1;
for(int row=0; row<frameRectRows; row++){
for(int col=0; col<frameRectCols; col++){
if(!occlusion0.at<char>(row,col))
continue;
float up0i = up0.at<float>(row,col);
for(int col1=0; col1<up1.cols-1; col1++){
if(!occlusion1.at<char>(row,col1) || !occlusion1.at<char>(row,col1+1))
continue;
float up1Left = up1.at<float>(row,col1);
float up1Right = up1.at<float>(row,col1+1);
if((up1Left <= up0i) && (up0i <= up1Right) && (up0i-up1Left < 1.0) && (up1Right-up0i < 1.0) && (up1Right-up1Left > 0.1)){
float col1i = col1 + (up0i-up1Left)/(up1Right-up1Left);
q0.push_back(cv::Point2f(col, row));
q1.push_back(cv::Point2f(col1i, row));
break;
}
}
}
}
int nMatches = q0.size();
if(nMatches < 1){
Q.resize(0);
color.resize(0);
return;
}
// Retrieve color information
color.resize(nMatches);
for(int i=0; i<nMatches; i++){
cv::Vec3b c0 = color0.at<cv::Vec3b>(q0[i][1], q0[i][0]);
cv::Vec3b c1 = color1.at<cv::Vec3b>(q1[i][1], q1[i][0]);
color[i] = 0.5*c0 + 0.5*c1;
}
// Triangulate points
cv::Mat QMatHomogenous, QMat;
cv::triangulatePoints(P0, P1, q0, q1, QMatHomogenous);
cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);
// Undo rectification
cv::Mat R0Inv;
cv::Mat(R0.t()).convertTo(R0Inv, CV_32F);
QMat = R0Inv*QMat;
cvtools::matToPoints3f(QMat, Q);
}