Subversion Repositories seema-scanner

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

}