21 #ifndef otbSubPixelDisparityImageFilter_hxx
22 #define otbSubPixelDisparityImageFilter_hxx
28 template <
class TInputImage,
class TOutputMetricImage,
class TDisparityImage,
class TMaskImage,
class TBlockMatchingFunctor>
32 this->SetNumberOfRequiredInputs(3);
35 this->SetNumberOfRequiredOutputs(3);
36 this->SetNthOutput(0, TDisparityImage::New());
37 this->SetNthOutput(1, TDisparityImage::New());
38 this->SetNthOutput(2, TOutputMetricImage::New());
47 m_MinimumHorizontalDisparity = -10;
48 m_MaximumHorizontalDisparity = 10;
49 m_MinimumVerticalDisparity = 0;
50 m_MaximumVerticalDisparity = 0;
63 template <
class TInputImage,
class TOutputMetricImage,
class TDisparityImage,
class TMaskImage,
class TBlockMatchingFunctor>
68 template <
class TInputImage,
class TOutputMetricImage,
class TDisparityImage,
class TMaskImage,
class TBlockMatchingFunctor>
72 this->SetNthInput(0,
const_cast<TInputImage*
>(image));
75 template <
class TInputImage,
class TOutputMetricImage,
class TDisparityImage,
class TMaskImage,
class TBlockMatchingFunctor>
79 this->SetNthInput(1,
const_cast<TInputImage*
>(image));
82 template <
class TInputImage,
class TOutputMetricImage,
class TDisparityImage,
class TMaskImage,
class TBlockMatchingFunctor>
84 const TDisparityImage* hfield)
87 this->SetNthInput(2,
const_cast<TDisparityImage*
>(hfield));
90 template <
class TInputImage,
class TOutputMetricImage,
class TDisparityImage,
class TMaskImage,
class TBlockMatchingFunctor>
92 const TDisparityImage* vfield)
95 this->SetNthInput(3,
const_cast<TDisparityImage*
>(vfield));
98 template <
class TInputImage,
class TOutputMetricImage,
class TDisparityImage,
class TMaskImage,
class TBlockMatchingFunctor>
100 const TMaskImage* image)
103 this->SetNthInput(4,
const_cast<TMaskImage*
>(image));
106 template <
class TInputImage,
class TOutputMetricImage,
class TDisparityImage,
class TMaskImage,
class TBlockMatchingFunctor>
108 const TMaskImage* image)
111 this->SetNthInput(5,
const_cast<TMaskImage*
>(image));
114 template <
class TInputImage,
class TOutputMetricImage,
class TDisparityImage,
class TMaskImage,
class TBlockMatchingFunctor>
116 const TOutputMetricImage* image)
119 this->SetNthInput(6,
const_cast<TOutputMetricImage*
>(image));
122 template <
class TInputImage,
class TOutputMetricImage,
class TDisparityImage,
class TMaskImage,
class TBlockMatchingFunctor>
125 if (this->GetNumberOfIndexedInputs() < 1)
129 return static_cast<const TInputImage*
>(this->itk::ProcessObject::GetInput(0));
132 template <
class TInputImage,
class TOutputMetricImage,
class TDisparityImage,
class TMaskImage,
class TBlockMatchingFunctor>
135 if (this->GetNumberOfIndexedInputs() < 2)
139 return static_cast<const TInputImage*
>(this->itk::ProcessObject::GetInput(1));
142 template <
class TInputImage,
class TOutputMetricImage,
class TDisparityImage,
class TMaskImage,
class TBlockMatchingFunctor>
143 const TDisparityImage*
146 if (this->GetNumberOfIndexedInputs() < 3)
150 return static_cast<const TDisparityImage*
>(this->itk::ProcessObject::GetInput(2));
153 template <
class TInputImage,
class TOutputMetricImage,
class TDisparityImage,
class TMaskImage,
class TBlockMatchingFunctor>
154 const TDisparityImage*
157 if (this->GetNumberOfIndexedInputs() < 4)
161 return static_cast<const TDisparityImage*
>(this->itk::ProcessObject::GetInput(3));
164 template <
class TInputImage,
class TOutputMetricImage,
class TDisparityImage,
class TMaskImage,
class TBlockMatchingFunctor>
167 if (this->GetNumberOfIndexedInputs() < 5)
171 return static_cast<const TMaskImage*
>(this->itk::ProcessObject::GetInput(4));
174 template <
class TInputImage,
class TOutputMetricImage,
class TDisparityImage,
class TMaskImage,
class TBlockMatchingFunctor>
177 if (this->GetNumberOfIndexedInputs() < 6)
181 return static_cast<const TMaskImage*
>(this->itk::ProcessObject::GetInput(5));
184 template <
class TInputImage,
class TOutputMetricImage,
class TDisparityImage,
class TMaskImage,
class TBlockMatchingFunctor>
185 const TDisparityImage*
188 if (this->GetNumberOfOutputs() < 1)
192 return static_cast<const TDisparityImage*
>(this->itk::ProcessObject::GetOutput(0));
195 template <
class TInputImage,
class TOutputMetricImage,
class TDisparityImage,
class TMaskImage,
class TBlockMatchingFunctor>
199 if (this->GetNumberOfOutputs() < 1)
203 return static_cast<TDisparityImage*
>(this->itk::ProcessObject::GetOutput(0));
206 template <
class TInputImage,
class TOutputMetricImage,
class TDisparityImage,
class TMaskImage,
class TBlockMatchingFunctor>
207 const TDisparityImage*
210 if (this->GetNumberOfOutputs() < 2)
214 return static_cast<const TDisparityImage*
>(this->itk::ProcessObject::GetOutput(1));
217 template <
class TInputImage,
class TOutputMetricImage,
class TDisparityImage,
class TMaskImage,
class TBlockMatchingFunctor>
220 if (this->GetNumberOfOutputs() < 2)
224 return static_cast<TDisparityImage*
>(this->itk::ProcessObject::GetOutput(1));
227 template <
class TInputImage,
class TOutputMetricImage,
class TDisparityImage,
class TMaskImage,
class TBlockMatchingFunctor>
228 const TOutputMetricImage*
231 if (this->GetNumberOfOutputs() < 3)
235 return static_cast<const TOutputMetricImage*
>(this->itk::ProcessObject::GetOutput(2));
238 template <
class TInputImage,
class TOutputMetricImage,
class TDisparityImage,
class TMaskImage,
class TBlockMatchingFunctor>
241 if (this->GetNumberOfOutputs() < 3)
245 return static_cast<TOutputMetricImage*
>(this->itk::ProcessObject::GetOutput(2));
248 template <
class TInputImage,
class TOutputMetricImage,
class TDisparityImage,
class TMaskImage,
class TBlockMatchingFunctor>
285 template <
class TInputImage,
class TOutputMetricImage,
class TDisparityImage,
class TMaskImage,
class TBlockMatchingFunctor>
289 const TInputImage* inLeftPtr = this->GetLeftInput();
290 const TInputImage* inRightPtr = this->GetRightInput();
291 const TMaskImage* inLeftMaskPtr = this->GetLeftMaskInput();
292 const TMaskImage* inRightMaskPtr = this->GetRightMaskInput();
293 const TDisparityImage* inHDispPtr = this->GetHorizontalDisparityInput();
294 const TDisparityImage* inVDispPtr = this->GetVerticalDisparityInput();
297 if (!inLeftPtr || !inRightPtr)
299 itkExceptionMacro(<<
"Missing input, need left and right input images.");
304 itkExceptionMacro(<<
"Input horizontal disparity map is missing");
308 if (inLeftPtr->GetLargestPossibleRegion() != inRightPtr->GetLargestPossibleRegion())
310 itkExceptionMacro(<<
"Left and right images do not have the same size ! Left largest region: " << inLeftPtr->GetLargestPossibleRegion()
311 <<
", right largest region: " << inRightPtr->GetLargestPossibleRegion());
315 if (inLeftMaskPtr && inLeftPtr->GetLargestPossibleRegion() != inLeftMaskPtr->GetLargestPossibleRegion())
317 itkExceptionMacro(<<
"Left and mask images do not have the same size ! Left largest region: " << inLeftPtr->GetLargestPossibleRegion()
318 <<
", mask largest region: " << inLeftMaskPtr->GetLargestPossibleRegion());
322 if (inRightMaskPtr && inRightPtr->GetLargestPossibleRegion() != inRightMaskPtr->GetLargestPossibleRegion())
324 itkExceptionMacro(<<
"Right and mask images do not have the same size ! Right largest region: " << inRightPtr->GetLargestPossibleRegion()
325 <<
", mask largest region: " << inRightMaskPtr->GetLargestPossibleRegion());
329 if (inHDispPtr && inVDispPtr && inHDispPtr->GetLargestPossibleRegion() != inVDispPtr->GetLargestPossibleRegion())
331 itkExceptionMacro(<<
"Initial horizontal and vertical disparity maps don't have the same size ! Horizontal disparity largest region: "
332 << inHDispPtr->GetLargestPossibleRegion() <<
", vertical disparity largest region: " << inVDispPtr->GetLargestPossibleRegion());
336 template <
class TInputImage,
class TOutputMetricImage,
class TDisparityImage,
class TMaskImage,
class TBlockMatchingFunctor>
340 const TInputImage* inLeftPtr = this->GetLeftInput();
341 const TDisparityImage* inHDispPtr = this->GetHorizontalDisparityInput();
343 TOutputMetricImage* outMetricPtr = this->GetMetricOutput();
344 TDisparityImage* outHDispPtr = this->GetHorizontalDisparityOutput();
345 TDisparityImage* outVDispPtr = this->GetVerticalDisparityOutput();
347 outMetricPtr->CopyInformation(inHDispPtr);
348 outHDispPtr->CopyInformation(inHDispPtr);
349 outVDispPtr->CopyInformation(inHDispPtr);
352 SpacingType leftSpacing = inLeftPtr->GetSignedSpacing();
353 SpacingType dispSpacing = inHDispPtr->GetSignedSpacing();
354 PointType leftOrigin = inLeftPtr->GetOrigin();
355 PointType dispOrigin = inHDispPtr->GetOrigin();
357 double ratioX = dispSpacing[0] / leftSpacing[0];
358 double ratioY = dispSpacing[1] / leftSpacing[1];
359 int stepX =
static_cast<int>(std::floor(ratioX + 0.5));
360 int stepY =
static_cast<int>(std::floor(ratioY + 0.5));
361 if (stepX < 1 || stepY < 1 || stepX != stepY)
363 itkExceptionMacro(<<
"Incompatible spacing values between disparity map and input image. Left spacing: " << leftSpacing
364 <<
", disparity spacing: " << dispSpacing);
366 this->m_Step =
static_cast<unsigned int>(stepX);
368 double shiftX = (dispOrigin[0] - leftOrigin[0]) / leftSpacing[0];
369 double shiftY = (dispOrigin[1] - leftOrigin[1]) / leftSpacing[1];
370 this->m_GridIndex[0] =
static_cast<typename IndexType::IndexValueType
>(std::floor(shiftX + 0.5));
371 this->m_GridIndex[1] =
static_cast<typename IndexType::IndexValueType
>(std::floor(shiftY + 0.5));
374 template <
class TInputImage,
class TOutputMetricImage,
class TDisparityImage,
class TMaskImage,
class TBlockMatchingFunctor>
378 Superclass::GenerateInputRequestedRegion();
381 TInputImage* inLeftPtr =
const_cast<TInputImage*
>(this->GetLeftInput());
382 TInputImage* inRightPtr =
const_cast<TInputImage*
>(this->GetRightInput());
383 TMaskImage* inLeftMaskPtr =
const_cast<TMaskImage*
>(this->GetLeftMaskInput());
384 TMaskImage* inRightMaskPtr =
const_cast<TMaskImage*
>(this->GetRightMaskInput());
385 TDisparityImage* inHDispPtr =
const_cast<TDisparityImage*
>(this->GetHorizontalDisparityInput());
386 TDisparityImage* inVDispPtr =
const_cast<TDisparityImage*
>(this->GetVerticalDisparityInput());
388 TDisparityImage* outHDispPtr = this->GetHorizontalDisparityOutput();
392 RegionType outputRequestedRegion = outHDispPtr->GetRequestedRegion();
393 RegionType fullRequestedRegion = BlockMatchingFilterType::ConvertSubsampledToFullRegion(outputRequestedRegion, this->m_Step, this->m_GridIndex);
396 RegionType inputLeftRegion = fullRequestedRegion;
397 inputLeftRegion.PadByRadius(m_Radius);
400 IndexType rightRequestedRegionIndex = fullRequestedRegion.GetIndex();
401 rightRequestedRegionIndex[0] += m_MinimumHorizontalDisparity;
402 rightRequestedRegionIndex[1] += m_MinimumVerticalDisparity;
404 SizeType rightRequestedRegionSize = fullRequestedRegion.GetSize();
405 rightRequestedRegionSize[0] += m_MaximumHorizontalDisparity - m_MinimumHorizontalDisparity;
406 rightRequestedRegionSize[1] += m_MaximumVerticalDisparity - m_MinimumVerticalDisparity;
409 inputRightRegion.SetIndex(rightRequestedRegionIndex);
410 inputRightRegion.SetSize(rightRequestedRegionSize);
412 RegionType inputRightMaskRegion = inputRightRegion;
414 inputRightRegion.PadByRadius(m_Radius);
417 if (inputLeftRegion.Crop(inLeftPtr->GetLargestPossibleRegion()))
419 inLeftPtr->SetRequestedRegion(inputLeftRegion);
426 inLeftPtr->SetRequestedRegion(inputLeftRegion);
429 itk::InvalidRequestedRegionError e(__FILE__, __LINE__);
430 std::ostringstream msg;
431 msg << this->GetNameOfClass() <<
"::GenerateInputRequestedRegion()";
432 e.SetLocation(msg.str());
433 e.SetDescription(
"Requested region is (at least partially) outside the largest possible region of left image.");
434 e.SetDataObject(inLeftPtr);
440 if (inputRightRegion.Crop(inRightPtr->GetLargestPossibleRegion()))
442 inRightPtr->SetRequestedRegion(inputRightRegion);
443 inputRightMaskRegion.Crop(inRightPtr->GetLargestPossibleRegion());
449 inputRightRegion.SetIndex(inRightPtr->GetLargestPossibleRegion().GetIndex());
450 inputRightRegion.SetSize(0, 0);
451 inputRightRegion.SetSize(1, 0);
452 inRightPtr->SetRequestedRegion(inputRightRegion);
453 inputRightMaskRegion = inputRightRegion;
475 inLeftMaskPtr->SetRequestedRegion(fullRequestedRegion);
480 inRightMaskPtr->SetRequestedRegion(inputRightMaskRegion);
485 inHDispPtr->SetRequestedRegion(outputRequestedRegion);
490 inVDispPtr->SetRequestedRegion(outputRequestedRegion);
494 template <
class TInputImage,
class TOutputMetricImage,
class TDisparityImage,
class TMaskImage,
class TBlockMatchingFunctor>
497 TOutputMetricImage* outMetricPtr = this->GetMetricOutput();
498 TDisparityImage* outHDispPtr = this->GetHorizontalDisparityOutput();
499 TDisparityImage* outVDispPtr = this->GetVerticalDisparityOutput();
502 outMetricPtr->FillBuffer(0.);
506 m_WrongExtrema.resize(this->GetNumberOfThreads());
509 template <
class TInputImage,
class TOutputMetricImage,
class TDisparityImage,
class TMaskImage,
class TBlockMatchingFunctor>
511 const RegionType& outputRegionForThread, itk::ThreadIdType threadId)
514 switch (m_RefineMethod)
518 ParabolicRefinement(outputRegionForThread, threadId);
523 TriangularRefinement(outputRegionForThread, threadId);
528 DichotomyRefinement(outputRegionForThread, threadId);
535 template <
class TInputImage,
class TOutputMetricImage,
class TDisparityImage,
class TMaskImage,
class TBlockMatchingFunctor>
537 const RegionType& outputRegionForThread, itk::ThreadIdType threadId)
540 const TInputImage* inLeftPtr = this->GetLeftInput();
541 const TInputImage* inRightPtr = this->GetRightInput();
542 const TMaskImage* inLeftMaskPtr = this->GetLeftMaskInput();
543 const TMaskImage* inRightMaskPtr = this->GetRightMaskInput();
544 const TDisparityImage* inHDispPtr = this->GetHorizontalDisparityInput();
545 const TDisparityImage* inVDispPtr = this->GetVerticalDisparityInput();
546 TOutputMetricImage* outMetricPtr = this->GetMetricOutput();
547 TDisparityImage* outHDispPtr = this->GetHorizontalDisparityOutput();
548 TDisparityImage* outVDispPtr = this->GetVerticalDisparityOutput();
550 unsigned int nb_WrongExtrema = 0;
554 itk::ProgressReporter progress(
this, threadId, outputRegionForThread.GetNumberOfPixels(), 100);
556 RegionType fullRegionForThread = BlockMatchingFilterType::ConvertSubsampledToFullRegion(outputRegionForThread, this->m_Step, this->m_GridIndex);
558 itk::ConstNeighborhoodIterator<TInputImage> leftIt(m_Radius, inLeftPtr, fullRegionForThread);
559 itk::ImageRegionIterator<TDisparityImage> outHDispIt(outHDispPtr, outputRegionForThread);
560 itk::ImageRegionIterator<TDisparityImage> outVDispIt(outVDispPtr, outputRegionForThread);
561 itk::ImageRegionIterator<TOutputMetricImage> outMetricIt(outMetricPtr, outputRegionForThread);
562 itk::ImageRegionConstIterator<TDisparityImage> inHDispIt;
563 itk::ImageRegionConstIterator<TDisparityImage> inVDispIt;
564 itk::ImageRegionConstIterator<TMaskImage> inLeftMaskIt;
565 itk::ImageRegionConstIterator<TMaskImage> inRightMaskIt;
567 bool useHorizontalDisparity =
false;
568 bool useVerticalDisparity =
false;
572 useHorizontalDisparity =
true;
573 inHDispIt = itk::ImageRegionConstIterator<TDisparityImage>(inHDispPtr, outputRegionForThread);
574 inHDispIt.GoToBegin();
578 useVerticalDisparity =
true;
579 inVDispIt = itk::ImageRegionConstIterator<TDisparityImage>(inVDispPtr, outputRegionForThread);
580 inVDispIt.GoToBegin();
585 inLeftMaskIt = itk::ImageRegionConstIterator<TMaskImage>(inLeftMaskPtr, fullRegionForThread);
586 inLeftMaskIt.GoToBegin();
588 RegionType rightBufferedRegion = inRightPtr->GetBufferedRegion();
591 RegionType inRightMaskRegion = inRightMaskPtr->GetBufferedRegion();
592 inRightMaskIt = itk::ImageRegionConstIterator<TMaskImage>(inRightMaskPtr, inRightMaskRegion);
595 itk::ConstantBoundaryCondition<TInputImage> nbc1;
596 leftIt.OverrideBoundaryCondition(&nbc1);
599 outHDispIt.GoToBegin();
600 outVDispIt.GoToBegin();
601 outMetricIt.GoToBegin();
613 bool horizontalInterpolation =
false;
614 bool verticalInterpolation =
false;
616 typename ResamplerFilterType::Pointer resampler;
623 double neighborsMetric[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
625 while (!leftIt.IsAtEnd() || !outHDispIt.IsAtEnd() || !outVDispIt.IsAtEnd() || !outMetricIt.IsAtEnd())
628 curLeftPos = leftIt.GetIndex();
629 if (((curLeftPos[0] - this->m_GridIndex[0] + this->m_Step) % this->m_Step == 0) &&
630 ((curLeftPos[1] - this->m_GridIndex[1] + this->m_Step) % this->m_Step == 0))
632 horizontalInterpolation =
false;
633 verticalInterpolation =
false;
636 if (useHorizontalDisparity)
638 hDisp_f =
static_cast<float>(inHDispIt.Get()) * stepDisparity;
639 hDisp_i =
static_cast<int>(std::floor(hDisp_f + 0.5));
640 curRightPos[0] = curLeftPos[0] + hDisp_i;
645 curRightPos[0] = curLeftPos[0];
649 if (useVerticalDisparity)
651 vDisp_f =
static_cast<float>(inVDispIt.Get()) * stepDisparity;
652 vDisp_i =
static_cast<int>(std::floor(vDisp_f + 0.5));
653 curRightPos[1] = curLeftPos[1] + vDisp_i;
658 curRightPos[1] = curLeftPos[1];
662 if (rightBufferedRegion.IsInside(curRightPos))
667 inRightMaskIt.SetIndex(curRightPos);
670 if (!inLeftMaskPtr || (inLeftMaskIt.Get() > 0))
672 if (!inRightMaskPtr || (inRightMaskIt.Get() > 0))
675 smallRightRegion.SetIndex(0, curRightPos[0] - 1);
676 smallRightRegion.SetIndex(1, curRightPos[1] - 1);
677 smallRightRegion.SetSize(0, 3);
678 smallRightRegion.SetSize(1, 3);
680 itk::ConstNeighborhoodIterator<TInputImage> rightIt(m_Radius, inRightPtr, smallRightRegion);
681 itk::ConstantBoundaryCondition<TInputImage> nbc2;
682 rightIt.OverrideBoundaryCondition(&nbc2);
685 rightIt.SetLocation(curRightPos);
686 neighborsMetric[1][1] = m_Functor(leftIt, rightIt);
688 if (m_MinimumVerticalDisparity < vDisp_i && vDisp_i < m_MaximumVerticalDisparity)
694 if (rightBufferedRegion.IsInside(upIndex) && rightBufferedRegion.IsInside(downIndex))
696 rightIt.SetLocation(upIndex);
697 neighborsMetric[1][0] = m_Functor(leftIt, rightIt);
699 rightIt.SetLocation(downIndex);
700 neighborsMetric[1][2] = m_Functor(leftIt, rightIt);
705 if (neighborsMetric[1][1] < neighborsMetric[1][0] && neighborsMetric[1][1] < neighborsMetric[1][2])
707 verticalInterpolation =
true;
712 if (neighborsMetric[1][1] > neighborsMetric[1][0] && neighborsMetric[1][1] > neighborsMetric[1][2])
714 verticalInterpolation =
true;
720 if (m_MinimumHorizontalDisparity < hDisp_i && hDisp_i < m_MaximumHorizontalDisparity)
723 leftIndex[0] += (-1);
726 if (rightBufferedRegion.IsInside(leftIndex) && rightBufferedRegion.IsInside(rightIndex))
728 rightIt.SetLocation(rightIndex);
729 neighborsMetric[2][1] = m_Functor(leftIt, rightIt);
731 rightIt.SetLocation(leftIndex);
732 neighborsMetric[0][1] = m_Functor(leftIt, rightIt);
737 if (neighborsMetric[1][1] < neighborsMetric[0][1] && neighborsMetric[1][1] < neighborsMetric[2][1])
739 horizontalInterpolation =
true;
744 if (neighborsMetric[1][1] > neighborsMetric[0][1] && neighborsMetric[1][1] > neighborsMetric[2][1])
746 horizontalInterpolation =
true;
753 if (verticalInterpolation && horizontalInterpolation)
756 uprightIndex[0] += 1;
757 uprightIndex[1] += (-1);
758 rightIt.SetLocation(uprightIndex);
759 neighborsMetric[2][0] = m_Functor(leftIt, rightIt);
762 downrightIndex[0] += 1;
763 downrightIndex[1] += 1;
764 rightIt.SetLocation(downrightIndex);
765 neighborsMetric[2][2] = m_Functor(leftIt, rightIt);
768 downleftIndex[0] += (-1);
769 downleftIndex[1] += 1;
770 rightIt.SetLocation(downleftIndex);
771 neighborsMetric[0][2] = m_Functor(leftIt, rightIt);
774 upleftIndex[0] += (-1);
775 upleftIndex[1] += (-1);
776 rightIt.SetLocation(upleftIndex);
777 neighborsMetric[0][0] = m_Functor(leftIt, rightIt);
782 if (neighborsMetric[1][1] > neighborsMetric[2][0] || neighborsMetric[1][1] > neighborsMetric[2][2] ||
783 neighborsMetric[1][1] > neighborsMetric[0][2] || neighborsMetric[1][1] > neighborsMetric[0][0])
785 verticalInterpolation =
false;
786 horizontalInterpolation =
false;
791 if (neighborsMetric[1][1] < neighborsMetric[2][0] || neighborsMetric[1][1] < neighborsMetric[2][2] ||
792 neighborsMetric[1][1] < neighborsMetric[0][2] || neighborsMetric[1][1] < neighborsMetric[0][0])
794 verticalInterpolation =
false;
795 horizontalInterpolation =
false;
804 if (verticalInterpolation && !horizontalInterpolation)
807 double deltaV = 0.5 - (1.0 / (1.0 + (neighborsMetric[1][0] - neighborsMetric[1][1]) / (neighborsMetric[1][2] - neighborsMetric[1][1])));
808 if (deltaV > (-0.5) && deltaV < 0.5)
810 outVDispIt.Set((
static_cast<double>(vDisp_i) + deltaV) * stepDisparityInv);
814 verticalInterpolation =
false;
817 else if (!verticalInterpolation && horizontalInterpolation)
820 double deltaH = 0.5 - (1.0 / (1.0 + (neighborsMetric[0][1] - neighborsMetric[1][1]) / (neighborsMetric[2][1] - neighborsMetric[1][1])));
821 if (deltaH > (-0.5) && deltaH < 0.5)
823 outHDispIt.Set((
static_cast<double>(hDisp_i) + deltaH) * stepDisparityInv);
827 horizontalInterpolation =
false;
830 else if (verticalInterpolation && horizontalInterpolation)
833 double dx = 0.5 * (neighborsMetric[2][1] - neighborsMetric[0][1]);
834 double dy = 0.5 * (neighborsMetric[1][2] - neighborsMetric[1][0]);
835 double dxx = neighborsMetric[2][1] + neighborsMetric[0][1] - 2.0 * neighborsMetric[1][1];
836 double dyy = neighborsMetric[1][2] + neighborsMetric[1][0] - 2.0 * neighborsMetric[1][1];
837 double dxy = 0.25 * (neighborsMetric[2][2] + neighborsMetric[0][0] - neighborsMetric[0][2] - neighborsMetric[2][0]);
838 double det = dxx * dyy - dxy * dxy;
839 if (std::abs(det) < (1e-10))
841 verticalInterpolation =
false;
842 horizontalInterpolation =
false;
846 double deltaH = (-dx * dyy + dy * dxy) / det;
847 double deltaV = (dx * dxy - dy * dxx) / det;
848 if (deltaH > (-1.0) && deltaH < 1.0 && deltaV > (-1.0) && deltaV < 1.0)
850 outHDispIt.Set((
static_cast<double>(hDisp_i) + deltaH) * stepDisparityInv);
851 outVDispIt.Set((
static_cast<double>(vDisp_i) + deltaV) * stepDisparityInv);
855 verticalInterpolation =
false;
856 horizontalInterpolation =
false;
861 if (!verticalInterpolation)
864 outVDispIt.Set(
static_cast<double>(vDisp_i) * stepDisparityInv);
867 if (!horizontalInterpolation)
870 outHDispIt.Set(
static_cast<double>(hDisp_i) * stepDisparityInv);
873 if (!verticalInterpolation && !horizontalInterpolation)
876 outMetricIt.Set(
static_cast<double>(neighborsMetric[1][1]));
881 typename TInputImage::Pointer fakeRightPtr = TInputImage::New();
882 fakeRightPtr->SetRegions(rightBufferedRegion);
883 fakeRightPtr->SetPixelContainer((
const_cast<TInputImage*
>(inRightPtr))->GetPixelContainer());
885 itk::ConstNeighborhoodIterator<TInputImage> shiftedIt;
886 itk::ConstantBoundaryCondition<TInputImage> nbc3;
887 shiftedIt.OverrideBoundaryCondition(&nbc3);
890 windowSize[0] = 2 * m_Radius[0] + 1;
891 windowSize[1] = 2 * m_Radius[1] + 1;
894 upleftCorner[0] = curRightPos[0] - m_Radius[0];
895 upleftCorner[1] = curRightPos[1] - m_Radius[1];
897 TransformationType::Pointer transfo = TransformationType::New();
898 TransformationType::OutputVectorType offsetTransfo(2);
901 tinyShiftedRegion.SetSize(0, 1);
902 tinyShiftedRegion.SetSize(1, 1);
903 tinyShiftedRegion.SetIndex(0, curRightPos[0]);
904 tinyShiftedRegion.SetIndex(1, curRightPos[1]);
906 resampler = ResamplerFilterType::New();
907 resampler->SetInput(fakeRightPtr);
908 resampler->SetSize(windowSize);
909 resampler->SetNumberOfThreads(1);
910 resampler->SetTransform(transfo);
911 resampler->SetOutputStartIndex(upleftCorner);
913 offsetTransfo[0] = outHDispIt.Get() * stepDisparity -
static_cast<double>(hDisp_i);
914 offsetTransfo[1] = outVDispIt.Get() * stepDisparity -
static_cast<double>(vDisp_i);
915 transfo->SetOffset(offsetTransfo);
916 resampler->Modified();
918 shiftedIt.Initialize(m_Radius, resampler->GetOutput(), tinyShiftedRegion);
919 outMetricIt.Set(m_Functor(leftIt, shiftedIt));
921 if ((outMetricIt.Get() > neighborsMetric[1][1] && m_Minimize) || (outMetricIt.Get() < neighborsMetric[1][1] && !m_Minimize))
927 progress.CompletedPixel();
931 if (useHorizontalDisparity)
935 if (useVerticalDisparity)
949 m_WrongExtrema[threadId] =
static_cast<double>(nb_WrongExtrema) /
static_cast<double>(outputRegionForThread.GetNumberOfPixels());
952 template <
class TInputImage,
class TOutputMetricImage,
class TDisparityImage,
class TMaskImage,
class TBlockMatchingFunctor>
954 const RegionType& outputRegionForThread, itk::ThreadIdType threadId)
957 const TInputImage* inLeftPtr = this->GetLeftInput();
958 const TInputImage* inRightPtr = this->GetRightInput();
959 const TMaskImage* inLeftMaskPtr = this->GetLeftMaskInput();
960 const TMaskImage* inRightMaskPtr = this->GetRightMaskInput();
961 const TDisparityImage* inHDispPtr = this->GetHorizontalDisparityInput();
962 const TDisparityImage* inVDispPtr = this->GetVerticalDisparityInput();
963 TOutputMetricImage* outMetricPtr = this->GetMetricOutput();
964 TDisparityImage* outHDispPtr = this->GetHorizontalDisparityOutput();
965 TDisparityImage* outVDispPtr = this->GetVerticalDisparityOutput();
967 unsigned int nb_WrongExtrema = 0;
971 itk::ProgressReporter progress(
this, threadId, outputRegionForThread.GetNumberOfPixels(), 100);
973 RegionType fullRegionForThread = BlockMatchingFilterType::ConvertSubsampledToFullRegion(outputRegionForThread, this->m_Step, this->m_GridIndex);
975 itk::ConstNeighborhoodIterator<TInputImage> leftIt(m_Radius, inLeftPtr, fullRegionForThread);
976 itk::ImageRegionIterator<TDisparityImage> outHDispIt(outHDispPtr, outputRegionForThread);
977 itk::ImageRegionIterator<TDisparityImage> outVDispIt(outVDispPtr, outputRegionForThread);
978 itk::ImageRegionIterator<TOutputMetricImage> outMetricIt(outMetricPtr, outputRegionForThread);
979 itk::ImageRegionConstIterator<TDisparityImage> inHDispIt;
980 itk::ImageRegionConstIterator<TDisparityImage> inVDispIt;
981 itk::ImageRegionConstIterator<TMaskImage> inLeftMaskIt;
982 itk::ImageRegionConstIterator<TMaskImage> inRightMaskIt;
984 bool useHorizontalDisparity =
false;
985 bool useVerticalDisparity =
false;
989 useHorizontalDisparity =
true;
990 inHDispIt = itk::ImageRegionConstIterator<TDisparityImage>(inHDispPtr, outputRegionForThread);
991 inHDispIt.GoToBegin();
995 useVerticalDisparity =
true;
996 inVDispIt = itk::ImageRegionConstIterator<TDisparityImage>(inVDispPtr, outputRegionForThread);
997 inVDispIt.GoToBegin();
1002 inLeftMaskIt = itk::ImageRegionConstIterator<TMaskImage>(inLeftMaskPtr, fullRegionForThread);
1003 inLeftMaskIt.GoToBegin();
1005 RegionType rightBufferedRegion = inRightPtr->GetBufferedRegion();
1008 RegionType inRightMaskRegion = inRightMaskPtr->GetBufferedRegion();
1009 inRightMaskIt = itk::ImageRegionConstIterator<TMaskImage>(inRightMaskPtr, inRightMaskRegion);
1012 itk::ConstantBoundaryCondition<TInputImage> nbc1;
1013 leftIt.OverrideBoundaryCondition(&nbc1);
1016 outHDispIt.GoToBegin();
1017 outVDispIt.GoToBegin();
1018 outMetricIt.GoToBegin();
1030 bool horizontalInterpolation =
false;
1031 bool verticalInterpolation =
false;
1033 typename ResamplerFilterType::Pointer resampler;
1041 double neighborsMetric[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
1043 while (!leftIt.IsAtEnd() || !outHDispIt.IsAtEnd() || !outVDispIt.IsAtEnd() || !outMetricIt.IsAtEnd())
1046 curLeftPos = leftIt.GetIndex();
1047 if (((curLeftPos[0] - this->m_GridIndex[0] + this->m_Step) % this->m_Step == 0) &&
1048 ((curLeftPos[1] - this->m_GridIndex[1] + this->m_Step) % this->m_Step == 0))
1050 horizontalInterpolation =
false;
1051 verticalInterpolation =
false;
1054 if (useHorizontalDisparity)
1056 hDisp_f =
static_cast<float>(inHDispIt.Get()) * stepDisparity;
1057 hDisp_i =
static_cast<int>(std::floor(hDisp_f + 0.5));
1058 curRightPos[0] = curLeftPos[0] + hDisp_i;
1063 curRightPos[0] = curLeftPos[0];
1067 if (useVerticalDisparity)
1069 vDisp_f =
static_cast<float>(inVDispIt.Get()) * stepDisparity;
1070 vDisp_i =
static_cast<int>(std::floor(vDisp_f + 0.5));
1071 curRightPos[1] = curLeftPos[1] + vDisp_i;
1076 curRightPos[1] = curLeftPos[1];
1080 if (rightBufferedRegion.IsInside(curRightPos))
1085 inRightMaskIt.SetIndex(curRightPos);
1088 if (!inLeftMaskPtr || (inLeftMaskIt.Get() > 0))
1090 if (!inRightMaskPtr || (inRightMaskIt.Get() > 0))
1093 smallRightRegion.SetIndex(0, curRightPos[0] - 1);
1094 smallRightRegion.SetIndex(1, curRightPos[1] - 1);
1095 smallRightRegion.SetSize(0, 3);
1096 smallRightRegion.SetSize(1, 3);
1098 itk::ConstNeighborhoodIterator<TInputImage> rightIt(m_Radius, inRightPtr, smallRightRegion);
1099 itk::ConstantBoundaryCondition<TInputImage> nbc2;
1100 rightIt.OverrideBoundaryCondition(&nbc2);
1103 rightIt.SetLocation(curRightPos);
1104 neighborsMetric[1][1] = m_Functor(leftIt, rightIt);
1106 if (m_MinimumVerticalDisparity < vDisp_i && vDisp_i < m_MaximumVerticalDisparity)
1112 if (rightBufferedRegion.IsInside(upIndex) && rightBufferedRegion.IsInside(downIndex))
1114 rightIt.SetLocation(upIndex);
1115 neighborsMetric[1][0] = m_Functor(leftIt, rightIt);
1117 rightIt.SetLocation(downIndex);
1118 neighborsMetric[1][2] = m_Functor(leftIt, rightIt);
1123 if (neighborsMetric[1][1] < neighborsMetric[1][0] && neighborsMetric[1][1] < neighborsMetric[1][2])
1125 verticalInterpolation =
true;
1130 if (neighborsMetric[1][1] > neighborsMetric[1][0] && neighborsMetric[1][1] > neighborsMetric[1][2])
1132 verticalInterpolation =
true;
1138 if (m_MinimumHorizontalDisparity < hDisp_i && hDisp_i < m_MaximumHorizontalDisparity)
1141 leftIndex[0] += (-1);
1144 if (rightBufferedRegion.IsInside(leftIndex) && rightBufferedRegion.IsInside(rightIndex))
1146 rightIt.SetLocation(rightIndex);
1147 neighborsMetric[2][1] = m_Functor(leftIt, rightIt);
1149 rightIt.SetLocation(leftIndex);
1150 neighborsMetric[0][1] = m_Functor(leftIt, rightIt);
1155 if (neighborsMetric[1][1] < neighborsMetric[0][1] && neighborsMetric[1][1] < neighborsMetric[2][1])
1157 horizontalInterpolation =
true;
1162 if (neighborsMetric[1][1] > neighborsMetric[0][1] && neighborsMetric[1][1] > neighborsMetric[2][1])
1164 horizontalInterpolation =
true;
1171 if (verticalInterpolation && horizontalInterpolation)
1174 uprightIndex[0] += 1;
1175 uprightIndex[1] += (-1);
1176 rightIt.SetLocation(uprightIndex);
1177 neighborsMetric[2][0] = m_Functor(leftIt, rightIt);
1180 downrightIndex[0] += 1;
1181 downrightIndex[1] += 1;
1182 rightIt.SetLocation(downrightIndex);
1183 neighborsMetric[2][2] = m_Functor(leftIt, rightIt);
1186 downleftIndex[0] += (-1);
1187 downleftIndex[1] += 1;
1188 rightIt.SetLocation(downleftIndex);
1189 neighborsMetric[0][2] = m_Functor(leftIt, rightIt);
1192 upleftIndex[0] += (-1);
1193 upleftIndex[1] += (-1);
1194 rightIt.SetLocation(upleftIndex);
1195 neighborsMetric[0][0] = m_Functor(leftIt, rightIt);
1200 if (neighborsMetric[1][1] > neighborsMetric[2][0] || neighborsMetric[1][1] > neighborsMetric[2][2] ||
1201 neighborsMetric[1][1] > neighborsMetric[0][2] || neighborsMetric[1][1] > neighborsMetric[0][0])
1203 verticalInterpolation =
false;
1204 horizontalInterpolation =
false;
1209 if (neighborsMetric[1][1] < neighborsMetric[2][0] || neighborsMetric[1][1] < neighborsMetric[2][2] ||
1210 neighborsMetric[1][1] < neighborsMetric[0][2] || neighborsMetric[1][1] < neighborsMetric[0][0])
1212 verticalInterpolation =
false;
1213 horizontalInterpolation =
false;
1222 if (verticalInterpolation && !horizontalInterpolation)
1226 if ((neighborsMetric[1][0] < neighborsMetric[1][2] && m_Minimize) || (neighborsMetric[1][0] > neighborsMetric[1][2] && !m_Minimize))
1228 deltaV = 0.5 * ((neighborsMetric[1][0] - neighborsMetric[1][2]) / (neighborsMetric[1][2] - neighborsMetric[1][1]));
1232 deltaV = 0.5 * ((neighborsMetric[1][0] - neighborsMetric[1][2]) / (neighborsMetric[1][0] - neighborsMetric[1][1]));
1234 if (deltaV > (-0.5) && deltaV < 0.5)
1236 outVDispIt.Set((
static_cast<double>(vDisp_i) + deltaV) * stepDisparityInv);
1240 verticalInterpolation =
false;
1243 else if (!verticalInterpolation && horizontalInterpolation)
1247 if ((neighborsMetric[0][1] < neighborsMetric[2][1] && m_Minimize) || (neighborsMetric[0][1] > neighborsMetric[2][1] && !m_Minimize))
1249 deltaH = 0.5 * ((neighborsMetric[0][1] - neighborsMetric[2][1]) / (neighborsMetric[2][1] - neighborsMetric[1][1]));
1253 deltaH = 0.5 * ((neighborsMetric[0][1] - neighborsMetric[2][1]) / (neighborsMetric[0][1] - neighborsMetric[1][1]));
1255 if (deltaH > (-0.5) && deltaH < 0.5)
1257 outHDispIt.Set((
static_cast<double>(hDisp_i) + deltaH) * stepDisparityInv);
1261 horizontalInterpolation =
false;
1264 else if (verticalInterpolation && horizontalInterpolation)
1269 if ((neighborsMetric[1][0] < neighborsMetric[1][2] && m_Minimize) || (neighborsMetric[1][0] > neighborsMetric[1][2] && !m_Minimize))
1271 deltaV = 0.5 * ((neighborsMetric[1][0] - neighborsMetric[1][2]) / (neighborsMetric[1][2] - neighborsMetric[1][1]));
1275 deltaV = 0.5 * ((neighborsMetric[1][0] - neighborsMetric[1][2]) / (neighborsMetric[1][0] - neighborsMetric[1][1]));
1278 if ((neighborsMetric[0][1] < neighborsMetric[2][1] && m_Minimize) || (neighborsMetric[0][1] > neighborsMetric[2][1] && !m_Minimize))
1280 deltaH = 0.5 * ((neighborsMetric[0][1] - neighborsMetric[2][1]) / (neighborsMetric[2][1] - neighborsMetric[1][1]));
1284 deltaH = 0.5 * ((neighborsMetric[0][1] - neighborsMetric[2][1]) / (neighborsMetric[0][1] - neighborsMetric[1][1]));
1287 if (deltaV > (-1.0) && deltaV < 1.0 && deltaH > (-1.0) && deltaH < 1.0)
1289 outVDispIt.Set((
static_cast<double>(vDisp_i) + deltaV) * stepDisparityInv);
1290 outHDispIt.Set((
static_cast<double>(hDisp_i) + deltaH) * stepDisparityInv);
1294 verticalInterpolation =
false;
1295 horizontalInterpolation =
false;
1299 if (!verticalInterpolation)
1302 outVDispIt.Set(
static_cast<double>(vDisp_i) * stepDisparityInv);
1305 if (!horizontalInterpolation)
1308 outHDispIt.Set(
static_cast<double>(hDisp_i) * stepDisparityInv);
1311 if (!verticalInterpolation && !horizontalInterpolation)
1314 outMetricIt.Set(
static_cast<double>(neighborsMetric[1][1]));
1319 typename TInputImage::Pointer fakeRightPtr = TInputImage::New();
1320 fakeRightPtr->SetRegions(rightBufferedRegion);
1321 fakeRightPtr->SetPixelContainer((
const_cast<TInputImage*
>(inRightPtr))->GetPixelContainer());
1323 itk::ConstNeighborhoodIterator<TInputImage> shiftedIt;
1324 itk::ConstantBoundaryCondition<TInputImage> nbc3;
1325 shiftedIt.OverrideBoundaryCondition(&nbc3);
1328 windowSize[0] = 2 * m_Radius[0] + 1;
1329 windowSize[1] = 2 * m_Radius[1] + 1;
1332 upleftCorner[0] = curRightPos[0] - m_Radius[0];
1333 upleftCorner[1] = curRightPos[1] - m_Radius[1];
1335 TransformationType::Pointer transfo = TransformationType::New();
1336 TransformationType::OutputVectorType offsetTransfo(2);
1339 tinyShiftedRegion.SetSize(0, 1);
1340 tinyShiftedRegion.SetSize(1, 1);
1341 tinyShiftedRegion.SetIndex(0, curRightPos[0]);
1342 tinyShiftedRegion.SetIndex(1, curRightPos[1]);
1344 resampler = ResamplerFilterType::New();
1345 resampler->SetInput(fakeRightPtr);
1346 resampler->SetSize(windowSize);
1347 resampler->SetNumberOfThreads(1);
1348 resampler->SetTransform(transfo);
1349 resampler->SetOutputStartIndex(upleftCorner);
1351 offsetTransfo[0] = outHDispIt.Get() * stepDisparity -
static_cast<double>(hDisp_i);
1352 offsetTransfo[1] = outVDispIt.Get() * stepDisparity -
static_cast<double>(vDisp_i);
1353 transfo->SetOffset(offsetTransfo);
1354 resampler->Modified();
1355 resampler->Update();
1356 shiftedIt.Initialize(m_Radius, resampler->GetOutput(), tinyShiftedRegion);
1357 outMetricIt.Set(m_Functor(leftIt, shiftedIt));
1359 if ((outMetricIt.Get() > neighborsMetric[1][1] && m_Minimize) || (outMetricIt.Get() < neighborsMetric[1][1] && !m_Minimize))
1365 progress.CompletedPixel();
1371 if (useHorizontalDisparity)
1375 if (useVerticalDisparity)
1388 m_WrongExtrema[threadId] =
static_cast<double>(nb_WrongExtrema) /
static_cast<double>(outputRegionForThread.GetNumberOfPixels());
1391 template <
class TInputImage,
class TOutputMetricImage,
class TDisparityImage,
class TMaskImage,
class TBlockMatchingFunctor>
1393 const RegionType& outputRegionForThread, itk::ThreadIdType threadId)
1396 const TInputImage* inLeftPtr = this->GetLeftInput();
1397 const TInputImage* inRightPtr = this->GetRightInput();
1398 const TMaskImage* inLeftMaskPtr = this->GetLeftMaskInput();
1399 const TMaskImage* inRightMaskPtr = this->GetRightMaskInput();
1400 const TDisparityImage* inHDispPtr = this->GetHorizontalDisparityInput();
1401 const TDisparityImage* inVDispPtr = this->GetVerticalDisparityInput();
1402 TOutputMetricImage* outMetricPtr = this->GetMetricOutput();
1403 TDisparityImage* outHDispPtr = this->GetHorizontalDisparityOutput();
1404 TDisparityImage* outVDispPtr = this->GetVerticalDisparityOutput();
1406 unsigned int nb_WrongExtrema = 0;
1410 itk::ProgressReporter progress(
this, threadId, outputRegionForThread.GetNumberOfPixels(), 100);
1412 RegionType fullRegionForThread = BlockMatchingFilterType::ConvertSubsampledToFullRegion(outputRegionForThread, this->m_Step, this->m_GridIndex);
1414 itk::ConstNeighborhoodIterator<TInputImage> leftIt(m_Radius, inLeftPtr, fullRegionForThread);
1415 itk::ImageRegionIterator<TDisparityImage> outHDispIt(outHDispPtr, outputRegionForThread);
1416 itk::ImageRegionIterator<TDisparityImage> outVDispIt(outVDispPtr, outputRegionForThread);
1417 itk::ImageRegionIterator<TOutputMetricImage> outMetricIt(outMetricPtr, outputRegionForThread);
1418 itk::ImageRegionConstIterator<TDisparityImage> inHDispIt;
1419 itk::ImageRegionConstIterator<TDisparityImage> inVDispIt;
1420 itk::ImageRegionConstIterator<TMaskImage> inLeftMaskIt;
1421 itk::ImageRegionConstIterator<TMaskImage> inRightMaskIt;
1423 bool useHorizontalDisparity =
false;
1424 bool useVerticalDisparity =
false;
1428 useHorizontalDisparity =
true;
1429 inHDispIt = itk::ImageRegionConstIterator<TDisparityImage>(inHDispPtr, outputRegionForThread);
1430 inHDispIt.GoToBegin();
1434 useVerticalDisparity =
true;
1435 inVDispIt = itk::ImageRegionConstIterator<TDisparityImage>(inVDispPtr, outputRegionForThread);
1436 inVDispIt.GoToBegin();
1441 inLeftMaskIt = itk::ImageRegionConstIterator<TMaskImage>(inLeftMaskPtr, fullRegionForThread);
1442 inLeftMaskIt.GoToBegin();
1444 RegionType rightBufferedRegion = inRightPtr->GetBufferedRegion();
1447 RegionType inRightMaskRegion = inRightMaskPtr->GetBufferedRegion();
1448 inRightMaskIt = itk::ImageRegionConstIterator<TMaskImage>(inRightMaskPtr, inRightMaskRegion);
1451 itk::ConstantBoundaryCondition<TInputImage> nbc1;
1452 leftIt.OverrideBoundaryCondition(&nbc1);
1455 outHDispIt.GoToBegin();
1456 outVDispIt.GoToBegin();
1457 outMetricIt.GoToBegin();
1468 typename TInputImage::Pointer fakeRightPtr = TInputImage::New();
1469 fakeRightPtr->SetRegions(rightBufferedRegion);
1470 fakeRightPtr->SetPixelContainer((
const_cast<TInputImage*
>(inRightPtr))->GetPixelContainer());
1473 bool horizontalInterpolation =
false;
1474 bool verticalInterpolation =
false;
1477 double neighborsMetric[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
1478 unsigned int nbIterMax = 10;
1484 windowSize[0] = 2 * m_Radius[0] + 1;
1485 windowSize[1] = 2 * m_Radius[1] + 1;
1487 TransformationType::Pointer transfo = TransformationType::New();
1488 TransformationType::OutputVectorType offsetTransfo(2);
1489 offsetTransfo[0] = 0.0;
1490 offsetTransfo[1] = 0.0;
1492 typename ResamplerFilterType::Pointer resampler;
1499 tinyShiftedRegion.SetIndex(0, m_Radius[0]);
1500 tinyShiftedRegion.SetIndex(1, m_Radius[1]);
1501 tinyShiftedRegion.SetSize(0, 1);
1502 tinyShiftedRegion.SetSize(1, 1);
1505 itk::ConstNeighborhoodIterator<TInputImage> rightIt;
1506 itk::ConstNeighborhoodIterator<TInputImage> shiftedIt;
1507 itk::ConstantBoundaryCondition<TInputImage> nbc2;
1508 itk::ConstantBoundaryCondition<TInputImage> nbc3;
1509 rightIt.OverrideBoundaryCondition(&nbc2);
1510 shiftedIt.OverrideBoundaryCondition(&nbc3);
1511 bool centreHasMoved;
1513 while (!leftIt.IsAtEnd() || !outHDispIt.IsAtEnd() || !outVDispIt.IsAtEnd() || !outMetricIt.IsAtEnd())
1516 curLeftPos = leftIt.GetIndex();
1517 if (((curLeftPos[0] - this->m_GridIndex[0] + this->m_Step) % this->m_Step == 0) &&
1518 ((curLeftPos[1] - this->m_GridIndex[1] + this->m_Step) % this->m_Step == 0))
1520 horizontalInterpolation =
false;
1521 verticalInterpolation =
false;
1524 if (useHorizontalDisparity)
1526 hDisp_f =
static_cast<float>(inHDispIt.Get()) * stepDisparity;
1527 hDisp_i =
static_cast<int>(std::floor(hDisp_f + 0.5));
1528 curRightPos[0] = curLeftPos[0] + hDisp_i;
1533 curRightPos[0] = curLeftPos[0];
1537 if (useVerticalDisparity)
1539 vDisp_f =
static_cast<float>(inVDispIt.Get()) * stepDisparity;
1540 vDisp_i =
static_cast<int>(std::floor(vDisp_f + 0.5));
1541 curRightPos[1] = curLeftPos[1] + vDisp_i;
1546 curRightPos[1] = curLeftPos[1];
1550 upleftCorner[0] = curRightPos[0] - m_Radius[0];
1551 upleftCorner[1] = curRightPos[1] - m_Radius[1];
1554 resampler = ResamplerFilterType::New();
1555 resampler->SetInput(fakeRightPtr);
1556 resampler->SetSize(windowSize);
1557 resampler->SetNumberOfThreads(1);
1558 resampler->SetTransform(transfo);
1559 resampler->SetOutputStartIndex(upleftCorner);
1561 tinyShiftedRegion.SetIndex(0, curRightPos[0]);
1562 tinyShiftedRegion.SetIndex(1, curRightPos[1]);
1565 if (rightBufferedRegion.IsInside(curRightPos))
1570 inRightMaskIt.SetIndex(curRightPos);
1573 if (!inLeftMaskPtr || (inLeftMaskIt.Get() > 0))
1575 if (!inRightMaskPtr || (inRightMaskIt.Get() > 0))
1578 smallRightRegion.SetIndex(0, curRightPos[0] - 1);
1579 smallRightRegion.SetIndex(1, curRightPos[1] - 1);
1580 smallRightRegion.SetSize(0, 3);
1581 smallRightRegion.SetSize(1, 3);
1583 rightIt.Initialize(m_Radius, inRightPtr, smallRightRegion);
1586 rightIt.SetLocation(curRightPos);
1587 neighborsMetric[1][1] = m_Functor(leftIt, rightIt);
1589 if (m_MinimumVerticalDisparity < vDisp_i && vDisp_i < m_MaximumVerticalDisparity)
1595 if (rightBufferedRegion.IsInside(upIndex) && rightBufferedRegion.IsInside(downIndex))
1597 rightIt.SetLocation(upIndex);
1598 neighborsMetric[1][0] = m_Functor(leftIt, rightIt);
1600 rightIt.SetLocation(downIndex);
1601 neighborsMetric[1][2] = m_Functor(leftIt, rightIt);
1606 if (neighborsMetric[1][1] < neighborsMetric[1][0] && neighborsMetric[1][1] < neighborsMetric[1][2])
1608 verticalInterpolation =
true;
1613 if (neighborsMetric[1][1] > neighborsMetric[1][0] && neighborsMetric[1][1] > neighborsMetric[1][2])
1615 verticalInterpolation =
true;
1621 if (m_MinimumHorizontalDisparity < hDisp_i && hDisp_i < m_MaximumHorizontalDisparity)
1624 leftIndex[0] += (-1);
1627 if (rightBufferedRegion.IsInside(leftIndex) && rightBufferedRegion.IsInside(rightIndex))
1629 rightIt.SetLocation(rightIndex);
1630 neighborsMetric[2][1] = m_Functor(leftIt, rightIt);
1632 rightIt.SetLocation(leftIndex);
1633 neighborsMetric[0][1] = m_Functor(leftIt, rightIt);
1638 if (neighborsMetric[1][1] < neighborsMetric[0][1] && neighborsMetric[1][1] < neighborsMetric[2][1])
1640 horizontalInterpolation =
true;
1645 if (neighborsMetric[1][1] > neighborsMetric[0][1] && neighborsMetric[1][1] > neighborsMetric[2][1])
1647 horizontalInterpolation =
true;
1654 if (verticalInterpolation && horizontalInterpolation)
1657 uprightIndex[0] += 1;
1658 uprightIndex[1] += (-1);
1659 rightIt.SetLocation(uprightIndex);
1660 neighborsMetric[2][0] = m_Functor(leftIt, rightIt);
1663 downrightIndex[0] += 1;
1664 downrightIndex[1] += 1;
1665 rightIt.SetLocation(downrightIndex);
1666 neighborsMetric[2][2] = m_Functor(leftIt, rightIt);
1669 downleftIndex[0] += (-1);
1670 downleftIndex[1] += 1;
1671 rightIt.SetLocation(downleftIndex);
1672 neighborsMetric[0][2] = m_Functor(leftIt, rightIt);
1675 upleftIndex[0] += (-1);
1676 upleftIndex[1] += (-1);
1677 rightIt.SetLocation(upleftIndex);
1678 neighborsMetric[0][0] = m_Functor(leftIt, rightIt);
1683 if (neighborsMetric[1][1] > neighborsMetric[2][0] || neighborsMetric[1][1] > neighborsMetric[2][2] ||
1684 neighborsMetric[1][1] > neighborsMetric[0][2] || neighborsMetric[1][1] > neighborsMetric[0][0])
1686 verticalInterpolation =
false;
1687 horizontalInterpolation =
false;
1692 if (neighborsMetric[1][1] < neighborsMetric[2][0] || neighborsMetric[1][1] < neighborsMetric[2][2] ||
1693 neighborsMetric[1][1] < neighborsMetric[0][2] || neighborsMetric[1][1] < neighborsMetric[0][0])
1695 verticalInterpolation =
false;
1696 horizontalInterpolation =
false;
1705 if (verticalInterpolation && !horizontalInterpolation)
1707 double ya =
static_cast<double>(vDisp_i - 1);
1708 double yb =
static_cast<double>(vDisp_i);
1709 double yc =
static_cast<double>(vDisp_i + 1);
1710 double s_yb = neighborsMetric[1][1];
1715 offsetTransfo[0] = 0.0;
1717 for (
unsigned int k = 0; k < nbIterMax; k++)
1719 if ((yb - ya) < (yc - yb))
1721 yd = 0.5 * (yc + yb);
1722 offsetTransfo[1] = yd -
static_cast<double>(vDisp_i);
1723 transfo->SetOffset(offsetTransfo);
1724 resampler->Modified();
1725 resampler->Update();
1726 shiftedIt.Initialize(m_Radius, resampler->GetOutput(), tinyShiftedRegion);
1727 s_yd = m_Functor(leftIt, shiftedIt);
1729 if ((s_yd < s_yb && m_Minimize) || (s_yd > s_yb && !m_Minimize))
1743 yd = 0.5 * (ya + yb);
1744 offsetTransfo[1] = yd -
static_cast<double>(vDisp_i);
1745 transfo->SetOffset(offsetTransfo);
1746 resampler->Modified();
1747 resampler->Update();
1748 shiftedIt.Initialize(m_Radius, resampler->GetOutput(), tinyShiftedRegion);
1749 s_yd = m_Functor(leftIt, shiftedIt);
1751 if ((s_yd < s_yb && m_Minimize) || (s_yd > s_yb && !m_Minimize))
1765 outVDispIt.Set(yb * stepDisparityInv);
1766 outMetricIt.Set(s_yb);
1768 else if (!verticalInterpolation && horizontalInterpolation)
1770 double xa =
static_cast<double>(hDisp_i - 1);
1771 double xb =
static_cast<double>(hDisp_i);
1772 double xc =
static_cast<double>(hDisp_i + 1);
1773 double s_xb = neighborsMetric[1][1];
1778 offsetTransfo[1] = 0.0;
1780 for (
unsigned int k = 0; k < nbIterMax; k++)
1782 if ((xb - xa) < (xc - xb))
1784 xd = 0.5 * (xc + xb);
1785 offsetTransfo[0] = xd -
static_cast<double>(hDisp_i);
1786 transfo->SetOffset(offsetTransfo);
1787 resampler->Modified();
1788 resampler->Update();
1789 shiftedIt.Initialize(m_Radius, resampler->GetOutput(), tinyShiftedRegion);
1790 s_xd = m_Functor(leftIt, shiftedIt);
1792 if ((s_xd < s_xb && m_Minimize) || (s_xd > s_xb && !m_Minimize))
1806 xd = 0.5 * (xa + xb);
1807 offsetTransfo[0] = xd -
static_cast<double>(hDisp_i);
1808 transfo->SetOffset(offsetTransfo);
1809 resampler->Modified();
1810 resampler->Update();
1811 shiftedIt.Initialize(m_Radius, resampler->GetOutput(), tinyShiftedRegion);
1812 s_xd = m_Functor(leftIt, shiftedIt);
1814 if ((s_xd < s_xb && m_Minimize) || (s_xd > s_xb && !m_Minimize))
1828 outHDispIt.Set(xb * stepDisparityInv);
1829 outMetricIt.Set(s_xb);
1831 else if (verticalInterpolation && horizontalInterpolation)
1833 double xa =
static_cast<double>(hDisp_i);
1834 double ya =
static_cast<double>(vDisp_i - 1);
1835 double xb =
static_cast<double>(hDisp_i);
1836 double yb =
static_cast<double>(vDisp_i);
1837 double yc =
static_cast<double>(vDisp_i + 1);
1838 double xe =
static_cast<double>(hDisp_i - 1);
1839 double ye =
static_cast<double>(vDisp_i);
1840 double xf =
static_cast<double>(hDisp_i + 1);
1842 double s_b = neighborsMetric[1][1];
1848 for (
unsigned int k = 0; k < nbIterMax; k++)
1851 centreHasMoved =
false;
1852 if ((yb - ya) < (yc - yb))
1854 yd = 0.5 * (yc + yb);
1855 offsetTransfo[0] = xd -
static_cast<double>(hDisp_i);
1856 offsetTransfo[1] = yd -
static_cast<double>(vDisp_i);
1857 transfo->SetOffset(offsetTransfo);
1858 resampler->Modified();
1859 resampler->Update();
1860 shiftedIt.Initialize(m_Radius, resampler->GetOutput(), tinyShiftedRegion);
1861 s_d = m_Functor(leftIt, shiftedIt);
1863 if ((s_d < s_b && m_Minimize) || (s_d > s_b && !m_Minimize))
1865 centreHasMoved =
true;
1879 yd = 0.5 * (ya + yb);
1880 offsetTransfo[0] = xd -
static_cast<double>(hDisp_i);
1881 offsetTransfo[1] = yd -
static_cast<double>(vDisp_i);
1882 transfo->SetOffset(offsetTransfo);
1883 resampler->Modified();
1884 resampler->Update();
1885 shiftedIt.Initialize(m_Radius, resampler->GetOutput(), tinyShiftedRegion);
1886 s_d = m_Functor(leftIt, shiftedIt);
1888 if ((s_d < s_b && m_Minimize) || (s_d > s_b && !m_Minimize))
1890 centreHasMoved =
true;
1907 offsetTransfo[0] = xe -
static_cast<double>(hDisp_i);
1908 offsetTransfo[1] = ye -
static_cast<double>(vDisp_i);
1909 transfo->SetOffset(offsetTransfo);
1910 resampler->Modified();
1911 resampler->Update();
1912 shiftedIt.Initialize(m_Radius, resampler->GetOutput(), tinyShiftedRegion);
1915 offsetTransfo[0] = xf -
static_cast<double>(hDisp_i);
1916 transfo->SetOffset(offsetTransfo);
1917 resampler->Modified();
1918 resampler->Update();
1919 shiftedIt.Initialize(m_Radius, resampler->GetOutput(), tinyShiftedRegion);
1923 centreHasMoved =
false;
1924 if ((xb - xe) < (xf - xb))
1926 xd = 0.5 * (xf + xb);
1927 offsetTransfo[0] = xd -
static_cast<double>(hDisp_i);
1928 offsetTransfo[1] = yd -
static_cast<double>(vDisp_i);
1929 transfo->SetOffset(offsetTransfo);
1930 resampler->Modified();
1931 resampler->Update();
1932 shiftedIt.Initialize(m_Radius, resampler->GetOutput(), tinyShiftedRegion);
1933 s_d = m_Functor(leftIt, shiftedIt);
1935 if ((s_d < s_b && m_Minimize) || (s_d > s_b && !m_Minimize))
1937 centreHasMoved =
true;
1951 xd = 0.5 * (xe + xb);
1952 offsetTransfo[0] = xd -
static_cast<double>(hDisp_i);
1953 offsetTransfo[1] = yd -
static_cast<double>(vDisp_i);
1954 transfo->SetOffset(offsetTransfo);
1955 resampler->Modified();
1956 resampler->Update();
1957 shiftedIt.Initialize(m_Radius, resampler->GetOutput(), tinyShiftedRegion);
1958 s_d = m_Functor(leftIt, shiftedIt);
1960 if ((s_d < s_b && m_Minimize) || (s_d > s_b && !m_Minimize))
1962 centreHasMoved =
true;
1979 offsetTransfo[0] = xa -
static_cast<double>(hDisp_i);
1980 offsetTransfo[1] = ya -
static_cast<double>(vDisp_i);
1981 transfo->SetOffset(offsetTransfo);
1982 resampler->Modified();
1983 resampler->Update();
1984 shiftedIt.Initialize(m_Radius, resampler->GetOutput(), tinyShiftedRegion);
1986 offsetTransfo[1] = yc -
static_cast<double>(vDisp_i);
1987 transfo->SetOffset(offsetTransfo);
1988 resampler->Modified();
1989 resampler->Update();
1990 shiftedIt.Initialize(m_Radius, resampler->GetOutput(), tinyShiftedRegion);
1994 outHDispIt.Set(xb * stepDisparityInv);
1995 outVDispIt.Set(yb * stepDisparityInv);
1996 outMetricIt.Set(s_b);
1999 if (!verticalInterpolation)
2002 outVDispIt.Set(
static_cast<double>(vDisp_i) * stepDisparityInv);
2005 if (!horizontalInterpolation)
2008 outHDispIt.Set(
static_cast<double>(hDisp_i) * stepDisparityInv);
2011 if (!verticalInterpolation && !horizontalInterpolation)
2013 outMetricIt.Set(
static_cast<double>(neighborsMetric[1][1]));
2017 if ((outMetricIt.Get() > neighborsMetric[1][1] && m_Minimize) || (outMetricIt.Get() < neighborsMetric[1][1] && !m_Minimize))
2023 progress.CompletedPixel();
2029 if (useHorizontalDisparity)
2033 if (useVerticalDisparity)
2046 m_WrongExtrema[threadId] =
static_cast<double>(nb_WrongExtrema) /
static_cast<double>(outputRegionForThread.GetNumberOfPixels());
2049 template <
class TInputImage,
class TOutputMetricImage,
class TDisparityImage,
class TMaskImage,
class TBlockMatchingFunctor>
2052 double wrongExtremaPercent = 0;
2053 for (
unsigned int i = 0; i < m_WrongExtrema.size(); i++)
2055 wrongExtremaPercent += m_WrongExtrema[i];
2057 wrongExtremaPercent /= m_WrongExtrema.size();