Subversion Repositories seema-scanner

//

// Two Frequency Phase Shifting using the Heterodyne Principle with horizontal and vertical encoding.

//

// This implementation is based on "AlgorithmPhaseShiftTwoFreq", but uses horizontal and vertial fringes, which adds some encoding redundancy,

// but avoids interpolation effects from rectifying homographies.

#include "AlgorithmPhaseShiftTwoFreqHorzVert.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

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

    // Set N

    N = 2+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 (horizontal)

    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 (horizontal)

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

    }

    // Primary encoding patterns (vertical)

    for(unsigned int i=0; i<nStepsPrimary; i++){

        float phase = 2.0*CV_PI/nStepsPrimary * i;

        float pitch = screenRows/nPeriodsPrimary;

        cv::Mat patternI(1,1,CV_8U);

        patternI = computePhaseVector(screenRows, phase, pitch);

        patterns.push_back(patternI);

    }

    // Secondary encoding patterns (vertical)

    for(unsigned int i=0; i<nStepsSecondary; i++){

        float phase = 2.0*CV_PI/nStepsSecondary * i;

        float pitch = screenRows/nPeriodsSecondary;

        cv::Mat patternI(1,1,CV_8U);

        patternI = computePhaseVector(screenRows, phase, pitch);

        patterns.push_back(patternI);

    }

}

cv::Mat AlgorithmPhaseShiftTwoFreqHorzVert::getEncodingPattern(unsigned int depth){

    return patterns[depth];

}

static void decodePhaseShiftingSequence(const std::vector<cv::Mat> &frames, cv::Mat &up, cv::Mat &amplitude, cv::Mat &energy){

    std::vector<cv::Mat> F = getDFTComponents(frames);

    cv::phase(F[2], -F[3], up);

    // Collected signal energy at higher frequencies

    energy.create(frames[0].rows, frames[0].cols, CV_32F);

    energy.setTo(0.0);

    for(unsigned int i=0; i<frames.size()-1; i++){

        cv::Mat magnitude;

        cv::magnitude(F[i*2 + 2], F[i*2 + 3], magnitude);

        cv::add(energy, magnitude, energy, cv::noArray(), CV_32F);

    }

    cv::magnitude(F[2], -F[3], amplitude);

}

static void unwrapHeterodynePhases(const cv::Mat &upPrimary, const cv::Mat &upSecondary, cv::Mat &up){

    // Determine equivalent phase

    cv::Mat upEquivalent = upSecondary - upPrimary;

    upEquivalent = cvtools::modulo(upEquivalent, 2.0*CV_PI);

    up = unwrapWithCue(upPrimary, upEquivalent, nPeriodsPrimary);

}

void AlgorithmPhaseShiftTwoFreqHorzVert::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);

    int frameRows = frames0[0].rows;

    int frameCols = frames0[0].cols;

    // Gray-scale

    std::vector<cv::Mat> frames0Gray(N);

    std::vector<cv::Mat> frames1Gray(N);

    for(unsigned int i=0; i<N; i++){

        if(frames0[i].depth() == CV_8U)

            cv::cvtColor(frames0[i], frames0Gray[i], CV_BayerBG2GRAY);

        else if(frames0[i].channels() == 3)

            cv::cvtColor(frames0[i], frames0Gray[i], CV_RGB2GRAY);

        else

            frames0Gray[i] = frames0[i];

        if(frames1[i].depth() == CV_8U)

            cv::cvtColor(frames1[i], frames1Gray[i], CV_BayerBG2GRAY);

        else if(frames1[i].channels() == 3)

            cv::cvtColor(frames1[i], frames1Gray[i], CV_RGB2GRAY);

        else

            frames1Gray[i] = frames1[i];

    }

    // Decode camera0 (horizontal)

    std::vector<cv::Mat> frames0PrimaryHorz(frames0Gray.begin()+2, frames0Gray.begin()+2+nStepsPrimary);

    cv::Mat up0Primary, amplitude0PrimaryHorz, energy0PrimaryHorz;

    decodePhaseShiftingSequence(frames0PrimaryHorz, up0Primary, amplitude0PrimaryHorz, energy0PrimaryHorz);

    std::vector<cv::Mat> frames0SecondaryHorz(frames0Gray.begin()+2+nStepsPrimary, frames0Gray.end());

    cv::Mat up0Secondary, amplitude0SecondaryHorz, energy0SecondaryHorz;

    decodePhaseShiftingSequence(frames0SecondaryHorz, up0Secondary, amplitude0SecondaryHorz, energy0SecondaryHorz);

    cv::Mat up0;

    unwrapHeterodynePhases(up0Primary, up0Secondary, up0);

    up0 *= screenCols/(2.0*CV_PI);

    #ifdef QT_DEBUG

        cvtools::writeMat(up0, "up0.mat", "up0");

    #endif

    // Decode camera0 (vertical)

    std::vector<cv::Mat> frames0PrimaryVert(frames0Gray.begin()+2+nStepsPrimary+nStepsSecondary, frames0Gray.end()-nStepsSecondary);

    cv::Mat vp0Primary, amplitude0PrimaryVert, energy0PrimaryVert;

    decodePhaseShiftingSequence(frames0PrimaryVert, vp0Primary, amplitude0PrimaryVert, energy0PrimaryVert);

    std::vector<cv::Mat> frames0SecondaryVert(frames0Gray.end()-nStepsSecondary, frames0Gray.end());

    cv::Mat vp0Secondary, amplitude0SecondaryVert, energy0SecondaryVert;

    decodePhaseShiftingSequence(frames0SecondaryVert, vp0Secondary, amplitude0SecondaryVert, energy0SecondaryVert);

    cv::Mat vp0;

    unwrapHeterodynePhases(vp0Primary, vp0Secondary, vp0);

    vp0 *= screenCols/(2.0*CV_PI);

    #ifdef QT_DEBUG

        cvtools::writeMat(vp0, "vp0.mat", "vp0");

    #endif

    // Decode camera1 (horizontal)

    std::vector<cv::Mat> frames1PrimaryHorz(frames1Gray.begin()+2, frames1Gray.begin()+2+nStepsPrimary);

    cv::Mat up1Primary, amplitude1PrimaryHorz, energy1PrimaryHorz;

    decodePhaseShiftingSequence(frames1PrimaryHorz, up1Primary, amplitude1PrimaryHorz, energy1PrimaryHorz);

    std::vector<cv::Mat> frames1SecondaryHorz(frames1Gray.begin()+2+nStepsPrimary, frames1Gray.end());

    cv::Mat up1Secondary, amplitude1SecondaryHorz, energy1SecondaryHorz;

    decodePhaseShiftingSequence(frames1SecondaryHorz, up1Secondary, amplitude1SecondaryHorz, energy1SecondaryHorz);

    cv::Mat up1;

    unwrapHeterodynePhases(up1Primary, up1Secondary, up1);

    up1 *= screenCols/(2.0*CV_PI);

    #ifdef QT_DEBUG

        cvtools::writeMat(up1, "up1.mat", "up1");

    #endif

    // Decode camera1 (vertical)

    std::vector<cv::Mat> frames1PrimaryVert(frames1Gray.begin()+2+nStepsPrimary+nStepsSecondary, frames1Gray.end()-nStepsSecondary);

    cv::Mat vp1Primary, amplitude1PrimaryVert, energy1PrimaryVert;

    decodePhaseShiftingSequence(frames1PrimaryVert, vp1Primary, amplitude1PrimaryVert, energy1PrimaryVert);

    std::vector<cv::Mat> frames1SecondaryVert(frames1Gray.end()-nStepsSecondary, frames1Gray.end());

    cv::Mat vp1Secondary, amplitude1SecondaryVert, energy1SecondaryVert;

    decodePhaseShiftingSequence(frames1SecondaryVert, vp1Secondary, amplitude1SecondaryVert, energy1SecondaryVert);

    cv::Mat vp1;

    unwrapHeterodynePhases(vp1Primary, vp1Secondary, vp1);

    vp1 *= screenCols/(2.0*CV_PI);

    #ifdef QT_DEBUG

        cvtools::writeMat(vp1, "vp1.mat", "vp1");

    #endif

    // color debayer and remap

    cv::Mat color0, color1;

    if(frames0[0].depth() == CV_8U)

        cv::cvtColor(frames0[0], color0, CV_BayerBG2RGB);

    else

        color0 = frames0[0];

    if(frames1[0].depth() == CV_8U)

        cv::cvtColor(frames1[0], color1, CV_BayerBG2GRAY);

    else

        color1 = frames1[0];

    #ifdef QT_DEBUG

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

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

    #endif

    // Occlusion masks

    cv::Mat occlusion0, occlusion1;

    cv::subtract(frames0Gray[0], frames0Gray[1], occlusion0);

    occlusion0 = (occlusion0 > 25) & (occlusion0 < 250);

    cv::subtract(frames1Gray[0], frames1Gray[1], occlusion1);

    occlusion1 = (occlusion1 > 25) & (occlusion1 < 250);

    // Threshold on energy at primary frequency

    occlusion0 = occlusion0 & (amplitude0PrimaryHorz > 5.0*nStepsPrimary);

    occlusion1 = occlusion1 & (amplitude1PrimaryHorz > 5.0*nStepsPrimary);

    // Threshold on energy ratios

    occlusion0 = occlusion0 & (amplitude0PrimaryHorz > 0.85*energy1PrimaryHorz);

    occlusion0 = occlusion0 & (amplitude0SecondaryHorz > 0.85*energy1SecondaryHorz);

    occlusion1 = occlusion1 & (amplitude1PrimaryHorz > 0.85*energy1PrimaryHorz);

    occlusion1 = occlusion1 & (amplitude1SecondaryHorz > 0.85*energy1SecondaryHorz);

//    // 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::Sobel(vp0, 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);

    cv::Sobel(vp1, 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<frameRows; row++){

        for(int col=0; col<frameCols; col++){

            if(!occlusion0.at<char>(row,col))

                continue;

            float up0i = up0.at<float>(row,col);

            float vp0i = vp0.at<float>(row,col);

            for(int row1=0; row1<vp1.rows-1; row1++){

                for(int col1=0; col1<up1.cols-1; col1++){

                    if(!occlusion1.at<char>(row1,col1) || !occlusion1.at<char>(row1,col1+1) || !occlusion1.at<char>(row1+1,col1) || !occlusion1.at<char>(row1+1,col1+1))

                        continue;

                    float up1Left = up1.at<float>(row1,col1);

                    float up1Right = up1.at<float>(row1,col1+1);

                    float vp1Up = vp1.at<float>(row1,col1);

                    float vp1Down = vp1.at<float>(row1+1,col1);

                    if((up1Left <= up0i) && (up0i <= up1Right) && (up0i-up1Left < 1.0) && (up1Right-up0i < 1.0) && (up1Right-up1Left > 0.1) &&

                       (vp1Up <= vp0i) && (vp0i <= vp1Down) && (vp0i-vp1Up < 1.0) && (vp1Down-vp0i < 1.0) && (vp1Down-vp1Up > 0.1)){

                        float col1i = col1 + (up0i-up1Left)/(up1Right-up1Left);

                        float row1i = row1 + (vp0i-vp1Up)/(vp1Up-vp1Down);

                        q0.push_back(cv::Point2f(col, row));

                        q1.push_back(cv::Point2f(col1i, row1i));

                        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;

    }

    // Correct for lens distortion

    cv::undistortPoints(q0, q0, calibration.K0, calibration.k0, cv::noArray(), calibration.K0);

    cv::undistortPoints(q1, q1, calibration.K1, calibration.k1, cv::noArray(), calibration.K1);

    cv::correctMatches(calibration.F, q0, q1, q0, q1);

    // Triangulate points

    cv::Mat P0(3, 4, CV_32F, cv::Scalar(0.0));

    cv::Mat(calibration.K0).copyTo(P0.colRange(0, 3));

    cv::Mat P1(3, 4, CV_32F), temp(3,4,CV_32F);

    cv::Mat(calibration.R1).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));

    cv::Mat(calibration.T1).copyTo(temp(cv::Range(0,3), cv::Range(3,4)));

    P1 = cv::Mat(calibration.K1) * temp;

    cv::Mat QMatHomogenous, QMat;

    cv::triangulatePoints(P0, P1, q0, q1, QMatHomogenous);

    cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);

    cvtools::matToPoints3f(QMat, Q);

}