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