OTB  9.0.0
Orfeo Toolbox
otbSubPixelDisparityImageFilter.hxx
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2005-2022 Centre National d'Etudes Spatiales (CNES)
3  *
4  * This file is part of Orfeo Toolbox
5  *
6  * https://www.orfeo-toolbox.org/
7  *
8  * Licensed under the Apache License, Version 2.0 (the "License");
9  * you may not use this file except in compliance with the License.
10  * You may obtain a copy of the License at
11  *
12  * http://www.apache.org/licenses/LICENSE-2.0
13  *
14  * Unless required by applicable law or agreed to in writing, software
15  * distributed under the License is distributed on an "AS IS" BASIS,
16  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
17  * See the License for the specific language governing permissions and
18  * limitations under the License.
19  */
20 
21 #ifndef otbSubPixelDisparityImageFilter_hxx
22 #define otbSubPixelDisparityImageFilter_hxx
23 
25 
26 namespace otb
27 {
28 template <class TInputImage, class TOutputMetricImage, class TDisparityImage, class TMaskImage, class TBlockMatchingFunctor>
30 {
31  // Set the number of required inputs
32  this->SetNumberOfRequiredInputs(3);
33 
34  // Set the outputs
35  this->SetNumberOfRequiredOutputs(3);
36  this->SetNthOutput(0, TDisparityImage::New());
37  this->SetNthOutput(1, TDisparityImage::New());
38  this->SetNthOutput(2, TOutputMetricImage::New());
39 
40  // Default parameters
41  m_Radius.Fill(2);
42 
43  // Minimize by default
44  m_Minimize = true;
45 
46  // Default disparity range
47  m_MinimumHorizontalDisparity = -10;
48  m_MaximumHorizontalDisparity = 10;
49  m_MinimumVerticalDisparity = 0;
50  m_MaximumVerticalDisparity = 0;
51 
52  // Default refinement method : parabolic
53  m_RefineMethod = 0;
54 
55  // Default step value
56  m_Step = 1;
57 
58  // Default grid index
59  m_GridIndex[0] = 0;
60  m_GridIndex[1] = 0;
61 }
62 
63 template <class TInputImage, class TOutputMetricImage, class TDisparityImage, class TMaskImage, class TBlockMatchingFunctor>
65 {
66 }
67 
68 template <class TInputImage, class TOutputMetricImage, class TDisparityImage, class TMaskImage, class TBlockMatchingFunctor>
70 {
71  // Process object is not const-correct so the const casting is required.
72  this->SetNthInput(0, const_cast<TInputImage*>(image));
73 }
74 
75 template <class TInputImage, class TOutputMetricImage, class TDisparityImage, class TMaskImage, class TBlockMatchingFunctor>
77 {
78  // Process object is not const-correct so the const casting is required.
79  this->SetNthInput(1, const_cast<TInputImage*>(image));
80 }
81 
82 template <class TInputImage, class TOutputMetricImage, class TDisparityImage, class TMaskImage, class TBlockMatchingFunctor>
84  const TDisparityImage* hfield)
85 {
86  // Process object is not const-correct so the const casting is required.
87  this->SetNthInput(2, const_cast<TDisparityImage*>(hfield));
88 }
89 
90 template <class TInputImage, class TOutputMetricImage, class TDisparityImage, class TMaskImage, class TBlockMatchingFunctor>
92  const TDisparityImage* vfield)
93 {
94  // Process object is not const-correct so the const casting is required.
95  this->SetNthInput(3, const_cast<TDisparityImage*>(vfield));
96 }
97 
98 template <class TInputImage, class TOutputMetricImage, class TDisparityImage, class TMaskImage, class TBlockMatchingFunctor>
100  const TMaskImage* image)
101 {
102  // Process object is not const-correct so the const casting is required.
103  this->SetNthInput(4, const_cast<TMaskImage*>(image));
104 }
105 
106 template <class TInputImage, class TOutputMetricImage, class TDisparityImage, class TMaskImage, class TBlockMatchingFunctor>
108  const TMaskImage* image)
109 {
110  // Process object is not const-correct so the const casting is required.
111  this->SetNthInput(5, const_cast<TMaskImage*>(image));
112 }
113 
114 template <class TInputImage, class TOutputMetricImage, class TDisparityImage, class TMaskImage, class TBlockMatchingFunctor>
116  const TOutputMetricImage* image)
117 {
118  // Process object is not const-correct so the const casting is required.
119  this->SetNthInput(6, const_cast<TOutputMetricImage*>(image));
120 }
121 
122 template <class TInputImage, class TOutputMetricImage, class TDisparityImage, class TMaskImage, class TBlockMatchingFunctor>
124 {
125  if (this->GetNumberOfIndexedInputs() < 1)
126  {
127  return nullptr;
128  }
129  return static_cast<const TInputImage*>(this->itk::ProcessObject::GetInput(0));
130 }
131 
132 template <class TInputImage, class TOutputMetricImage, class TDisparityImage, class TMaskImage, class TBlockMatchingFunctor>
134 {
135  if (this->GetNumberOfIndexedInputs() < 2)
136  {
137  return nullptr;
138  }
139  return static_cast<const TInputImage*>(this->itk::ProcessObject::GetInput(1));
140 }
141 
142 template <class TInputImage, class TOutputMetricImage, class TDisparityImage, class TMaskImage, class TBlockMatchingFunctor>
143 const TDisparityImage*
145 {
146  if (this->GetNumberOfIndexedInputs() < 3)
147  {
148  return nullptr;
149  }
150  return static_cast<const TDisparityImage*>(this->itk::ProcessObject::GetInput(2));
151 }
152 
153 template <class TInputImage, class TOutputMetricImage, class TDisparityImage, class TMaskImage, class TBlockMatchingFunctor>
154 const TDisparityImage*
156 {
157  if (this->GetNumberOfIndexedInputs() < 4)
158  {
159  return nullptr;
160  }
161  return static_cast<const TDisparityImage*>(this->itk::ProcessObject::GetInput(3));
162 }
163 
164 template <class TInputImage, class TOutputMetricImage, class TDisparityImage, class TMaskImage, class TBlockMatchingFunctor>
166 {
167  if (this->GetNumberOfIndexedInputs() < 5)
168  {
169  return nullptr;
170  }
171  return static_cast<const TMaskImage*>(this->itk::ProcessObject::GetInput(4));
172 }
173 
174 template <class TInputImage, class TOutputMetricImage, class TDisparityImage, class TMaskImage, class TBlockMatchingFunctor>
176 {
177  if (this->GetNumberOfIndexedInputs() < 6)
178  {
179  return nullptr;
180  }
181  return static_cast<const TMaskImage*>(this->itk::ProcessObject::GetInput(5));
182 }
183 
184 template <class TInputImage, class TOutputMetricImage, class TDisparityImage, class TMaskImage, class TBlockMatchingFunctor>
185 const TDisparityImage*
187 {
188  if (this->GetNumberOfOutputs() < 1)
189  {
190  return 0;
191  }
192  return static_cast<const TDisparityImage*>(this->itk::ProcessObject::GetOutput(0));
193 }
194 
195 template <class TInputImage, class TOutputMetricImage, class TDisparityImage, class TMaskImage, class TBlockMatchingFunctor>
196 TDisparityImage*
198 {
199  if (this->GetNumberOfOutputs() < 1)
200  {
201  return nullptr;
202  }
203  return static_cast<TDisparityImage*>(this->itk::ProcessObject::GetOutput(0));
204 }
205 
206 template <class TInputImage, class TOutputMetricImage, class TDisparityImage, class TMaskImage, class TBlockMatchingFunctor>
207 const TDisparityImage*
209 {
210  if (this->GetNumberOfOutputs() < 2)
211  {
212  return 0;
213  }
214  return static_cast<const TDisparityImage*>(this->itk::ProcessObject::GetOutput(1));
215 }
216 
217 template <class TInputImage, class TOutputMetricImage, class TDisparityImage, class TMaskImage, class TBlockMatchingFunctor>
219 {
220  if (this->GetNumberOfOutputs() < 2)
221  {
222  return nullptr;
223  }
224  return static_cast<TDisparityImage*>(this->itk::ProcessObject::GetOutput(1));
225 }
226 
227 template <class TInputImage, class TOutputMetricImage, class TDisparityImage, class TMaskImage, class TBlockMatchingFunctor>
228 const TOutputMetricImage*
230 {
231  if (this->GetNumberOfOutputs() < 3)
232  {
233  return 0;
234  }
235  return static_cast<const TOutputMetricImage*>(this->itk::ProcessObject::GetOutput(2));
236 }
237 
238 template <class TInputImage, class TOutputMetricImage, class TDisparityImage, class TMaskImage, class TBlockMatchingFunctor>
240 {
241  if (this->GetNumberOfOutputs() < 3)
242  {
243  return nullptr;
244  }
245  return static_cast<TOutputMetricImage*>(this->itk::ProcessObject::GetOutput(2));
246 }
247 
248 template <class TInputImage, class TOutputMetricImage, class TDisparityImage, class TMaskImage, class TBlockMatchingFunctor>
250  const BlockMatchingFilterType* filter)
251 {
252  this->SetLeftInput(filter->GetLeftInput());
253  this->SetRightInput(filter->GetRightInput());
254 
255  this->SetRadius(filter->GetRadius());
256 
257  this->SetMinimumHorizontalDisparity(filter->GetMinimumHorizontalDisparity());
258  this->SetMaximumHorizontalDisparity(filter->GetMaximumHorizontalDisparity());
259 
260  this->SetMinimumVerticalDisparity(filter->GetMinimumVerticalDisparity());
261  this->SetMaximumVerticalDisparity(filter->GetMaximumVerticalDisparity());
262 
263  this->SetMinimize(filter->GetMinimize());
264 
265  this->SetMetricInput(filter->GetMetricOutput());
266 
267  if (filter->GetHorizontalDisparityOutput())
268  {
269  this->SetHorizontalDisparityInput(filter->GetHorizontalDisparityOutput());
270  }
271  if (filter->GetVerticalDisparityOutput())
272  {
273  this->SetVerticalDisparityInput(filter->GetVerticalDisparityOutput());
274  }
275  if (filter->GetLeftMaskInput())
276  {
277  this->SetLeftMaskInput(filter->GetLeftMaskInput());
278  }
279  if (filter->GetRightMaskInput())
280  {
281  this->SetRightMaskInput(filter->GetRightMaskInput());
282  }
283 }
284 
285 template <class TInputImage, class TOutputMetricImage, class TDisparityImage, class TMaskImage, class TBlockMatchingFunctor>
287 {
288  // Retrieve input pointers
289  const TInputImage* inLeftPtr = this->GetLeftInput();
290  const TInputImage* inRightPtr = this->GetRightInput();
291  const TMaskImage* inLeftMaskPtr = this->GetLeftMaskInput();
292  const TMaskImage* inRightMaskPtr = this->GetRightMaskInput();
293  const TDisparityImage* inHDispPtr = this->GetHorizontalDisparityInput();
294  const TDisparityImage* inVDispPtr = this->GetVerticalDisparityInput();
295 
296  // Check pointers before using them
297  if (!inLeftPtr || !inRightPtr)
298  {
299  itkExceptionMacro(<< "Missing input, need left and right input images.");
300  }
301 
302  if (!inHDispPtr)
303  {
304  itkExceptionMacro(<< "Input horizontal disparity map is missing");
305  }
306 
307  // Now, we impose that both inputs have the same size
308  if (inLeftPtr->GetLargestPossibleRegion() != inRightPtr->GetLargestPossibleRegion())
309  {
310  itkExceptionMacro(<< "Left and right images do not have the same size ! Left largest region: " << inLeftPtr->GetLargestPossibleRegion()
311  << ", right largest region: " << inRightPtr->GetLargestPossibleRegion());
312  }
313 
314  // We also check that left mask image has same size if present
315  if (inLeftMaskPtr && inLeftPtr->GetLargestPossibleRegion() != inLeftMaskPtr->GetLargestPossibleRegion())
316  {
317  itkExceptionMacro(<< "Left and mask images do not have the same size ! Left largest region: " << inLeftPtr->GetLargestPossibleRegion()
318  << ", mask largest region: " << inLeftMaskPtr->GetLargestPossibleRegion());
319  }
320 
321  // We also check that right mask image has same size if present
322  if (inRightMaskPtr && inRightPtr->GetLargestPossibleRegion() != inRightMaskPtr->GetLargestPossibleRegion())
323  {
324  itkExceptionMacro(<< "Right and mask images do not have the same size ! Right largest region: " << inRightPtr->GetLargestPossibleRegion()
325  << ", mask largest region: " << inRightMaskPtr->GetLargestPossibleRegion());
326  }
327 
328  // We check that the input initial disparity maps have the same size if present
329  if (inHDispPtr && inVDispPtr && inHDispPtr->GetLargestPossibleRegion() != inVDispPtr->GetLargestPossibleRegion())
330  {
331  itkExceptionMacro(<< "Initial horizontal and vertical disparity maps don't have the same size ! Horizontal disparity largest region: "
332  << inHDispPtr->GetLargestPossibleRegion() << ", vertical disparity largest region: " << inVDispPtr->GetLargestPossibleRegion());
333  }
334 }
335 
336 template <class TInputImage, class TOutputMetricImage, class TDisparityImage, class TMaskImage, class TBlockMatchingFunctor>
338 {
339  // Retrieve input pointers
340  const TInputImage* inLeftPtr = this->GetLeftInput();
341  const TDisparityImage* inHDispPtr = this->GetHorizontalDisparityInput();
342 
343  TOutputMetricImage* outMetricPtr = this->GetMetricOutput();
344  TDisparityImage* outHDispPtr = this->GetHorizontalDisparityOutput();
345  TDisparityImage* outVDispPtr = this->GetVerticalDisparityOutput();
346 
347  outMetricPtr->CopyInformation(inHDispPtr);
348  outHDispPtr->CopyInformation(inHDispPtr);
349  outVDispPtr->CopyInformation(inHDispPtr);
350 
351  // Check the size of the input disparity maps (possible subsampled grid)
352  SpacingType leftSpacing = inLeftPtr->GetSignedSpacing();
353  SpacingType dispSpacing = inHDispPtr->GetSignedSpacing();
354  PointType leftOrigin = inLeftPtr->GetOrigin();
355  PointType dispOrigin = inHDispPtr->GetOrigin();
356 
357  double ratioX = dispSpacing[0] / leftSpacing[0];
358  double ratioY = dispSpacing[1] / leftSpacing[1];
359  int stepX = static_cast<int>(std::floor(ratioX + 0.5));
360  int stepY = static_cast<int>(std::floor(ratioY + 0.5));
361  if (stepX < 1 || stepY < 1 || stepX != stepY)
362  {
363  itkExceptionMacro(<< "Incompatible spacing values between disparity map and input image. Left spacing: " << leftSpacing
364  << ", disparity spacing: " << dispSpacing);
365  }
366  this->m_Step = static_cast<unsigned int>(stepX);
367 
368  double shiftX = (dispOrigin[0] - leftOrigin[0]) / leftSpacing[0];
369  double shiftY = (dispOrigin[1] - leftOrigin[1]) / leftSpacing[1];
370  this->m_GridIndex[0] = static_cast<typename IndexType::IndexValueType>(std::floor(shiftX + 0.5));
371  this->m_GridIndex[1] = static_cast<typename IndexType::IndexValueType>(std::floor(shiftY + 0.5));
372 }
373 
374 template <class TInputImage, class TOutputMetricImage, class TDisparityImage, class TMaskImage, class TBlockMatchingFunctor>
376 {
377  // Call superclass implementation
378  Superclass::GenerateInputRequestedRegion();
379 
380  // Retrieve input pointers
381  TInputImage* inLeftPtr = const_cast<TInputImage*>(this->GetLeftInput());
382  TInputImage* inRightPtr = const_cast<TInputImage*>(this->GetRightInput());
383  TMaskImage* inLeftMaskPtr = const_cast<TMaskImage*>(this->GetLeftMaskInput());
384  TMaskImage* inRightMaskPtr = const_cast<TMaskImage*>(this->GetRightMaskInput());
385  TDisparityImage* inHDispPtr = const_cast<TDisparityImage*>(this->GetHorizontalDisparityInput());
386  TDisparityImage* inVDispPtr = const_cast<TDisparityImage*>(this->GetVerticalDisparityInput());
387 
388  TDisparityImage* outHDispPtr = this->GetHorizontalDisparityOutput();
389 
390  // Retrieve requested region (TODO: check if we need to handle
391  // region for outHDispPtr)
392  RegionType outputRequestedRegion = outHDispPtr->GetRequestedRegion();
393  RegionType fullRequestedRegion = BlockMatchingFilterType::ConvertSubsampledToFullRegion(outputRequestedRegion, this->m_Step, this->m_GridIndex);
394 
395  // Pad by the appropriate radius
396  RegionType inputLeftRegion = fullRequestedRegion;
397  inputLeftRegion.PadByRadius(m_Radius);
398 
399  // Now, we must find the corresponding region in moving image
400  IndexType rightRequestedRegionIndex = fullRequestedRegion.GetIndex();
401  rightRequestedRegionIndex[0] += m_MinimumHorizontalDisparity;
402  rightRequestedRegionIndex[1] += m_MinimumVerticalDisparity;
403 
404  SizeType rightRequestedRegionSize = fullRequestedRegion.GetSize();
405  rightRequestedRegionSize[0] += m_MaximumHorizontalDisparity - m_MinimumHorizontalDisparity;
406  rightRequestedRegionSize[1] += m_MaximumVerticalDisparity - m_MinimumVerticalDisparity;
407 
408  RegionType inputRightRegion;
409  inputRightRegion.SetIndex(rightRequestedRegionIndex);
410  inputRightRegion.SetSize(rightRequestedRegionSize);
411 
412  RegionType inputRightMaskRegion = inputRightRegion;
413 
414  inputRightRegion.PadByRadius(m_Radius);
415 
416  // crop the left region at the left's largest possible region
417  if (inputLeftRegion.Crop(inLeftPtr->GetLargestPossibleRegion()))
418  {
419  inLeftPtr->SetRequestedRegion(inputLeftRegion);
420  }
421  else
422  {
423  // Couldn't crop the region (requested region is outside the largest
424  // possible region). Throw an exception.
425  // store what we tried to request (prior to trying to crop)
426  inLeftPtr->SetRequestedRegion(inputLeftRegion);
427 
428  // build an exception
429  itk::InvalidRequestedRegionError e(__FILE__, __LINE__);
430  std::ostringstream msg;
431  msg << this->GetNameOfClass() << "::GenerateInputRequestedRegion()";
432  e.SetLocation(msg.str());
433  e.SetDescription("Requested region is (at least partially) outside the largest possible region of left image.");
434  e.SetDataObject(inLeftPtr);
435  throw e;
436  }
437 
438 
439  // crop the right region at the right's largest possible region
440  if (inputRightRegion.Crop(inRightPtr->GetLargestPossibleRegion()))
441  {
442  inRightPtr->SetRequestedRegion(inputRightRegion);
443  inputRightMaskRegion.Crop(inRightPtr->GetLargestPossibleRegion());
444  }
445  else
446  {
447  // Depending on the minimum and maximum disparities, the right side patch can
448  // be outside the image : in this case, just request an empty region
449  inputRightRegion.SetIndex(inRightPtr->GetLargestPossibleRegion().GetIndex());
450  inputRightRegion.SetSize(0, 0);
451  inputRightRegion.SetSize(1, 0);
452  inRightPtr->SetRequestedRegion(inputRightRegion);
453  inputRightMaskRegion = inputRightRegion;
454 
455 
456  // // Couldn't crop the region (requested region is outside the largest
457  // // possible region). Throw an exception.
458  // // store what we tried to request (prior to trying to crop)
459  // inRightPtr->SetRequestedRegion( inputRightRegion );
460  //
461  // // build an exception
462  // itk::InvalidRequestedRegionError e(__FILE__, __LINE__);
463  // std::ostringstream msg;
464  // msg << this->GetNameOfClass()
465  // << "::GenerateInputRequestedRegion()";
466  // e.SetLocation(msg.str());
467  // e.SetDescription("Requested region is (at least partially) outside the largest possible region of right image.");
468  // e.SetDataObject(inRightPtr);
469  // throw e;
470  }
471 
472  if (inLeftMaskPtr)
473  {
474  // no need to crop the mask region : left mask and left image have same largest possible region
475  inLeftMaskPtr->SetRequestedRegion(fullRequestedRegion);
476  }
477 
478  if (inRightMaskPtr)
479  {
480  inRightMaskPtr->SetRequestedRegion(inputRightMaskRegion);
481  }
482 
483  if (inHDispPtr)
484  {
485  inHDispPtr->SetRequestedRegion(outputRequestedRegion);
486  }
487 
488  if (inVDispPtr)
489  {
490  inVDispPtr->SetRequestedRegion(outputRequestedRegion);
491  }
492 }
493 
494 template <class TInputImage, class TOutputMetricImage, class TDisparityImage, class TMaskImage, class TBlockMatchingFunctor>
496 {
497  TOutputMetricImage* outMetricPtr = this->GetMetricOutput();
498  TDisparityImage* outHDispPtr = this->GetHorizontalDisparityOutput();
499  TDisparityImage* outVDispPtr = this->GetVerticalDisparityOutput();
500 
501  // Fill buffers with default values
502  outMetricPtr->FillBuffer(0.);
503  outHDispPtr->FillBuffer(static_cast<DisparityPixelType>(m_MinimumHorizontalDisparity) / static_cast<DisparityPixelType>(this->m_Step));
504  outVDispPtr->FillBuffer(static_cast<DisparityPixelType>(m_MinimumVerticalDisparity) / static_cast<DisparityPixelType>(this->m_Step));
505 
506  m_WrongExtrema.resize(this->GetNumberOfThreads());
507 }
508 
509 template <class TInputImage, class TOutputMetricImage, class TDisparityImage, class TMaskImage, class TBlockMatchingFunctor>
511  const RegionType& outputRegionForThread, itk::ThreadIdType threadId)
512 {
513  // choose the refinement method to use
514  switch (m_RefineMethod)
515  {
516  // 0 = Parabolic fit
517  case 0:
518  ParabolicRefinement(outputRegionForThread, threadId);
519  break;
520 
521  // 1 = triangular fit
522  case 1:
523  TriangularRefinement(outputRegionForThread, threadId);
524  break;
525 
526  // 2 = dichotomy search
527  case 2:
528  DichotomyRefinement(outputRegionForThread, threadId);
529  break;
530  default:
531  break;
532  }
533 }
534 
535 template <class TInputImage, class TOutputMetricImage, class TDisparityImage, class TMaskImage, class TBlockMatchingFunctor>
537  const RegionType& outputRegionForThread, itk::ThreadIdType threadId)
538 {
539  // Retrieve pointers
540  const TInputImage* inLeftPtr = this->GetLeftInput();
541  const TInputImage* inRightPtr = this->GetRightInput();
542  const TMaskImage* inLeftMaskPtr = this->GetLeftMaskInput();
543  const TMaskImage* inRightMaskPtr = this->GetRightMaskInput();
544  const TDisparityImage* inHDispPtr = this->GetHorizontalDisparityInput();
545  const TDisparityImage* inVDispPtr = this->GetVerticalDisparityInput();
546  TOutputMetricImage* outMetricPtr = this->GetMetricOutput();
547  TDisparityImage* outHDispPtr = this->GetHorizontalDisparityOutput();
548  TDisparityImage* outVDispPtr = this->GetVerticalDisparityOutput();
549 
550  unsigned int nb_WrongExtrema = 0;
551 
552  // Set-up progress reporting (this is not exact, since we do not
553  // account for pixels that won't be refined)
554  itk::ProgressReporter progress(this, threadId, outputRegionForThread.GetNumberOfPixels(), 100);
555 
556  RegionType fullRegionForThread = BlockMatchingFilterType::ConvertSubsampledToFullRegion(outputRegionForThread, this->m_Step, this->m_GridIndex);
557 
558  itk::ConstNeighborhoodIterator<TInputImage> leftIt(m_Radius, inLeftPtr, fullRegionForThread);
559  itk::ImageRegionIterator<TDisparityImage> outHDispIt(outHDispPtr, outputRegionForThread);
560  itk::ImageRegionIterator<TDisparityImage> outVDispIt(outVDispPtr, outputRegionForThread);
561  itk::ImageRegionIterator<TOutputMetricImage> outMetricIt(outMetricPtr, outputRegionForThread);
562  itk::ImageRegionConstIterator<TDisparityImage> inHDispIt;
563  itk::ImageRegionConstIterator<TDisparityImage> inVDispIt;
564  itk::ImageRegionConstIterator<TMaskImage> inLeftMaskIt;
565  itk::ImageRegionConstIterator<TMaskImage> inRightMaskIt;
566 
567  bool useHorizontalDisparity = false;
568  bool useVerticalDisparity = false;
569 
570  if (inHDispPtr)
571  {
572  useHorizontalDisparity = true;
573  inHDispIt = itk::ImageRegionConstIterator<TDisparityImage>(inHDispPtr, outputRegionForThread);
574  inHDispIt.GoToBegin();
575  }
576  if (inVDispPtr)
577  {
578  useVerticalDisparity = true;
579  inVDispIt = itk::ImageRegionConstIterator<TDisparityImage>(inVDispPtr, outputRegionForThread);
580  inVDispIt.GoToBegin();
581  }
582 
583  if (inLeftMaskPtr)
584  {
585  inLeftMaskIt = itk::ImageRegionConstIterator<TMaskImage>(inLeftMaskPtr, fullRegionForThread);
586  inLeftMaskIt.GoToBegin();
587  }
588  RegionType rightBufferedRegion = inRightPtr->GetBufferedRegion();
589  if (inRightMaskPtr)
590  {
591  RegionType inRightMaskRegion = inRightMaskPtr->GetBufferedRegion();
592  inRightMaskIt = itk::ImageRegionConstIterator<TMaskImage>(inRightMaskPtr, inRightMaskRegion);
593  }
594 
595  itk::ConstantBoundaryCondition<TInputImage> nbc1;
596  leftIt.OverrideBoundaryCondition(&nbc1);
597 
598  leftIt.GoToBegin();
599  outHDispIt.GoToBegin();
600  outVDispIt.GoToBegin();
601  outMetricIt.GoToBegin();
602 
603  /* Specific variables */
604  IndexType curLeftPos;
605  IndexType curRightPos;
606 
607  float hDisp_f;
608  float vDisp_f;
609  int hDisp_i;
610  int vDisp_i;
611 
612  // compute metric around current right position
613  bool horizontalInterpolation = false;
614  bool verticalInterpolation = false;
615 
616  typename ResamplerFilterType::Pointer resampler;
617 
618  // step value as disparityType
619  DisparityPixelType stepDisparity = static_cast<DisparityPixelType>(this->m_Step);
620  DisparityPixelType stepDisparityInv = 1. / stepDisparity;
621 
622  // metrics for neighbors positions : first index is x, second is y
623  double neighborsMetric[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
624 
625  while (!leftIt.IsAtEnd() || !outHDispIt.IsAtEnd() || !outVDispIt.IsAtEnd() || !outMetricIt.IsAtEnd())
626  {
627  // If the pixel location is on the subsampled grid
628  curLeftPos = leftIt.GetIndex();
629  if (((curLeftPos[0] - this->m_GridIndex[0] + this->m_Step) % this->m_Step == 0) &&
630  ((curLeftPos[1] - this->m_GridIndex[1] + this->m_Step) % this->m_Step == 0))
631  {
632  horizontalInterpolation = false;
633  verticalInterpolation = false;
634 
635  /* compute estimated right position */
636  if (useHorizontalDisparity)
637  {
638  hDisp_f = static_cast<float>(inHDispIt.Get()) * stepDisparity;
639  hDisp_i = static_cast<int>(std::floor(hDisp_f + 0.5));
640  curRightPos[0] = curLeftPos[0] + hDisp_i;
641  }
642  else
643  {
644  hDisp_i = 0;
645  curRightPos[0] = curLeftPos[0];
646  }
647 
648 
649  if (useVerticalDisparity)
650  {
651  vDisp_f = static_cast<float>(inVDispIt.Get()) * stepDisparity;
652  vDisp_i = static_cast<int>(std::floor(vDisp_f + 0.5));
653  curRightPos[1] = curLeftPos[1] + vDisp_i;
654  }
655  else
656  {
657  vDisp_i = 0;
658  curRightPos[1] = curLeftPos[1];
659  }
660 
661  // check if the current right position is inside the right image
662  if (rightBufferedRegion.IsInside(curRightPos))
663  {
664  if (inRightMaskPtr)
665  {
666  // Set right mask iterator position
667  inRightMaskIt.SetIndex(curRightPos);
668  }
669  // check that the current positions are not masked
670  if (!inLeftMaskPtr || (inLeftMaskIt.Get() > 0))
671  {
672  if (!inRightMaskPtr || (inRightMaskIt.Get() > 0))
673  {
674  RegionType smallRightRegion;
675  smallRightRegion.SetIndex(0, curRightPos[0] - 1);
676  smallRightRegion.SetIndex(1, curRightPos[1] - 1);
677  smallRightRegion.SetSize(0, 3);
678  smallRightRegion.SetSize(1, 3);
679 
680  itk::ConstNeighborhoodIterator<TInputImage> rightIt(m_Radius, inRightPtr, smallRightRegion);
681  itk::ConstantBoundaryCondition<TInputImage> nbc2;
682  rightIt.OverrideBoundaryCondition(&nbc2);
683 
684  // compute metric at centre position
685  rightIt.SetLocation(curRightPos);
686  neighborsMetric[1][1] = m_Functor(leftIt, rightIt);
687 
688  if (m_MinimumVerticalDisparity < vDisp_i && vDisp_i < m_MaximumVerticalDisparity)
689  {
690  IndexType upIndex(curRightPos);
691  upIndex[1] += (-1);
692  IndexType downIndex(curRightPos);
693  downIndex[1] += 1;
694  if (rightBufferedRegion.IsInside(upIndex) && rightBufferedRegion.IsInside(downIndex))
695  {
696  rightIt.SetLocation(upIndex);
697  neighborsMetric[1][0] = m_Functor(leftIt, rightIt);
698 
699  rightIt.SetLocation(downIndex);
700  neighborsMetric[1][2] = m_Functor(leftIt, rightIt);
701 
702  // check that current position is an extrema
703  if (m_Minimize)
704  {
705  if (neighborsMetric[1][1] < neighborsMetric[1][0] && neighborsMetric[1][1] < neighborsMetric[1][2])
706  {
707  verticalInterpolation = true;
708  }
709  }
710  else
711  {
712  if (neighborsMetric[1][1] > neighborsMetric[1][0] && neighborsMetric[1][1] > neighborsMetric[1][2])
713  {
714  verticalInterpolation = true;
715  }
716  }
717  }
718  }
719 
720  if (m_MinimumHorizontalDisparity < hDisp_i && hDisp_i < m_MaximumHorizontalDisparity)
721  {
722  IndexType leftIndex(curRightPos);
723  leftIndex[0] += (-1);
724  IndexType rightIndex(curRightPos);
725  rightIndex[0] += 1;
726  if (rightBufferedRegion.IsInside(leftIndex) && rightBufferedRegion.IsInside(rightIndex))
727  {
728  rightIt.SetLocation(rightIndex);
729  neighborsMetric[2][1] = m_Functor(leftIt, rightIt);
730 
731  rightIt.SetLocation(leftIndex);
732  neighborsMetric[0][1] = m_Functor(leftIt, rightIt);
733 
734  // check that current position is an extrema
735  if (m_Minimize)
736  {
737  if (neighborsMetric[1][1] < neighborsMetric[0][1] && neighborsMetric[1][1] < neighborsMetric[2][1])
738  {
739  horizontalInterpolation = true;
740  }
741  }
742  else
743  {
744  if (neighborsMetric[1][1] > neighborsMetric[0][1] && neighborsMetric[1][1] > neighborsMetric[2][1])
745  {
746  horizontalInterpolation = true;
747  }
748  }
749  }
750  }
751 
752  // if both vertical and horizontal interpolation, compute metrics on corners
753  if (verticalInterpolation && horizontalInterpolation)
754  {
755  IndexType uprightIndex(curRightPos);
756  uprightIndex[0] += 1;
757  uprightIndex[1] += (-1);
758  rightIt.SetLocation(uprightIndex);
759  neighborsMetric[2][0] = m_Functor(leftIt, rightIt);
760 
761  IndexType downrightIndex(curRightPos);
762  downrightIndex[0] += 1;
763  downrightIndex[1] += 1;
764  rightIt.SetLocation(downrightIndex);
765  neighborsMetric[2][2] = m_Functor(leftIt, rightIt);
766 
767  IndexType downleftIndex(curRightPos);
768  downleftIndex[0] += (-1);
769  downleftIndex[1] += 1;
770  rightIt.SetLocation(downleftIndex);
771  neighborsMetric[0][2] = m_Functor(leftIt, rightIt);
772 
773  IndexType upleftIndex(curRightPos);
774  upleftIndex[0] += (-1);
775  upleftIndex[1] += (-1);
776  rightIt.SetLocation(upleftIndex);
777  neighborsMetric[0][0] = m_Functor(leftIt, rightIt);
778 
779  // check that it is an extrema
780  if (m_Minimize)
781  {
782  if (neighborsMetric[1][1] > neighborsMetric[2][0] || neighborsMetric[1][1] > neighborsMetric[2][2] ||
783  neighborsMetric[1][1] > neighborsMetric[0][2] || neighborsMetric[1][1] > neighborsMetric[0][0])
784  {
785  verticalInterpolation = false;
786  horizontalInterpolation = false;
787  }
788  }
789  else
790  {
791  if (neighborsMetric[1][1] < neighborsMetric[2][0] || neighborsMetric[1][1] < neighborsMetric[2][2] ||
792  neighborsMetric[1][1] < neighborsMetric[0][2] || neighborsMetric[1][1] < neighborsMetric[0][0])
793  {
794  verticalInterpolation = false;
795  horizontalInterpolation = false;
796  }
797  }
798  }
799  }
800  }
801  }
802 
803  // Interpolate position : parabola fit
804  if (verticalInterpolation && !horizontalInterpolation)
805  {
806  // vertical only
807  double deltaV = 0.5 - (1.0 / (1.0 + (neighborsMetric[1][0] - neighborsMetric[1][1]) / (neighborsMetric[1][2] - neighborsMetric[1][1])));
808  if (deltaV > (-0.5) && deltaV < 0.5)
809  {
810  outVDispIt.Set((static_cast<double>(vDisp_i) + deltaV) * stepDisparityInv);
811  }
812  else
813  {
814  verticalInterpolation = false;
815  }
816  }
817  else if (!verticalInterpolation && horizontalInterpolation)
818  {
819  // horizontal only
820  double deltaH = 0.5 - (1.0 / (1.0 + (neighborsMetric[0][1] - neighborsMetric[1][1]) / (neighborsMetric[2][1] - neighborsMetric[1][1])));
821  if (deltaH > (-0.5) && deltaH < 0.5)
822  {
823  outHDispIt.Set((static_cast<double>(hDisp_i) + deltaH) * stepDisparityInv);
824  }
825  else
826  {
827  horizontalInterpolation = false;
828  }
829  }
830  else if (verticalInterpolation && horizontalInterpolation)
831  {
832  // both horizontal and vertical
833  double dx = 0.5 * (neighborsMetric[2][1] - neighborsMetric[0][1]);
834  double dy = 0.5 * (neighborsMetric[1][2] - neighborsMetric[1][0]);
835  double dxx = neighborsMetric[2][1] + neighborsMetric[0][1] - 2.0 * neighborsMetric[1][1];
836  double dyy = neighborsMetric[1][2] + neighborsMetric[1][0] - 2.0 * neighborsMetric[1][1];
837  double dxy = 0.25 * (neighborsMetric[2][2] + neighborsMetric[0][0] - neighborsMetric[0][2] - neighborsMetric[2][0]);
838  double det = dxx * dyy - dxy * dxy;
839  if (std::abs(det) < (1e-10))
840  {
841  verticalInterpolation = false;
842  horizontalInterpolation = false;
843  }
844  else
845  {
846  double deltaH = (-dx * dyy + dy * dxy) / det;
847  double deltaV = (dx * dxy - dy * dxx) / det;
848  if (deltaH > (-1.0) && deltaH < 1.0 && deltaV > (-1.0) && deltaV < 1.0)
849  {
850  outHDispIt.Set((static_cast<double>(hDisp_i) + deltaH) * stepDisparityInv);
851  outVDispIt.Set((static_cast<double>(vDisp_i) + deltaV) * stepDisparityInv);
852  }
853  else
854  {
855  verticalInterpolation = false;
856  horizontalInterpolation = false;
857  }
858  }
859  }
860 
861  if (!verticalInterpolation)
862  {
863  // No vertical interpolation done : simply copy the integer vertical disparity
864  outVDispIt.Set(static_cast<double>(vDisp_i) * stepDisparityInv);
865  }
866 
867  if (!horizontalInterpolation)
868  {
869  // No horizontal interpolation done : simply copy the integer horizontal disparity
870  outHDispIt.Set(static_cast<double>(hDisp_i) * stepDisparityInv);
871  }
872 
873  if (!verticalInterpolation && !horizontalInterpolation)
874  {
875  // no interpolation done : keep current score
876  outMetricIt.Set(static_cast<double>(neighborsMetric[1][1]));
877  }
878  else
879  {
880  // interpolation done, use a resampler to compute new score
881  typename TInputImage::Pointer fakeRightPtr = TInputImage::New();
882  fakeRightPtr->SetRegions(rightBufferedRegion);
883  fakeRightPtr->SetPixelContainer((const_cast<TInputImage*>(inRightPtr))->GetPixelContainer());
884 
885  itk::ConstNeighborhoodIterator<TInputImage> shiftedIt;
886  itk::ConstantBoundaryCondition<TInputImage> nbc3;
887  shiftedIt.OverrideBoundaryCondition(&nbc3);
888 
889  SizeType windowSize;
890  windowSize[0] = 2 * m_Radius[0] + 1;
891  windowSize[1] = 2 * m_Radius[1] + 1;
892 
893  IndexType upleftCorner;
894  upleftCorner[0] = curRightPos[0] - m_Radius[0];
895  upleftCorner[1] = curRightPos[1] - m_Radius[1];
896 
897  TransformationType::Pointer transfo = TransformationType::New();
898  TransformationType::OutputVectorType offsetTransfo(2);
899 
900  RegionType tinyShiftedRegion;
901  tinyShiftedRegion.SetSize(0, 1);
902  tinyShiftedRegion.SetSize(1, 1);
903  tinyShiftedRegion.SetIndex(0, curRightPos[0]);
904  tinyShiftedRegion.SetIndex(1, curRightPos[1]);
905 
906  resampler = ResamplerFilterType::New();
907  resampler->SetInput(fakeRightPtr);
908  resampler->SetSize(windowSize);
909  resampler->SetNumberOfThreads(1);
910  resampler->SetTransform(transfo);
911  resampler->SetOutputStartIndex(upleftCorner);
912 
913  offsetTransfo[0] = outHDispIt.Get() * stepDisparity - static_cast<double>(hDisp_i);
914  offsetTransfo[1] = outVDispIt.Get() * stepDisparity - static_cast<double>(vDisp_i);
915  transfo->SetOffset(offsetTransfo);
916  resampler->Modified();
917  resampler->Update();
918  shiftedIt.Initialize(m_Radius, resampler->GetOutput(), tinyShiftedRegion);
919  outMetricIt.Set(m_Functor(leftIt, shiftedIt));
920 
921  if ((outMetricIt.Get() > neighborsMetric[1][1] && m_Minimize) || (outMetricIt.Get() < neighborsMetric[1][1] && !m_Minimize))
922  {
923  nb_WrongExtrema++;
924  }
925  }
926 
927  progress.CompletedPixel();
928  ++outHDispIt;
929  ++outVDispIt;
930  ++outMetricIt;
931  if (useHorizontalDisparity)
932  {
933  ++inHDispIt;
934  }
935  if (useVerticalDisparity)
936  {
937  ++inVDispIt;
938  }
939  }
940 
941  ++leftIt;
942 
943  if (inLeftMaskPtr)
944  {
945  ++inLeftMaskIt;
946  }
947  }
948 
949  m_WrongExtrema[threadId] = static_cast<double>(nb_WrongExtrema) / static_cast<double>(outputRegionForThread.GetNumberOfPixels());
950 }
951 
952 template <class TInputImage, class TOutputMetricImage, class TDisparityImage, class TMaskImage, class TBlockMatchingFunctor>
954  const RegionType& outputRegionForThread, itk::ThreadIdType threadId)
955 {
956  // Retrieve pointers
957  const TInputImage* inLeftPtr = this->GetLeftInput();
958  const TInputImage* inRightPtr = this->GetRightInput();
959  const TMaskImage* inLeftMaskPtr = this->GetLeftMaskInput();
960  const TMaskImage* inRightMaskPtr = this->GetRightMaskInput();
961  const TDisparityImage* inHDispPtr = this->GetHorizontalDisparityInput();
962  const TDisparityImage* inVDispPtr = this->GetVerticalDisparityInput();
963  TOutputMetricImage* outMetricPtr = this->GetMetricOutput();
964  TDisparityImage* outHDispPtr = this->GetHorizontalDisparityOutput();
965  TDisparityImage* outVDispPtr = this->GetVerticalDisparityOutput();
966 
967  unsigned int nb_WrongExtrema = 0;
968 
969  // Set-up progress reporting (this is not exact, since we do not
970  // account for pixels that won't be refined)
971  itk::ProgressReporter progress(this, threadId, outputRegionForThread.GetNumberOfPixels(), 100);
972 
973  RegionType fullRegionForThread = BlockMatchingFilterType::ConvertSubsampledToFullRegion(outputRegionForThread, this->m_Step, this->m_GridIndex);
974 
975  itk::ConstNeighborhoodIterator<TInputImage> leftIt(m_Radius, inLeftPtr, fullRegionForThread);
976  itk::ImageRegionIterator<TDisparityImage> outHDispIt(outHDispPtr, outputRegionForThread);
977  itk::ImageRegionIterator<TDisparityImage> outVDispIt(outVDispPtr, outputRegionForThread);
978  itk::ImageRegionIterator<TOutputMetricImage> outMetricIt(outMetricPtr, outputRegionForThread);
979  itk::ImageRegionConstIterator<TDisparityImage> inHDispIt;
980  itk::ImageRegionConstIterator<TDisparityImage> inVDispIt;
981  itk::ImageRegionConstIterator<TMaskImage> inLeftMaskIt;
982  itk::ImageRegionConstIterator<TMaskImage> inRightMaskIt;
983 
984  bool useHorizontalDisparity = false;
985  bool useVerticalDisparity = false;
986 
987  if (inHDispPtr)
988  {
989  useHorizontalDisparity = true;
990  inHDispIt = itk::ImageRegionConstIterator<TDisparityImage>(inHDispPtr, outputRegionForThread);
991  inHDispIt.GoToBegin();
992  }
993  if (inVDispPtr)
994  {
995  useVerticalDisparity = true;
996  inVDispIt = itk::ImageRegionConstIterator<TDisparityImage>(inVDispPtr, outputRegionForThread);
997  inVDispIt.GoToBegin();
998  }
999 
1000  if (inLeftMaskPtr)
1001  {
1002  inLeftMaskIt = itk::ImageRegionConstIterator<TMaskImage>(inLeftMaskPtr, fullRegionForThread);
1003  inLeftMaskIt.GoToBegin();
1004  }
1005  RegionType rightBufferedRegion = inRightPtr->GetBufferedRegion();
1006  if (inRightMaskPtr)
1007  {
1008  RegionType inRightMaskRegion = inRightMaskPtr->GetBufferedRegion();
1009  inRightMaskIt = itk::ImageRegionConstIterator<TMaskImage>(inRightMaskPtr, inRightMaskRegion);
1010  }
1011 
1012  itk::ConstantBoundaryCondition<TInputImage> nbc1;
1013  leftIt.OverrideBoundaryCondition(&nbc1);
1014 
1015  leftIt.GoToBegin();
1016  outHDispIt.GoToBegin();
1017  outVDispIt.GoToBegin();
1018  outMetricIt.GoToBegin();
1019 
1020  /* Specific variables */
1021  IndexType curLeftPos;
1022  IndexType curRightPos;
1023 
1024  float hDisp_f;
1025  float vDisp_f;
1026  int hDisp_i;
1027  int vDisp_i;
1028 
1029  // compute metric around current right position
1030  bool horizontalInterpolation = false;
1031  bool verticalInterpolation = false;
1032 
1033  typename ResamplerFilterType::Pointer resampler;
1034 
1035  // step value as disparityType
1036  DisparityPixelType stepDisparity = static_cast<DisparityPixelType>(this->m_Step);
1037  DisparityPixelType stepDisparityInv = 1. / stepDisparity;
1038 
1039 
1040  // metrics for neighbors positions : first index is x, second is y
1041  double neighborsMetric[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
1042 
1043  while (!leftIt.IsAtEnd() || !outHDispIt.IsAtEnd() || !outVDispIt.IsAtEnd() || !outMetricIt.IsAtEnd())
1044  {
1045  // If the pixel location is on the subsampled grid
1046  curLeftPos = leftIt.GetIndex();
1047  if (((curLeftPos[0] - this->m_GridIndex[0] + this->m_Step) % this->m_Step == 0) &&
1048  ((curLeftPos[1] - this->m_GridIndex[1] + this->m_Step) % this->m_Step == 0))
1049  {
1050  horizontalInterpolation = false;
1051  verticalInterpolation = false;
1052 
1053  /* compute estimated right position */
1054  if (useHorizontalDisparity)
1055  {
1056  hDisp_f = static_cast<float>(inHDispIt.Get()) * stepDisparity;
1057  hDisp_i = static_cast<int>(std::floor(hDisp_f + 0.5));
1058  curRightPos[0] = curLeftPos[0] + hDisp_i;
1059  }
1060  else
1061  {
1062  hDisp_i = 0;
1063  curRightPos[0] = curLeftPos[0];
1064  }
1065 
1066 
1067  if (useVerticalDisparity)
1068  {
1069  vDisp_f = static_cast<float>(inVDispIt.Get()) * stepDisparity;
1070  vDisp_i = static_cast<int>(std::floor(vDisp_f + 0.5));
1071  curRightPos[1] = curLeftPos[1] + vDisp_i;
1072  }
1073  else
1074  {
1075  vDisp_i = 0;
1076  curRightPos[1] = curLeftPos[1];
1077  }
1078 
1079  // check if the current right position is inside the right image
1080  if (rightBufferedRegion.IsInside(curRightPos))
1081  {
1082  if (inRightMaskPtr)
1083  {
1084  // Set right mask iterator position
1085  inRightMaskIt.SetIndex(curRightPos);
1086  }
1087  // check that the current positions are not masked
1088  if (!inLeftMaskPtr || (inLeftMaskIt.Get() > 0))
1089  {
1090  if (!inRightMaskPtr || (inRightMaskIt.Get() > 0))
1091  {
1092  RegionType smallRightRegion;
1093  smallRightRegion.SetIndex(0, curRightPos[0] - 1);
1094  smallRightRegion.SetIndex(1, curRightPos[1] - 1);
1095  smallRightRegion.SetSize(0, 3);
1096  smallRightRegion.SetSize(1, 3);
1097 
1098  itk::ConstNeighborhoodIterator<TInputImage> rightIt(m_Radius, inRightPtr, smallRightRegion);
1099  itk::ConstantBoundaryCondition<TInputImage> nbc2;
1100  rightIt.OverrideBoundaryCondition(&nbc2);
1101 
1102  // compute metric at centre position
1103  rightIt.SetLocation(curRightPos);
1104  neighborsMetric[1][1] = m_Functor(leftIt, rightIt);
1105 
1106  if (m_MinimumVerticalDisparity < vDisp_i && vDisp_i < m_MaximumVerticalDisparity)
1107  {
1108  IndexType upIndex(curRightPos);
1109  upIndex[1] += (-1);
1110  IndexType downIndex(curRightPos);
1111  downIndex[1] += 1;
1112  if (rightBufferedRegion.IsInside(upIndex) && rightBufferedRegion.IsInside(downIndex))
1113  {
1114  rightIt.SetLocation(upIndex);
1115  neighborsMetric[1][0] = m_Functor(leftIt, rightIt);
1116 
1117  rightIt.SetLocation(downIndex);
1118  neighborsMetric[1][2] = m_Functor(leftIt, rightIt);
1119 
1120  // check that current position is an extrema
1121  if (m_Minimize)
1122  {
1123  if (neighborsMetric[1][1] < neighborsMetric[1][0] && neighborsMetric[1][1] < neighborsMetric[1][2])
1124  {
1125  verticalInterpolation = true;
1126  }
1127  }
1128  else
1129  {
1130  if (neighborsMetric[1][1] > neighborsMetric[1][0] && neighborsMetric[1][1] > neighborsMetric[1][2])
1131  {
1132  verticalInterpolation = true;
1133  }
1134  }
1135  }
1136  }
1137 
1138  if (m_MinimumHorizontalDisparity < hDisp_i && hDisp_i < m_MaximumHorizontalDisparity)
1139  {
1140  IndexType leftIndex(curRightPos);
1141  leftIndex[0] += (-1);
1142  IndexType rightIndex(curRightPos);
1143  rightIndex[0] += 1;
1144  if (rightBufferedRegion.IsInside(leftIndex) && rightBufferedRegion.IsInside(rightIndex))
1145  {
1146  rightIt.SetLocation(rightIndex);
1147  neighborsMetric[2][1] = m_Functor(leftIt, rightIt);
1148 
1149  rightIt.SetLocation(leftIndex);
1150  neighborsMetric[0][1] = m_Functor(leftIt, rightIt);
1151 
1152  // check that current position is an extrema
1153  if (m_Minimize)
1154  {
1155  if (neighborsMetric[1][1] < neighborsMetric[0][1] && neighborsMetric[1][1] < neighborsMetric[2][1])
1156  {
1157  horizontalInterpolation = true;
1158  }
1159  }
1160  else
1161  {
1162  if (neighborsMetric[1][1] > neighborsMetric[0][1] && neighborsMetric[1][1] > neighborsMetric[2][1])
1163  {
1164  horizontalInterpolation = true;
1165  }
1166  }
1167  }
1168  }
1169 
1170  // if both vertical and horizontal interpolation, compute metrics on corners
1171  if (verticalInterpolation && horizontalInterpolation)
1172  {
1173  IndexType uprightIndex(curRightPos);
1174  uprightIndex[0] += 1;
1175  uprightIndex[1] += (-1);
1176  rightIt.SetLocation(uprightIndex);
1177  neighborsMetric[2][0] = m_Functor(leftIt, rightIt);
1178 
1179  IndexType downrightIndex(curRightPos);
1180  downrightIndex[0] += 1;
1181  downrightIndex[1] += 1;
1182  rightIt.SetLocation(downrightIndex);
1183  neighborsMetric[2][2] = m_Functor(leftIt, rightIt);
1184 
1185  IndexType downleftIndex(curRightPos);
1186  downleftIndex[0] += (-1);
1187  downleftIndex[1] += 1;
1188  rightIt.SetLocation(downleftIndex);
1189  neighborsMetric[0][2] = m_Functor(leftIt, rightIt);
1190 
1191  IndexType upleftIndex(curRightPos);
1192  upleftIndex[0] += (-1);
1193  upleftIndex[1] += (-1);
1194  rightIt.SetLocation(upleftIndex);
1195  neighborsMetric[0][0] = m_Functor(leftIt, rightIt);
1196 
1197  // check that it is an extrema
1198  if (m_Minimize)
1199  {
1200  if (neighborsMetric[1][1] > neighborsMetric[2][0] || neighborsMetric[1][1] > neighborsMetric[2][2] ||
1201  neighborsMetric[1][1] > neighborsMetric[0][2] || neighborsMetric[1][1] > neighborsMetric[0][0])
1202  {
1203  verticalInterpolation = false;
1204  horizontalInterpolation = false;
1205  }
1206  }
1207  else
1208  {
1209  if (neighborsMetric[1][1] < neighborsMetric[2][0] || neighborsMetric[1][1] < neighborsMetric[2][2] ||
1210  neighborsMetric[1][1] < neighborsMetric[0][2] || neighborsMetric[1][1] < neighborsMetric[0][0])
1211  {
1212  verticalInterpolation = false;
1213  horizontalInterpolation = false;
1214  }
1215  }
1216  }
1217  }
1218  }
1219  }
1220 
1221  // Interpolate position : triangular fit
1222  if (verticalInterpolation && !horizontalInterpolation)
1223  {
1224  // vertical only
1225  double deltaV;
1226  if ((neighborsMetric[1][0] < neighborsMetric[1][2] && m_Minimize) || (neighborsMetric[1][0] > neighborsMetric[1][2] && !m_Minimize))
1227  {
1228  deltaV = 0.5 * ((neighborsMetric[1][0] - neighborsMetric[1][2]) / (neighborsMetric[1][2] - neighborsMetric[1][1]));
1229  }
1230  else
1231  {
1232  deltaV = 0.5 * ((neighborsMetric[1][0] - neighborsMetric[1][2]) / (neighborsMetric[1][0] - neighborsMetric[1][1]));
1233  }
1234  if (deltaV > (-0.5) && deltaV < 0.5)
1235  {
1236  outVDispIt.Set((static_cast<double>(vDisp_i) + deltaV) * stepDisparityInv);
1237  }
1238  else
1239  {
1240  verticalInterpolation = false;
1241  }
1242  }
1243  else if (!verticalInterpolation && horizontalInterpolation)
1244  {
1245  // horizontal only
1246  double deltaH;
1247  if ((neighborsMetric[0][1] < neighborsMetric[2][1] && m_Minimize) || (neighborsMetric[0][1] > neighborsMetric[2][1] && !m_Minimize))
1248  {
1249  deltaH = 0.5 * ((neighborsMetric[0][1] - neighborsMetric[2][1]) / (neighborsMetric[2][1] - neighborsMetric[1][1]));
1250  }
1251  else
1252  {
1253  deltaH = 0.5 * ((neighborsMetric[0][1] - neighborsMetric[2][1]) / (neighborsMetric[0][1] - neighborsMetric[1][1]));
1254  }
1255  if (deltaH > (-0.5) && deltaH < 0.5)
1256  {
1257  outHDispIt.Set((static_cast<double>(hDisp_i) + deltaH) * stepDisparityInv);
1258  }
1259  else
1260  {
1261  horizontalInterpolation = false;
1262  }
1263  }
1264  else if (verticalInterpolation && horizontalInterpolation)
1265  {
1266  // both horizontal and vertical
1267  double deltaH;
1268  double deltaV;
1269  if ((neighborsMetric[1][0] < neighborsMetric[1][2] && m_Minimize) || (neighborsMetric[1][0] > neighborsMetric[1][2] && !m_Minimize))
1270  {
1271  deltaV = 0.5 * ((neighborsMetric[1][0] - neighborsMetric[1][2]) / (neighborsMetric[1][2] - neighborsMetric[1][1]));
1272  }
1273  else
1274  {
1275  deltaV = 0.5 * ((neighborsMetric[1][0] - neighborsMetric[1][2]) / (neighborsMetric[1][0] - neighborsMetric[1][1]));
1276  }
1277 
1278  if ((neighborsMetric[0][1] < neighborsMetric[2][1] && m_Minimize) || (neighborsMetric[0][1] > neighborsMetric[2][1] && !m_Minimize))
1279  {
1280  deltaH = 0.5 * ((neighborsMetric[0][1] - neighborsMetric[2][1]) / (neighborsMetric[2][1] - neighborsMetric[1][1]));
1281  }
1282  else
1283  {
1284  deltaH = 0.5 * ((neighborsMetric[0][1] - neighborsMetric[2][1]) / (neighborsMetric[0][1] - neighborsMetric[1][1]));
1285  }
1286 
1287  if (deltaV > (-1.0) && deltaV < 1.0 && deltaH > (-1.0) && deltaH < 1.0)
1288  {
1289  outVDispIt.Set((static_cast<double>(vDisp_i) + deltaV) * stepDisparityInv);
1290  outHDispIt.Set((static_cast<double>(hDisp_i) + deltaH) * stepDisparityInv);
1291  }
1292  else
1293  {
1294  verticalInterpolation = false;
1295  horizontalInterpolation = false;
1296  }
1297  }
1298 
1299  if (!verticalInterpolation)
1300  {
1301  // No vertical interpolation done : simply copy the integer vertical disparity
1302  outVDispIt.Set(static_cast<double>(vDisp_i) * stepDisparityInv);
1303  }
1304 
1305  if (!horizontalInterpolation)
1306  {
1307  // No horizontal interpolation done : simply copy the integer horizontal disparity
1308  outHDispIt.Set(static_cast<double>(hDisp_i) * stepDisparityInv);
1309  }
1310 
1311  if (!verticalInterpolation && !horizontalInterpolation)
1312  {
1313  // no interpolation done : keep current score
1314  outMetricIt.Set(static_cast<double>(neighborsMetric[1][1]));
1315  }
1316  else
1317  {
1318  // interpolation done, use a resampler to compute new score
1319  typename TInputImage::Pointer fakeRightPtr = TInputImage::New();
1320  fakeRightPtr->SetRegions(rightBufferedRegion);
1321  fakeRightPtr->SetPixelContainer((const_cast<TInputImage*>(inRightPtr))->GetPixelContainer());
1322 
1323  itk::ConstNeighborhoodIterator<TInputImage> shiftedIt;
1324  itk::ConstantBoundaryCondition<TInputImage> nbc3;
1325  shiftedIt.OverrideBoundaryCondition(&nbc3);
1326 
1327  SizeType windowSize;
1328  windowSize[0] = 2 * m_Radius[0] + 1;
1329  windowSize[1] = 2 * m_Radius[1] + 1;
1330 
1331  IndexType upleftCorner;
1332  upleftCorner[0] = curRightPos[0] - m_Radius[0];
1333  upleftCorner[1] = curRightPos[1] - m_Radius[1];
1334 
1335  TransformationType::Pointer transfo = TransformationType::New();
1336  TransformationType::OutputVectorType offsetTransfo(2);
1337 
1338  RegionType tinyShiftedRegion;
1339  tinyShiftedRegion.SetSize(0, 1);
1340  tinyShiftedRegion.SetSize(1, 1);
1341  tinyShiftedRegion.SetIndex(0, curRightPos[0]);
1342  tinyShiftedRegion.SetIndex(1, curRightPos[1]);
1343 
1344  resampler = ResamplerFilterType::New();
1345  resampler->SetInput(fakeRightPtr);
1346  resampler->SetSize(windowSize);
1347  resampler->SetNumberOfThreads(1);
1348  resampler->SetTransform(transfo);
1349  resampler->SetOutputStartIndex(upleftCorner);
1350 
1351  offsetTransfo[0] = outHDispIt.Get() * stepDisparity - static_cast<double>(hDisp_i);
1352  offsetTransfo[1] = outVDispIt.Get() * stepDisparity - static_cast<double>(vDisp_i);
1353  transfo->SetOffset(offsetTransfo);
1354  resampler->Modified();
1355  resampler->Update();
1356  shiftedIt.Initialize(m_Radius, resampler->GetOutput(), tinyShiftedRegion);
1357  outMetricIt.Set(m_Functor(leftIt, shiftedIt));
1358 
1359  if ((outMetricIt.Get() > neighborsMetric[1][1] && m_Minimize) || (outMetricIt.Get() < neighborsMetric[1][1] && !m_Minimize))
1360  {
1361  nb_WrongExtrema++;
1362  }
1363  }
1364 
1365  progress.CompletedPixel();
1366 
1367  ++outHDispIt;
1368  ++outVDispIt;
1369  ++outMetricIt;
1370 
1371  if (useHorizontalDisparity)
1372  {
1373  ++inHDispIt;
1374  }
1375  if (useVerticalDisparity)
1376  {
1377  ++inVDispIt;
1378  }
1379  }
1380  ++leftIt;
1381 
1382  if (inLeftMaskPtr)
1383  {
1384  ++inLeftMaskIt;
1385  }
1386  }
1387 
1388  m_WrongExtrema[threadId] = static_cast<double>(nb_WrongExtrema) / static_cast<double>(outputRegionForThread.GetNumberOfPixels());
1389 }
1390 
1391 template <class TInputImage, class TOutputMetricImage, class TDisparityImage, class TMaskImage, class TBlockMatchingFunctor>
1393  const RegionType& outputRegionForThread, itk::ThreadIdType threadId)
1394 {
1395  // Retrieve pointers
1396  const TInputImage* inLeftPtr = this->GetLeftInput();
1397  const TInputImage* inRightPtr = this->GetRightInput();
1398  const TMaskImage* inLeftMaskPtr = this->GetLeftMaskInput();
1399  const TMaskImage* inRightMaskPtr = this->GetRightMaskInput();
1400  const TDisparityImage* inHDispPtr = this->GetHorizontalDisparityInput();
1401  const TDisparityImage* inVDispPtr = this->GetVerticalDisparityInput();
1402  TOutputMetricImage* outMetricPtr = this->GetMetricOutput();
1403  TDisparityImage* outHDispPtr = this->GetHorizontalDisparityOutput();
1404  TDisparityImage* outVDispPtr = this->GetVerticalDisparityOutput();
1405 
1406  unsigned int nb_WrongExtrema = 0;
1407 
1408  // Set-up progress reporting (this is not exact, since we do not
1409  // account for pixels that won't be refined)
1410  itk::ProgressReporter progress(this, threadId, outputRegionForThread.GetNumberOfPixels(), 100);
1411 
1412  RegionType fullRegionForThread = BlockMatchingFilterType::ConvertSubsampledToFullRegion(outputRegionForThread, this->m_Step, this->m_GridIndex);
1413 
1414  itk::ConstNeighborhoodIterator<TInputImage> leftIt(m_Radius, inLeftPtr, fullRegionForThread);
1415  itk::ImageRegionIterator<TDisparityImage> outHDispIt(outHDispPtr, outputRegionForThread);
1416  itk::ImageRegionIterator<TDisparityImage> outVDispIt(outVDispPtr, outputRegionForThread);
1417  itk::ImageRegionIterator<TOutputMetricImage> outMetricIt(outMetricPtr, outputRegionForThread);
1418  itk::ImageRegionConstIterator<TDisparityImage> inHDispIt;
1419  itk::ImageRegionConstIterator<TDisparityImage> inVDispIt;
1420  itk::ImageRegionConstIterator<TMaskImage> inLeftMaskIt;
1421  itk::ImageRegionConstIterator<TMaskImage> inRightMaskIt;
1422 
1423  bool useHorizontalDisparity = false;
1424  bool useVerticalDisparity = false;
1425 
1426  if (inHDispPtr)
1427  {
1428  useHorizontalDisparity = true;
1429  inHDispIt = itk::ImageRegionConstIterator<TDisparityImage>(inHDispPtr, outputRegionForThread);
1430  inHDispIt.GoToBegin();
1431  }
1432  if (inVDispPtr)
1433  {
1434  useVerticalDisparity = true;
1435  inVDispIt = itk::ImageRegionConstIterator<TDisparityImage>(inVDispPtr, outputRegionForThread);
1436  inVDispIt.GoToBegin();
1437  }
1438 
1439  if (inLeftMaskPtr)
1440  {
1441  inLeftMaskIt = itk::ImageRegionConstIterator<TMaskImage>(inLeftMaskPtr, fullRegionForThread);
1442  inLeftMaskIt.GoToBegin();
1443  }
1444  RegionType rightBufferedRegion = inRightPtr->GetBufferedRegion();
1445  if (inRightMaskPtr)
1446  {
1447  RegionType inRightMaskRegion = inRightMaskPtr->GetBufferedRegion();
1448  inRightMaskIt = itk::ImageRegionConstIterator<TMaskImage>(inRightMaskPtr, inRightMaskRegion);
1449  }
1450 
1451  itk::ConstantBoundaryCondition<TInputImage> nbc1;
1452  leftIt.OverrideBoundaryCondition(&nbc1);
1453 
1454  leftIt.GoToBegin();
1455  outHDispIt.GoToBegin();
1456  outVDispIt.GoToBegin();
1457  outMetricIt.GoToBegin();
1458 
1459  /* Specific variables */
1460  IndexType curLeftPos;
1461  IndexType curRightPos;
1462 
1463  float hDisp_f;
1464  float vDisp_f;
1465  int hDisp_i;
1466  int vDisp_i;
1467 
1468  typename TInputImage::Pointer fakeRightPtr = TInputImage::New();
1469  fakeRightPtr->SetRegions(rightBufferedRegion);
1470  fakeRightPtr->SetPixelContainer((const_cast<TInputImage*>(inRightPtr))->GetPixelContainer());
1471 
1472  // compute metric around current right position
1473  bool horizontalInterpolation = false;
1474  bool verticalInterpolation = false;
1475 
1476  // metrics for neighbors positions : first index is x, second is y
1477  double neighborsMetric[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
1478  unsigned int nbIterMax = 10;
1479 
1480  // set the output size and start index to the center position
1481  // then set the smaller shift in the transform
1482  IndexType upleftCorner;
1483  SizeType windowSize;
1484  windowSize[0] = 2 * m_Radius[0] + 1;
1485  windowSize[1] = 2 * m_Radius[1] + 1;
1486 
1487  TransformationType::Pointer transfo = TransformationType::New();
1488  TransformationType::OutputVectorType offsetTransfo(2);
1489  offsetTransfo[0] = 0.0;
1490  offsetTransfo[1] = 0.0;
1491 
1492  typename ResamplerFilterType::Pointer resampler;
1493 
1494  // step value as disparityType
1495  DisparityPixelType stepDisparity = static_cast<DisparityPixelType>(this->m_Step);
1496  DisparityPixelType stepDisparityInv = 1. / stepDisparity;
1497 
1498  RegionType tinyShiftedRegion;
1499  tinyShiftedRegion.SetIndex(0, m_Radius[0]);
1500  tinyShiftedRegion.SetIndex(1, m_Radius[1]);
1501  tinyShiftedRegion.SetSize(0, 1);
1502  tinyShiftedRegion.SetSize(1, 1);
1503 
1504  // iterators on right image
1505  itk::ConstNeighborhoodIterator<TInputImage> rightIt;
1506  itk::ConstNeighborhoodIterator<TInputImage> shiftedIt;
1507  itk::ConstantBoundaryCondition<TInputImage> nbc2;
1508  itk::ConstantBoundaryCondition<TInputImage> nbc3;
1509  rightIt.OverrideBoundaryCondition(&nbc2);
1510  shiftedIt.OverrideBoundaryCondition(&nbc3);
1511  bool centreHasMoved;
1512 
1513  while (!leftIt.IsAtEnd() || !outHDispIt.IsAtEnd() || !outVDispIt.IsAtEnd() || !outMetricIt.IsAtEnd())
1514  {
1515  // If the pixel location is on the subsampled grid
1516  curLeftPos = leftIt.GetIndex();
1517  if (((curLeftPos[0] - this->m_GridIndex[0] + this->m_Step) % this->m_Step == 0) &&
1518  ((curLeftPos[1] - this->m_GridIndex[1] + this->m_Step) % this->m_Step == 0))
1519  {
1520  horizontalInterpolation = false;
1521  verticalInterpolation = false;
1522 
1523  /* compute estimated right position */
1524  if (useHorizontalDisparity)
1525  {
1526  hDisp_f = static_cast<float>(inHDispIt.Get()) * stepDisparity;
1527  hDisp_i = static_cast<int>(std::floor(hDisp_f + 0.5));
1528  curRightPos[0] = curLeftPos[0] + hDisp_i;
1529  }
1530  else
1531  {
1532  hDisp_i = 0;
1533  curRightPos[0] = curLeftPos[0];
1534  }
1535 
1536 
1537  if (useVerticalDisparity)
1538  {
1539  vDisp_f = static_cast<float>(inVDispIt.Get()) * stepDisparity;
1540  vDisp_i = static_cast<int>(std::floor(vDisp_f + 0.5));
1541  curRightPos[1] = curLeftPos[1] + vDisp_i;
1542  }
1543  else
1544  {
1545  vDisp_i = 0;
1546  curRightPos[1] = curLeftPos[1];
1547  }
1548 
1549  // update resampler input position
1550  upleftCorner[0] = curRightPos[0] - m_Radius[0];
1551  upleftCorner[1] = curRightPos[1] - m_Radius[1];
1552 
1553  // resampler
1554  resampler = ResamplerFilterType::New();
1555  resampler->SetInput(fakeRightPtr);
1556  resampler->SetSize(windowSize);
1557  resampler->SetNumberOfThreads(1);
1558  resampler->SetTransform(transfo);
1559  resampler->SetOutputStartIndex(upleftCorner);
1560 
1561  tinyShiftedRegion.SetIndex(0, curRightPos[0]);
1562  tinyShiftedRegion.SetIndex(1, curRightPos[1]);
1563 
1564  // check if the current right position is inside the right image
1565  if (rightBufferedRegion.IsInside(curRightPos))
1566  {
1567  if (inRightMaskPtr)
1568  {
1569  // Set right mask iterator position
1570  inRightMaskIt.SetIndex(curRightPos);
1571  }
1572  // check that the current positions are not masked
1573  if (!inLeftMaskPtr || (inLeftMaskIt.Get() > 0))
1574  {
1575  if (!inRightMaskPtr || (inRightMaskIt.Get() > 0))
1576  {
1577  RegionType smallRightRegion;
1578  smallRightRegion.SetIndex(0, curRightPos[0] - 1);
1579  smallRightRegion.SetIndex(1, curRightPos[1] - 1);
1580  smallRightRegion.SetSize(0, 3);
1581  smallRightRegion.SetSize(1, 3);
1582 
1583  rightIt.Initialize(m_Radius, inRightPtr, smallRightRegion);
1584 
1585  // compute metric at centre position
1586  rightIt.SetLocation(curRightPos);
1587  neighborsMetric[1][1] = m_Functor(leftIt, rightIt);
1588 
1589  if (m_MinimumVerticalDisparity < vDisp_i && vDisp_i < m_MaximumVerticalDisparity)
1590  {
1591  IndexType upIndex(curRightPos);
1592  upIndex[1] += (-1);
1593  IndexType downIndex(curRightPos);
1594  downIndex[1] += 1;
1595  if (rightBufferedRegion.IsInside(upIndex) && rightBufferedRegion.IsInside(downIndex))
1596  {
1597  rightIt.SetLocation(upIndex);
1598  neighborsMetric[1][0] = m_Functor(leftIt, rightIt);
1599 
1600  rightIt.SetLocation(downIndex);
1601  neighborsMetric[1][2] = m_Functor(leftIt, rightIt);
1602 
1603  // check that current position is an extrema
1604  if (m_Minimize)
1605  {
1606  if (neighborsMetric[1][1] < neighborsMetric[1][0] && neighborsMetric[1][1] < neighborsMetric[1][2])
1607  {
1608  verticalInterpolation = true;
1609  }
1610  }
1611  else
1612  {
1613  if (neighborsMetric[1][1] > neighborsMetric[1][0] && neighborsMetric[1][1] > neighborsMetric[1][2])
1614  {
1615  verticalInterpolation = true;
1616  }
1617  }
1618  }
1619  }
1620 
1621  if (m_MinimumHorizontalDisparity < hDisp_i && hDisp_i < m_MaximumHorizontalDisparity)
1622  {
1623  IndexType leftIndex(curRightPos);
1624  leftIndex[0] += (-1);
1625  IndexType rightIndex(curRightPos);
1626  rightIndex[0] += 1;
1627  if (rightBufferedRegion.IsInside(leftIndex) && rightBufferedRegion.IsInside(rightIndex))
1628  {
1629  rightIt.SetLocation(rightIndex);
1630  neighborsMetric[2][1] = m_Functor(leftIt, rightIt);
1631 
1632  rightIt.SetLocation(leftIndex);
1633  neighborsMetric[0][1] = m_Functor(leftIt, rightIt);
1634 
1635  // check that current position is an extrema
1636  if (m_Minimize)
1637  {
1638  if (neighborsMetric[1][1] < neighborsMetric[0][1] && neighborsMetric[1][1] < neighborsMetric[2][1])
1639  {
1640  horizontalInterpolation = true;
1641  }
1642  }
1643  else
1644  {
1645  if (neighborsMetric[1][1] > neighborsMetric[0][1] && neighborsMetric[1][1] > neighborsMetric[2][1])
1646  {
1647  horizontalInterpolation = true;
1648  }
1649  }
1650  }
1651  }
1652 
1653  // if both vertical and horizontal interpolation, compute metrics on corners
1654  if (verticalInterpolation && horizontalInterpolation)
1655  {
1656  IndexType uprightIndex(curRightPos);
1657  uprightIndex[0] += 1;
1658  uprightIndex[1] += (-1);
1659  rightIt.SetLocation(uprightIndex);
1660  neighborsMetric[2][0] = m_Functor(leftIt, rightIt);
1661 
1662  IndexType downrightIndex(curRightPos);
1663  downrightIndex[0] += 1;
1664  downrightIndex[1] += 1;
1665  rightIt.SetLocation(downrightIndex);
1666  neighborsMetric[2][2] = m_Functor(leftIt, rightIt);
1667 
1668  IndexType downleftIndex(curRightPos);
1669  downleftIndex[0] += (-1);
1670  downleftIndex[1] += 1;
1671  rightIt.SetLocation(downleftIndex);
1672  neighborsMetric[0][2] = m_Functor(leftIt, rightIt);
1673 
1674  IndexType upleftIndex(curRightPos);
1675  upleftIndex[0] += (-1);
1676  upleftIndex[1] += (-1);
1677  rightIt.SetLocation(upleftIndex);
1678  neighborsMetric[0][0] = m_Functor(leftIt, rightIt);
1679 
1680  // check that it is an extrema
1681  if (m_Minimize)
1682  {
1683  if (neighborsMetric[1][1] > neighborsMetric[2][0] || neighborsMetric[1][1] > neighborsMetric[2][2] ||
1684  neighborsMetric[1][1] > neighborsMetric[0][2] || neighborsMetric[1][1] > neighborsMetric[0][0])
1685  {
1686  verticalInterpolation = false;
1687  horizontalInterpolation = false;
1688  }
1689  }
1690  else
1691  {
1692  if (neighborsMetric[1][1] < neighborsMetric[2][0] || neighborsMetric[1][1] < neighborsMetric[2][2] ||
1693  neighborsMetric[1][1] < neighborsMetric[0][2] || neighborsMetric[1][1] < neighborsMetric[0][0])
1694  {
1695  verticalInterpolation = false;
1696  horizontalInterpolation = false;
1697  }
1698  }
1699  }
1700  }
1701  }
1702  }
1703 
1704  // Interpolate position : dichotomy
1705  if (verticalInterpolation && !horizontalInterpolation)
1706  {
1707  double ya = static_cast<double>(vDisp_i - 1);
1708  double yb = static_cast<double>(vDisp_i);
1709  double yc = static_cast<double>(vDisp_i + 1);
1710  double s_yb = neighborsMetric[1][1];
1711 
1712  double yd;
1713  double s_yd;
1714 
1715  offsetTransfo[0] = 0.0;
1716 
1717  for (unsigned int k = 0; k < nbIterMax; k++)
1718  {
1719  if ((yb - ya) < (yc - yb))
1720  {
1721  yd = 0.5 * (yc + yb);
1722  offsetTransfo[1] = yd - static_cast<double>(vDisp_i);
1723  transfo->SetOffset(offsetTransfo);
1724  resampler->Modified();
1725  resampler->Update();
1726  shiftedIt.Initialize(m_Radius, resampler->GetOutput(), tinyShiftedRegion);
1727  s_yd = m_Functor(leftIt, shiftedIt);
1728 
1729  if ((s_yd < s_yb && m_Minimize) || (s_yd > s_yb && !m_Minimize))
1730  {
1731  ya = yb;
1732 
1733  yb = yd;
1734  s_yb = s_yd;
1735  }
1736  else
1737  {
1738  yc = yd;
1739  }
1740  }
1741  else
1742  {
1743  yd = 0.5 * (ya + yb);
1744  offsetTransfo[1] = yd - static_cast<double>(vDisp_i);
1745  transfo->SetOffset(offsetTransfo);
1746  resampler->Modified();
1747  resampler->Update();
1748  shiftedIt.Initialize(m_Radius, resampler->GetOutput(), tinyShiftedRegion);
1749  s_yd = m_Functor(leftIt, shiftedIt);
1750 
1751  if ((s_yd < s_yb && m_Minimize) || (s_yd > s_yb && !m_Minimize))
1752  {
1753  yc = yb;
1754 
1755  yb = yd;
1756  s_yb = s_yd;
1757  }
1758  else
1759  {
1760  ya = yd;
1761  }
1762  }
1763  }
1764 
1765  outVDispIt.Set(yb * stepDisparityInv);
1766  outMetricIt.Set(s_yb);
1767  }
1768  else if (!verticalInterpolation && horizontalInterpolation)
1769  {
1770  double xa = static_cast<double>(hDisp_i - 1);
1771  double xb = static_cast<double>(hDisp_i);
1772  double xc = static_cast<double>(hDisp_i + 1);
1773  double s_xb = neighborsMetric[1][1];
1774 
1775  double xd;
1776  double s_xd;
1777 
1778  offsetTransfo[1] = 0.0;
1779 
1780  for (unsigned int k = 0; k < nbIterMax; k++)
1781  {
1782  if ((xb - xa) < (xc - xb))
1783  {
1784  xd = 0.5 * (xc + xb);
1785  offsetTransfo[0] = xd - static_cast<double>(hDisp_i);
1786  transfo->SetOffset(offsetTransfo);
1787  resampler->Modified();
1788  resampler->Update();
1789  shiftedIt.Initialize(m_Radius, resampler->GetOutput(), tinyShiftedRegion);
1790  s_xd = m_Functor(leftIt, shiftedIt);
1791 
1792  if ((s_xd < s_xb && m_Minimize) || (s_xd > s_xb && !m_Minimize))
1793  {
1794  xa = xb;
1795 
1796  xb = xd;
1797  s_xb = s_xd;
1798  }
1799  else
1800  {
1801  xc = xd;
1802  }
1803  }
1804  else
1805  {
1806  xd = 0.5 * (xa + xb);
1807  offsetTransfo[0] = xd - static_cast<double>(hDisp_i);
1808  transfo->SetOffset(offsetTransfo);
1809  resampler->Modified();
1810  resampler->Update();
1811  shiftedIt.Initialize(m_Radius, resampler->GetOutput(), tinyShiftedRegion);
1812  s_xd = m_Functor(leftIt, shiftedIt);
1813 
1814  if ((s_xd < s_xb && m_Minimize) || (s_xd > s_xb && !m_Minimize))
1815  {
1816  xc = xb;
1817 
1818  xb = xd;
1819  s_xb = s_xd;
1820  }
1821  else
1822  {
1823  xa = xd;
1824  }
1825  }
1826  }
1827 
1828  outHDispIt.Set(xb * stepDisparityInv);
1829  outMetricIt.Set(s_xb);
1830  }
1831  else if (verticalInterpolation && horizontalInterpolation)
1832  {
1833  double xa = static_cast<double>(hDisp_i);
1834  double ya = static_cast<double>(vDisp_i - 1);
1835  double xb = static_cast<double>(hDisp_i);
1836  double yb = static_cast<double>(vDisp_i);
1837  double yc = static_cast<double>(vDisp_i + 1);
1838  double xe = static_cast<double>(hDisp_i - 1);
1839  double ye = static_cast<double>(vDisp_i);
1840  double xf = static_cast<double>(hDisp_i + 1);
1841 
1842  double s_b = neighborsMetric[1][1];
1843 
1844  double xd = xb;
1845  double yd = yb;
1846  double s_d;
1847 
1848  for (unsigned int k = 0; k < nbIterMax; k++)
1849  {
1850  // Vertical step
1851  centreHasMoved = false;
1852  if ((yb - ya) < (yc - yb))
1853  {
1854  yd = 0.5 * (yc + yb);
1855  offsetTransfo[0] = xd - static_cast<double>(hDisp_i);
1856  offsetTransfo[1] = yd - static_cast<double>(vDisp_i);
1857  transfo->SetOffset(offsetTransfo);
1858  resampler->Modified();
1859  resampler->Update();
1860  shiftedIt.Initialize(m_Radius, resampler->GetOutput(), tinyShiftedRegion);
1861  s_d = m_Functor(leftIt, shiftedIt);
1862 
1863  if ((s_d < s_b && m_Minimize) || (s_d > s_b && !m_Minimize))
1864  {
1865  centreHasMoved = true;
1866 
1867  ya = yb;
1868 
1869  yb = yd;
1870  s_b = s_d;
1871  }
1872  else
1873  {
1874  yc = yd;
1875  }
1876  }
1877  else
1878  {
1879  yd = 0.5 * (ya + yb);
1880  offsetTransfo[0] = xd - static_cast<double>(hDisp_i);
1881  offsetTransfo[1] = yd - static_cast<double>(vDisp_i);
1882  transfo->SetOffset(offsetTransfo);
1883  resampler->Modified();
1884  resampler->Update();
1885  shiftedIt.Initialize(m_Radius, resampler->GetOutput(), tinyShiftedRegion);
1886  s_d = m_Functor(leftIt, shiftedIt);
1887 
1888  if ((s_d < s_b && m_Minimize) || (s_d > s_b && !m_Minimize))
1889  {
1890  centreHasMoved = true;
1891 
1892  yc = yb;
1893 
1894  yb = yd;
1895  s_b = s_d;
1896  }
1897  else
1898  {
1899  ya = yd;
1900  }
1901  }
1902  if (centreHasMoved)
1903  {
1904  // update points e and f
1905  ye = yb;
1906 
1907  offsetTransfo[0] = xe - static_cast<double>(hDisp_i);
1908  offsetTransfo[1] = ye - static_cast<double>(vDisp_i);
1909  transfo->SetOffset(offsetTransfo);
1910  resampler->Modified();
1911  resampler->Update();
1912  shiftedIt.Initialize(m_Radius, resampler->GetOutput(), tinyShiftedRegion);
1913 
1914 
1915  offsetTransfo[0] = xf - static_cast<double>(hDisp_i);
1916  transfo->SetOffset(offsetTransfo);
1917  resampler->Modified();
1918  resampler->Update();
1919  shiftedIt.Initialize(m_Radius, resampler->GetOutput(), tinyShiftedRegion);
1920  }
1921 
1922  // Horizontal step
1923  centreHasMoved = false;
1924  if ((xb - xe) < (xf - xb))
1925  {
1926  xd = 0.5 * (xf + xb);
1927  offsetTransfo[0] = xd - static_cast<double>(hDisp_i);
1928  offsetTransfo[1] = yd - static_cast<double>(vDisp_i);
1929  transfo->SetOffset(offsetTransfo);
1930  resampler->Modified();
1931  resampler->Update();
1932  shiftedIt.Initialize(m_Radius, resampler->GetOutput(), tinyShiftedRegion);
1933  s_d = m_Functor(leftIt, shiftedIt);
1934 
1935  if ((s_d < s_b && m_Minimize) || (s_d > s_b && !m_Minimize))
1936  {
1937  centreHasMoved = true;
1938 
1939  xe = xb;
1940 
1941  xb = xd;
1942  s_b = s_d;
1943  }
1944  else
1945  {
1946  xf = xd;
1947  }
1948  }
1949  else
1950  {
1951  xd = 0.5 * (xe + xb);
1952  offsetTransfo[0] = xd - static_cast<double>(hDisp_i);
1953  offsetTransfo[1] = yd - static_cast<double>(vDisp_i);
1954  transfo->SetOffset(offsetTransfo);
1955  resampler->Modified();
1956  resampler->Update();
1957  shiftedIt.Initialize(m_Radius, resampler->GetOutput(), tinyShiftedRegion);
1958  s_d = m_Functor(leftIt, shiftedIt);
1959 
1960  if ((s_d < s_b && m_Minimize) || (s_d > s_b && !m_Minimize))
1961  {
1962  centreHasMoved = true;
1963 
1964  xf = xb;
1965 
1966  xb = xd;
1967  s_b = s_d;
1968  }
1969  else
1970  {
1971  xe = xd;
1972  }
1973  }
1974  if (centreHasMoved)
1975  {
1976  // update a and c
1977  xa = xb;
1978 
1979  offsetTransfo[0] = xa - static_cast<double>(hDisp_i);
1980  offsetTransfo[1] = ya - static_cast<double>(vDisp_i);
1981  transfo->SetOffset(offsetTransfo);
1982  resampler->Modified();
1983  resampler->Update();
1984  shiftedIt.Initialize(m_Radius, resampler->GetOutput(), tinyShiftedRegion);
1985 
1986  offsetTransfo[1] = yc - static_cast<double>(vDisp_i);
1987  transfo->SetOffset(offsetTransfo);
1988  resampler->Modified();
1989  resampler->Update();
1990  shiftedIt.Initialize(m_Radius, resampler->GetOutput(), tinyShiftedRegion);
1991  }
1992  }
1993 
1994  outHDispIt.Set(xb * stepDisparityInv);
1995  outVDispIt.Set(yb * stepDisparityInv);
1996  outMetricIt.Set(s_b);
1997  }
1998 
1999  if (!verticalInterpolation)
2000  {
2001  // No vertical interpolation done : simply copy the integer vertical disparity
2002  outVDispIt.Set(static_cast<double>(vDisp_i) * stepDisparityInv);
2003  }
2004 
2005  if (!horizontalInterpolation)
2006  {
2007  // No horizontal interpolation done : simply copy the integer horizontal disparity
2008  outHDispIt.Set(static_cast<double>(hDisp_i) * stepDisparityInv);
2009  }
2010 
2011  if (!verticalInterpolation && !horizontalInterpolation)
2012  {
2013  outMetricIt.Set(static_cast<double>(neighborsMetric[1][1]));
2014  }
2015  else
2016  {
2017  if ((outMetricIt.Get() > neighborsMetric[1][1] && m_Minimize) || (outMetricIt.Get() < neighborsMetric[1][1] && !m_Minimize))
2018  {
2019  nb_WrongExtrema++;
2020  }
2021  }
2022 
2023  progress.CompletedPixel();
2024 
2025  ++outHDispIt;
2026  ++outVDispIt;
2027  ++outMetricIt;
2028 
2029  if (useHorizontalDisparity)
2030  {
2031  ++inHDispIt;
2032  }
2033  if (useVerticalDisparity)
2034  {
2035  ++inVDispIt;
2036  }
2037  }
2038  ++leftIt;
2039 
2040  if (inLeftMaskPtr)
2041  {
2042  ++inLeftMaskIt;
2043  }
2044  }
2045 
2046  m_WrongExtrema[threadId] = static_cast<double>(nb_WrongExtrema) / static_cast<double>(outputRegionForThread.GetNumberOfPixels());
2047 }
2048 
2049 template <class TInputImage, class TOutputMetricImage, class TDisparityImage, class TMaskImage, class TBlockMatchingFunctor>
2051 {
2052  double wrongExtremaPercent = 0;
2053  for (unsigned int i = 0; i < m_WrongExtrema.size(); i++)
2054  {
2055  wrongExtremaPercent += m_WrongExtrema[i];
2056  }
2057  wrongExtremaPercent /= m_WrongExtrema.size();
2058 
2059  // std::cout << "Wrong extrema percentage = "<< wrongExtremaPercent << std::endl;
2060 }
2061 
2062 } // End namespace otb
2063 
2064 #endif
otb::SubPixelDisparityImageFilter::ParabolicRefinement
void ParabolicRefinement(const RegionType &outputRegionForThread, itk::ThreadIdType threadId)
Definition: otbSubPixelDisparityImageFilter.hxx:536
otb::SubPixelDisparityImageFilter::BeforeThreadedGenerateData
void BeforeThreadedGenerateData() override
Definition: otbSubPixelDisparityImageFilter.hxx:495
otb::SubPixelDisparityImageFilter::IndexType
InputImageType::IndexType IndexType
Definition: otbSubPixelDisparityImageFilter.h:90
otb::SubPixelDisparityImageFilter::SetRightInput
void SetRightInput(const TInputImage *image)
Definition: otbSubPixelDisparityImageFilter.hxx:76
otb::SubPixelDisparityImageFilter::SubPixelDisparityImageFilter
SubPixelDisparityImageFilter()
Definition: otbSubPixelDisparityImageFilter.hxx:29
otb::SubPixelDisparityImageFilter::AfterThreadedGenerateData
void AfterThreadedGenerateData() override
Definition: otbSubPixelDisparityImageFilter.hxx:2050
otb::SubPixelDisparityImageFilter::SetVerticalDisparityInput
void SetVerticalDisparityInput(const TDisparityImage *vfield)
Definition: otbSubPixelDisparityImageFilter.hxx:91
otb::SubPixelDisparityImageFilter::SizeType
InputImageType::SizeType SizeType
Definition: otbSubPixelDisparityImageFilter.h:89
otb::PixelWiseBlockMatchingImageFilter::GetRightMaskInput
const TMaskImage * GetRightMaskInput() const
Definition: otbPixelWiseBlockMatchingImageFilter.hxx:146
otbSubPixelDisparityImageFilter.h
otb::SubPixelDisparityImageFilter::SetRightMaskInput
void SetRightMaskInput(const TMaskImage *image)
Definition: otbSubPixelDisparityImageFilter.hxx:107
otb::SubPixelDisparityImageFilter::TriangularRefinement
void TriangularRefinement(const RegionType &outputRegionForThread, itk::ThreadIdType threadId)
Definition: otbSubPixelDisparityImageFilter.hxx:953
otb
The "otb" namespace contains all Orfeo Toolbox (OTB) classes.
Definition: otbJoinContainer.h:32
otb::PixelWiseBlockMatchingImageFilter::GetRightInput
const TInputImage * GetRightInput() const
Definition: otbPixelWiseBlockMatchingImageFilter.hxx:124
otb::PixelWiseBlockMatchingImageFilter::GetMinimumHorizontalDisparity
virtual const int & GetMinimumHorizontalDisparity() const
otb::SubPixelDisparityImageFilter::SetInputsFromBlockMatchingFilter
void SetInputsFromBlockMatchingFilter(const BlockMatchingFilterType *filter)
Definition: otbSubPixelDisparityImageFilter.hxx:249
otb::SubPixelDisparityImageFilter::SetMetricInput
void SetMetricInput(const TOutputMetricImage *image)
Definition: otbSubPixelDisparityImageFilter.hxx:115
otb::PixelWiseBlockMatchingImageFilter::GetMaximumVerticalDisparity
virtual const int & GetMaximumVerticalDisparity() const
otb::SubPixelDisparityImageFilter::DisparityPixelType
OutputDisparityImageType::PixelType DisparityPixelType
Definition: otbSubPixelDisparityImageFilter.h:97
otb::SubPixelDisparityImageFilter::SetLeftMaskInput
void SetLeftMaskInput(const TMaskImage *image)
Definition: otbSubPixelDisparityImageFilter.hxx:99
otb::SubPixelDisparityImageFilter::GetVerticalDisparityInput
const TDisparityImage * GetVerticalDisparityInput() const
Definition: otbSubPixelDisparityImageFilter.hxx:155
otb::SubPixelDisparityImageFilter::GetHorizontalDisparityOutput
const TDisparityImage * GetHorizontalDisparityOutput() const
Definition: otbSubPixelDisparityImageFilter.hxx:186
otb::PixelWiseBlockMatchingImageFilter
Perform 2D block matching between two images.
Definition: otbPixelWiseBlockMatchingImageFilter.h:298
otb::SubPixelDisparityImageFilter::DichotomyRefinement
void DichotomyRefinement(const RegionType &outputRegionForThread, itk::ThreadIdType threadId)
Definition: otbSubPixelDisparityImageFilter.hxx:1392
otb::SubPixelDisparityImageFilter::GenerateInputRequestedRegion
void GenerateInputRequestedRegion() override
Definition: otbSubPixelDisparityImageFilter.hxx:375
otb::SubPixelDisparityImageFilter::SetHorizontalDisparityInput
void SetHorizontalDisparityInput(const TDisparityImage *hfield)
Definition: otbSubPixelDisparityImageFilter.hxx:83
otb::PixelWiseBlockMatchingImageFilter::GetMinimumVerticalDisparity
virtual const int & GetMinimumVerticalDisparity() const
otb::SubPixelDisparityImageFilter::GetRightMaskInput
const TMaskImage * GetRightMaskInput() const
Definition: otbSubPixelDisparityImageFilter.hxx:175
otb::PixelWiseBlockMatchingImageFilter::GetMinimize
virtual const bool & GetMinimize() const
otb::SubPixelDisparityImageFilter::SetLeftInput
void SetLeftInput(const TInputImage *image)
Definition: otbSubPixelDisparityImageFilter.hxx:69
otb::SubPixelDisparityImageFilter::RegionType
InputImageType::RegionType RegionType
Definition: otbSubPixelDisparityImageFilter.h:91
otb::SubPixelDisparityImageFilter::GetHorizontalDisparityInput
const TDisparityImage * GetHorizontalDisparityInput() const
Definition: otbSubPixelDisparityImageFilter.hxx:144
otb::SubPixelDisparityImageFilter::PointType
InputImageType::PointType PointType
Definition: otbSubPixelDisparityImageFilter.h:93
otb::SubPixelDisparityImageFilter::GetMetricOutput
const TOutputMetricImage * GetMetricOutput() const
Definition: otbSubPixelDisparityImageFilter.hxx:229
otb::PixelWiseBlockMatchingImageFilter::GetLeftMaskInput
const TMaskImage * GetLeftMaskInput() const
Definition: otbPixelWiseBlockMatchingImageFilter.hxx:135
otb::SubPixelDisparityImageFilter::~SubPixelDisparityImageFilter
~SubPixelDisparityImageFilter() override
Definition: otbSubPixelDisparityImageFilter.hxx:64
otb::SubPixelDisparityImageFilter::GetLeftMaskInput
const TMaskImage * GetLeftMaskInput() const
Definition: otbSubPixelDisparityImageFilter.hxx:165
otb::SubPixelDisparityImageFilter::SpacingType
InputImageType::SpacingType SpacingType
Definition: otbSubPixelDisparityImageFilter.h:92
otb::SubPixelDisparityImageFilter::GenerateOutputInformation
void GenerateOutputInformation() override
Definition: otbSubPixelDisparityImageFilter.hxx:337
otb::PixelWiseBlockMatchingImageFilter::GetMetricOutput
const TOutputMetricImage * GetMetricOutput() const
Definition: otbPixelWiseBlockMatchingImageFilter.hxx:157
otb::SubPixelDisparityImageFilter::ThreadedGenerateData
void ThreadedGenerateData(const RegionType &outputRegionForThread, itk::ThreadIdType threadId) override
Definition: otbSubPixelDisparityImageFilter.hxx:510
otb::SubPixelDisparityImageFilter::GetRightInput
const TInputImage * GetRightInput() const
Definition: otbSubPixelDisparityImageFilter.hxx:133
otb::PixelWiseBlockMatchingImageFilter::GetLeftInput
const TInputImage * GetLeftInput() const
Definition: otbPixelWiseBlockMatchingImageFilter.hxx:113
otb::PixelWiseBlockMatchingImageFilter::GetRadius
virtual const SizeType & GetRadius() const
otb::PixelWiseBlockMatchingImageFilter::GetHorizontalDisparityOutput
const TOutputDisparityImage * GetHorizontalDisparityOutput() const
Definition: otbPixelWiseBlockMatchingImageFilter.hxx:180
otb::SubPixelDisparityImageFilter::GetLeftInput
const TInputImage * GetLeftInput() const
Definition: otbSubPixelDisparityImageFilter.hxx:123
otb::PixelWiseBlockMatchingImageFilter::GetVerticalDisparityOutput
const TOutputDisparityImage * GetVerticalDisparityOutput() const
Definition: otbPixelWiseBlockMatchingImageFilter.hxx:202
otb::SubPixelDisparityImageFilter::GetVerticalDisparityOutput
const TDisparityImage * GetVerticalDisparityOutput() const
Definition: otbSubPixelDisparityImageFilter.hxx:208
otb::PixelWiseBlockMatchingImageFilter::GetMaximumHorizontalDisparity
virtual const int & GetMaximumHorizontalDisparity() const
otb::SubPixelDisparityImageFilter::VerifyInputInformation
void VerifyInputInformation() override
Verify that the input images are compatible.
Definition: otbSubPixelDisparityImageFilter.hxx:286