WebSVN – seema-scanner – Diff – /src/algorithm/AlgorithmPhaseShiftTwoFreq.cpp


//
// 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(const 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);
 
    cv::Mat map0X, map0Y, map1X, map1Y;
    cv::Mat R0, P0, P1;
{
    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  R1, 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::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;
 
    cv::Mat up0;
    cv::Mat occlusion0;
{
        // Gray-scale and remap
        std::vector<cv::Mat> frames0Rect(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);
        }
        // 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();
 
    std::vector<cv::Mat> F0Primary = getDFTComponents(frames0Primary);
    frames0Primary.clear();
    cv::Mat up0Primary;
    cv::phase(F0Primary[2], -F0Primary[3], up0Primary);
 
    std::vector<cv::Mat> F0Secondary = getDFTComponents(frames0Secondary);
    frames0Secondary.clear();
    cv::Mat up0Secondary;
    cv::phase(F0Secondary[2], -F0Secondary[3], up0Secondary);
 
    cv::Mat up0Equivalent = up0Secondary - up0Primary;
    up0Equivalent = cvtools::modulo(up0Equivalent, 2.0*CV_PI);
    up0 = unwrapWithCue(up0Primary, up0Equivalent, nPeriodsPrimary);
    up0 *= screenCols/(2.0*CV_PI);
 
    // Signal energy at unit frequency
    cv::Mat amplitude0Primary, amplitude0Secondary;
    cv::magnitude(F0Primary[2], -F0Primary[3], amplitude0Primary);
    cv::magnitude(F0Secondary[2], -F0Secondary[3], amplitude0Secondary);
 
    // 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);
    }
 
    // 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);
 
    #ifdef SM_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(amplitude0Primary, "amplitude0Primary.mat", "amplitude0Primary");
        cvtools::writeMat(energy0Primary, "energy0Primary.mat", "energy0Primary");
        cvtools::writeMat(energy0Secondary, "energy0Secondary.mat", "energy0Secondary");
    #endif
}
 
    cv::Mat up1;
    cv::Mat occlusion1;
    {
 
        // Gray-scale and remap
        std::vector<cv::Mat> frames1Rect(N);
        for(unsigned int i=0; i<N; i++){
            cv::Mat temp;
            cv::cvtColor(frames1[i], temp, CV_BayerBG2GRAY);
            cv::remap(temp, frames1Rect[i], map1X, map1Y, CV_INTER_LINEAR);
        }
 
        // 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();
 
    std::vector<cv::Mat> F1Primary = getDFTComponents(frames1Primary);
    frames1Primary.clear();
    cv::Mat up1Primary;
    cv::phase(F1Primary[2], -F1Primary[3], up1Primary);
 
    std::vector<cv::Mat> F1Secondary = getDFTComponents(frames1Secondary);
    frames1Secondary.clear();
    cv::Mat up1Secondary;
    cv::phase(F1Secondary[2], -F1Secondary[3], up1Secondary);
 
    cv::Mat up1Equivalent = up1Secondary - up1Primary;
    up1Equivalent = cvtools::modulo(up1Equivalent, 2.0*CV_PI);
    up1 = unwrapWithCue(up1Primary, up1Equivalent, nPeriodsPrimary);
    up1 *= screenCols/(2.0*CV_PI);
 
    // Signal energy at unit frequency
    cv::Mat amplitude1Primary, amplitude1Secondary;
    cv::magnitude(F1Primary[2], -F1Primary[3], amplitude1Primary);
    cv::magnitude(F1Secondary[2], -F1Secondary[3], amplitude1Secondary);
 
    // 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);
    }
 
    // 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);
 
    #ifdef SM_DEBUG
        cvtools::writeMat(up1, "up1.mat", "up1");
    #endif
}
 
//    // 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 SM_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;
    }
{
    // 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 SM_DEBUG
        cvtools::writeMat(color0, "color0.mat", "color0");
        cvtools::writeMat(color1, "color1.mat", "color1");
    #endif
 
    // 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);
 
}
 

Subversion Repositories seema-scanner

(root)/src/algorithm/AlgorithmPhaseShiftTwoFreq.cpp – Rev 228 → 229

Rev 228		Rev 229
1	`//`	1	`//`
2	`// Two Frequency Phase Shifting using the Heterodyne Principle`	2	`// Two Frequency Phase Shifting using the Heterodyne Principle`
3	`//`	3	`//`
4	`// This implementation follows "Reich, Ritter, Thesing, White light heterodyne principle for 3D-measurement", SPIE (1997)`	4	`// This implementation follows "Reich, Ritter, Thesing, White light heterodyne principle for 3D-measurement", SPIE (1997)`
5	`// Different from the paper, it uses only two different frequencies.`	5	`// Different from the paper, it uses only two different frequencies.`
6	`//`	6	`//`
7	`// The number of periods in the primary frequency can be chosen freely, but small changes can have a considerable impact on quality.`	7	`// The number of periods in the primary frequency can be chosen freely, but small changes can have a considerable impact on quality.`
8	`// 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.`	8	`// 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.`
9	`//`	9	`//`
10		10
11	`#include "AlgorithmPhaseShiftTwoFreq.h"`	11	`#include "AlgorithmPhaseShiftTwoFreq.h"`
12	`#include <math.h>`	12	`#include <math.h>`
13		13
14	`#include "cvtools.h"`	14	`#include "cvtools.h"`
15	`#include "algorithmtools.h"`	15	`#include "algorithmtools.h"`
16		16
17	`#define SM_DEBUG 1`	-
18		-
19	`static unsigned int nStepsPrimary = 16; // number of shifts/steps in primary`	17	`static unsigned int nStepsPrimary = 16; // number of shifts/steps in primary`
20	`static unsigned int nStepsSecondary = 8; // number of shifts/steps in secondary`	18	`static unsigned int nStepsSecondary = 8; // number of shifts/steps in secondary`
21	`static float nPeriodsPrimary = 40; // primary period`	19	`static float nPeriodsPrimary = 40; // primary period`
22		20
23	`AlgorithmPhaseShiftTwoFreq::AlgorithmPhaseShiftTwoFreq(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){`	21	`AlgorithmPhaseShiftTwoFreq::AlgorithmPhaseShiftTwoFreq(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){`
24		22
25	`// Set N`	23	`// Set N`
26	`N = 2+nStepsPrimary+nStepsSecondary;`	24	`N = 2+nStepsPrimary+nStepsSecondary;`
27		25
28	`// Determine the secondary (wider) period to fulfill the heterodyne condition`	26	`// Determine the secondary (wider) period to fulfill the heterodyne condition`
29	`float nPeriodsSecondary = nPeriodsPrimary + 1;`	27	`float nPeriodsSecondary = nPeriodsPrimary + 1;`
30		28
31	`// all on pattern`	29	`// all on pattern`
32	`cv::Mat allOn(1, screenCols, CV_8UC3, cv::Scalar::all(255));`	30	`cv::Mat allOn(1, screenCols, CV_8UC3, cv::Scalar::all(255));`
33	`patterns.push_back(allOn);`	31	`patterns.push_back(allOn);`
34		32
35	`// all off pattern`	33	`// all off pattern`
36	`cv::Mat allOff(1, screenCols, CV_8UC3, cv::Scalar::all(0));`	34	`cv::Mat allOff(1, screenCols, CV_8UC3, cv::Scalar::all(0));`
37	`patterns.push_back(allOff);`	35	`patterns.push_back(allOff);`
38		36
39	`// Primary encoding patterns`	37	`// Primary encoding patterns`
40	`for(unsigned int i=0; i<nStepsPrimary; i++){`	38	`for(unsigned int i=0; i<nStepsPrimary; i++){`