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(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 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 195	Rev 207
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){`	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.setTo(0.0);`
86	`A.col(0).setTo(1.0);`	86	`A.col(0).setTo(1.0);`
87		87
88	`int rowBegin = 0;`	88	`int rowBegin = 0;`
89	`for(int m=0; m<M; m++){`	89	`for(int m=0; m<M; m++){`
90		90
91	`int nSteps = Nm[m];`	91	`int nSteps = Nm[m];`
92		92
93	`cv::Mat Am(nSteps, 2, CV_32F);`	93	`cv::Mat Am(nSteps, 2, CV_32F);`
94		94
95	`for(unsigned int i=0; i<nSteps; i++){`	95	`for(unsigned int i=0; i<nSteps; i++){`
96	`float phase = 2.0CV_PI/std::max(nSteps, 3) i;`	96	`float phase = 2.0CV_PI/std::max(nSteps, 3) i;`
97		97
98	`Am.at<float>(i, 0) = std::cos(phase);`	98	`Am.at<float>(i, 0) = std::cos(phase);`
99	`Am.at<float>(i, 1) = -std::sin(phase);`	99	`Am.at<float>(i, 1) = -std::sin(phase);`
100	`}`	100	`}`
101		101
102	`// Copy into the A matrix`	102	`// Copy into the A matrix`
103	`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)));`
104	`rowBegin += nSteps;`	104	`rowBegin += nSteps;`
105	`}`	105	`}`
106	`// std::cout << A << std::endl << std::endl;`	106	`// std::cout << A << std::endl << std::endl;`
107		107
108	`// Invert A`	108	`// Invert A`
109	`cv::Mat Ainv;`	109	`cv::Mat Ainv;`
110	`cv::invert(A, Ainv, cv::DECOMP_SVD);`	110	`cv::invert(A, Ainv, cv::DECOMP_SVD);`
111		111
112	`int frameRows = frames[0].rows;`	112	`int frameRows = frames[0].rows;`
113	`int frameCols = frames[0].cols;`	113	`int frameCols = frames[0].cols;`
114		114
115	`// DC-offset`	115	`// DC-offset`
116	`cv::Mat O(frameRows, frameCols, CV_32F);`	116	`cv::Mat O(frameRows, frameCols, CV_32F);`
117		117
118	`// Relative phase maps`	118	`// Relative phase maps`
119	`std::vector<cv::Mat> phim;`	119	`std::vector<cv::Mat> phim;`
120	`for(int i=0; i<N; i++)`	120	`for(int i=0; i<N; i++)`
121	`phim.push_back(cv::Mat(frameRows, frameCols, CV_32F));`	121	`phim.push_back(cv::Mat(frameRows, frameCols, CV_32F));`
122		122
123	`// Solve for relative phase values`	123	`// Solve for relative phase values`
124	`for(int row=0; row<frameRows; row++){`	124	`for(int row=0; row<frameRows; row++){`
125	`for(int col=0; col<frameCols; col++){`	125	`for(int col=0; col<frameCols; col++){`
126		126
127	`// Measurement vector`	127	`// Measurement vector`
128	`cv::Mat r(N, 1, CV_32F);`	128	`cv::Mat r(N, 1, CV_32F);`
129	`for(int i=0; i<N; i++)`	129	`for(int i=0; i<N; i++)`
130	`r.at<float>(i) = frames[i].at<uchar>(row, col);`	130	`r.at<float>(i) = frames[i].at<uchar>(row, col);`
131		131
132	`// Solve`	132	`// Solve`
133	`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, ...]`
134	`//cv::solve(A, r, u, cv::DECOMP_SVD);`	134	`//cv::solve(A, r, u, cv::DECOMP_SVD);`
135	`u = Ainv*r;`	135	`u = Ainv*r;`
136		136
137	`for(int m=0; m<M; m++)`	137	`for(int m=0; m<M; m++)`
138	`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));`
139		139
140	`O.at<float>(row, col) = u.at<float>(0);`	140	`O.at<float>(row, col) = u.at<float>(0);`
141	`}`	141	`}`
142		142
143	`}`	143	`}`
144		144
145	`#if 0`	145	`#if 0`
146	`for(int i=0; i<N; i++)`	146	`for(int i=0; i<N; i++)`
147	`cv::imwrite(QString("frames_%1.png").arg(i).toStdString().c_str(), frames[i]);`	147	`cv::imwrite(QString("frames_%1.png").arg(i).toStdString().c_str(), frames[i]);`
148	`cvtools::writeMat(O, "O.mat");`	148	`cvtools::writeMat(O, "O.mat");`
149	`for(int m=0; m<M; m++)`	149	`for(int m=0; m<M; m++)`
150	`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());`
151	`#endif`	151	`#endif`
152		152
153	`// Determine phase cue sequence`	153	`// Determine phase cue sequence`
154	`std::vector<cv::Mat> Phim(M);`	154	`std::vector<cv::Mat> Phim(M);`
155	`Phim[0] = phim[0];`	155	`Phim[0] = phim[0];`
156	`for(int m=1; m<M; m++){`	156	`for(int m=1; m<M; m++){`
157	`cv::subtract(phim[m], phim[0], Phim[m]);`	157	`cv::subtract(phim[m], phim[0], Phim[m]);`
158	`Phim[m] = cvtools::modulo(Phim[m], 2.0*CV_PI);`	158	`Phim[m] = cvtools::modulo(Phim[m], 2.0*CV_PI);`
159	`}`	159	`}`
160		160
161	`// Note: Phim[1] is the cue of highest quality`	161	`// Note: Phim[1] is the cue of highest quality`
162		162
163	`#if 0`	163	`#if 0`
164	`for(int m=0; m<M; m++)`	164	`for(int m=0; m<M; m++)`
165	`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());`
166	`#endif`	166	`#endif`
167		167
168	`// Compute embedded frequencies`	168	`// Compute embedded frequencies`
169	`float Fm[M];`	169	`float Fm[M];`
170	`for(int m=0; m<M; m++){`	170	`for(int m=0; m<M; m++){`
171	`Fm[m] = 1.0;`	171	`Fm[m] = 1.0;`
172	`for(int i=0; i<=m; i++)`	172	`for(int i=0; i<=m; i++)`
173	`Fm[m] *= 1.0/Tm[i];`	173	`Fm[m] *= 1.0/Tm[i];`
174	`}`	174	`}`
175		175
176	`// Compute pattern frequencies`	176	`// Compute pattern frequencies`
177	`float fm[M];`	177	`float fm[M];`
178	`for(int m=0; m<M; m++)`	178	`for(int m=0; m<M; m++)`
179	`fm[m] = Fm[0];`	179	`fm[m] = Fm[0];`
180	`for(int m=1; m<M; m++)`	180	`for(int m=1; m<M; m++)`
181	`fm[m] += Fm[m];`	181	`fm[m] += Fm[m];`
182		182
183	`// Unwrap phase cue sequence`	183	`// Unwrap phase cue sequence`
184	`cv::Mat upCue = Phim[M-1];`	184	`cv::Mat upCue = Phim[M-1];`
185	`for(int m=M-2; m>0; m--){`	185	`for(int m=M-2; m>0; m--){`
186	`upCue = unwrapWithCue(Phim[m], upCue, Fm[m]/Fm[m+1]);`	186	`upCue = unwrapWithCue(Phim[m], upCue, Fm[m]/Fm[m+1]);`
187	`#if 0`	187	`#if 0`
188	`cvtools::writeMat(upCue, "upCue.mat", "upCue");`	188	`cvtools::writeMat(upCue, "upCue.mat", "upCue");`
189	`#endif`	189	`#endif`
190	`}`	190	`}`
191		191
192	`// Unwrap high frequency patterns`	192	`// Unwrap high frequency patterns`
193	`std::vector<cv::Mat> upm(M);`	193	`std::vector<cv::Mat> upm(M);`
194	`for(int m=0; m<M; m++){`	194	`for(int m=0; m<M; m++){`
195	`upm[m] = unwrapWithCue(phim[m], upCue, fm[m]/Fm[M-1]);`	195	`upm[m] = unwrapWithCue(phim[m], upCue, fm[m]/Fm[M-1]);`
196	`}`	196	`}`
197		197
198	`#if 1`	198	`#if 1`
199	`for(int m=0; m<M; m++)`	199	`for(int m=0; m<M; m++)`
200	`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());`
201	`#endif`	201	`#endif`
202		202
203	`// Determine range of phases (for outlier detection)`	203	`// Determine range of phases (for outlier detection)`
204	`cv::Mat upMin = upm[0].clone();`	204	`cv::Mat upMin = upm[0].clone();`
205	`cv::Mat upMax = upm[0].clone();`	205	`cv::Mat upMax = upm[0].clone();`
206	`for(int m=1; m<M; m++){`	206	`for(int m=1; m<M; m++){`
207	`upMin = cv::min(upm[m], upMin);`	207	`upMin = cv::min(upm[m], upMin);`
208	`upMax = cv::max(upm[m], upMax);`	208	`upMax = cv::max(upm[m], upMax);`
209	`}`	209	`}`
210		210
211	`#if 1`	211	`#if 1`
212	`cvtools::writeMat(upMin, "upMin.mat");`	212	`cvtools::writeMat(upMin, "upMin.mat");`
213	`cvtools::writeMat(upMax, "upMax.mat");`	213	`cvtools::writeMat(upMax, "upMax.mat");`
214	`#endif`	214	`#endif`
215		215
216	`upRange = upMax-upMin;`	216	`upRange = upMax-upMin;`
217		217
218	`// Return average of phase maps`	218	`// Return average of phase maps`
219	`up = upm[0].clone();`	219	`up = upm[0].clone();`
220	`for(int m=1; m<M; m++)`	220	`for(int m=1; m<M; m++)`
221	`up += upm[m];`	221	`up += upm[m];`
222	`up /= M;`	222	`up /= M;`
223	`}`	223	`}`
224		224
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){`	225	`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){`
226		226
227	`assert(frames0.size() == N);`	227	`assert(frames0.size() == N);`
228	`assert(frames1.size() == N);`	228	`assert(frames1.size() == N);`
229		229
230	`int frameRows = frames0[0].rows;`	230	`int frameRows = frames0[0].rows;`
231	`int frameCols = frames0[0].cols;`	231	`int frameCols = frames0[0].cols;`
232		232
233	`// Rectifying homographies (rotation+projections)`	233	`// Rectifying homographies (rotation+projections)`
234	`cv::Size frameSize(frameCols, frameRows);`	234	`cv::Size frameSize(frameCols, frameRows);`
235	`cv::Mat R, T;`	235	`cv::Mat R, T;`
236	`// stereoRectify segfaults unless R is double precision`	236	`// stereoRectify segfaults unless R is double precision`
237	`cv::Mat(calibration.R1).convertTo(R, CV_64F);`	237	`cv::Mat(calibration.R1).convertTo(R, CV_64F);`
238	`cv::Mat(calibration.T1).convertTo(T, CV_64F);`	238	`cv::Mat(calibration.T1).convertTo(T, CV_64F);`
239	`cv::Mat R0, R1, P0, P1, QRect;`	239	`cv::Mat R0, R1, P0, P1, QRect;`
240	`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);`
241		241
242	`// Interpolation maps (lens distortion and rectification)`	242	`// Interpolation maps (lens distortion and rectification)`
243	`cv::Mat map0X, map0Y, map1X, map1Y;`	243	`cv::Mat map0X, map0Y, map1X, map1Y;`
244	`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);`
245	`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);`
246		246
247	`int frameRectRows = map0X.rows;`	247	`int frameRectRows = map0X.rows;`
248	`int frameRectCols = map0X.cols;`	248	`int frameRectCols = map0X.cols;`
249		249
250	`// Gray-scale and remap`	250	`// Gray-scale and remap`
251	`std::vector<cv::Mat> frames0Rect(N);`	251	`std::vector<cv::Mat> frames0Rect(N);`
252	`std::vector<cv::Mat> frames1Rect(N);`	252	`std::vector<cv::Mat> frames1Rect(N);`
253	`for(unsigned int i=0; i<N; i++){`	253	`for(unsigned int i=0; i<N; i++){`
254	`cv::Mat temp;`	254	`cv::Mat temp;`
255	`cv::cvtColor(frames0[i], temp, CV_BayerBG2GRAY);`	255	`cv::cvtColor(frames0[i], temp, CV_BayerBG2GRAY);`
256	`cv::remap(temp, frames0Rect[i], map0X, map0Y, CV_INTER_LINEAR);`	256	`cv::remap(temp, frames0Rect[i], map0X, map0Y, CV_INTER_LINEAR);`
257	`cv::cvtColor(frames1[i], temp, CV_BayerBG2GRAY);`	257	`cv::cvtColor(frames1[i], temp, CV_BayerBG2GRAY);`
258	`cv::remap(temp, frames1Rect[i], map1X, map1Y, CV_INTER_LINEAR);`	258	`cv::remap(temp, frames1Rect[i], map1X, map1Y, CV_INTER_LINEAR);`
259	`}`	259	`}`
260		260
261	`// Decode camera 0`	261	`// Decode camera 0`
262	`std::vector<cv::Mat> frames0Patterns(frames0Rect.begin()+2, frames0Rect.end());`	262	`std::vector<cv::Mat> frames0Patterns(frames0Rect.begin()+2, frames0Rect.end());`
263		263
264	`cv::Mat up0, up0Range;`	264	`cv::Mat up0, up0Range;`
265	`decodeEmbeddedPS(frames0Patterns, up0, up0Range);`	265	`decodeEmbeddedPS(frames0Patterns, up0, up0Range);`
266	`up0 *= screenCols;`	266	`up0 *= screenCols;`
267		267
268	`#ifdef QT_DEBUG`	268	`#ifdef QT_DEBUG`
269	`cvtools::writeMat(up0, "up0.mat", "up0");`	269	`cvtools::writeMat(up0, "up0.mat", "up0");`
270	`cvtools::writeMat(up0Range, "up0Range.mat", "up0Range");`	270	`cvtools::writeMat(up0Range, "up0Range.mat", "up0Range");`
271	`#endif`	271	`#endif`
272		272
273	`// Decode camera 1`	273	`// Decode camera 1`
274	`std::vector<cv::Mat> frames1Patterns(frames1Rect.begin()+2, frames1Rect.end());`	274	`std::vector<cv::Mat> frames1Patterns(frames1Rect.begin()+2, frames1Rect.end());`
275		275
276	`cv::Mat up1, up1Range;`	276	`cv::Mat up1, up1Range;`
277	`decodeEmbeddedPS(frames1Patterns, up1, up1Range);`	277	`decodeEmbeddedPS(frames1Patterns, up1, up1Range);`
278	`up1 *= screenCols;`	278	`up1 *= screenCols;`
279		279
280	`#ifdef QT_DEBUG`	280	`#ifdef QT_DEBUG`
281	`cvtools::writeMat(up1, "up1.mat", "up1");`	281	`cvtools::writeMat(up1, "up1.mat", "up1");`
282	`#endif`	282	`#endif`
283		283
284	`// Color debayer and remap`	284	`// Color debayer and remap`
285	`cv::Mat color0, color1;`	285	`cv::Mat color0, color1;`
286	`cv::cvtColor(frames0[0], color0, CV_BayerBG2RGB);`	286	`cv::cvtColor(frames0[0], color0, CV_BayerBG2RGB);`
287	`cv::remap(color0, color0, map0X, map0Y, CV_INTER_LINEAR);`	287	`cv::remap(color0, color0, map0X, map0Y, CV_INTER_LINEAR);`
288		288
289	`cv::cvtColor(frames1[0], color1, CV_BayerBG2RGB);`	289	`cv::cvtColor(frames1[0], color1, CV_BayerBG2RGB);`
290	`cv::remap(color1, color1, map1X, map1Y, CV_INTER_LINEAR);`	290	`cv::remap(color1, color1, map1X, map1Y, CV_INTER_LINEAR);`
291		291
292	`#ifdef QT_DEBUG`	292	`#ifdef QT_DEBUG`
293	`cvtools::writeMat(color0, "color0.mat", "color0");`	293	`cvtools::writeMat(color0, "color0.mat", "color0");`
294	`cvtools::writeMat(color1, "color1.mat", "color1");`	294	`cvtools::writeMat(color1, "color1.mat", "color1");`
295	`#endif`	295	`#endif`
296		296
297	`// Occlusion masks`	297	`// Occlusion masks`
298	`cv::Mat occlusion0, occlusion1;`	298	`cv::Mat occlusion0, occlusion1;`
299	`cv::subtract(frames0Rect[0], frames0Rect[1], occlusion0);`	299	`cv::subtract(frames0Rect[0], frames0Rect[1], occlusion0);`
300	`occlusion0 = (occlusion0 > 25) & (occlusion0 < 250);`	300	`occlusion0 = (occlusion0 > 25) & (occlusion0 < 250);`
301	`cv::subtract(frames1Rect[0], frames1Rect[1], occlusion1);`	301	`cv::subtract(frames1Rect[0], frames1Rect[1], occlusion1);`
302	`occlusion1 = (occlusion1 > 25) & (occlusion1 < 250);`	302	`occlusion1 = (occlusion1 > 25) & (occlusion1 < 250);`
303		303
304	`// // Threshold on energy at primary frequency`	304	`// // Threshold on energy at primary frequency`
305	`// occlusion0 = occlusion0 & (amplitude0 > 5.0*nStepsPrimary);`	305	`// occlusion0 = occlusion0 & (amplitude0 > 5.0*nStepsPrimary);`
306	`// occlusion1 = occlusion1 & (amplitude1 > 5.0*nStepsPrimary);`	306	`// occlusion1 = occlusion1 & (amplitude1 > 5.0*nStepsPrimary);`
307		307
308	`// // Erode occlusion masks`	308	`// // Erode occlusion masks`
309	`// 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));`
310	`// cv::erode(occlusion0, occlusion0, strel);`	310	`// cv::erode(occlusion0, occlusion0, strel);`
311	`// cv::erode(occlusion1, occlusion1, strel);`	311	`// cv::erode(occlusion1, occlusion1, strel);`
312		312
313	`// Threshold on gradient of phase`	313	`// Threshold on gradient of phase`
314	`cv::Mat edges0;`	314	`cv::Mat edges0;`
315	`cv::Sobel(up0, edges0, -1, 1, 1, 5);`	315	`cv::Sobel(up0, edges0, -1, 1, 1, 5);`
316	`occlusion0 = occlusion0 & (abs(edges0) < 150);`	316	`occlusion0 = occlusion0 & (abs(edges0) < 150);`
317		317
318	`cv::Mat edges1;`	318	`cv::Mat edges1;`
319	`cv::Sobel(up1, edges1, -1, 1, 1, 5);`	319	`cv::Sobel(up1, edges1, -1, 1, 1, 5);`
320	`occlusion1 = occlusion1 & (abs(edges1) < 150);`	320	`occlusion1 = occlusion1 & (abs(edges1) < 150);`
321		321
322	`#ifdef QT_DEBUG`	322	`#ifdef QT_DEBUG`
323	`cvtools::writeMat(occlusion0, "occlusion0.mat", "occlusion0");`	323	`cvtools::writeMat(occlusion0, "occlusion0.mat", "occlusion0");`
324	`cvtools::writeMat(occlusion1, "occlusion1.mat", "occlusion1");`	324	`cvtools::writeMat(occlusion1, "occlusion1.mat", "occlusion1");`
325	`#endif`	325	`#endif`
326		326
327	`// Match phase maps`	327	`// Match phase maps`
328		328
329	`// camera0 against camera1`	329	`// camera0 against camera1`
330	`std::vector<cv::Vec2f> q0, q1;`	330	`std::vector<cv::Vec2f> q0, q1;`
331	`for(int row=0; row<frameRectRows; row++){`	331	`for(int row=0; row<frameRectRows; row++){`
332	`for(int col=0; col<frameRectCols; col++){`	332	`for(int col=0; col<frameRectCols; col++){`
333		333
334	`if(!occlusion0.at<char>(row,col))`	334	`if(!occlusion0.at<char>(row,col))`
335	`continue;`	335	`continue;`
336		336
337	`float up0i = up0.at<float>(row,col);`	337	`float up0i = up0.at<float>(row,col);`
338	`for(int col1=0; col1<up1.cols-1; col1++){`	338	`for(int col1=0; col1<up1.cols-1; col1++){`
339		339
340	`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))`
341	`continue;`	341	`continue;`
342		342
343	`float up1Left = up1.at<float>(row,col1);`	343	`float up1Left = up1.at<float>(row,col1);`
344	`float up1Right = up1.at<float>(row,col1+1);`	344	`float up1Right = up1.at<float>(row,col1+1);`
345		345
346	`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)){`
347		347
348	`float col1i = col1 + (up0i-up1Left)/(up1Right-up1Left);`	348	`float col1i = col1 + (up0i-up1Left)/(up1Right-up1Left);`
349		349
350	`q0.push_back(cv::Point2f(col, row));`	350	`q0.push_back(cv::Point2f(col, row));`
351	`q1.push_back(cv::Point2f(col1i, row));`	351	`q1.push_back(cv::Point2f(col1i, row));`
352		352
353	`break;`	353	`break;`
354	`}`	354	`}`
355	`}`	355	`}`
356	`}`	356	`}`
357	`}`	357	`}`
358		358
359		359
360	`int nMatches = q0.size();`	360	`int nMatches = q0.size();`
361		361
362	`if(nMatches < 1){`	362	`if(nMatches < 1){`
363	`Q.resize(0);`	363	`Q.resize(0);`
364	`color.resize(0);`	364	`color.resize(0);`
365		365
366	`return;`	366	`return;`
367	`}`	367	`}`
368		368
369	`// Retrieve color information`	369	`// Retrieve color information`
370	`color.resize(nMatches);`	370	`color.resize(nMatches);`
371	`for(int i=0; i<nMatches; i++){`	371	`for(int i=0; i<nMatches; i++){`
372		372
373	`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]);`
374	`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]);`
375		375
376	`color[i] = 0.5c0 + 0.5c1;`	376	`color[i] = 0.5c0 + 0.5c1;`
377	`}`	377	`}`
378		378
379	`// Triangulate points`	379	`// Triangulate points`
380	`cv::Mat QMatHomogenous, QMat;`	380	`cv::Mat QMatHomogenous, QMat;`
381	`cv::triangulatePoints(P0, P1, q0, q1, QMatHomogenous);`	381	`cv::triangulatePoints(P0, P1, q0, q1, QMatHomogenous);`
382	`cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);`	382	`cvtools::convertMatFromHomogeneous(QMatHomogenous, QMat);`
383		383
384	`// Undo rectification`	384	`// Undo rectification`
385	`cv::Mat R0Inv;`	385	`cv::Mat R0Inv;`
386	`cv::Mat(R0.t()).convertTo(R0Inv, CV_32F);`	386	`cv::Mat(R0.t()).convertTo(R0Inv, CV_32F);`
387	`QMat = R0Inv*QMat;`	387	`QMat = R0Inv*QMat;`
388		388
389	`cvtools::matToPoints3f(QMat, Q);`	389	`cvtools::matToPoints3f(QMat, Q);`
390		390
391	`}`	391	`}`
392		392

Subversion Repositories seema-scanner

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