Subversion Repositories seema-scanner

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

#include "cvtools.h"

#ifndef M_PI
    #define M_PI 3.14159265358979323846
#endif

static unsigned int nPhases = 8;

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

EncoderPhaseShift::EncoderPhaseShift(unsigned int _screenCols, unsigned int _screenRows, CodecDir _dir) : Encoder(_screenCols, _screenRows, _dir){

    // Set N
    if(dir == CodecDirBoth)
        this->N = 12;
    else
        this->N = 6;

    // Precompute encoded patterns
    const float pi = M_PI;

    if(dir & CodecDirHorizontal){
        // Horizontally encoding patterns
        for(unsigned int i=0; i<3; i++){
            float phase = 2.0*pi/3.0 * (1.0 - (float)i);
            float pitch = (float)screenCols/(float)nPhases;
            cv::Mat patternI(1,1,CV_8U);
            patternI = computePhaseVector(screenCols, phase, pitch);
            patternI = patternI.t();
            patterns.push_back(patternI);
        }

        // Phase cue patterns
        for(unsigned int i=0; i<3; i++){
            float phase = 2.0*pi/3.0 * (1.0 - (float)i);
            float pitch = screenCols;
            cv::Mat patternI;
            patternI = computePhaseVector(screenCols, phase, pitch);
            patternI = patternI.t();
            patterns.push_back(patternI);
        }
    }
    if(dir & CodecDirVertical){
        // Precompute vertically encoding patterns
        for(unsigned int i=0; i<3; i++){
            float phase = 2.0*pi/3.0 * (1.0 - (float)i);
            float pitch = (float)screenRows/(float)nPhases;
            cv::Mat patternI;
            patternI = computePhaseVector(screenRows, phase, pitch);
            patterns.push_back(patternI);
        }

        // Precompute vertically phase cue patterns
        for(unsigned int i=0; i<3; i++){
            float phase = 2.0*pi/3.0 * (1.0 - (float)i);
            float pitch = screenRows;
            cv::Mat patternI;
            patternI = computePhaseVector(screenRows, phase, pitch);
            patterns.push_back(patternI);
        }
    }
}

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

// Decoder
DecoderPhaseShift::DecoderPhaseShift(CodecDir _dir) : Decoder(_dir){

}

static cv::Mat absolutePhase(cv::Mat _I1, cv::Mat _I2, cv::Mat _I3){

    const float pi = M_PI;

    // Mat_ wrapper for easier indexing
    cv::Mat_<float> I1(_I1);
    cv::Mat_<float> I2(_I2);
    cv::Mat_<float> I3(_I3);

    cv::Mat absPhase(I1.size(), CV_32F);

    for(int i = 0; i < absPhase.rows; i++){
        for(int j = 0; j < absPhase.cols; j++){
            float phase = atan2(sqrt(3.0) * (I1(i,j)-I3(i,j)), I1(i,j) + I3(i,j) - 2.0*I2(i,j)) + pi;
            absPhase.at<float>(i,j) = phase;
        }
    }

    //absPhase.addref();
    return absPhase;
}

void DecoderPhaseShift::decodeFrames(const std::vector<cv::Mat> frames, cv::Mat &up, cv::Mat &vp, cv::Mat &mask, cv::Mat &shading){

    const float pi = M_PI;

    if(dir & CodecDirHorizontal){
        std::vector<cv::Mat> framesHorz(frames.begin(), frames.begin()+6);

        // Horizontal decoding
        up = absolutePhase(framesHorz[0], framesHorz[1], framesHorz[2]);

        cv::Mat upPhaseCue = absolutePhase(framesHorz[3], framesHorz[4], framesHorz[5]);
        upPhaseCue = (upPhaseCue*nPhases-up)/(2*pi);
        upPhaseCue.convertTo(upPhaseCue, CV_8U);
        upPhaseCue.convertTo(upPhaseCue, CV_32F);

        // unwrap absolute phase using phase cue
        up += upPhaseCue*2*pi;

    }
    if(dir & CodecDirVertical){
        std::vector<cv::Mat> framesVert(frames.end()-6, frames.end());

        // Vertical decoding
        vp = absolutePhase(framesVert[0], framesVert[1], framesVert[2]);

        cv::Mat vpPhaseCue = absolutePhase(framesVert[3], framesVert[4], framesVert[5]);
        vpPhaseCue = (vpPhaseCue*nPhases-vp)/(2*pi);
        vpPhaseCue.convertTo(vpPhaseCue, CV_8U);
        vpPhaseCue.convertTo(vpPhaseCue, CV_32F);

        // unwrap absolute phase using phase cue
        vp += vpPhaseCue*2*pi;

    }

    // Calculate modulation
    cv::Mat I1, I2, I3;
    frames[3].convertTo(I1, CV_32F);
    frames[4].convertTo(I2, CV_32F);
    frames[5].convertTo(I3, CV_32F);
    cv::Mat modulation;
    modulation = 3.0*(I1-I3).mul(I1-I3) + (2.0*I2-I1-I3).mul(2.0*I2-I1-I3);
    cv::sqrt(modulation, modulation);
    modulation.convertTo(shading, CV_8U);
    //shading = (1.0/3.0)*(framesHorz[3]+framesHorz[4]+framesHorz[5]);

    // Threshold modulation image for mask
    mask = shading > 50;

    cv::Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(7,7));
    cv::erode(mask, mask, element);
}