WebSVN – seema-scanner – Diff – /src/algorithm/AlgorithmPhaseShiftEmbedded.cpp


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

Rev 193	Rev 195
1	`//`	1	`//`
2	`// Embedded Phase Shifting`	2	`// Embedded Phase Shifting`
3	`//`	3	`//`
4	`// This implementation follows "Moreno, Son, Taubin: Embedded Phase Shifting: Robust Phase Shifting with Embedded Signals, CVPR 2015"`	4	`// This implementation follows "Moreno, Son, Taubin: Embedded Phase Shifting: Robust Phase Shifting with Embedded Signals, CVPR 2015"`
5	`//`	5	`//`
6	`//`	6	`//`
7		7
8	`#include "AlgorithmPhaseShiftEmbedded.h"`	8	`#include "AlgorithmPhaseShiftEmbedded.h"`
9	`#include <math.h>`	9	`#include <math.h>`
10		10
11	`#include "cvtools.h"`	11	`#include "cvtools.h"`
12	`#include "algorithmtools.h"`	12	`#include "algorithmtools.h"`
13		13
14	`// Number of frequencies`	14	`// Number of frequencies`
15	`static const int M = 4;`	15	`static const int M = 4;`
16		16
17	`// Embedded periods (product of these must be greater than screenCols)`	17	`// Embedded periods (product of these must be greater than screenCols)`
18	`static const float Tm[M] = {16, 8, 8, 8};`	18	`static const float Tm[M] = {16, 8, 8, 8};`
19		19
20	`// Number of patterns at each frequency`	20	`// Number of patterns at each frequency`
21	`static const int Nm[M] = {3, 3, 3, 3};`	21	`static const int Nm[M] = {3, 3, 3, 3};`
22		22
23		23
24	`AlgorithmPhaseShiftEmbedded::AlgorithmPhaseShiftEmbedded(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){`	24	`AlgorithmPhaseShiftEmbedded::AlgorithmPhaseShiftEmbedded(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){`
25		25
26	`// Set N`	26	`// Set N`
27	`N = 2;`	27	`N = 2;`
28	`for(int m=0; m<M; m++)`	28	`for(int m=0; m<M; m++)`
29	`N += Nm[m];`	29	`N += Nm[m];`
30		30
31	`// all on pattern`	31	`// all on pattern`
32	`cv::Mat allOn(1, screenCols, CV_8UC3, cv::Scalar::all(255));`	32	`cv::Mat allOn(1, screenCols, CV_8UC3, cv::Scalar::all(255));`
33	`patterns.push_back(allOn);`	33	`patterns.push_back(allOn);`
34		34
35	`// all off pattern`	35	`// all off pattern`
36	`cv::Mat allOff(1, screenCols, CV_8UC3, cv::Scalar::all(0));`	36	`cv::Mat allOff(1, screenCols, CV_8UC3, cv::Scalar::all(0));`
37	`patterns.push_back(allOff);`	37	`patterns.push_back(allOff);`
38		38
39	`// Precompute encoded patterns`	39	`// Precompute encoded patterns`
40	`const float pi = M_PI;`	40	`const float pi = M_PI;`
41		41
42	`// Compute embedded frequencies`	42	`// Compute embedded frequencies`
43	`float Fm[M];`	43	`float Fm[M];`
44	`for(int m=0; m<M; m++){`	44	`for(int m=0; m<M; m++){`
45	`Fm[m] = 1.0;`	45	`Fm[m] = 1.0;`
46	`for(int i=0; i<=m; i++)`	46	`for(int i=0; i<=m; i++)`
47	`Fm[m] *= 1.0/Tm[i];`	47	`Fm[m] *= 1.0/Tm[i];`
48	`}`	48	`}`
49		49
50	`// Compute pattern frequencies`	50	`// Compute pattern frequencies`
51	`float fm[M];`	51	`float fm[M];`
52	`for(int m=0; m<M; m++)`	52	`for(int m=0; m<M; m++)`
53	`fm[m] = Fm[0];`	53	`fm[m] = Fm[0];`
54	`for(int m=1; m<M; m++)`	54	`for(int m=1; m<M; m++)`
55	`fm[m] += Fm[m];`	55	`fm[m] += Fm[m];`
56		56
57	`for(int m=0; m<M; m++)`	57	`for(int m=0; m<M; m++)`
58	`std::cout << fm[m] << std::endl;`	58	`std::cout << fm[m] << std::endl;`
59		59
60	`// Encoding patterns`	60	`// Encoding patterns`
61	`for(int m=0; m<M; m++){`	61	`for(int m=0; m<M; m++){`
62	`int nSteps = Nm[m];`	62	`int nSteps = Nm[m];`
63	`float frequency = fm[m];`	63	`float frequency = fm[m];`
64	`for(unsigned int i=0; i<nSteps; i++){`	64	`for(unsigned int i=0; i<nSteps; i++){`
65	`float phase = 2.0pi/std::max(nSteps, 3) i;`	65	`float phase = 2.0pi/std::max(nSteps, 3) i;`
66	`float pitch = 1.0/frequency;`	66	`float pitch = 1.0/frequency;`
67	`cv::Mat patternI(1,1,CV_8U);`	67	`cv::Mat patternI(1,1,CV_8U);`
68	`patternI = computePhaseVector(screenCols, phase, pitch);`	68	`patternI = computePhaseVector(screenCols, phase, pitch);`
69	`patterns.push_back(patternI.t());`	69	`patterns.push_back(patternI.t());`
70	`}`	70	`}`
71	`}`	71	`}`
72		72
73	`}`	73	`}`
74		74
75	`cv::Mat AlgorithmPhaseShiftEmbedded::getEncodingPattern(unsigned int depth){`	75	`cv::Mat AlgorithmPhaseShiftEmbedded::getEncodingPattern(unsigned int depth){`
76	`return patterns[depth];`	76	`return patterns[depth];`
77	`}`	77	`}`
78		78
79	`static void decodeEmbeddedPS(const std::vector<cv::Mat> &frames, cv::Mat &up, cv::Mat &upRange, float screenCols){`	79	`static void decodeEmbeddedPS(const std::vector<cv::Mat> &frames, cv::Mat &up, cv::Mat &upRange){`
80		80
81	`const int N = frames.size();`	81	`const int N = frames.size();`
82		82
83	`// Construct shift matrix`	83	`// Construct shift matrix`
84	`cv::Mat A(N, 1 + 2*M, CV_32F);`	84	`cv::Mat A(N, 1 + 2*M, CV_32F);`
-		85	`A.setTo(0.0);`
85	`A.col(0).setTo(1.0);`	86	`A.col(0).setTo(1.0);`
86		87
87	`int rowBegin = 0;`	88	`int rowBegin = 0;`
88	`for(int m=0; m<M; m++){`	89	`for(int m=0; m<M; m++){`
89		90
90	`int nSteps = Nm[m];`	91	`int nSteps = Nm[m];`
91		92
92	`cv::Mat Am(nSteps, 2, CV_32F);`	93	`cv::Mat Am(nSteps, 2, CV_32F);`
93		94
94	`for(unsigned int i=0; i<nSteps; i++){`	95	`for(unsigned int i=0; i<nSteps; i++){`
95	`float phase = 2.0CV_PI/std::max(nSteps, 3) i;`	96	`float phase = 2.0CV_PI/std::max(nSteps, 3) i;`
96		97
97	`Am.at<float>(i, 0) = std::cos(phase);`	98	`Am.at<float>(i, 0) = std::cos(phase);`
98	`Am.at<float>(i, 1) = -std::sin(phase);`	99	`Am.at<float>(i, 1) = -std::sin(phase);`
99	`}`	100	`}`
100		101
101	`// Copy into the A matrix`	102	`// Copy into the A matrix`
102	`Am.copyTo(A.rowRange(rowBegin, rowBegin+nSteps).colRange(1+2m, 1+2(m+1)));`	103	`Am.copyTo(A.rowRange(rowBegin, rowBegin+nSteps).colRange(1+2m, 1+2(m+1)));`
103	`rowBegin += nSteps;`	104	`rowBegin += nSteps;`
104	`}`	105	`}`
105	`//std::cout << A << std::endl << std::endl;`	106	`// std::cout << A << std::endl << std::endl;`
-		107
-		108	`// Invert A`
-		109	`cv::Mat Ainv;`
-		110	`cv::invert(A, Ainv, cv::DECOMP_SVD);`
106		111
107	`int frameRows = frames[0].rows;`	112	`int frameRows = frames[0].rows;`
108	`int frameCols = frames[0].cols;`	113	`int frameCols = frames[0].cols;`
109		114
110	`// DC-offset`	115	`// DC-offset`
111	`cv::Mat O(frameRows, frameCols, CV_32F);`	116	`cv::Mat O(frameRows, frameCols, CV_32F);`
112		117
113	`// Relative phase maps`	118	`// Relative phase maps`
114	`std::vector<cv::Mat> phim;`	119	`std::vector<cv::Mat> phim;`
115	`for(int i=0; i<N; i++)`	120	`for(int i=0; i<N; i++)`
116	`phim.push_back(cv::Mat(frameRows, frameCols, CV_32F));`	121	`phim.push_back(cv::Mat(frameRows, frameCols, CV_32F));`
117		122
118	`// Solve for relative phase values`	123	`// Solve for relative phase values`
119	`for(int row=0; row<frameRows; row++){`	124	`for(int row=0; row<frameRows; row++){`
120	`for(int col=0; col<frameCols; col++){`	125	`for(int col=0; col<frameCols; col++){`
121		126
122	`// Measurement vector`	127	`// Measurement vector`
123	`cv::Mat r(N, 1, CV_32F);`	128	`cv::Mat r(N, 1, CV_32F);`
124	`for(int i=0; i<N; i++)`	129	`for(int i=0; i<N; i++)`
125	`r.at<float>(i) = frames[i].at<uchar>(row, col);`	130	`r.at<float>(i) = frames[i].at<uchar>(row, col);`
126		131
127	`// Solve`	132	`// Solve`
128	`cv::Mat u; //[o, a cos1, a sin1, a cos2, a sin2, ...]`	133	`cv::Mat u; //[o, a cos1, a sin1, a cos2, a sin2, ...]`
129	`cv::solve(A, r, u, cv::DECOMP_SVD);`	134	`//cv::solve(A, r, u, cv::DECOMP_SVD);`
-		135	`u = Ainv*r;`
130		136
131	`for(int m=0; m<M; m++)`	137	`for(int m=0; m<M; m++)`
132	`phim[m].at<float>(row, col) = std::atan2(u.at<float>(m2+1), u.at<float>(m2+2));`	138	`phim[m].at<float>(row, col) = std::atan2(u.at<float>(m2+1), u.at<float>(m2+2));`
133		139
134	`O.at<float>(row, col) = u.at<float>(0);`	140	`O.at<float>(row, col) = u.at<float>(0);`
135	`}`	141	`}`
136		142
137	`}`	143	`}`
138		144
139	`#if 0`	145	`#if 0`
140	`for(int i=0; i<N; i++)`	146	`for(int i=0; i<N; i++)`
141	`cvtools::writeMat(frames[i], QString("frames_%1.mat").arg(i).toStdString().c_str());`	147	`cv::imwrite(QString("frames_%1.png").arg(i).toStdString().c_str(), frames[i]);`
142	`cvtools::writeMat(O, "O.mat");`	148	`cvtools::writeMat(O, "O.mat");`
143	`for(int m=0; m<M; m++)`	149	`for(int m=0; m<M; m++)`
144	`cvtools::writeMat(phim[m], QString("phim_%1.mat").arg(m).toStdString().c_str());`	150	`cvtools::writeMat(phim[m], QString("phim_%1.mat").arg(m).toStdString().c_str());`
145	`#endif`	151	`#endif`
146		152
147	`// Determine phase cue sequence`	153	`// Determine phase cue sequence`
148	`std::vector<cv::Mat> Phim(M);`	154	`std::vector<cv::Mat> Phim(M);`
149	`Phim[0] = phim[0];`	155	`Phim[0] = phim[0];`
150	`for(int m=1; m<M; m++){`	156	`for(int m=1; m<M; m++){`
151	`cv::subtract(phim[m], phim[0], Phim[m]);`	157	`cv::subtract(phim[m], phim[0], Phim[m]);`
152	`Phim[m] = cvtools::modulo(Phim[m], 2.0*CV_PI);`	158	`Phim[m] = cvtools::modulo(Phim[m], 2.0*CV_PI);`
153	`}`	159	`}`
154		160
155	`// Note: Phim[1] is the cue of highest quality`	161	`// Note: Phim[1] is the cue of highest quality`
156		162
157	`#if 0`	163	`#if 0`
158	`for(int m=0; m<M; m++)`	164	`for(int m=0; m<M; m++)`
159	`cvtools::writeMat(Phim[m], QString("Phim_%1.mat").arg(m).toStdString().c_str());`	165	`cvtools::writeMat(Phim[m], QString("Phim_%1.mat").arg(m).toStdString().c_str());`
160	`#endif`	166	`#endif`
161		167
162	`// Compute embedded frequencies`	168	`// Compute embedded frequencies`
163	`float Fm[M];`	169	`float Fm[M];`
164	`for(int m=0; m<M; m++){`	170	`for(int m=0; m<M; m++){`
165	`Fm[m] = 1.0;`	171	`Fm[m] = 1.0;`
166	`for(int i=0; i<=m; i++)`	172	`for(int i=0; i<=m; i++)`
167	`Fm[m] *= 1.0/Tm[i];`	173	`Fm[m] *= 1.0/Tm[i];`
168	`}`	174	`}`
169		175
-		176	`// Compute pattern frequencies`
-		177	`float fm[M];`
-		178	`for(int m=0; m<M; m++)`
-		179	`fm[m] = Fm[0];`
-		180	`for(int m=1; m<M; m++)`
-		181	`fm[m] += Fm[m];`
-		182
170	`// Unwrap phase cue sequence`	183	`// Unwrap phase cue sequence`
171	`cv::Mat upCue = Phim[M-1];`	184	`cv::Mat upCue = Phim[M-1];`
172	`for(int m=M-2; m>0; m--){`	185	`for(int m=M-2; m>0; m--){`
173	`upCue = unwrapWithCue(Phim[m], upCue, screenCols*Fm[m]);`	186	`upCue = unwrapWithCue(Phim[m], upCue, Fm[m]/Fm[m+1]);`
174	`#if 1`	187	`#if 0`
175	`cvtools::writeMat(upCue, "upCue.mat", "upCue");`	188	`cvtools::writeMat(upCue, "upCue.mat", "upCue");`
176	`#endif`	189	`#endif`
177	`}`	190	`}`
178		191
179	`// Unwrap high frequency patterns`	192	`// Unwrap high frequency patterns`
180	`std::vector<cv::Mat> upm(M);`	193	`std::vector<cv::Mat> upm(M);`
181	`for(int m=0; m<M; m++){`	194	`for(int m=0; m<M; m++){`
182	`upm[m] = unwrapWithCue(phim[m], upCue, 1.0/Fm[1]);`	195	`upm[m] = unwrapWithCue(phim[m], upCue, fm[m]/Fm[M-1]);`
183	`}`	196	`}`
184		197
185	`#if 1`	198	`#if 1`
186	`for(int m=0; m<M; m++)`	199	`for(int m=0; m<M; m++)`
187	`cvtools::writeMat(upm[m], QString("upm_%1.mat").arg(m).toStdString().c_str());`	200	`cvtools::writeMat(upm[m], QString("upm_%1.mat").arg(m).toStdString().c_str());`
188	`#endif`	201	`#endif`
189		202
190	`// Determine range of phases (for outlier detection)`	203	`// Determine range of phases (for outlier detection)`
191	`cv::Mat upMin = upm[0];`	204	`cv::Mat upMin = upm[0].clone();`
192	`cv::Mat upMax = upm[0];`	205	`cv::Mat upMax = upm[0].clone();`
193	`for(int m=1; m<M; m++){`	206	`for(int m=1; m<M; m++){`
194	`upMin = cv::min(upMin, upm[m]);`	207	`upMin = cv::min(upm[m], upMin);`
195	`upMax = cv::max(upMax, upm[m]);`	208	`upMax = cv::max(upm[m], upMax);`
196	`}`	209	`}`
-		210
-		211	`#if 1`
-		212	`cvtools::writeMat(upMin, "upMin.mat");`
-		213	`cvtools::writeMat(upMax, "upMax.mat");`
-		214	`#endif`
-		215
197	`upRange = upMax-upMin;`	216	`upRange = upMax-upMin;`
198		217
199	`// Return average of phase maps`	218	`// Return average of phase maps`
200	`up = upm[0];`	219	`up = upm[0].clone();`
201	`for(int m=1; m<M; m++)`	220	`for(int m=1; m<M; m++)`
202	`up += upm[m];`	221	`up += upm[m];`
203	`up /= M;`	222	`up /= M;`
204	`}`	223	`}`
205		224
206	`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){`	225	`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){`
207		226
208	`assert(frames0.size() == N);`	227	`assert(frames0.size() == N);`
209	`assert(frames1.size() == N);`	228	`assert(frames1.size() == N);`
210		229
211	`int frameRows = frames0[0].rows;`	230	`int frameRows = frames0[0].rows;`
212	`int frameCols = frames0[0].cols;`	231	`int frameCols = frames0[0].cols;`
213		232
214	`// Rectifying homographies (rotation+projections)`	233	`// Rectifying homographies (rotation+projections)`
215	`cv::Size frameSize(frameCols, frameRows);`	234	`cv::Size frameSize(frameCols, frameRows);`
216	`cv::Mat R, T;`	235	`cv::Mat R, T;`
217	`// stereoRectify segfaults unless R is double precision`	236	`// stereoRectify segfaults unless R is double precision`
218	`cv::Mat(calibration.R1).convertTo(R, CV_64F);`	237	`cv::Mat(calibration.R1).convertTo(R, CV_64F);`
219	`cv::Mat(calibration.T1).convertTo(T, CV_64F);`	238	`cv::Mat(calibration.T1).convertTo(T, CV_64F);`
220	`cv::Mat R0, R1, P0, P1, QRect;`	239	`cv::Mat R0, R1, P0, P1, QRect;`
221	`cv::stereoRectify(calibration.K0, calibration.k0, calibration.K1, calibration.k1, frameSize, R, T, R0, R1, P0, P1, QRect, 0);`	240	`cv::stereoRectify(calibration.K0, calibration.k0, calibration.K1, calibration.k1, frameSize, R, T, R0, R1, P0, P1, QRect, 0);`
222		241
223	`// Interpolation maps (lens distortion and rectification)`	242	`// Interpolation maps (lens distortion and rectification)`
224	`cv::Mat map0X, map0Y, map1X, map1Y;`	243	`cv::Mat map0X, map0Y, map1X, map1Y;`
225	`cv::initUndistortRectifyMap(calibration.K0, calibration.k0, R0, P0, frameSize, CV_32F, map0X, map0Y);`	244	`cv::initUndistortRectifyMap(calibration.K0, calibration.k0, R0, P0, frameSize, CV_32F, map0X, map0Y);`
226	`cv::initUndistortRectifyMap(calibration.K1, calibration.k1, R1, P1, frameSize, CV_32F, map1X, map1Y);`	245	`cv::initUndistortRectifyMap(calibration.K1, calibration.k1, R1, P1, frameSize, CV_32F, map1X, map1Y);`
227		246
228	`int frameRectRows = map0X.rows;`	247	`int frameRectRows = map0X.rows;`
229	`int frameRectCols = map0X.cols;`	248	`int frameRectCols = map0X.cols;`
230		249
231	`// Gray-scale and remap`	250	`// Gray-scale and remap`
232	`std::vector<cv::Mat> frames0Rect(N);`	251	`std::vector<cv::Mat> frames0Rect(N);`
233	`std::vector<cv::Mat> frames1Rect(N);`	252	`std::vector<cv::Mat> frames1Rect(N);`
234	`for(unsigned int i=0; i<N; i++){`	253	`for(unsigned int i=0; i<N; i++){`
235	`cv::Mat temp;`	254	`cv::Mat temp;`
236	`cv::cvtColor(frames0[i], temp, CV_BayerBG2GRAY);`	255	`cv::cvtColor(frames0[i], temp, CV_BayerBG2GRAY);`
237	`cv::remap(temp, frames0Rect[i], map0X, map0Y, CV_INTER_LINEAR);`	256	`cv::remap(temp, frames0Rect[i], map0X, map0Y, CV_INTER_LINEAR);`
238	`cv::cvtColor(frames1[i], temp, CV_BayerBG2GRAY);`	257	`cv::cvtColor(frames1[i], temp, CV_BayerBG2GRAY);`
239	`cv::remap(temp, frames1Rect[i], map1X, map1Y, CV_INTER_LINEAR);`	258	`cv::remap(temp, frames1Rect[i], map1X, map1Y, CV_INTER_LINEAR);`
240	`}`	259	`}`
241		260
242	`// Decode camera 0`	261	`// Decode camera 0`
243	`std::vector<cv::Mat> frames0Patterns(frames0Rect.begin()+2, frames0Rect.end());`	262	`std::vector<cv::Mat> frames0Patterns(frames0Rect.begin()+2, frames0Rect.end());`
244		263
245	`cv::Mat up0, up0Range;`	264	`cv::Mat up0, up0Range;`
246	`decodeEmbeddedPS(frames0Patterns, up0, up0Range, screenCols);`	265	`decodeEmbeddedPS(frames0Patterns, up0, up0Range);`
247	`up0 *= screenCols;`	266	`up0 *= screenCols;`
248		267
249	`#ifdef QT_DEBUG`	268	`#ifdef QT_DEBUG`
250	`cvtools::writeMat(up0, "up0.mat", "up0");`	269	`cvtools::writeMat(up0, "up0.mat", "up0");`
251	`cvtools::writeMat(up0Range, "up0Range.mat", "up0Range");`	270	`cvtools::writeMat(up0Range, "up0Range.mat", "up0Range");`
252	`#endif`	271	`#endif`
253		272
254	`// Decode camera 1`	273	`// Decode camera 1`
255	`std::vector<cv::Mat> frames1Patterns(frames1Rect.begin()+2, frames1Rect.end());`	274	`std::vector<cv::Mat> frames1Patterns(frames1Rect.begin()+2, frames1Rect.end());`
256		275
257	`cv::Mat up1, up1Range;`	276	`cv::Mat up1, up1Range;`
258	`decodeEmbeddedPS(frames1Patterns, up1, up1Range, screenCols);`	277	`decodeEmbeddedPS(frames1Patterns, up1, up1Range);`
259	`up1 *= screenCols;`	278	`up1 *= screenCols;`
260		279
261	`#ifdef QT_DEBUG`	280	`#ifdef QT_DEBUG`
262	`cvtools::writeMat(up1, "up1.mat", "up1");`	281	`cvtools::writeMat(up1, "up1.mat", "up1");`
263	`#endif`	282	`#endif`
264		283
265	`// Color debayer and remap`	284	`// Color debayer and remap`
266	`cv::Mat color0, color1;`	285	`cv::Mat color0, color1;`
267	`cv::cvtColor(frames0[0], color0, CV_BayerBG2RGB);`	286	`cv::cvtColor(frames0[0], color0, CV_BayerBG2RGB);`
268	`cv::remap(color0, color0, map0X, map0Y, CV_INTER_LINEAR);`	287	`cv::remap(color0, color0, map0X, map0Y, CV_INTER_LINEAR);`
269		288
270	`cv::cvtColor(frames1[0], color1, CV_BayerBG2RGB);`	289	`cv::cvtColor(frames1[0], color1, CV_BayerBG2RGB);`
271	`cv::remap(color1, color1, map1X, map1Y, CV_INTER_LINEAR);`	290	`cv::remap(color1, color1, map1X, map1Y, CV_INTER_LINEAR);`
272		291
273	`#ifdef QT_DEBUG`	292	`#ifdef QT_DEBUG`
274	`cvtools::writeMat(color0, "color0.mat", "color0");`	293	`cvtools::writeMat(color0, "color0.mat", "color0");`
275	`cvtools::writeMat(color1, "color1.mat", "color1");`	294	`cvtools::writeMat(color1, "color1.mat", "color1");`
276	`#endif`	295	`#endif`
277		296
278	`// Occlusion masks`	297	`// Occlusion masks`
279	`cv::Mat occlusion0, occlusion1;`	298	`cv::Mat occlusion0, occlusion1;`
280	`cv::subtract(frames0Rect[0], frames0Rect[1], occlusion0);`	299	`cv::subtract(frames0Rect[0], frames0Rect[1], occlusion0);`
281	`occlusion0 = (occlusion0 > 25) & (occlusion0 < 250);`	300	`occlusion0 = (occlusion0 > 25) & (occlusion0 < 250);`
282	`cv::subtract(frames1Rect[0], frames1Rect[1], occlusion1);`	301	`cv::subtract(frames1Rect[0], frames1Rect[1], occlusion1);`
283	`occlusion1 = (occlusion1 > 25) & (occlusion1 < 250);`	302	`occlusion1 = (occlusion1 > 25) & (occlusion1 < 250);`
284		303
285	`// // Threshold on energy at primary frequency`	304	`// // Threshold on energy at primary frequency`
286	`// occlusion0 = occlusion0 & (amplitude0 > 5.0*nStepsPrimary);`	305	`// occlusion0 = occlusion0 & (amplitude0 > 5.0*nStepsPrimary);`
287	`// occlusion1 = occlusion1 & (amplitude1 > 5.0*nStepsPrimary);`	306	`// occlusion1 = occlusion1 & (amplitude1 > 5.0*nStepsPrimary);`
288		307
289	`// // Erode occlusion masks`	308	`// // Erode occlusion masks`
290	`// cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5,5));`	309	`// cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5,5));`
291	`// cv::erode(occlusion0, occlusion0, strel);`	310	`// cv::erode(occlusion0, occlusion0, strel);`
292	`// cv::erode(occlusion1, occlusion1, strel);`	311	`// cv::erode(occlusion1, occlusion1, strel);`
293		312
294	`// Threshold on gradient of phase`	313	`// Threshold on gradient of phase`
295	`cv::Mat edges0;`	314	`cv::Mat edges0;`
296	`cv::Sobel(up0, edges0, -1, 1, 1, 5);`	315	`cv::Sobel(up0, edges0, -1, 1, 1, 5);`
297	`occlusion0 = occlusion0 & (abs(edges0) < 150);`	316	`occlusion0 = occlusion0 & (abs(edges0) < 150);`
298		317
299	`cv::Mat edges1;`	318	`cv::Mat edges1;`
300	`cv::Sobel(up1, edges1, -1, 1, 1, 5);`	319	`cv::Sobel(up1, edges1, -1, 1, 1, 5);`
301	`occlusion1 = occlusion1 & (abs(edges1) < 150);`	320	`occlusion1 = occlusion1 & (abs(edges1) < 150);`
302		321
303	`#ifdef QT_DEBUG`	322	`#ifdef QT_DEBUG`
304	`cvtools::writeMat(occlusion0, "occlusion0.mat", "occlusion0");`	323	`cvtools::writeMat(occlusion0, "occlusion0.mat", "occlusion0");`
305	`cvtools::writeMat(occlusion1, "occlusion1.mat", "occlusion1");`	324	`cvtools::writeMat(occlusion1, "occlusion1.mat", "occlusion1");`
306	`#endif`	325	`#endif`
307		326
308	`// Match phase maps`	327	`// Match phase maps`
309		328
310	`// camera0 against camera1`	329	`// camera0 against camera1`
311	`std::vector<cv::Vec2f> q0, q1;`	330	`std::vector<cv::Vec2f> q0, q1;`
312	`for(int row=0; row<frameRectRows; row++){`	331	`for(int row=0; row<frameRectRows; row++){`
313	`for(int col=0; col<frameRectCols; col++){`	332	`for(int col=0; col<frameRectCols; col++){`
314		333
315	`if(!occlusion0.at<char>(row,col))`	334	`if(!occlusion0.at<char>(row,col))`
316	`continue;`	335	`continue;`
317		336
318	`float up0i = up0.at<float>(row,col);`	337	`float up0i = up0.at<float>(row,col);`
319	`for(int col1=0; col1<up1.cols-1; col1++){`	338	`for(int col1=0; col1<up1.cols-1; col1++){`
320		339
321	`if(!occlusion1.at<char>(row,col1) \|\| !occlusion1.at<char>(row,col1+1))`	340	`if(!occlusion1.at<char>(row,col1) \|\| !occlusion1.at<char>(row,col1+1))`
322	`continue;`	341	`continue;`
323		342
324	`float up1Left = up1.at<float>(row,col1);`	343	`float up1Left = up1.at<float>(row,col1);`
325	`float up1Right = up1.at<float>(row,col1+1);`	344	`float up1Right = up1.at<float>(row,col1+1);`
326		345
327	`if((up1Left <= up0i) && (up0i <= up1Right) && (up0i-up1Left < 1.0) && (up1Right-up0i < 1.0) && (up1Right-up1Left > 0.1)){`	346	`if((up1Left <= up0i) && (up0i <= up1Right) && (up0i-up1Left < 1.0) && (up1Right-up0i < 1.0) && (up1Right-up1Left > 0.1)){`
328		347
329	`float col1i = col1 + (up0i-up1Left)/(up1Right-up1Left);`	348	`float col1i = col1 + (up0i-up1Left)/(up1Right-up1Left);`
330		349
331	`q0.push_back(cv::Point2f(col, row));`	350	`q0.push_back(cv::Point2f(col, row));`
332	`q1.push_back(cv::Point2f(col1i, row));`	351	`q1.push_back(cv::Point2f(col1i, row));`
333		352
334	`break;`	353	`break;`
335	`}`	354	`}`
336	`}`	355	`}`
337	`}`	356	`}`
338	`}`	357	`}`
339		358
340		359
341	`int nMatches = q0.size();`	360	`int nMatches = q0.size();`
342		361
343	`if(nMatches < 1){`	362	`if(nMatches < 1){`
344	`Q.resize(0);`	363	`Q.resize(0);`
345	`color.resize(0);`	364	`color.resize(0);`
346		365
347	`return;`	366	`return;`
348	`}`	367	`}`
349		368
350	`// Retrieve color information`	369	`// Retrieve color information`
351	`color.resize(nMatches);`	370	`color.resize(nMatches);`
352	`for(int i=0; i<nMatches; i++){`	371	`for(int i=0; i<nMatches; i++){`
353		372
354	`cv::Vec3b c0 = color0.at<cv::Vec3b>(q0[i][1], q0[i][0]);`	373	`cv::Vec3b c0 = color0.at<cv::Vec3b>(q0[i][1], q0[i][0]);`
355	`cv::Vec3b c1 = color1.at<cv::Vec3b>(q1[i][1], q1[i][0]);`	374	`cv::Vec3b c1 = color1.at<cv::Vec3b>(q1[i][1], q1[i][0]);`
356		375
357	`color[i] = 0.5c0 + 0.5c1;`	376	`color[i] = 0.5c0 + 0.5c1;`
358	`}`	377	`}`
359		378
360	`// Triangulate points`	379	`// Triangulate points`
361	`cv::Mat QMatHomogenous, QMat;`	380	`cv::Mat QMatHomogenous, QMat;`
362	`cv::triangulatePoints(P0, P1, q0, q1, QMatHomogenous);`	381	`cv::triangulatePoints(P0, P1, q0, q1, QMatHomogenous);`
363	`cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);`	382	`cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);`
364		383
365	`// Undo rectification`	384	`// Undo rectification`
366	`cv::Mat R0Inv;`	385	`cv::Mat R0Inv;`
367	`cv::Mat(R0.t()).convertTo(R0Inv, CV_32F);`	386	`cv::Mat(R0.t()).convertTo(R0Inv, CV_32F);`
368	`QMat = R0Inv*QMat;`	387	`QMat = R0Inv*QMat;`
369		388
370	`cvtools::matToPoints3f(QMat, Q);`	389	`cvtools::matToPoints3f(QMat, Q);`
371		390
372	`}`	391	`}`
373		392

Subversion Repositories seema-scanner

(root)/src/algorithm/AlgorithmPhaseShiftEmbedded.cpp – Rev 193 → 195