Subversion Repositories seema-scanner

//
// 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 (higher frequency) can in principle 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"

#ifndef M_PI
    #define M_PI 3.14159265358979323846
#endif

static unsigned int nStepsPrimary = 16; // number of shifts/steps in primary
static unsigned int nStepsSecondary = 8; // number of shifts/steps in secondary
static float periodPrimary = 48; // 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;
}

AlgorithmPhaseShiftTwoFreq::AlgorithmPhaseShiftTwoFreq(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){

    // Set N
    N = 2+nStepsPrimary+nStepsSecondary;

    // Determine the secondary (wider) period
    float pSecondary = (screenCols*periodPrimary)/(screenCols-periodPrimary);

    // 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<nStepsPrimary; i++){
        float phase = 2.0*pi/nStepsPrimary * i;
        float pitch = periodPrimary;
        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*pi/nStepsSecondary * i;
        float pitch = pSecondary;
        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];
}


//// Absolute phase from 3 frames
//static 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
static 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.0*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
static 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 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){

    const float pi = M_PI;

    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);

    // 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);
    //cv::Mat up0Secondary = getPhase(frames0Secondary[0], frames0Secondary[1], frames0Secondary[2]);
    std::vector<cv::Mat> F0Secondary = getDFTComponents(frames0Secondary);
    cv::Mat up0Secondary;
    cv::phase(F0Secondary[2], -F0Secondary[3], up0Secondary);
    cv::Mat up0Equivalent = up0Primary - up0Secondary;
    up0Equivalent = cvtools::modulo(up0Equivalent, 2.0*pi);
    cv::Mat up0 = unwrapWithCue(up0Primary, up0Equivalent, (float)screenCols/periodPrimary);
    up0 *= screenCols/(2.0*pi);
    cv::Mat amplitude0;
    cv::magnitude(F0Primary[2], -F0Primary[3], amplitude0);

    // 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);
    //cv::Mat up1Secondary = getPhase(frames1Secondary[0], frames1Secondary[1], frames1Secondary[2]);
    std::vector<cv::Mat> F1Secondary = getDFTComponents(frames1Secondary);
    cv::Mat up1Secondary;
    cv::phase(F1Secondary[2], -F1Secondary[3], up1Secondary);
    cv::Mat up1Equivalent = up1Primary - up1Secondary;
    up1Equivalent = cvtools::modulo(up1Equivalent, 2.0*pi);
    cv::Mat up1 = unwrapWithCue(up1Primary, up1Equivalent, (float)screenCols/periodPrimary);
    up1 *= screenCols/(2.0*pi);
    cv::Mat amplitude1;
    cv::magnitude(F1Primary[2], -F1Primary[3], amplitude1);

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");

    // color debayer and remap
    cv::Mat color0, color1;
//    frames0[0].convertTo(color0Rect, CV_8UC1, 1.0/256.0);
    cv::cvtColor(frames0[0], color0, CV_BayerBG2RGB);
    cv::remap(color0, color0, map0X, map0Y, CV_INTER_LINEAR);

//    frames1[0].convertTo(color1Rect, CV_8UC1, 1.0/256.0);
    cv::cvtColor(frames1[0], color1, CV_BayerBG2RGB);
    cv::remap(color1, color1, map1X, map1Y, CV_INTER_LINEAR);

//cvtools::writeMat(color0, "color0.mat", "color0");
//cvtools::writeMat(color1, "color1.mat", "color1");

    // 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);

//cvtools::writeMat(occlusion1, "occlusion1.mat", "occlusion1");
//cvtools::writeMat(occlusion1, "occlusion1.mat", "occlusion1");

    // 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) < 150);

    cv::Mat edges1;
    cv::Sobel(up1, edges1, -1, 1, 1, 5);
    occlusion1 = occlusion1 & (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> 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) && (up1Right-up0i < 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);

}