Rev 73 | Rev 76 | Go to most recent revision | Blame | Compare with Previous | Last modification | View Log | RSS feed
#include "AlgorithmPhaseShift.h"
#include <math.h>
#include "cvtools.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
static unsigned int nSteps = 20; // number of shifts/steps in primary
static float pPrimary = 200; // primary period
// Algorithm
static cv::Mat computePhaseVector(unsigned int length, float phase, float pitch){
cv::Mat phaseVector(length, 1, CV_8UC3);
//phaseVector.setTo(0);
const float pi = M_PI;
// Loop through vector
for(int i=0; i<phaseVector.rows; i++){
// Amplitude of channels
float amp = 0.5*(1+cos(2*pi*i/pitch - phase));
phaseVector.at<cv::Vec3b>(i, 0) = cv::Vec3b(255.0*amp,255.0*amp,255.0*amp);
}
return phaseVector;
}
AlgorithmPhaseShift::AlgorithmPhaseShift(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){
// Set N
N = 2+nSteps+3;
// Determine the secondary (wider) period
float pSecondary = (screenCols*pPrimary)/(screenCols-pPrimary);
// 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);
// Precompute encoded patterns
const float pi = M_PI;
// Primary encoding patterns
for(unsigned int i=0; i<nSteps; i++){
float phase = 2.0*pi/nSteps * i;
float pitch = pPrimary;
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<3; i++){
float phase = 2.0*pi/3 * i;
float pitch = pSecondary;
cv::Mat patternI(1,1,CV_8U);
patternI = computePhaseVector(screenCols, phase, pitch);
patterns.push_back(patternI.t());
}
}
cv::Mat AlgorithmPhaseShift::getEncodingPattern(unsigned int depth){
return patterns[depth];
}
// Absolute phase from 3 frames
cv::Mat getPhase(const cv::Mat I1, const cv::Mat I2, const cv::Mat I3){
cv::Mat_<float> I1_(I1);
cv::Mat_<float> I2_(I2);
cv::Mat_<float> I3_(I3);
cv::Mat phase;
// One call approach
cv::phase(2.0*I1_-I3_-I2_, sqrt(3.0)*(I2_-I3_), phase);
return phase;
}
// Phase unwrapping by means of a phase cue
cv::Mat unwrapWithCue(const cv::Mat up, const cv::Mat upCue, float nPhases){
const float pi = M_PI;
// Determine number of jumps
cv::Mat P = (upCue*nPhases-up)/(2*pi);
// Round to integers
P.convertTo(P, CV_8U);
P.convertTo(P, CV_32F);
// Add to phase
cv::Mat upUnwrapped = up + P*2*pi;
// Scale to range [0; 2pi]
upUnwrapped *= 1.0/nPhases;
return upUnwrapped;
}
// Absolute phase and magnitude from N frames
std::vector<cv::Mat> getDFTComponents(const std::vector<cv::Mat> frames){
unsigned int N = frames.size();
// std::vector<cv::Mat> framesReverse = frames;
// std::reverse(framesReverse.begin(), framesReverse.end());
// DFT approach
cv::Mat I;
cv::merge(frames, I);
unsigned int w = I.cols;
unsigned int h = I.rows;
I = I.reshape(1, h*w);
I.convertTo(I, CV_32F);
cv::Mat fI;
cv::dft(I, fI, cv::DFT_ROWS + cv::DFT_COMPLEX_OUTPUT);
fI = fI.reshape(N*2, h);
std::vector<cv::Mat> fIcomp;
cv::split(fI, fIcomp);
return fIcomp;
}
void AlgorithmPhaseShift::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){
const float pi = M_PI;
assert(frames0.size() == N);
assert(frames1.size() == N);
int frameRows = frames0[0].rows;
int frameCols = frames0[0].cols;
// Gray-scale everything
std::vector<cv::Mat> frames0Gray(N);
std::vector<cv::Mat> frames1Gray(N);
for(int i=0; i<N; i++){
cv::cvtColor(frames0[i], frames0Gray[i], CV_RGB2GRAY);
cv::cvtColor(frames1[i], frames1Gray[i], CV_RGB2GRAY);
}
// Decode camera0
std::vector<cv::Mat> frames0Primary(frames0Gray.begin()+2, frames0Gray.begin()+2+nSteps);
std::vector<cv::Mat> frames0Secondary(frames0Gray.begin()+2+nSteps, frames0Gray.begin()+2+nSteps+3);
std::vector<cv::Mat> F0Enc = getDFTComponents(frames0Primary);
cv::Mat up0Primary;
cv::phase(F0Enc[2], -F0Enc[3], up0Primary);
cv::Mat up0Secondary = getPhase(frames0Secondary[0], frames0Secondary[1], frames0Secondary[2]);
cv::Mat up0Equivalent = up0Primary - up0Secondary;
up0Equivalent = cvtools::modulo(up0Equivalent, 2*pi);
cv::Mat up0 = unwrapWithCue(up0Primary, up0Equivalent, (float)screenCols/pPrimary);
up0 *= screenCols/(2*pi);
// Decode camera1
std::vector<cv::Mat> frames1Primary(frames1Gray.begin()+2, frames1Gray.begin()+2+nSteps);
std::vector<cv::Mat> frames1Secondary(frames1Gray.begin()+2+nSteps, frames1Gray.begin()+2+nSteps+3);
std::vector<cv::Mat> F1Enc = getDFTComponents(frames1Primary);
cv::Mat up1Primary;
cv::phase(F1Enc[2], -F1Enc[3], up1Primary);
cv::Mat up1Secondary = getPhase(frames1Secondary[0], frames1Secondary[1], frames1Secondary[2]);
cv::Mat up1Equivalent = up1Primary - up1Secondary;
up1Equivalent = cvtools::modulo(up1Equivalent, 2*pi);
cv::Mat up1 = unwrapWithCue(up1Primary, up1Equivalent, (float)screenCols/pPrimary);
up1 *= screenCols/(2*pi);
// 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);
// Phase remaps
cv::Mat up0Rect, up1Rect;
cv::remap(up0, up0Rect, map0X, map0Y, CV_INTER_CUBIC);
cv::remap(up1, up1Rect, map1X, map1Y, CV_INTER_CUBIC);
//cvtools::writeMat(up0Rect, "up0Rect.mat", "up0Rect");
//cvtools::writeMat(up1Rect, "up1Rect.mat", "up1Rect");
// Color remaps
cv::Mat color0Rect, color1Rect;
cv::remap(frames0[0], color0Rect, map0X, map0Y, CV_INTER_CUBIC);
cv::remap(frames1[0], color1Rect, map1X, map1Y, CV_INTER_CUBIC);
//cvtools::writeMat(color0Rect, "color0Rect.mat", "color0Rect");
//cvtools::writeMat(color1Rect, "color1Rect.mat", "color1Rect");
// On/off remaps
cv::Mat frames0OnRect, frames0OffRect;
cv::remap(frames0Gray[0], frames0OnRect, map0X, map0Y, CV_INTER_CUBIC);
cv::remap(frames0Gray[1], frames0OffRect, map0X, map0Y, CV_INTER_CUBIC);
cv::Mat frames1OnRect, frames1OffRect;
cv::remap(frames1Gray[0], frames1OnRect, map1X, map1Y, CV_INTER_CUBIC);
cv::remap(frames1Gray[1], frames1OffRect, map1X, map1Y, CV_INTER_CUBIC);
// Occlusion masks
cv::Mat occlusion0Rect, occlusion1Rect;
cv::subtract(frames0OnRect, frames0OffRect, occlusion0Rect);
occlusion0Rect = occlusion0Rect > 50;
cv::subtract(frames1OnRect, frames1OffRect, occlusion1Rect);
occlusion1Rect = occlusion1Rect > 50;
//cvtools::writeMat(occlusion0Rect, "occlusion0Rect.mat", "occlusion0Rect");
//cvtools::writeMat(occlusion1Rect, "occlusion1Rect.mat", "occlusion1Rect");
// Erode occlusion masks
cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5,5));
cv::erode(occlusion0Rect, occlusion0Rect, strel);
cv::erode(occlusion1Rect, occlusion1Rect, strel);
// Threshold on gradient of phase
cv::Mat edges0;
cv::Sobel(up0Rect, edges0, -1, 1, 0, 5);
occlusion0Rect = occlusion0Rect & (abs(edges0) < 150);
cv::Mat edges1;
cv::Sobel(up1Rect, edges1, -1, 1, 0, 5);
occlusion1Rect = occlusion1Rect & (abs(edges1) < 150);
//cvtools::writeMat(edges0, "edges0.mat", "edges0");
//cvtools::writeMat(edges1, "edges1.mat", "edges1");
// Match phase maps
int frameRectRows = map0X.rows;
int frameRectCols = map0X.cols;
// camera0 against camera1
std::vector<cv::Vec2f> q0Rect, q1Rect;
for(int row=0; row<frameRectRows; row++){
for(int col=0; col<frameRectCols; col++){
if(!occlusion0Rect.at<char>(row,col))
continue;
float up0i = up0Rect.at<float>(row,col);
for(int col1=0; col1<up1Rect.cols-1; col1++){
if(!occlusion1Rect.at<char>(row,col1) || !occlusion1Rect.at<char>(row,col1+1))
continue;
float up1Left = up1Rect.at<float>(row,col1);
float up1Right = up1Rect.at<float>(row,col1+1);
if((up1Left <= up0i) && (up0i <= up1Right) && (up0i-up1Left < 1) && (up1Right-up0i < 1)){
float col1i = col1 + (up0i-up1Left)/(up1Right-up1Left);
q0Rect.push_back(cv::Point2f(col, row));
q1Rect.push_back(cv::Point2f(col1i, row));
break;
}
}
}
}
// camera1 against camera0
for(int row=0; row<frameRectRows; row++){
for(int col=0; col<frameRectCols; col++){
if(!occlusion1Rect.at<char>(row,col))
continue;
float up1i = up1Rect.at<float>(row,col);
for(int col0=0; col0<up0Rect.cols-1; col0++){
if(!occlusion0Rect.at<char>(row,col0) || !occlusion0Rect.at<char>(row,col0+1))
continue;
float up0Left = up0Rect.at<float>(row,col0);
float up0Right = up0Rect.at<float>(row,col0+1);
if((up0Left <= up1i) && (up1i <= up0Right) && (up1i-up0Left < 1) && (up0Right-up1i < 1)){
float col0i = col0 + (up1i-up0Left)/(up0Right-up0Left);
q1Rect.push_back(cv::Point2f(col, row));
q0Rect.push_back(cv::Point2f(col0i, row));
break;
}
}
}
}
int nMatches = q0Rect.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 = color0Rect.at<cv::Vec3b>(q0Rect[i][1], q0Rect[i][0]);
cv::Vec3b c1 = color1Rect.at<cv::Vec3b>(q1Rect[i][1], q1Rect[i][0]);
color[i] = 0.5*c0 + 0.5*c1;
}
// Triangulate points
cv::Mat QMatHomogenous, QMat;
cv::triangulatePoints(P0, P1, q0Rect, q1Rect, 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);
}