1 /*M///////////////////////////////////////////////////////////////////////////////////////
2 //
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
4 //
5 // By downloading, copying, installing or using the software you agree to this license.
6 // If you do not agree to this license, do not download, install,
7 // copy or use the software.
8 //
9 //
10 // License Agreement
11 // For Open Source Computer Vision Library
12 //
13 // Copyright (C) 2015, OpenCV Foundation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
15 //
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
18 //
19 // * Redistribution's of source code must retain the above copyright notice,
20 // this list of conditions and the following disclaimer.
21 //
22 // * Redistribution's in binary form must reproduce the above copyright notice,
23 // this list of conditions and the following disclaimer in the documentation
24 // and/or other materials provided with the distribution.
25 //
26 // * The name of the copyright holders may not be used to endorse or promote products
27 // derived from this software without specific prior written permission.
28 //
29 // This software is provided by the copyright holders and contributors "as is" and
30 // any express or implied warranties, including, but not limited to, the implied
31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
32 // In no event shall the Intel Corporation or contributors be liable for any direct,
33 // indirect, incidental, special, exemplary, or consequential damages
34 // (including, but not limited to, procurement of substitute goods or services;
35 // loss of use, data, or profits; or business interruption) however caused
36 // and on any theory of liability, whether in contract, strict liability,
37 // or tort (including negligence or otherwise) arising in any way out of
38 // the use of this software, even if advised of the possibility of such damage.
39 //
40 //M*/
41
42
43 #include "precomp.hpp"
44
45 namespace cv {
46 namespace structured_light {
47 class CV_EXPORTS_W SinusoidalPatternProfilometry_Impl CV_FINAL : public SinusoidalPattern
48 {
49 public:
50 // Constructor
51 explicit SinusoidalPatternProfilometry_Impl( const SinusoidalPattern::Params ¶meters =
52 SinusoidalPattern::Params() );
53 // Destructor
~SinusoidalPatternProfilometry_Impl()54 virtual ~SinusoidalPatternProfilometry_Impl() CV_OVERRIDE {};
55
56 // Generate sinusoidal patterns
57 bool generate( OutputArrayOfArrays patternImages ) CV_OVERRIDE;
58
59 bool decode( const std::vector< std::vector<Mat> >& patternImages, OutputArray disparityMap,
60 InputArrayOfArrays blackImages = noArray(), InputArrayOfArrays whiteImages =
61 noArray(), int flags = 0 ) const CV_OVERRIDE;
62
63 // Compute a wrapped phase map from the sinusoidal patterns
64 void computePhaseMap( InputArrayOfArrays patternImages, OutputArray wrappedPhaseMap,
65 OutputArray shadowMask = noArray(), InputArray fundamental = noArray()) CV_OVERRIDE;
66 // Unwrap the wrapped phase map to retrieve correspondences
67 void unwrapPhaseMap( InputArray wrappedPhaseMap,
68 OutputArray unwrappedPhaseMap,
69 cv::Size camSize,
70 InputArray shadowMask = noArray() ) CV_OVERRIDE;
71 // Find correspondences between the devices
72 void findProCamMatches( InputArray projUnwrappedPhaseMap, InputArray camUnwrappedPhaseMap,
73 OutputArrayOfArrays matches ) CV_OVERRIDE;
74
75 void computeDataModulationTerm( InputArrayOfArrays patternImages,
76 OutputArray dataModulationTerm,
77 InputArray shadowMask ) CV_OVERRIDE;
78
79 private:
80 // Compute The Fourier transform of a pattern. Output is complex. Taken from the DFT example in OpenCV
81 void computeDft( InputArray patternImage, OutputArray FourierTransform );
82 // Compute the inverse Fourier transform. Output can be complex or real
83 void computeInverseDft( InputArray FourierTransform, OutputArray inverseFourierTransform,
84 bool realOutput );
85 // Compute the DFT magnitude which is used to find maxima in the spectrum
86 void computeDftMagnitude( InputArray FourierTransform, OutputArray FourierTransformMagnitude );
87 // Compute phase map from the complex signal given by non-symmetrical filtering of DFT
88 void computeFtPhaseMap( InputArray inverseFourierTransform,
89 InputArray shadowMask,
90 OutputArray wrappedPhaseMap );
91 // Swap DFT quadrants. Come from opencv example
92 void swapQuadrants( InputOutputArray image, int centerX, int centerY );
93 // Filter (non)-symmetrically the DFT.
94 void frequencyFiltering( InputOutputArray FourierTransform, int centerX1, int centerY1,
95 int halfRegionWidth, int halfRegionHeight, bool keepInsideRegion,
96 int centerX2 = -1, int centerY2 = -1 );
97 // Find maxima in the spectrum so that we know how it should be filtered
98 bool findMaxInHalvesTransform( InputArray FourierTransformMag, Point &maxPosition1,
99 Point &maxPosition2 );
100 // Compute phase map from the three sinusoidal patterns
101 void computePsPhaseMap( InputArrayOfArrays patternImages,
102 InputArray shadowMask,
103 OutputArray wrappedPhaseMap );
104
105 void computeFapsPhaseMap( InputArray a, InputArray b, InputArray theta1, InputArray theta2,
106 InputArray shadowMask, OutputArray wrappedPhaseMap );
107 // Compute a shadow mask to discard shadow regions
108 void computeShadowMask( InputArrayOfArrays patternImages, OutputArray shadowMask );
109 // Data modulation term is used to isolate cross markers
110
111 void extractMarkersLocation( InputArray dataModulationTerm,
112 std::vector<Point> &markersLocation );
113
114 void convertToAbsolutePhaseMap( InputArrayOfArrays camPatterns,
115 InputArray unwrappedProjPhaseMap,
116 InputArray unwrappedCamPhaseMap,
117 InputArray shadowMask,
118 InputArray fundamentalMatrix );
119
120 Params params;
121 phase_unwrapping::HistogramPhaseUnwrapping::Params unwrappingParams;
122 // Class describing markers that are added to the patterns
123 class Marker{
124 private:
125 Point center, up, right, left, down;
126 public:
127 Marker();
128 Marker( Point c );
129 void drawMarker( OutputArray pattern );
130 };
131 };
132 // Default parameters value
Params()133 SinusoidalPattern::Params::Params()
134 {
135 width = 800;
136 height = 600;
137 nbrOfPeriods = 20;
138 shiftValue = (float)(2 * CV_PI / 3);
139 methodId = FAPS;
140 nbrOfPixelsBetweenMarkers = 56;
141 horizontal = false;
142 setMarkers = false;
143 }
Marker()144 SinusoidalPatternProfilometry_Impl::Marker::Marker(){};
145
Marker(Point c)146 SinusoidalPatternProfilometry_Impl::Marker::Marker( Point c )
147 {
148 center = c;
149 up.x = c.x;
150 up.y = c.y - 1;
151 left.x = c.x - 1;
152 left.y = c.y;
153
154 down.x = c.x;
155 down.y = c.y + 1;
156 right.x = c.x + 1;
157 right.y = c.y;
158 }
159 // Draw marker on a pattern
drawMarker(OutputArray pattern)160 void SinusoidalPatternProfilometry_Impl::Marker::drawMarker( OutputArray pattern )
161 {
162 Mat &pattern_ = *(Mat*) pattern.getObj();
163
164 pattern_.at<uchar>(center.x, center.y) = 255;
165 pattern_.at<uchar>(up.x, up.y) = 255;
166 pattern_.at<uchar>(right.x, right.y) = 255;
167 pattern_.at<uchar>(left.x, left.y) = 255;
168 pattern_.at<uchar>(down.x, down.y) = 255;
169 }
170
SinusoidalPatternProfilometry_Impl(const SinusoidalPattern::Params & parameters)171 SinusoidalPatternProfilometry_Impl::SinusoidalPatternProfilometry_Impl(
172 const SinusoidalPattern::Params ¶meters ) : params(parameters)
173 {
174
175 }
176 // Generate sinusoidal patterns. Markers are optional
generate(OutputArrayOfArrays pattern)177 bool SinusoidalPatternProfilometry_Impl::generate( OutputArrayOfArrays pattern )
178 {
179 // Three patterns are used in the reference paper.
180 int nbrOfPatterns = 3;
181 float meanAmpl = 127.5;
182 float sinAmpl = 127.5;
183 // Period in number of pixels
184 int period;
185 float frequency;
186 // m and n are parameters described in the reference paper
187 int m = params.nbrOfPixelsBetweenMarkers;
188 int n;
189 // Offset for the first marker of the first row.
190 int firstMarkerOffset = 10;
191 int mnRatio;
192 int nbrOfMarkersOnOneRow;
193 std::vector<Mat> &pattern_ = *(std::vector<Mat>*) pattern.getObj();
194
195 n = params.nbrOfPeriods / nbrOfPatterns;
196 mnRatio = m / n;
197
198 pattern_.resize(nbrOfPatterns);
199
200 if( params.horizontal )
201 {
202 period = params.height / params.nbrOfPeriods;
203 nbrOfMarkersOnOneRow = (int)floor(static_cast<float>((params.width - firstMarkerOffset) / m));
204 }
205 else
206 {
207 period = params.width / params.nbrOfPeriods;
208 nbrOfMarkersOnOneRow = (int)floor(static_cast<float>((params.height - firstMarkerOffset) / m));
209 }
210 frequency = (float) 1 / period;
211
212 for( int i = 0; i < nbrOfPatterns; ++i )
213 {
214 pattern_[i] = Mat(params.height, params.width, CV_8UC1);
215
216 if( params.horizontal )
217 pattern_[i] = pattern_[i].t();
218 }
219 // Patterns vary along one direction only so, a row Mat can be created and copied to the pattern's rows
220 for( int i = 0; i < nbrOfPatterns; ++i )
221 {
222 Mat rowValues(1, pattern_[i].cols, CV_8UC1);
223
224 for( int j = 0; j < pattern_[i].cols; ++j )
225 {
226 rowValues.at<uchar>(0, j) = saturate_cast<uchar>(
227 meanAmpl + sinAmpl * sin(2 * CV_PI * frequency * j + i * params.shiftValue));
228 }
229
230 for( int j = 0; j < pattern_[i].rows; ++j )
231 {
232 rowValues.row(0).copyTo(pattern_[i].row(j));
233 }
234 }
235 // Add cross markers to the patterns.
236 if( params.setMarkers )
237 {
238 for( int i = 0; i < nbrOfPatterns; ++i )
239 {
240 for( int j = 0; j < n; ++j )
241 {
242 for( int k = 0; k < nbrOfMarkersOnOneRow; ++k )
243 {
244 Marker mark(Point(firstMarkerOffset + k * m + j * mnRatio,
245 3 * period / 4 + j * period + i * period * n - i * period / 3));
246 mark.drawMarker(pattern_[i]);
247 params.markersLocation.push_back(Point2f((float)(firstMarkerOffset + k * m + j * mnRatio),
248 (float) (3 * period / 4 + j * period + i * period * n - i * period / 3)));
249 }
250 }
251 }
252 }
253 if( params.horizontal )
254 for( int i = 0; i < nbrOfPatterns; ++i )
255 {
256 pattern_[i] = pattern_[i].t();
257 }
258 return true;
259 }
260
decode(const std::vector<std::vector<Mat>> & patternImages,OutputArray disparityMap,InputArrayOfArrays blackImages,InputArrayOfArrays whiteImages,int flags) const261 bool SinusoidalPatternProfilometry_Impl::decode(const std::vector< std::vector<Mat> >& patternImages,
262 OutputArray disparityMap,
263 InputArrayOfArrays blackImages,
264 InputArrayOfArrays whiteImages, int flags ) const
265 {
266 CV_UNUSED(patternImages);
267 CV_UNUSED(disparityMap);
268 CV_UNUSED(blackImages);
269 CV_UNUSED(whiteImages);
270 CV_UNUSED(flags);
271 return true;
272 }
273 // Most of the steps described in the paper to get the wrapped phase map take place here
computePhaseMap(InputArrayOfArrays patternImages,OutputArray wrappedPhaseMap,OutputArray shadowMask,InputArray fundamental)274 void SinusoidalPatternProfilometry_Impl::computePhaseMap( InputArrayOfArrays patternImages,
275 OutputArray wrappedPhaseMap,
276 OutputArray shadowMask,
277 InputArray fundamental )
278 {
279 std::vector<Mat> &pattern_ = *(std::vector<Mat>*) patternImages.getObj();
280 Mat &wrappedPhaseMap_ = *(Mat*) wrappedPhaseMap.getObj();
281 int rows = pattern_[0].rows;
282 int cols = pattern_[0].cols;
283 int dcWidth = 5;
284 int dcHeight = 5;
285 int bpWidth = 21;
286 int bpHeight = 21;
287 // Compute wrapped phase map for FTP
288 if( params.methodId == FTP )
289 {
290 Mat &shadowMask_ = *(Mat*) shadowMask.getObj();
291 Mat dftImage, complexInverseDft;
292 Mat dftMag;
293 int halfWidth = cols/2;
294 int halfHeight = rows/2;
295 Point m1, m2;
296 computeShadowMask(pattern_, shadowMask_);
297
298 computeDft(pattern_[0], dftImage); //compute the complex pattern DFT
299 swapQuadrants(dftImage, halfWidth, halfHeight); //swap quadrants to get 0 frequency in (halfWidth, halfHeight)
300 frequencyFiltering(dftImage, halfHeight, halfWidth, dcHeight, dcWidth, false); //get rid of 0 frequency
301 computeDftMagnitude(dftImage, dftMag); //compute magnitude to find maxima
302 findMaxInHalvesTransform(dftMag, m1, m2); //look for maxima in the magnitude. Useful information is located around maxima
303 frequencyFiltering(dftImage, m2.y, m2.x, bpHeight, bpWidth, true); //keep useful information only
304 swapQuadrants(dftImage,halfWidth, halfHeight); //swap quadrants again to compute inverse dft
305 computeInverseDft(dftImage, complexInverseDft, false); //compute inverse dft. Result is complex since we only keep half of the spectrum
306 computeFtPhaseMap(complexInverseDft, shadowMask_, wrappedPhaseMap_); //compute phaseMap from the complex image.
307 }
308 // Compute wrapped pahse map for PSP
309 else if( params.methodId == PSP )
310 {
311 Mat &shadowMask_ = *(Mat*) shadowMask.getObj();
312 //Mat &fundamental_ = *(Mat*) fundamental.getObj();
313 CV_UNUSED(fundamental);
314 Mat dmt;
315 int nbrOfPatterns = static_cast<int>(pattern_.size());
316 std::vector<Mat> filteredPatterns(nbrOfPatterns);
317 std::vector<Mat> dftImages(nbrOfPatterns);
318 std::vector<Mat> dftMags(nbrOfPatterns);
319 int halfWidth = cols/2;
320 int halfHeight = rows/2;
321 Point m1, m2;
322
323 computeShadowMask(pattern_, shadowMask_);
324
325 //this loop symmetrically filters pattern to remove cross markers.
326 for( int i = 0; i < nbrOfPatterns; ++i )
327 {
328 computeDft(pattern_[i], dftImages[i]);
329 swapQuadrants(dftImages[i], halfWidth, halfHeight);
330 frequencyFiltering(dftImages[i], halfHeight, halfWidth, dcHeight, dcWidth, false);
331 computeDftMagnitude(dftImages[i], dftMags[i]);
332 findMaxInHalvesTransform(dftMags[i], m1, m2);
333 frequencyFiltering(dftImages[i], m1.y, m1.x, bpHeight, bpWidth, true, m2.y, m2.x);//symmetrical filtering
334 swapQuadrants(dftImages[i], halfWidth, halfHeight);
335 computeInverseDft(dftImages[i], filteredPatterns[i], true);
336
337 }
338 computePsPhaseMap(filteredPatterns, shadowMask_, wrappedPhaseMap_);
339 }
340 else if( params.methodId == FAPS )
341 {
342 Mat &shadowMask_ = *(Mat*) shadowMask.getObj();
343 int nbrOfPatterns = static_cast<int>(pattern_.size());
344 std::vector<Mat> unwrappedFTPhaseMaps;
345 std::vector<Mat> filteredPatterns(nbrOfPatterns);
346 Mat dmt;
347 Mat theta1, theta2, a, b;
348 std::vector<Point> markersLoc;
349 cv::Size camSize;
350 camSize.height = pattern_[0].rows;
351 camSize.width = pattern_[0].cols;
352 computeShadowMask(pattern_, shadowMask_);
353
354 for( int i = 0; i < nbrOfPatterns; ++i )
355 {
356 Mat dftImage, complexInverseDft;
357 Mat dftMag;
358 Mat tempWrappedPhaseMap;
359 Mat tempUnwrappedPhaseMap;
360 int halfWidth = cols/2;
361 int halfHeight = rows/2;
362 Point m1, m2;
363
364 computeDft(pattern_[i], dftImage); //compute the complex pattern DFT
365 swapQuadrants(dftImage, halfWidth, halfHeight); //swap quadrants to get 0 frequency in (halfWidth, halfHeight)
366 frequencyFiltering(dftImage, halfHeight, halfWidth, dcHeight, dcWidth, false); //get rid of 0 frequency
367 computeDftMagnitude(dftImage, dftMag); //compute magnitude to find maxima
368 findMaxInHalvesTransform(dftMag, m1, m2); //look for maxima in the magnitude. Useful information is located around maxima
369 frequencyFiltering(dftImage, m2.y, m2.x, bpHeight, bpWidth, true); //keep useful information only
370 swapQuadrants(dftImage,halfWidth, halfHeight); //swap quadrants again to compute inverse dft
371 computeInverseDft(dftImage, complexInverseDft, false); //compute inverse dft. Result is complex since we only keep half of the spectrum
372 computeFtPhaseMap(complexInverseDft, shadowMask_, tempWrappedPhaseMap); //compute phaseMap from the complex image.
373 unwrapPhaseMap(tempWrappedPhaseMap, tempUnwrappedPhaseMap, camSize, shadowMask);
374 unwrappedFTPhaseMaps.push_back(tempUnwrappedPhaseMap);
375 computeInverseDft(dftImage, filteredPatterns[i], true);
376 }
377
378 theta1.create(camSize.height, camSize.width, unwrappedFTPhaseMaps[0].type());
379 theta2.create(camSize.height, camSize.width, unwrappedFTPhaseMaps[0].type());
380 a.create(camSize.height, camSize.width, CV_32FC1);
381 b.create(camSize.height, camSize.width, CV_32FC1);
382
383 a = filteredPatterns[0] - filteredPatterns[1];
384 b = filteredPatterns[1] - filteredPatterns[2];
385
386 theta1 = unwrappedFTPhaseMaps[1] - unwrappedFTPhaseMaps[0];
387 theta2 = unwrappedFTPhaseMaps[2] - unwrappedFTPhaseMaps[1];
388
389 computeFapsPhaseMap(a, b, theta1, theta2, shadowMask_, wrappedPhaseMap_);
390 }
391 }
392
unwrapPhaseMap(InputArray wrappedPhaseMap,OutputArray unwrappedPhaseMap,cv::Size camSize,InputArray shadowMask)393 void SinusoidalPatternProfilometry_Impl::unwrapPhaseMap( InputArray wrappedPhaseMap,
394 OutputArray unwrappedPhaseMap,
395 cv::Size camSize,
396 InputArray shadowMask )
397 {
398 int rows = params.height;
399 int cols = params.width;
400 unwrappingParams.width = camSize.width;
401 unwrappingParams.height = camSize.height;
402
403 Mat &wPhaseMap = *(Mat*) wrappedPhaseMap.getObj();
404 Mat &uPhaseMap = *(Mat*) unwrappedPhaseMap.getObj();
405 Mat mask;
406
407 if( shadowMask.empty() )
408 {
409 mask.create(rows, cols, CV_8UC1);
410 mask = Scalar::all(255);
411 }
412 else
413 {
414 Mat &temp = *(Mat*) shadowMask.getObj();
415 temp.copyTo(mask);
416 }
417
418 Ptr<phase_unwrapping::HistogramPhaseUnwrapping> phaseUnwrapping =
419 phase_unwrapping::HistogramPhaseUnwrapping::create(unwrappingParams);
420
421 phaseUnwrapping->unwrapPhaseMap(wPhaseMap, uPhaseMap, mask);
422 }
423
findProCamMatches(InputArray projUnwrappedPhaseMap,InputArray camUnwrappedPhaseMap,OutputArrayOfArrays matches)424 void SinusoidalPatternProfilometry_Impl::findProCamMatches( InputArray projUnwrappedPhaseMap,
425 InputArray camUnwrappedPhaseMap,
426 OutputArrayOfArrays matches )
427 {
428 CV_UNUSED(projUnwrappedPhaseMap);
429 CV_UNUSED(camUnwrappedPhaseMap);
430 CV_UNUSED(matches);
431 }
432
computeDft(InputArray patternImage,OutputArray FourierTransform)433 void SinusoidalPatternProfilometry_Impl::computeDft( InputArray patternImage,
434 OutputArray FourierTransform )
435 {
436 Mat &pattern_ = *(Mat*) patternImage.getObj();
437 Mat &FourierTransform_ = *(Mat*) FourierTransform.getObj();
438 Mat padded;
439 int m = getOptimalDFTSize(pattern_.rows);
440 int n = getOptimalDFTSize(pattern_.cols);
441 copyMakeBorder(pattern_, padded, 0, m - pattern_.rows, 0, n - pattern_.cols, BORDER_CONSTANT,
442 Scalar::all(0));
443 Mat planes[] = {Mat_<float>(padded), Mat::zeros(padded.size(), CV_32F)};
444 merge(planes, 2, FourierTransform_);
445 dft(FourierTransform_, FourierTransform_);
446 }
447
computeInverseDft(InputArray FourierTransform,OutputArray inverseFourierTransform,bool realOutput)448 void SinusoidalPatternProfilometry_Impl::computeInverseDft( InputArray FourierTransform,
449 OutputArray inverseFourierTransform,
450 bool realOutput )
451 {
452 Mat &FourierTransform_ = *(Mat*) FourierTransform.getObj();
453 Mat &inverseFourierTransform_ = *(Mat*) inverseFourierTransform.getObj();
454 if( realOutput )
455 idft(FourierTransform_, inverseFourierTransform_, DFT_SCALE | DFT_REAL_OUTPUT);
456 else
457 idft(FourierTransform_, inverseFourierTransform_, DFT_SCALE);
458 }
459
computeDftMagnitude(InputArray FourierTransform,OutputArray FourierTransformMagnitude)460 void SinusoidalPatternProfilometry_Impl::computeDftMagnitude( InputArray FourierTransform,
461 OutputArray FourierTransformMagnitude )
462 {
463 Mat &FourierTransform_ = *(Mat*) FourierTransform.getObj();
464 Mat &FourierTransformMagnitude_ = *(Mat*) FourierTransformMagnitude.getObj();
465 Mat planes[2];
466 split(FourierTransform_, planes);
467 magnitude(planes[0], planes[1], planes[0]);
468 FourierTransformMagnitude_ = planes[0];
469 FourierTransformMagnitude_ += Scalar::all(1);
470 log(FourierTransformMagnitude_, FourierTransformMagnitude_);
471 FourierTransformMagnitude_ = FourierTransformMagnitude_(
472 Rect(0, 0, FourierTransformMagnitude_.cols & -2, FourierTransformMagnitude_.rows & - 2));
473 normalize(FourierTransformMagnitude_, FourierTransformMagnitude_, 0, 1, NORM_MINMAX);
474 }
475
computeFtPhaseMap(InputArray inverseFourierTransform,InputArray shadowMask,OutputArray wrappedPhaseMap)476 void SinusoidalPatternProfilometry_Impl::computeFtPhaseMap( InputArray inverseFourierTransform,
477 InputArray shadowMask,
478 OutputArray wrappedPhaseMap )
479 {
480
481 Mat &inverseFourierTransform_ = *(Mat*) inverseFourierTransform.getObj();
482 Mat &wrappedPhaseMap_ = *(Mat*) wrappedPhaseMap.getObj();
483 Mat &shadowMask_ = *(Mat*) shadowMask.getObj();
484 Mat planes[2];
485
486 int rows = inverseFourierTransform_.rows;
487 int cols = inverseFourierTransform_.cols;
488
489 if( wrappedPhaseMap_.empty () )
490 wrappedPhaseMap_.create(rows, cols, CV_32FC1);
491
492 split(inverseFourierTransform_, planes);
493
494 for( int i = 0; i < rows; ++i )
495 {
496 for( int j = 0; j < cols; ++j )
497 {
498 if( shadowMask_.at<uchar>(i, j) != 0 )
499 {
500 float im = planes[1].at<float>(i, j);
501 float re = planes[0].at<float>(i, j);
502 wrappedPhaseMap_.at<float>(i, j) = atan2(re, im);
503 }
504 else
505 {
506 wrappedPhaseMap_.at<float>(i, j) = 0;
507 }
508 }
509 }
510 }
swapQuadrants(InputOutputArray image,int centerX,int centerY)511 void SinusoidalPatternProfilometry_Impl::swapQuadrants( InputOutputArray image,
512 int centerX, int centerY )
513 {
514 Mat &image_ = *(Mat*) image.getObj();
515 Mat q0(image_, Rect(0, 0, centerX, centerY));
516 Mat q1(image_, Rect(centerX, 0, centerX, centerY));
517 Mat q2(image_, Rect(0, centerY, centerX, centerY));
518 Mat q3(image_, Rect(centerX, centerY, centerX, centerY));
519 Mat tmp;
520
521 q0.copyTo(tmp);
522 q3.copyTo(q0);
523 tmp.copyTo(q3);
524
525 q1.copyTo(tmp);
526 q2.copyTo(q1);
527 tmp.copyTo(q2);
528 }
529
frequencyFiltering(InputOutputArray FourierTransform,int centerX1,int centerY1,int halfRegionWidth,int halfRegionHeight,bool keepInsideRegion,int centerX2,int centerY2)530 void SinusoidalPatternProfilometry_Impl::frequencyFiltering( InputOutputArray FourierTransform,
531 int centerX1, int centerY1,
532 int halfRegionWidth, int halfRegionHeight,
533 bool keepInsideRegion, int centerX2,
534 int centerY2 )
535 {
536 Mat &FourierTransform_ = *(Mat*) FourierTransform.getObj();
537 int rows = FourierTransform_.rows;
538 int cols = FourierTransform_.cols;
539 int type = FourierTransform_.type();
540 if( keepInsideRegion )
541 {
542 Mat maskedTransform(rows, cols, type);
543 maskedTransform = Scalar::all(0);
544 Mat roi1 = FourierTransform_(
545 Rect(centerY1 - halfRegionHeight, centerX1 - halfRegionWidth,
546 2 * halfRegionHeight, 2 * halfRegionWidth));
547 Mat dstRoi1 = maskedTransform(
548 Rect(centerY1 - halfRegionHeight, centerX1 - halfRegionWidth,
549 2 * halfRegionHeight, 2 * halfRegionWidth));
550 roi1.copyTo(dstRoi1);
551
552 if( centerY2 != -1 || centerX2 != -1 )
553 {
554 Mat roi2 = FourierTransform_(
555 Rect(centerY2 - halfRegionHeight, centerX2 - halfRegionWidth,
556 2 * halfRegionHeight, 2 * halfRegionWidth));
557 Mat dstRoi2 = maskedTransform(
558 Rect(centerY2 - halfRegionHeight, centerX2 - halfRegionWidth,
559 2 * halfRegionHeight, 2 * halfRegionWidth));
560 roi2.copyTo(dstRoi2);
561 }
562 FourierTransform_ = maskedTransform;
563 }
564 else
565 {
566 Mat roi(2 * halfRegionHeight, 2 * halfRegionWidth, type);
567 roi = Scalar::all(0);
568
569 Mat dstRoi1 = FourierTransform_(
570 Rect(centerY1 - halfRegionHeight, centerX1 - halfRegionWidth,
571 2 * halfRegionHeight, 2 * halfRegionWidth));
572 roi.copyTo(dstRoi1);
573
574 if( centerY2 != -1 || centerX2 != -1 )
575 {
576 Mat dstRoi2 = FourierTransform_(
577 Rect(centerY2 - halfRegionHeight, centerX2 - halfRegionWidth,
578 2 * halfRegionHeight, 2 * halfRegionWidth));
579 roi.copyTo(dstRoi2);
580 }
581 }
582 }
findMaxInHalvesTransform(InputArray FourierTransformMag,Point & maxPosition1,Point & maxPosition2)583 bool SinusoidalPatternProfilometry_Impl::findMaxInHalvesTransform( InputArray FourierTransformMag,
584 Point &maxPosition1,
585 Point &maxPosition2 )
586 {
587 Mat &FourierTransformMag_ = *(Mat*) FourierTransformMag.getObj();
588
589 int centerX = FourierTransformMag_.cols / 2;
590 int centerY = FourierTransformMag_.rows / 2;
591 Mat h0, h1;
592 double maxV1 = -1;
593 double maxV2 = -1;
594 int margin = 5;
595
596 if( params.horizontal )
597 {
598 h0 = FourierTransformMag_(Rect(0, 0, FourierTransformMag_.cols, centerY - margin));
599 h1 = FourierTransformMag_(
600 Rect(0, centerY + margin, FourierTransformMag_.cols, centerY - margin));
601 }
602 else
603 {
604 h0 = FourierTransformMag_(Rect(0, 0, centerX - margin, FourierTransformMag_.rows));
605 h1 = FourierTransformMag_(
606 Rect(centerX + margin, 0, centerX - margin, FourierTransformMag_.rows));
607 }
608
609 minMaxLoc(h0, NULL, &maxV1, NULL, &maxPosition1);
610 minMaxLoc(h1, NULL, &maxV2, NULL, &maxPosition2);
611
612 if( params.horizontal )
613 {
614 maxPosition2.y = maxPosition2.y + centerY + margin;
615 }
616 else
617 {
618 maxPosition2.x = maxPosition2.x + centerX + margin;
619 }
620
621 if( maxV1 == -1 || maxV2 == -1 )
622 {
623 return false;
624 }
625
626 return true;
627 }
628
computePsPhaseMap(InputArrayOfArrays patternImages,InputArray shadowMask,OutputArray wrappedPhaseMap)629 void SinusoidalPatternProfilometry_Impl::computePsPhaseMap( InputArrayOfArrays patternImages,
630 InputArray shadowMask,
631 OutputArray wrappedPhaseMap )
632 {
633 std::vector<Mat> &pattern_ = *(std::vector<Mat>*) patternImages.getObj();
634 Mat &wrappedPhaseMap_ = *(Mat*) wrappedPhaseMap.getObj();
635 Mat &shadowMask_ = *(Mat*) shadowMask.getObj();
636
637 int rows = pattern_[0].rows;
638 int cols = pattern_[0].cols;
639
640 float i1 = 0;
641 float i2 = 0;
642 float i3 = 0;
643
644 if( wrappedPhaseMap_.empty() )
645 wrappedPhaseMap_.create(rows, cols, CV_32FC1);
646
647 for( int i = 0; i < rows; ++i )
648 {
649 for( int j = 0; j < cols; ++j )
650 {
651 if( shadowMask_.at<uchar>(i, j) != 0 )
652 {
653 if( pattern_[0].type() == CV_8UC1 )
654 {
655 i1 = pattern_[0].at<uchar>(i, j);
656 i2 = pattern_[1].at<uchar>(i, j);
657 i3 = pattern_[2].at<uchar>(i, j);
658 }
659 else if( pattern_[0].type() == CV_32FC1 )
660 {
661 i1 = pattern_[0].at<float>(i, j);
662 i2 = pattern_[1].at<float>(i, j);
663 i3 = pattern_[2].at<float>(i, j);
664 }
665 float num = (1- cos(params.shiftValue)) * (i3 - i2);
666 float den = sin(params.shiftValue) * (2 * i1 - i2 - i3);
667 wrappedPhaseMap_.at<float>(i,j) = atan2(num, den);
668 }
669 else
670 {
671 wrappedPhaseMap_.at<float>(i,j) = 0;
672 }
673 }
674 }
675 }
676
computeFapsPhaseMap(InputArray a,InputArray b,InputArray theta1,InputArray theta2,InputArray shadowMask,OutputArray wrappedPhaseMap)677 void SinusoidalPatternProfilometry_Impl::computeFapsPhaseMap( InputArray a,
678 InputArray b,
679 InputArray theta1,
680 InputArray theta2,
681 InputArray shadowMask,
682 OutputArray wrappedPhaseMap )
683 {
684 Mat &a_ = *(Mat*) a.getObj();
685 Mat &b_ = *(Mat*) b.getObj();
686 Mat &theta1_ = *(Mat*) theta1.getObj();
687 Mat &theta2_ = *(Mat*) theta2.getObj();
688 Mat &wrappedPhaseMap_ = *(Mat*) wrappedPhaseMap.getObj();
689 Mat &shadowMask_ = *(Mat*) shadowMask.getObj();
690
691 int rows = a_.rows;
692 int cols = a_.cols;
693
694 if( wrappedPhaseMap_.empty() )
695 wrappedPhaseMap_.create(rows, cols, CV_32FC1);
696
697 for( int i = 0; i < rows; ++i )
698 {
699 for( int j = 0; j < cols; ++j )
700 {
701 if( shadowMask_.at<uchar>(i, j ) != 0 )
702 {
703 float num = (1 - cos(theta2_.at<float>(i, j))) * a_.at<float>(i, j) +
704 (1 - cos(theta1_.at<float>(i, j))) * b_.at<float>(i, j);
705
706 float den = sin(theta1_.at<float>(i, j)) * b_.at<float>(i, j) -
707 sin(theta2_.at<float>(i, j)) * a_.at<float>(i, j);
708
709 wrappedPhaseMap_.at<float>(i, j) = atan2(num, den);
710 }
711 else
712 {
713 wrappedPhaseMap_.at<float>(i, j) = 0;
714 }
715 }
716 }
717 }
718
719 //compute shadow mask from three patterns. Valid pixels are lit at least by one pattern
computeShadowMask(InputArrayOfArrays patternImages,OutputArray shadowMask)720 void SinusoidalPatternProfilometry_Impl::computeShadowMask( InputArrayOfArrays patternImages,
721 OutputArray shadowMask )
722 {
723 std::vector<Mat> &patternImages_ = *(std::vector<Mat>*) patternImages.getObj();
724 Mat &shadowMask_ = *(Mat*) shadowMask.getObj();
725 Mat mean;
726 int rows = patternImages_[0].rows;
727 int cols = patternImages_[0].cols;
728 float i1, i2, i3;
729
730 mean.create(rows, cols, CV_32FC1);
731
732 for( int i = 0; i < rows; ++i )
733 {
734 for( int j = 0; j < cols; ++j )
735 {
736 i1 = (float) patternImages_[0].at<uchar>(i, j);
737 i2 = (float) patternImages_[1].at<uchar>(i, j);
738 i3 = (float) patternImages_[2].at<uchar>(i, j);
739 mean.at<float>(i, j) = (i1 + i2 + i3) / 3;
740 }
741 }
742 mean.convertTo(mean, CV_8UC1);
743 threshold(mean, shadowMask_, 10, 255, 0);
744
745 }
746 // Compute the data modulation term according to the formula given in the reference paper
computeDataModulationTerm(InputArrayOfArrays patternImages,OutputArray dataModulationTerm,InputArray shadowMask)747 void SinusoidalPatternProfilometry_Impl::computeDataModulationTerm( InputArrayOfArrays patternImages,
748 OutputArray dataModulationTerm,
749 InputArray shadowMask )
750 {
751 std::vector<Mat> &patternImages_ = *(std::vector<Mat>*) patternImages.getObj();
752 Mat &dataModulationTerm_ = *(Mat*) dataModulationTerm.getObj();
753 Mat &shadowMask_ = *(Mat*) shadowMask.getObj();
754 int rows = patternImages_[0].rows;
755 int cols = patternImages_[0].cols;
756 float num = 0;
757 float den = 0;
758 float i1 = 0;
759 float i2 = 0;
760 float i3 = 0;
761
762 int iOffset, jOffset;
763 Mat dmt(rows, cols, CV_32FC1);
764 Mat threshedDmt;
765
766 if( dataModulationTerm_.empty() )
767 {
768 dataModulationTerm_.create(rows, cols, CV_8UC1);
769 }
770 if( shadowMask_.empty() )
771 {
772 shadowMask_.create(rows, cols, CV_8U);
773 shadowMask_ = Scalar::all(255);
774 }
775 for( int i = 0; i < rows; ++i )
776 {
777 for( int j = 0; j < cols; ++j )
778 {
779 if( shadowMask_.at<uchar>(i, j) != 0 ){
780 if( i - 2 == - 2 )
781 {
782 iOffset = 0;
783 }
784 else if( i - 2 == - 1 )
785 {
786 iOffset = -1;
787 }
788 else if( i - 2 + 4 == rows + 1 )
789 {
790 iOffset = -3;
791 }
792 else
793 {
794 iOffset = -2;
795 }
796 if( j - 2 == -2 )
797 {
798 jOffset = 0;
799 }
800 else if( j - 2 == -1 )
801 {
802 jOffset = -1;
803 }
804 else if( j - 2 + 4 == cols + 1 )
805 {
806 jOffset = -3;
807 }
808 else
809 {
810 jOffset = -2;
811 }
812 Mat roi = shadowMask_(Rect(j + jOffset, i + iOffset, 4, 4));
813 Scalar nbrOfValidPixels = sum(roi);
814 if( nbrOfValidPixels[0] < 14*255 )
815 {
816 dmt.at<float>(i, j) = 0;
817 }
818 else
819 {
820 i1 = patternImages_[0].at<uchar>(i, j);
821 i2 = patternImages_[1].at<uchar>(i, j);
822 i3 = patternImages_[2].at<uchar>(i, j);
823
824 num = sqrt(3 * ( i1 - i3 ) * ( i1 - i3 ) + ( 2 * i2 - i1 - i3 ) * ( 2 * i2 - i1 - i3 ));
825 den = i1 + i2 + i3;
826 dmt.at<float>(i, j) = 1 - num / den;
827 }
828 }
829 else
830 {
831 dmt.at<float>(i, j) = 0;
832 }
833 }
834 }
835 Mat kernel(3, 3, CV_32F);
836 kernel.at<float>(0, 0) = 1.f/16.f;
837 kernel.at<float>(1, 0) = 2.f/16.f;
838 kernel.at<float>(2, 0) = 1.f/16.f;
839
840 kernel.at<float>(0, 1) = 2.f/16.f;
841 kernel.at<float>(1, 1) = 4.f/16.f;
842 kernel.at<float>(2, 1) = 2.f/16.f;
843
844 kernel.at<float>(0, 2) = 1.f/16.f;
845 kernel.at<float>(1, 2) = 2.f/16.f;
846 kernel.at<float>(2, 2) = 1.f/16.f;
847
848 Point anchor = Point(-1, -1);
849 double delta = 0;
850 int ddepth = -1;
851
852 filter2D(dmt, dmt, ddepth, kernel, anchor, delta, BORDER_DEFAULT);
853
854 threshold(dmt, threshedDmt, 0.4, 1, THRESH_BINARY);
855 threshedDmt.convertTo(dataModulationTerm_, CV_8UC1, 255, 0);
856 }
857
858 //Extract marker location on the DMT. Duplicates are removed
extractMarkersLocation(InputArray dataModulationTerm,std::vector<Point> & markersLocation)859 void SinusoidalPatternProfilometry_Impl::extractMarkersLocation( InputArray dataModulationTerm,
860 std::vector<Point> &markersLocation )
861 {
862 Mat &dmt = *(Mat*) dataModulationTerm.getObj();
863 int rows = dmt.rows;
864 int cols = dmt.cols;
865 int halfRegionSize = 6;
866
867 for( int i = 0; i < rows; ++i )
868 {
869 for( int j = 0; j < cols; ++j )
870 {
871 if( dmt.at<uchar>(i,j) != 0 )
872 {
873 bool addToVector = true;
874 for(int k = 0; k < (int)markersLocation.size(); ++k)
875 {
876 if( markersLocation[k].x - halfRegionSize < i &&
877 markersLocation[k].x + halfRegionSize > i &&
878 markersLocation[k].y - halfRegionSize < j &&
879 markersLocation[k].y + halfRegionSize > j ){
880 addToVector = false;
881 }
882 }
883 if(addToVector)
884 {
885 Point temp(i,j);
886 markersLocation.push_back(temp);
887 }
888 }
889 }
890 }
891 }
convertToAbsolutePhaseMap(InputArrayOfArrays camPatterns,InputArray unwrappedProjPhaseMap,InputArray unwrappedCamPhaseMap,InputArray shadowMask,InputArray fundamentalMatrix)892 void SinusoidalPatternProfilometry_Impl::convertToAbsolutePhaseMap( InputArrayOfArrays camPatterns,
893 InputArray unwrappedProjPhaseMap,
894 InputArray unwrappedCamPhaseMap,
895 InputArray shadowMask,
896 InputArray fundamentalMatrix )
897 {
898 std::vector<Mat> &camPatterns_ = *(std::vector<Mat>*) camPatterns.getObj();
899 CV_UNUSED(unwrappedCamPhaseMap);
900 CV_UNUSED(unwrappedProjPhaseMap);
901
902 Mat &fundamental = *(Mat*) fundamentalMatrix.getObj();
903
904 Mat camDmt;
905
906 std::vector<Point> markersLocation;
907
908 computeDataModulationTerm(camPatterns_, camDmt, shadowMask);
909
910 std::vector<Vec3f> epilines;
911 computeCorrespondEpilines(params.markersLocation, 2, fundamental, epilines);
912
913 }
create(Ptr<SinusoidalPattern::Params> params)914 Ptr<SinusoidalPattern> SinusoidalPattern::create( Ptr<SinusoidalPattern::Params> params )
915 {
916 return makePtr<SinusoidalPatternProfilometry_Impl>(*params);
917 }
918 }
919 }
920