Subversion Repositories seema-scanner

//
// Embedded Phase Shifting
//
// This implementation follows "Moreno, Son, Taubin: Embedded Phase Shifting: Robust Phase Shifting with Embedded Signals, CVPR 2015"
//
//

#include "AlgorithmPhaseShiftEmbedded.h"
#include <math.h>

#include "cvtools.h"
#include "algorithmtools.h"

// Number of frequencies
static const int M = 4;

// Embedded periods (product of these must be greater than screenCols)
static const float Tm[M] = {16, 8, 8, 8};

// Number of patterns at each frequency
static const int Nm[M] = {3, 3, 3, 3};


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

    // Set N
    N = 2;
    for(int m=0; m<M; m++)
        N += Nm[m];

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

    // Compute embedded frequencies
    float Fm[M];
    for(int m=0; m<M; m++){
        Fm[m] = 1.0;
        for(int i=0; i<=m; i++)
            Fm[m] *= 1.0/Tm[i];
    }

    // Compute pattern frequencies
    float fm[M];
    for(int m=0; m<M; m++)
        fm[m] = Fm[0];
    for(int m=1; m<M; m++)
        fm[m] += Fm[m];

    for(int m=0; m<M; m++)
        std::cout << fm[m] << std::endl;

    // Encoding patterns
    for(int m=0; m<M; m++){
        int nSteps = Nm[m];
        float frequency = fm[m];
        for(unsigned int i=0; i<nSteps; i++){
            float phase = 2.0*pi/std::max(nSteps, 3) * i;
            float pitch = 1.0/frequency;
            cv::Mat patternI(1,1,CV_8U);
            patternI = computePhaseVector(screenCols, phase, pitch);
            patterns.push_back(patternI.t());
        }
    }

}

cv::Mat AlgorithmPhaseShiftEmbedded::getEncodingPattern(unsigned int depth){
    return patterns[depth];
}

static void decodeEmbeddedPS(const std::vector<cv::Mat> &frames, cv::Mat &up, cv::Mat &upRange){

    const int N = frames.size();

    // Construct shift matrix
    cv::Mat A(N, 1 + 2*M, CV_32F);
    A.setTo(0.0);
    A.col(0).setTo(1.0);

    int rowBegin = 0;
    for(int m=0; m<M; m++){

        int nSteps = Nm[m];

        cv::Mat Am(nSteps, 2, CV_32F);

        for(unsigned int i=0; i<nSteps; i++){
            float phase = 2.0*CV_PI/std::max(nSteps, 3) * i;

            Am.at<float>(i, 0) = std::cos(phase);
            Am.at<float>(i, 1) = -std::sin(phase);
        }

        // Copy into the A matrix
        Am.copyTo(A.rowRange(rowBegin, rowBegin+nSteps).colRange(1+2*m, 1+2*(m+1)));
        rowBegin += nSteps;
    }
//    std::cout << A << std::endl << std::endl;

    // Invert A
    cv::Mat Ainv;
    cv::invert(A, Ainv, cv::DECOMP_SVD);

    int frameRows = frames[0].rows;
    int frameCols = frames[0].cols;

    // DC-offset
    cv::Mat O(frameRows, frameCols, CV_32F);

    // Relative phase maps
    std::vector<cv::Mat> phim;
    for(int i=0; i<N; i++)
        phim.push_back(cv::Mat(frameRows, frameCols, CV_32F));

    // Solve for relative phase values
    for(int row=0; row<frameRows; row++){
        for(int col=0; col<frameCols; col++){

            // Measurement vector
            cv::Mat r(N, 1, CV_32F);
            for(int i=0; i<N; i++)
                r.at<float>(i) = frames[i].at<uchar>(row, col);

            // Solve
            cv::Mat u; //[o, a cos1, a sin1, a cos2, a sin2, ...]
            //cv::solve(A, r, u, cv::DECOMP_SVD);
            u = Ainv*r;

            for(int m=0; m<M; m++)
                phim[m].at<float>(row, col) = std::atan2(u.at<float>(m*2+1), u.at<float>(m*2+2));

            O.at<float>(row, col) = u.at<float>(0);
        }

    }

    #if 0
        for(int i=0; i<N; i++)
            cv::imwrite(QString("frames_%1.png").arg(i).toStdString().c_str(), frames[i]);
        cvtools::writeMat(O, "O.mat");
        for(int m=0; m<M; m++)
            cvtools::writeMat(phim[m], QString("phim_%1.mat").arg(m).toStdString().c_str());
    #endif

    // Determine phase cue sequence
    std::vector<cv::Mat> Phim(M);
    Phim[0] = phim[0];
    for(int m=1; m<M; m++){
        cv::subtract(phim[m], phim[0], Phim[m]);
        Phim[m] = cvtools::modulo(Phim[m], 2.0*CV_PI);
    }

    // Note: Phim[1] is the cue of highest quality

    #if 0
        for(int m=0; m<M; m++)
            cvtools::writeMat(Phim[m], QString("Phim_%1.mat").arg(m).toStdString().c_str());
    #endif

    // Compute embedded frequencies
    float Fm[M];
    for(int m=0; m<M; m++){
        Fm[m] = 1.0;
        for(int i=0; i<=m; i++)
            Fm[m] *= 1.0/Tm[i];
    }

    // Compute pattern frequencies
    float fm[M];
    for(int m=0; m<M; m++)
        fm[m] = Fm[0];
    for(int m=1; m<M; m++)
        fm[m] += Fm[m];

    // Unwrap phase cue sequence
    cv::Mat upCue = Phim[M-1];
    for(int m=M-2; m>0; m--){
        upCue = unwrapWithCue(Phim[m], upCue, Fm[m]/Fm[m+1]);
        #if 0
                cvtools::writeMat(upCue, "upCue.mat", "upCue");
        #endif
    }

    // Unwrap high frequency patterns
    std::vector<cv::Mat> upm(M);
    for(int m=0; m<M; m++){
        upm[m] = unwrapWithCue(phim[m], upCue, fm[m]/Fm[M-1]);
    }

    #if 1
        for(int m=0; m<M; m++)
            cvtools::writeMat(upm[m], QString("upm_%1.mat").arg(m).toStdString().c_str());
    #endif

    // Determine range of phases (for outlier detection)
    cv::Mat upMin = upm[0].clone();
    cv::Mat upMax = upm[0].clone();
    for(int m=1; m<M; m++){
        upMin = cv::min(upm[m], upMin);
        upMax = cv::max(upm[m], upMax);
    }

    #if 1
        cvtools::writeMat(upMin, "upMin.mat");
        cvtools::writeMat(upMax, "upMax.mat");
    #endif

    upRange = upMax-upMin;

    // Return average of phase maps
    up = upm[0].clone();
    for(int m=1; m<M; m++)
        up += upm[m];
    up /= M;
}

void AlgorithmPhaseShiftEmbedded::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){

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

    int frameRectRows = map0X.rows;
    int frameRectCols = map0X.cols;

    // 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 camera 0
    std::vector<cv::Mat> frames0Patterns(frames0Rect.begin()+2, frames0Rect.end());

    cv::Mat up0, up0Range;
    decodeEmbeddedPS(frames0Patterns, up0, up0Range);
    up0 *= screenCols;

    #ifdef QT_DEBUG
        cvtools::writeMat(up0, "up0.mat", "up0");
        cvtools::writeMat(up0Range, "up0Range.mat", "up0Range");
    #endif

    // Decode camera 1
    std::vector<cv::Mat> frames1Patterns(frames1Rect.begin()+2, frames1Rect.end());

    cv::Mat up1, up1Range;
    decodeEmbeddedPS(frames1Patterns, up1, up1Range);
    up1 *= screenCols;

    #ifdef QT_DEBUG
        cvtools::writeMat(up1, "up1.mat", "up1");
    #endif

    // 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 QT_DEBUG
        cvtools::writeMat(color0, "color0.mat", "color0");
        cvtools::writeMat(color1, "color1.mat", "color1");
    #endif

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

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

    #ifdef QT_DEBUG
        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;
    }

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

}