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