Subversion Repositories seema-scanner

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

#include "cvtools.h"

#ifndef M_PI
    #define M_PI 3.14159265358979323846
#endif

static unsigned int nPhases = 8;

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

AlgorithmPhaseShift::AlgorithmPhaseShift(unsigned int _screenCols, unsigned int _screenRows, CodingDir _dir) : Algorithm(_screenCols, _screenRows, _dir){

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

    // Precompute encoded patterns
    const float pi = M_PI;

    if(dir & CodingDirHorizontal){
        // 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 & CodingDirVertical){
        // 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 AlgorithmPhaseShift::getEncodingPattern(unsigned int depth){
    return patterns[depth];
}


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 AlgorithmPhaseShift::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){

}