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

    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 by means of disparity projection

    Q.resize(q0.size());

    cv::Matx44f QRectx = cv::Matx44f(QRect);

    cv::Matx33f R0invx = cv::Matx33f(cv::Mat(R0.t()));

    #pragma omp parallel for

    for(unsigned int i=0; i<q0.size(); i++){

        float disparity = q0[i][0]-q1[i][0];

        cv::Vec4f Qih = QRectx*cv::Vec4f(q0[i][0], q0[i][1], disparity, 1.0);

        float winv = float(1.0)/Qih[3];

        Q[i] = R0invx * cv::Point3f(Qih[0]*winv, Qih[1]*winv, Qih[2]*winv);

    }

}