Subversion Repositories seema-scanner

Rev

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

Rev 189 Rev 190
Line 27... Line 27...
27 
AlgorithmPhaseShiftThreeFreq::AlgorithmPhaseShiftThreeFreq(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){
 27 
AlgorithmPhaseShiftThreeFreq::AlgorithmPhaseShiftThreeFreq(unsigned int _screenCols, unsigned int _screenRows) : Algorithm(_screenCols, _screenRows){
28 
 
 28 
 
29 
    // Set N
 29 
    // Set N
30 
    N = 2+nStepsFirst+nStepsSecond+nStepsThird;
 30 
    N = 2+nStepsFirst+nStepsSecond+nStepsThird;
31 
 
 31 
 
32 
    // Determine the Third period to fulfill the heterodyne condition
 32 
    // Determine the third number of periods to fulfill the heterodyne condition
33 
    float nPeriodsThird = 1 + 2*nPeriodsSecond - nPeriodsFirst;
 33 
    float nPeriodsThird = 1 + 2*nPeriodsSecond - nPeriodsFirst;
34 
 
 34 
 
35 
    // all on pattern
 35 
    // all on pattern
36 
    cv::Mat allOn(1, screenCols, CV_8UC3, cv::Scalar::all(255));
 36 
    cv::Mat allOn(1, screenCols, CV_8UC3, cv::Scalar::all(255));
37 
    patterns.push_back(allOn);
 37 
    patterns.push_back(allOn);
Line 83... Line 83...
83 
    assert(frames1.size() == N);
 83 
    assert(frames1.size() == N);
84 
 
 84 
 
85 
    int frameRows = frames0[0].rows;
 85 
    int frameRows = frames0[0].rows;
86 
    int frameCols = frames0[0].cols;
 86 
    int frameCols = frames0[0].cols;
87 
 
 87 
 
- 
 
 88 
    float nPeriodsThird = 1 + 2*nPeriodsSecond - nPeriodsFirst;
- 
 
 89 
 
- 
 
 90 
 
88 
    // Rectifying homographies (rotation+projections)
 91 
    // Rectifying homographies (rotation+projections)
89 
    cv::Size frameSize(frameCols, frameRows);
 92 
    cv::Size frameSize(frameCols, frameRows);
90 
    cv::Mat R, T;
 93 
    cv::Mat R, T;
91 
    // stereoRectify segfaults unless R is double precision
 94 
    // stereoRectify segfaults unless R is double precision
92 
    cv::Mat(calibration.R1).convertTo(R, CV_64F);
 95 
    cv::Mat(calibration.R1).convertTo(R, CV_64F);
Line 124... Line 127...
124 
    cv::phase(F0Second[2], -F0Second[3], up0Second);
 127 
    cv::phase(F0Second[2], -F0Second[3], up0Second);
125 
    std::vector<cv::Mat> F0Third = getDFTComponents(frames0Third);
 128 
    std::vector<cv::Mat> F0Third = getDFTComponents(frames0Third);
126 
    cv::Mat up0Third;
 129 
    cv::Mat up0Third;
127 
    cv::phase(F0Third[2], -F0Third[3], up0Third);
 130 
    cv::phase(F0Third[2], -F0Third[3], up0Third);
128 
 
 131 
 
129 
    cv::Mat up0EquivalentFS = up0First - up0Second;
 132 
    cv::Mat up0FS = up0Second - up0First;
130 
    up0EquivalentFS = cvtools::modulo(up0EquivalentFS, 2.0*pi);
 133 
    up0FS = cvtools::modulo(up0FS, 2.0*pi);
131 
 
 134 
 
132 
    cv::Mat up0EquivalentST = up0Second - up0Third;
 135 
    cv::Mat up0ST = up0Third - up0Second;
133 
    up0EquivalentST = cvtools::modulo(up0EquivalentST, 2.0*pi);
 136 
    up0ST = cvtools::modulo(up0ST, 2.0*pi);
134 
 
 137 
 
135 
    cv::Mat up0EquivalentFST = up0EquivalentFS - up0EquivalentST;
 138 
    cv::Mat up0FST = up0ST - up0FS;
136 
    up0EquivalentFST = cvtools::modulo(up0EquivalentFST, 2.0*pi);
 139 
    up0FST = cvtools::modulo(up0FST, 2.0*pi);
137 
 
 140 
 
- 
 
 141 
    cv::Mat up0F = unwrapWithCue(up0First, up0FST, nPeriodsFirst);
- 
 
 142 
    cv::Mat up0S = unwrapWithCue(up0Second, up0FST, nPeriodsSecond);
138 
        // TODO: we should use backward phase unwrapping (Song Zhang term)...
 143 
    cv::Mat up0T = unwrapWithCue(up0Third, up0FST, nPeriodsThird);
- 
 
 144 
    cv::Mat up0Mean = 1.0/3.0 * (up0F + up0S + up0T);
- 
 
 145 
    cv::Mat up0Range = cv::max(up0F, cv::max(up0S, up0T)) - cv::min(up0F, cv::min(up0S, up0T));
- 
 
 146 
    cv::Mat up0 = up0Mean * screenCols/(2.0*pi);
139 
 
 147 
 
140 
    cv::Mat up0 = unwrapWithCue(up0First, up0EquivalentFST, (float)screenCols/nPeriodsFirst);
 - 
 
141 
    up0 *= screenCols/(2.0*pi);
 - 
 
142 
    cv::Mat amplitude0;
 148 
    cv::Mat amplitude0;
143 
    cv::magnitude(F0First[2], -F0First[3], amplitude0);
 149 
    cv::magnitude(F0First[2], -F0First[3], amplitude0);
144 
 
 150 
 
145 
    // Decode camera1
 151 
    // Decode camera1
146 
    std::vector<cv::Mat> frames1First(frames1Rect.begin()+2, frames1Rect.begin()+2+nStepsFirst);
 152 
    std::vector<cv::Mat> frames1First(frames1Rect.begin()+2, frames1Rect.begin()+2+nStepsFirst);
Line 155... Line 161...
155 
    cv::phase(F1Second[2], -F1Second[3], up1Second);
 161 
    cv::phase(F1Second[2], -F1Second[3], up1Second);
156 
    std::vector<cv::Mat> F1Third = getDFTComponents(frames1Third);
 162 
    std::vector<cv::Mat> F1Third = getDFTComponents(frames1Third);
157 
    cv::Mat up1Third;
 163 
    cv::Mat up1Third;
158 
    cv::phase(F1Third[2], -F1Third[3], up1Third);
 164 
    cv::phase(F1Third[2], -F1Third[3], up1Third);
159 
 
 165 
 
160 
    cv::Mat up1EquivalentFS = up1First - up1Second;
 166 
    cv::Mat up1FS = up1Second - up1First;
161 
    up1EquivalentFS = cvtools::modulo(up1EquivalentFS, 2.0*pi);
 167 
    up1FS = cvtools::modulo(up1FS, 2.0*pi);
162 
 
 168 
 
163 
    cv::Mat up1EquivalentST = up1Second - up1Third;
 169 
    cv::Mat up1ST = up1Third - up1Second;
164 
    up1EquivalentST = cvtools::modulo(up1EquivalentST, 2.0*pi);
 170 
    up1ST = cvtools::modulo(up1ST, 2.0*pi);
165 
 
 171 
 
166 
    cv::Mat up1EquivalentFST = up1EquivalentFS - up1EquivalentST;
 172 
    cv::Mat up1FST = up1ST - up1FS;
167 
    up1EquivalentFST = cvtools::modulo(up1EquivalentFST, 2.0*pi);
 173 
    up1FST = cvtools::modulo(up1FST, 2.0*pi);
- 
 
 174 
 
- 
 
 175 
    cv::Mat up1F = unwrapWithCue(up1First, up1FST, nPeriodsFirst);
- 
 
 176 
    cv::Mat up1S = unwrapWithCue(up1Second, up1FST, nPeriodsSecond);
- 
 
 177 
    cv::Mat up1T = unwrapWithCue(up1Third, up1FST, nPeriodsThird);
- 
 
 178 
    cv::Mat up1Mean = 1.0/3.0 * (up1F + up1S + up1T);
- 
 
 179 
    cv::Mat up1Range = cv::max(up1F, cv::max(up1S, up1T)) - cv::min(up1F, cv::min(up1S, up1T));
- 
 
 180 
    cv::Mat up1 = up1Mean * screenCols/(2.0*pi);
168 
 
 181 
 
169 
    cv::Mat up1 = unwrapWithCue(up1First, up1EquivalentFST, (float)screenCols/nPeriodsFirst);
 - 
 
170 
    up1 *= screenCols/(2.0*pi);
 - 
 
171 
    cv::Mat amplitude1;
 182 
    cv::Mat amplitude1;
172 
    cv::magnitude(F1First[2], -F1First[3], amplitude1);
 183 
    cv::magnitude(F1First[2], -F1First[3], amplitude1);
173 
 
 184 
 
174 
    #ifdef QT_DEBUG
 185 
    #ifdef QT_DEBUG
175 
        cvtools::writeMat(up0First, "up0First.mat", "up0First");
 186 
        cvtools::writeMat(up0First, "up0First.mat", "up0First");
176 
        cvtools::writeMat(up0Second, "up0Second.mat", "up0Second");
 187 
        cvtools::writeMat(up0Second, "up0Second.mat", "up0Second");
177 
        cvtools::writeMat(up0Third, "up0Third.mat", "up0Third");
 188 
        cvtools::writeMat(up0Third, "up0Third.mat", "up0Third");
178 
        cvtools::writeMat(up0EquivalentFS, "up0EquivalentFS.mat", "up0EquivalentFS");
 189 
        cvtools::writeMat(up0FS, "up0FS.mat", "up0FS");
179 
        cvtools::writeMat(up0EquivalentST, "up0EquivalentST.mat", "up0EquivalentST");
 190 
        cvtools::writeMat(up0ST, "up0ST.mat", "up0ST");
180 
        cvtools::writeMat(up0EquivalentFST, "up0EquivalentFST.mat", "up0EquivalentFST");
 191 
        cvtools::writeMat(up0FST, "up0FST.mat", "up0FST");
- 
 
 192 
        cvtools::writeMat(up0Mean, "up0Mean.mat", "up0Mean");
- 
 
 193 
        cvtools::writeMat(up0Range, "up0Range.mat", "up0Range");
181 
        cvtools::writeMat(up0, "up0.mat", "up0");
 194 
        cvtools::writeMat(up0, "up0.mat", "up0");
182 
        cvtools::writeMat(up1, "up1.mat", "up1");
 195 
        cvtools::writeMat(up1, "up1.mat", "up1");
183 
        cvtools::writeMat(amplitude0, "amplitude0.mat", "amplitude0");
 196 
        cvtools::writeMat(amplitude0, "amplitude0.mat", "amplitude0");
184 
 
 197 
 
185 
        cvtools::writeMat(amplitude0, "amplitude0.mat", "amplitude0");
 198 
        cvtools::writeMat(amplitude0, "amplitude0.mat", "amplitude0");
Line 200... Line 213...
200 
    #endif
 213 
    #endif
201 
 
 214 
 
202 
    // Occlusion masks
 215 
    // Occlusion masks
203 
    cv::Mat occlusion0, occlusion1;
 216 
    cv::Mat occlusion0, occlusion1;
204 
    cv::subtract(frames0Rect[0], frames0Rect[1], occlusion0);
 217 
    cv::subtract(frames0Rect[0], frames0Rect[1], occlusion0);
205 
    occlusion0 = (occlusion0 > 5) & (occlusion0 < 250);
 218 
    occlusion0 = (occlusion0 > 25) & (occlusion0 < 250);
206 
    cv::subtract(frames1Rect[0], frames1Rect[1], occlusion1);
 219 
    cv::subtract(frames1Rect[0], frames1Rect[1], occlusion1);
207 
    occlusion1 = (occlusion1 > 5) & (occlusion1 < 250);
 220 
    occlusion1 = (occlusion1 > 25) & (occlusion1 < 250);
- 
 
 221 
 
- 
 
 222 
    // Threshold on range
- 
 
 223 
    occlusion0 = occlusion0 & (up0Range < 0.05);
- 
 
 224 
    occlusion1 = occlusion1 & (up1Range < 0.05);
- 
 
 225 
 
- 
 
 226 
//    // Erode occlusion masks
- 
 
 227 
//    cv::Mat strel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5,5));
- 
 
 228 
//    cv::erode(occlusion0, occlusion0, strel);
- 
 
 229 
//    cv::erode(occlusion1, occlusion1, strel);
208 
 
 230 
 
209 
    // Threshold on gradient of phase
 231 
    // Threshold on gradient of phase
210 
    cv::Mat edges0;
 232 
    cv::Mat edges0;
211 
    cv::Sobel(up0, edges0, -1, 1, 1, 5);
 233 
    cv::Sobel(up0, edges0, -1, 1, 1, 5);
212 
    occlusion0 = occlusion0 & (abs(edges0) < 150);
 234 
    occlusion0 = occlusion0 & (abs(edges0) < 150);