Rev 247 | 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"
#include <omp.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];
}
struct StereoRectifyier {
cv::Mat map0X, map0Y, map1X, map1Y;
cv::Mat R0, R1, P0, P1, QRect;
};
void getStereoRectifier(const SMCalibrationParameters &calibration,
const cv::Size& frameSize,
StereoRectifyier& stereoRect){
// cv::stereoRectify segfaults unless R is double precision
cv::Mat R, T;
cv::Mat(calibration.R1).convertTo(R, CV_64F);
cv::Mat(calibration.T1).convertTo(T, CV_64F);
cv::stereoRectify(calibration.K0, calibration.k0,
calibration.K1, calibration.k1,
frameSize, R, T,
stereoRect.R0, stereoRect.R1,
stereoRect.P0, stereoRect.P1,
stereoRect.QRect, 0);
// Interpolation maps (lens distortion and rectification)
cv::initUndistortRectifyMap(calibration.K0, calibration.k0,
stereoRect.R0, stereoRect.P0,
frameSize, CV_32F,
stereoRect.map0X, stereoRect.map0Y);
cv::initUndistortRectifyMap(calibration.K1, calibration.k1,
stereoRect.R1, stereoRect.P1,
frameSize, CV_32F,
stereoRect.map1X, stereoRect.map1Y);
}
void determineAmplitudePhaseEnergy(std::vector<cv::Mat>& frames,
cv::Mat& amplitude,
cv::Mat& phase,
cv::Mat& energy) {
std::vector<cv::Mat> fourier = getDFTComponents(frames);
cv::phase(fourier[2], -fourier[3], phase);
// Signal energy at unit frequency
cv::magnitude(fourier[2], -fourier[3], amplitude);
// Collected signal energy at higher frequencies
energy = cv::Mat(phase.rows, phase.cols, CV_32F, cv::Scalar(0.0));
for(unsigned int i=0; i<frames.size()-1; i++){
cv::Mat magnitude;
cv::magnitude(fourier[i*2 + 2], fourier[i*2 + 3], magnitude);
cv::add(energy, magnitude, energy, cv::noArray(), CV_32F);
}
frames.clear();
}
void unWrapPhaseMap(const cv::Mat& phasePrimary,
const cv::Mat& phaseSecondary,
cv::Mat& phase) {
cv::Mat phaseEquivalent = phaseSecondary - phasePrimary;
phaseEquivalent = cvtools::modulo(phaseEquivalent, 2.0*CV_PI);
phase = unwrapWithCue(phasePrimary, phaseEquivalent, nPeriodsPrimary);
phase *= phasePrimary.cols/(2.0*CV_PI);
}
void matchPhaseMaps(const cv::Mat& occlusion0, const cv::Mat& occlusion1,
const cv::Mat& phase0, const cv::Mat& phase1,
std::vector<cv::Vec2f>& q0, std::vector<cv::Vec2f>& q1) {
#pragma omp parallel for
for(int row=0; row<occlusion0.rows; row++){
for(int col=0; col<occlusion0.cols; col++){
if(!occlusion0.at<char>(row,col))
continue;
float phase0i = phase0.at<float>(row,col);
for(int col1=0; col1<phase1.cols-1; col1++){
if(!occlusion1.at<char>(row,col1) || !occlusion1.at<char>(row,col1+1))
continue;
float phase1Left = phase1.at<float>(row,col1);
float phase1Right = phase1.at<float>(row,col1+1);
bool match = (phase1Left <= phase0i)
&& (phase0i <= phase1Right)
&& (phase0i-phase1Left < 1.0)
&& (phase1Right-phase0i < 1.0)
&& (phase1Right-phase1Left > 0.1);
if(match){
float col1i = col1 + (phase0i-phase1Left)/(phase1Right-phase1Left);
#pragma omp critical
{
q0.push_back(cv::Point2f(col, row));
q1.push_back(cv::Point2f(col1i, row));
}
break;
}
}
}
}
}
void triangulate(const StereoRectifyier& stereoRect,
const std::vector<cv::Vec2f>& q0,
const std::vector<cv::Vec2f>& q1,
std::vector<cv::Point3f>& Q) {
// 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);
// Triangulate by means of disparity projection
Q.resize(q0.size());
cv::Matx44f QRectx = cv::Matx44f(stereoRect.QRect);
cv::Matx33f R0invx = cv::Matx33f(cv::Mat(stereoRect.R0.t()));
#pragma omp parallel for
for(unsigned int i=0; i < q0.size(); i++){
float disparity = q0[i][0] - q1[i][0];
cv::Vec4f Qih = QRectx*cv::Vec4f(q0[i][0], q0[i][1], disparity, 1.0);
float winv = float(1.0) / Qih[3];
Q[i] = R0invx * cv::Point3f(Qih[0]*winv, Qih[1]*winv, Qih[2]*winv);
}
}
void AlgorithmPhaseShiftTwoFreq::
get3DPoints(const SMCalibrationParameters & calibration,
const std::vector<cv::Mat>& frames0,
const std::vector<cv::Mat>& frames1,
std::vector<cv::Point3f>& Q,
std::vector<cv::Vec3f>& color){
assert(frames0.size() == N);
assert(frames1.size() == N);
StereoRectifyier stereoRect;
getStereoRectifier(calibration,
cv::Size(frames0[0].cols, frames0[0].rows),
stereoRect);
// // Erode occlusion masks
// cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5,5));
cv::Mat up0, up1;
cv::Mat occlusion0, occlusion1;
cv::Mat color0, color1;
#pragma omp parallel sections
{
#pragma omp section
{
// Gray-scale and remap/rectify
std::vector<cv::Mat> frames0Rect(N);
for(unsigned int i=0; i<N; i++){
cv::Mat temp;
if(frames0[i].depth() == CV_8U)
cv::cvtColor(frames0[i], temp, CV_BayerBG2GRAY);
else
temp = frames0[i];
cv::remap(temp, frames0Rect[i],
stereoRect.map0X, stereoRect.map0Y,
CV_INTER_LINEAR);
}
// If images are HDR (float), we need to convert to uchar
frames0Rect[0].convertTo(color0, CV_32FC3);
// Occlusion masks
cv::subtract(frames0Rect[0], frames0Rect[1], occlusion0);
occlusion0 = (occlusion0 > 25) & (occlusion0 < 250);
// 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());
frames0Rect.clear();
cv::Mat amplitude0Primary, amplitude0Secondary;
cv::Mat up0Primary, up0Secondary;
cv::Mat energy0Primary, energy0Secondary;
determineAmplitudePhaseEnergy(frames0Primary,
amplitude0Primary,
up0Primary,
energy0Primary);
determineAmplitudePhaseEnergy(frames0Secondary,
amplitude0Secondary,
up0Secondary,
energy0Secondary);
unWrapPhaseMap(up0Primary, up0Secondary, up0);
// Threshold on energy at primary frequency
occlusion0 = occlusion0 & (amplitude0Primary > 5.0*nStepsPrimary);
// Threshold on energy ratios
occlusion0 = occlusion0 & (amplitude0Primary > 0.25*energy0Primary);
occlusion0 = occlusion0 & (amplitude0Secondary > 0.25*energy0Secondary);
// // Erode occlusion masks
// cv::erode(occlusion0, occlusion0, strel);
// Threshold on vertical gradient of phase
cv::Mat edges0;
cv::Sobel(up0, edges0, -1, 1, 1, 5);
occlusion0 = occlusion0 & (abs(edges0) < 10);
#ifdef SM_DEBUG
cvtools::writeMat(up0Primary, "up0Primary.mat", "up0Primary");
cvtools::writeMat(up0Secondary, "up0Secondary.mat", "up0Secondary");
cvtools::writeMat(up0, "up0.mat", "up0");
cvtools::writeMat(amplitude0Primary,
"amplitude0Primary.mat", "amplitude0Primary");
cvtools::writeMat(amplitude0Secondary,
"amplitude0Secondary.mat", "amplitude0Secondary");
cvtools::writeMat(energy0Primary,
"energy0Primary.mat", "energy0Primary");
cvtools::writeMat(energy0Secondary,
"energy0Secondary.mat", "energy0Secondary");
cvtools::writeMat(edges0, "edges0.mat", "edges0");
cvtools::writeMat(occlusion0, "occlusion0.mat", "occlusion0");
cvtools::writeMat(color0, "color0.mat", "color0");
#endif
}
#pragma omp section
{
// Gray-scale and remap
std::vector<cv::Mat> frames1Rect(N);
for(unsigned int i=0; i<N; i++){
cv::Mat temp;
if(frames1[i].depth() == CV_8U)
cv::cvtColor(frames1[i], temp, CV_BayerBG2GRAY);
else
temp = frames1[i];
cv::remap(temp, frames1Rect[i],
stereoRect.map1X, stereoRect.map1Y,
CV_INTER_LINEAR);
}
// If images are HDR (float), we need to convert to uchar
frames1Rect[0].convertTo(color1, CV_32FC3);
// Occlusion masks
cv::subtract(frames1Rect[0], frames1Rect[1], occlusion1);
occlusion1 = (occlusion1 > 25) & (occlusion1 < 250);
// 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());
frames1Rect.clear();
cv::Mat amplitude1Primary, amplitude1Secondary;
cv::Mat up1Primary, up1Secondary;
cv::Mat energy1Primary, energy1Secondary;
determineAmplitudePhaseEnergy(frames1Primary,
amplitude1Primary,
up1Primary,
energy1Primary);
determineAmplitudePhaseEnergy(frames1Secondary,
amplitude1Secondary,
up1Secondary,
energy1Secondary);
unWrapPhaseMap(up1Primary, up1Secondary, up1);
// Threshold on energy at primary frequency
occlusion1 = occlusion1 & (amplitude1Primary > 5.0*nStepsPrimary);
// Threshold on energy ratios
occlusion1 = occlusion1 & (amplitude1Primary > 0.25*energy1Primary);
occlusion1 = occlusion1 & (amplitude1Secondary > 0.25*energy1Secondary);
// // Erode occlusion masks
// cv::erode(occlusion1, occlusion1, strel);
// Threshold on vertical gradient of phase
cv::Mat edges1;
cv::Sobel(up1, edges1, -1, 1, 1, 5);
occlusion1 = occlusion1 & (abs(edges1) < 10);
#ifdef SM_DEBUG
cvtools::writeMat(up1Primary, "up1Primary.mat", "up1Primary");
cvtools::writeMat(up1Secondary, "up1Secondary.mat", "up1Secondary");
cvtools::writeMat(up1, "up1.mat", "up1");
cvtools::writeMat(amplitude1Primary,
"amplitude1Primary.mat", "amplitude1Primary");
cvtools::writeMat(amplitude1Secondary,
"amplitude1Secondary.mat", "amplitude1Secondary");
cvtools::writeMat(energy1Primary,
"energy1Primary.mat", "energy1Primary");
cvtools::writeMat(energy1Secondary,
"energy1Secondary.mat", "energy1Secondary");
cvtools::writeMat(edges1, "edges1.mat", "edges1");
cvtools::writeMat(occlusion1, "occlusion1.mat", "occlusion1");
cvtools::writeMat(color1, "color1.mat", "color1");
#endif
}
}
// Match phase maps
// camera0 against camera1
std::vector<cv::Vec2f> q0, q1;
matchPhaseMaps(occlusion0, occlusion1, up0, up1, q0, q1);
size_t nMatches = q0.size();
if(nMatches < 1){
Q.resize(0);
color.resize(0);
return;
}
else {
// Retrieve color information
color.resize(nMatches);
for(unsigned int i=0; i<nMatches; i++){
cv::Vec3f c0 = color0.at<cv::Vec3f>(int(q0[i][1]), int(q0[i][0]));
cv::Vec3f c1 = color1.at<cv::Vec3f>(int(q1[i][1]), int(q1[i][0]));
color[i] = 0.5*c0 + 0.5*c1;
}
}
// Triangulate points
triangulate(stereoRect, q0, q1, Q);
}