Subversion Repositories seema-scanner

Rev

Rev 192 | Rev 195 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 192 Rev 193
Line 10... Line 10...
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 = 3;
 15 
static const int M = 4;
16 
 
 16 
 
17 
// Embedded frequencies
 17 
// Embedded periods (product of these must be greater than screenCols)
18 
static const float Fm[M] = {1.0/16, 1.0/128, 1.0/1024};
 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, 2};
 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
Line 37... Line 37...
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
- 
 
 43 
    float Fm[M];
- 
 
 44 
    for(int m=0; m<M; m++){
- 
 
 45 
        Fm[m] = 1.0;
- 
 
 46 
        for(int i=0; i<=m; i++)
- 
 
 47 
            Fm[m] *= 1.0/Tm[i];
- 
 
 48 
    }
- 
 
 49 
 
- 
 
 50 
    // Compute pattern frequencies
- 
 
 51 
    float fm[M];
- 
 
 52 
    for(int m=0; m<M; m++)
- 
 
 53 
        fm[m] = Fm[0];
- 
 
 54 
    for(int m=1; m<M; m++)
- 
 
 55 
        fm[m] += Fm[m];
- 
 
 56 
 
- 
 
 57 
    for(int m=0; m<M; m++)
- 
 
 58 
        std::cout << fm[m] << std::endl;
- 
 
 59 
 
42 
    // Encoding patterns
 60 
    // Encoding patterns
43 
    for(int m=0; m<M; m++){
 61 
    for(int m=0; m<M; m++){
44 
        int nSteps = Nm[m];
 62 
        int nSteps = Nm[m];
45 
        float frequency = Fm[m];
 63 
        float frequency = fm[m];
46 
        for(unsigned int i=0; i<nSteps; i++){
 64 
        for(unsigned int i=0; i<nSteps; i++){
47 
            float phase = 2.0*pi/nSteps * i;
 65 
            float phase = 2.0*pi/std::max(nSteps, 3) * i;
48 
            float pitch = 1.0/frequency;
 66 
            float pitch = 1.0/frequency;
49 
            cv::Mat patternI(1,1,CV_8U);
 67 
            cv::Mat patternI(1,1,CV_8U);
50 
            patternI = computePhaseVector(screenCols, phase, pitch);
 68 
            patternI = computePhaseVector(screenCols, phase, pitch);
51 
            patterns.push_back(patternI.t());
 69 
            patterns.push_back(patternI.t());
52 
        }
 70 
        }
Line 56... Line 74...
56 
 
 74 
 
57 
cv::Mat AlgorithmPhaseShiftEmbedded::getEncodingPattern(unsigned int depth){
 75 
cv::Mat AlgorithmPhaseShiftEmbedded::getEncodingPattern(unsigned int depth){
58 
    return patterns[depth];
 76 
    return patterns[depth];
59 
}
 77 
}
60 
 
 78 
 
61 
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, float screenCols){
62 
 
 80 
 
63 
    const int N = frames.size();
 81 
    const int N = frames.size();
64 
 
 82 
 
65 
    // Construct shift matrix
 83 
    // Construct shift matrix
66 
    cv::Mat A(N, 1 + 2*N, CV_32F);
 84 
    cv::Mat A(N, 1 + 2*M, CV_32F);
- 
 
 85 
    A.col(0).setTo(1.0);
67 
 
 86 
 
68 
    int rowBegin = 0;
 87 
    int rowBegin = 0;
69 
    for(int m=0; m<M; m++){
 88 
    for(int m=0; m<M; m++){
70 
 
 89 
 
71 
        int nSteps = Nm[m];
 90 
        int nSteps = Nm[m];
72 
 
 91 
 
73 
        cv::Mat Am(nSteps, 2, CV_32F);
 92 
        cv::Mat Am(nSteps, 2, CV_32F);
74 
 
 93 
 
75 
        for(unsigned int i=0; i<nSteps; i++){
 94 
        for(unsigned int i=0; i<nSteps; i++){
76 
            float phase = 2.0*CV_PI/nSteps * i;
 95 
            float phase = 2.0*CV_PI/std::max(nSteps, 3) * i;
77 
 
 96 
 
78 
            Am.at<float>(i, 0) = std::cos(phase);
 97 
            Am.at<float>(i, 0) = std::cos(phase);
79 
            Am.at<float>(i, 1) = -std::sin(phase);
 98 
            Am.at<float>(i, 1) = -std::sin(phase);
80 
        }
 99 
        }
81 
 
 100 
 
82 
        // Copy into the A matrix
 101 
        // Copy into the A matrix
83 
        Am.copyTo(A.rowRange(rowBegin, rowBegin+nSteps).colRange(1+2*m, 1+2*(m+1)));
 102 
        Am.copyTo(A.rowRange(rowBegin, rowBegin+nSteps).colRange(1+2*m, 1+2*(m+1)));
84 
        rowBegin += nSteps;
 103 
        rowBegin += nSteps;
85 
    }
 104 
    }
- 
 
 105 
    //std::cout << A << std::endl << std::endl;
86 
 
 106 
 
87 
    int frameRows = frames[0].rows;
 107 
    int frameRows = frames[0].rows;
88 
    int frameCols = frames[0].cols;
 108 
    int frameCols = frames[0].cols;
89 
 
 109 
 
90 
    // DC-offset
 110 
    // DC-offset
Line 100... Line 120...
100 
        for(int col=0; col<frameCols; col++){
 120 
        for(int col=0; col<frameCols; col++){
101 
 
 121 
 
102 
            // Measurement vector
 122 
            // Measurement vector
103 
            cv::Mat r(N, 1, CV_32F);
 123 
            cv::Mat r(N, 1, CV_32F);
104 
            for(int i=0; i<N; i++)
 124 
            for(int i=0; i<N; i++)
105 
                r.at<float>(i) = frames[i].at<float>(row, col);
 125 
                r.at<float>(i) = frames[i].at<uchar>(row, col);
106 
 
 126 
 
107 
            // Solve
 127 
            // Solve
108 
            cv::Mat u; //[o, a cos1, a sin1, a cos2, a sin2, ...]
 128 
            cv::Mat u; //[o, a cos1, a sin1, a cos2, a sin2, ...]
109 
            cv::solve(A, r, u);
 129 
            cv::solve(A, r, u, cv::DECOMP_SVD);
110 
 
 130 
 
111 
            for(int m=0; m<M; m++)
 131 
            for(int m=0; m<M; m++)
112 
                phim[m].at<float>(row, col) = std::atan2(u.at<float>(m*2+1), u.at<float>(m*2+2));
 132 
                phim[m].at<float>(row, col) = std::atan2(u.at<float>(m*2+1), u.at<float>(m*2+2));
113 
 
 133 
 
114 
            O.at<float>(row, col) = u.at<float>(0);
 134 
            O.at<float>(row, col) = u.at<float>(0);
115 
        }
 135 
        }
116 
 
 136 
 
117 
    }
 137 
    }
118 
 
 138 
 
- 
 
 139 
    #if 0
- 
 
 140 
        for(int i=0; i<N; i++)
- 
 
 141 
            cvtools::writeMat(frames[i], QString("frames_%1.mat").arg(i).toStdString().c_str());
- 
 
 142 
        cvtools::writeMat(O, "O.mat");
- 
 
 143 
        for(int m=0; m<M; m++)
- 
 
 144 
            cvtools::writeMat(phim[m], QString("phim_%1.mat").arg(m).toStdString().c_str());
- 
 
 145 
    #endif
- 
 
 146 
 
119 
    // Determine phase cues
 147 
    // Determine phase cue sequence
120 
    std::vector<cv::Mat> Phim(M);
 148 
    std::vector<cv::Mat> Phim(M);
121 
    Phim[0] = phim[0];
 149 
    Phim[0] = phim[0];
122 
 
 - 
 
123 
    for(int i=1; i<M; i++)
 150 
    for(int m=1; m<M; m++){
124 
        cv::subtract(phim[i], phim[0], Phim[i]);
 151 
        cv::subtract(phim[m], phim[0], Phim[m]);
- 
 
 152 
        Phim[m] = cvtools::modulo(Phim[m], 2.0*CV_PI);
- 
 
 153 
    }
125 
 
 154 
 
126 
    // Note: Phim[1] is the cue of highest quality
 155 
    // Note: Phim[1] is the cue of highest quality
127 
 
 156 
 
- 
 
 157 
    #if 0
128 
    std::vector<cv::Mat> upm;
 158 
        for(int m=0; m<M; m++)
- 
 
 159 
            cvtools::writeMat(Phim[m], QString("Phim_%1.mat").arg(m).toStdString().c_str());
- 
 
 160 
    #endif
129 
 
 161 
 
- 
 
 162 
    // Compute embedded frequencies
- 
 
 163 
    float Fm[M];
130 
    for(int m=0; m<M; m++)
 164 
    for(int m=0; m<M; m++){
- 
 
 165 
        Fm[m] = 1.0;
- 
 
 166 
        for(int i=0; i<=m; i++)
131 
        upm[m] = unwrapWithCue(phim[m], Phim[1], 1.0/Fm[1]);
 167 
            Fm[m] *= 1.0/Tm[i];
- 
 
 168 
    }
132 
 
 169 
 
- 
 
 170 
    // Unwrap phase cue sequence
- 
 
 171 
    cv::Mat upCue = Phim[M-1];
- 
 
 172 
    for(int m=M-2; m>0; m--){
- 
 
 173 
        upCue = unwrapWithCue(Phim[m], upCue, screenCols*Fm[m]);
- 
 
 174 
        #if 1
- 
 
 175 
                cvtools::writeMat(upCue, "upCue.mat", "upCue");
- 
 
 176 
        #endif
- 
 
 177 
    }
- 
 
 178 
 
- 
 
 179 
    // Unwrap high frequency patterns
- 
 
 180 
    std::vector<cv::Mat> upm(M);
- 
 
 181 
    for(int m=0; m<M; m++){
- 
 
 182 
        upm[m] = unwrapWithCue(phim[m], upCue, 1.0/Fm[1]);
- 
 
 183 
    }
- 
 
 184 
 
- 
 
 185 
    #if 1
- 
 
 186 
        for(int m=0; m<M; m++)
- 
 
 187 
            cvtools::writeMat(upm[m], QString("upm_%1.mat").arg(m).toStdString().c_str());
- 
 
 188 
    #endif
133 
 
 189 
 
134 
    // Determine range of phases (for outlier detection)
 190 
    // Determine range of phases (for outlier detection)
135 
    cv::Mat upMin = upm[0];
 191 
    cv::Mat upMin = upm[0];
136 
    cv::Mat upMax = upm[0];
 192 
    cv::Mat upMax = upm[0];
137 
    for(int m=1; m<M; m++){
 193 
    for(int m=1; m<M; m++){
Line 185... Line 241...
185 
 
 241 
 
186 
    // Decode camera 0
 242 
    // Decode camera 0
187 
    std::vector<cv::Mat> frames0Patterns(frames0Rect.begin()+2, frames0Rect.end());
 243 
    std::vector<cv::Mat> frames0Patterns(frames0Rect.begin()+2, frames0Rect.end());
188 
 
 244 
 
189 
    cv::Mat up0, up0Range;
 245 
    cv::Mat up0, up0Range;
190 
    decodeEmbeddedPS(frames0Patterns, up0, up0Range);
 246 
    decodeEmbeddedPS(frames0Patterns, up0, up0Range, screenCols);
191 
    up0 *= screenCols;
 247 
    up0 *= screenCols;
192 
 
 248 
 
193 
    #ifdef QT_DEBUG
 249 
    #ifdef QT_DEBUG
194 
        cvtools::writeMat(up0, "up0.mat", "up0");
 250 
        cvtools::writeMat(up0, "up0.mat", "up0");
195 
        cvtools::writeMat(up0Range, "up0Range.mat", "up0Range");
 251 
        cvtools::writeMat(up0Range, "up0Range.mat", "up0Range");
Line 197... Line 253...
197 
 
 253 
 
198 
    // Decode camera 1
 254 
    // Decode camera 1
199 
    std::vector<cv::Mat> frames1Patterns(frames1Rect.begin()+2, frames1Rect.end());
 255 
    std::vector<cv::Mat> frames1Patterns(frames1Rect.begin()+2, frames1Rect.end());
200 
 
 256 
 
201 
    cv::Mat up1, up1Range;
 257 
    cv::Mat up1, up1Range;
202 
    decodeEmbeddedPS(frames1Patterns, up1, up1Range);
 258 
    decodeEmbeddedPS(frames1Patterns, up1, up1Range, screenCols);
203 
    up1 *= screenCols;
 259 
    up1 *= screenCols;
204 
 
 260 
 
205 
    #ifdef QT_DEBUG
 261 
    #ifdef QT_DEBUG
206 
        cvtools::writeMat(up1, "up1.mat", "up1");
 262 
        cvtools::writeMat(up1, "up1.mat", "up1");
207 
    #endif
 263 
    #endif