21 #ifndef otbDisparityTranslateFilter_hxx
22 #define otbDisparityTranslateFilter_hxx
26 #include "itkImageRegionIteratorWithIndex.h"
27 #include "itkImageRegionIterator.h"
33 template <
class TDisparityImage,
class TGr
idImage,
class TSensorImage,
class TMaskImage>
35 : m_NoDataValue(-32768)
38 this->SetNumberOfRequiredInputs(6);
39 this->SetNumberOfRequiredInputs(1);
42 this->SetNumberOfRequiredOutputs(2);
43 this->SetNthOutput(0, TDisparityImage::New());
44 this->SetNthOutput(1, TDisparityImage::New());
47 template <
class TDisparityImage,
class TGr
idImage,
class TSensorImage,
class TMaskImage>
51 this->SetNthInput(0,
const_cast<TDisparityImage*
>(hmap));
54 template <
class TDisparityImage,
class TGr
idImage,
class TSensorImage,
class TMaskImage>
58 this->SetNthInput(1,
const_cast<TDisparityImage*
>(vmap));
61 template <
class TDisparityImage,
class TGr
idImage,
class TSensorImage,
class TMaskImage>
65 this->SetNthInput(2,
const_cast<TGridImage*
>(lgrid));
68 template <
class TDisparityImage,
class TGr
idImage,
class TSensorImage,
class TMaskImage>
72 this->SetNthInput(3,
const_cast<TGridImage*
>(rgrid));
75 template <
class TDisparityImage,
class TGr
idImage,
class TSensorImage,
class TMaskImage>
79 this->SetNthInput(4,
const_cast<TMaskImage*
>(mask));
82 template <
class TDisparityImage,
class TGr
idImage,
class TSensorImage,
class TMaskImage>
86 this->SetNthInput(5,
const_cast<TSensorImage*
>(left));
89 template <
class TDisparityImage,
class TGr
idImage,
class TSensorImage,
class TMaskImage>
92 if (this->GetNumberOfInputs() < 1)
96 return static_cast<const TDisparityImage*
>(this->itk::ProcessObject::GetInput(0));
99 template <
class TDisparityImage,
class TGr
idImage,
class TSensorImage,
class TMaskImage>
102 if (this->GetNumberOfInputs() < 2)
106 return static_cast<const TDisparityImage*
>(this->itk::ProcessObject::GetInput(1));
109 template <
class TDisparityImage,
class TGr
idImage,
class TSensorImage,
class TMaskImage>
112 if (this->GetNumberOfInputs() < 3)
116 return static_cast<const TGridImage*
>(this->itk::ProcessObject::GetInput(2));
119 template <
class TDisparityImage,
class TGr
idImage,
class TSensorImage,
class TMaskImage>
122 if (this->GetNumberOfInputs() < 4)
126 return static_cast<const TGridImage*
>(this->itk::ProcessObject::GetInput(3));
129 template <
class TDisparityImage,
class TGr
idImage,
class TSensorImage,
class TMaskImage>
132 if (this->GetNumberOfInputs() < 5)
136 return static_cast<const TMaskImage*
>(this->itk::ProcessObject::GetInput(4));
139 template <
class TDisparityImage,
class TGr
idImage,
class TSensorImage,
class TMaskImage>
142 if (this->GetNumberOfInputs() < 6)
146 return static_cast<const TSensorImage*
>(this->itk::ProcessObject::GetInput(5));
149 template <
class TDisparityImage,
class TGr
idImage,
class TSensorImage,
class TMaskImage>
152 if (this->GetNumberOfOutputs() < 1)
156 return static_cast<TDisparityImage*
>(this->itk::ProcessObject::GetOutput(0));
159 template <
class TDisparityImage,
class TGr
idImage,
class TSensorImage,
class TMaskImage>
162 if (this->GetNumberOfOutputs() < 2)
166 return static_cast<TDisparityImage*
>(this->itk::ProcessObject::GetOutput(1));
169 template <
class TDisparityImage,
class TGr
idImage,
class TSensorImage,
class TMaskImage>
174 TDisparityImage* horizOut = this->GetHorizontalDisparityMapOutput();
175 TDisparityImage* vertiOut = this->GetVerticalDisparityMapOutput();
177 const TSensorImage* leftIn = this->GetLeftSensorImageInput();
179 horizOut->CopyInformation(leftIn);
180 vertiOut->CopyInformation(leftIn);
183 std::vector<bool> noDataValueAvailable = {
true};
184 std::vector<double> noDataValue = {m_NoDataValue};
186 WriteNoDataFlags(noDataValueAvailable, noDataValue, horizOut->GetImageMetadata());
187 WriteNoDataFlags(noDataValueAvailable, noDataValue, vertiOut->GetImageMetadata());
190 template <
class TDisparityImage,
class TGr
idImage,
class TSensorImage,
class TMaskImage>
193 this->Superclass::GenerateInputRequestedRegion();
195 TGridImage* leftGrid =
const_cast<TGridImage*
>(this->GetInverseEpipolarLeftGrid());
196 TGridImage* rightGrid =
const_cast<TGridImage*
>(this->GetDirectEpipolarRightGrid());
198 leftGrid->SetRequestedRegionToLargestPossibleRegion();
199 rightGrid->SetRequestedRegionToLargestPossibleRegion();
201 leftGrid->UpdateOutputData();
202 rightGrid->UpdateOutputData();
204 TSensorImage* leftIn =
const_cast<TSensorImage*
>(this->GetLeftSensorImageInput());
205 RegionType emptyRegion = leftIn->GetLargestPossibleRegion();
206 emptyRegion.SetSize(0, 0);
207 emptyRegion.SetSize(1, 0);
208 leftIn->SetRequestedRegion(emptyRegion);
210 TDisparityImage* horizOut = this->GetHorizontalDisparityMapOutput();
212 TDisparityImage* horizIn =
const_cast<TDisparityImage*
>(this->GetHorizontalDisparityMapInput());
213 TDisparityImage* vertiIn =
const_cast<TDisparityImage*
>(this->GetVerticalDisparityMapInput());
215 TMaskImage* maskIn =
const_cast<TMaskImage*
>(this->GetDisparityMaskInput());
217 RegionType requested = this->GetHorizontalDisparityMapOutput()->GetRequestedRegion();
218 RegionType inputlargest = this->GetHorizontalDisparityMapInput()->GetLargestPossibleRegion();
220 GridRegionType gridLargest = leftGrid->GetLargestPossibleRegion();
224 minIndex.Fill(itk::NumericTraits<IndexValueType>::Zero);
225 maxIndex.Fill(itk::NumericTraits<IndexValueType>::Zero);
228 for (
int i = 0; i < 4; i++)
229 corners[i].Fill(itk::NumericTraits<IndexValueType>::Zero);
231 corners[0] = requested.GetIndex();
232 corners[1] = requested.GetIndex();
233 corners[1][0] +=
static_cast<IndexValueType>(requested.GetSize()[0]) - 1;
234 corners[2] = requested.GetIndex();
235 corners[2][1] +=
static_cast<IndexValueType>(requested.GetSize()[1]) - 1;
236 corners[3] = requested.GetIndex();
237 corners[3][0] +=
static_cast<IndexValueType>(requested.GetSize()[0]) - 1;
238 corners[3][1] +=
static_cast<IndexValueType>(requested.GetSize()[1]) - 1;
239 for (
unsigned int k = 0; k < 4; ++k)
242 horizOut->TransformIndexToPhysicalPoint(corners[k], pointSensor);
243 itk::ContinuousIndex<double, 2> indexGrid;
244 leftGrid->TransformPhysicalPointToContinuousIndex(pointSensor, indexGrid);
246 ul[0] =
static_cast<long>(std::floor(indexGrid[0]));
247 ul[1] =
static_cast<long>(std::floor(indexGrid[1]));
248 if (ul[0] < gridLargest.GetIndex()[0])
249 ul[0] = gridLargest.GetIndex()[0];
250 if (ul[1] < gridLargest.GetIndex()[1])
251 ul[1] = gridLargest.GetIndex()[1];
252 if (ul[0] >
static_cast<IndexValueType>(gridLargest.GetIndex()[0] + gridLargest.GetSize()[0] - 2))
253 ul[0] = (gridLargest.GetIndex()[0] + gridLargest.GetSize()[0] - 2);
254 if (ul[1] >
static_cast<IndexValueType>(gridLargest.GetIndex()[1] + gridLargest.GetSize()[1] - 2))
255 ul[1] = (gridLargest.GetIndex()[1] + gridLargest.GetSize()[1] - 2);
265 double rx = indexGrid[0] -
static_cast<double>(ul[0]);
266 double ry = indexGrid[1] -
static_cast<double>(ul[1]);
269 pointEpi[0] += (1. - ry) * ((1. - rx) * leftGrid->GetPixel(ul)[0] + rx * leftGrid->GetPixel(ur)[0]) +
270 ry * ((1. - rx) * leftGrid->GetPixel(ll)[0] + rx * leftGrid->GetPixel(lr)[0]);
271 pointEpi[1] += (1. - ry) * ((1. - rx) * leftGrid->GetPixel(ul)[1] + rx * leftGrid->GetPixel(ur)[1]) +
272 ry * ((1. - rx) * leftGrid->GetPixel(ll)[1] + rx * leftGrid->GetPixel(lr)[1]);
273 itk::ContinuousIndex<double, 2> indexEpi;
275 horizIn->TransformPhysicalPointToContinuousIndex(pointEpi, indexEpi);
278 minIndex[0] =
static_cast<long>(std::floor(indexEpi[0]));
279 minIndex[1] =
static_cast<long>(std::floor(indexEpi[1]));
280 maxIndex[0] =
static_cast<long>(std::ceil(indexEpi[0]));
281 maxIndex[1] =
static_cast<long>(std::ceil(indexEpi[1]));
285 if (minIndex[0] >
static_cast<long>(std::floor(indexEpi[0])))
286 minIndex[0] =
static_cast<long>(std::floor(indexEpi[0]));
287 if (minIndex[1] >
static_cast<long>(std::floor(indexEpi[1])))
288 minIndex[1] =
static_cast<long>(std::floor(indexEpi[1]));
289 if (maxIndex[0] <
static_cast<long>(std::ceil(indexEpi[0])))
290 maxIndex[0] =
static_cast<long>(std::ceil(indexEpi[0]));
291 if (maxIndex[1] <
static_cast<long>(std::ceil(indexEpi[1])))
292 maxIndex[1] =
static_cast<long>(std::ceil(indexEpi[1]));
296 inputRequested.SetIndex(minIndex);
297 inputRequested.SetSize(0,
static_cast<unsigned long>(maxIndex[0] - minIndex[0]));
298 inputRequested.SetSize(1,
static_cast<unsigned long>(maxIndex[1] - minIndex[1]));
300 if (!inputRequested.Crop(inputlargest))
302 inputRequested.SetSize(0, 0);
303 inputRequested.SetSize(1, 0);
304 inputRequested.SetIndex(inputlargest.GetIndex());
307 horizIn->SetRequestedRegion(inputRequested);
309 vertiIn->SetRequestedRegion(inputRequested);
311 maskIn->SetRequestedRegion(inputRequested);
314 template <
class TDisparityImage,
class TGr
idImage,
class TSensorImage,
class TMaskImage>
316 itk::ThreadIdType itkNotUsed(threadId))
318 const TGridImage* leftGrid = this->GetInverseEpipolarLeftGrid();
319 const TGridImage* rightGrid = this->GetDirectEpipolarRightGrid();
321 TDisparityImage* horizOut = this->GetHorizontalDisparityMapOutput();
322 TDisparityImage* vertiOut = this->GetVerticalDisparityMapOutput();
324 const TDisparityImage* horizIn = this->GetHorizontalDisparityMapInput();
325 const TDisparityImage* vertiIn = this->GetVerticalDisparityMapInput();
327 const TMaskImage* maskIn = this->GetDisparityMaskInput();
329 GridRegionType leftLargest = leftGrid->GetLargestPossibleRegion();
330 GridRegionType rightLargest = rightGrid->GetLargestPossibleRegion();
331 RegionType buffered = horizIn->GetBufferedRegion();
333 const bool emptyInputRegion = buffered.GetNumberOfPixels() == 0;
335 typedef itk::ImageRegionIteratorWithIndex<TDisparityImage> DispIterator;
336 DispIterator horizIter(horizOut, outputRegionForThread);
337 DispIterator vertiIter(vertiOut, outputRegionForThread);
339 horizIter.GoToBegin();
340 vertiIter.GoToBegin();
342 while (!horizIter.IsAtEnd() && !vertiIter.IsAtEnd())
345 if (!emptyInputRegion)
348 horizOut->TransformIndexToPhysicalPoint(horizIter.GetIndex(), pointSensor);
350 itk::ContinuousIndex<double, 2> indexGrid;
351 leftGrid->TransformPhysicalPointToContinuousIndex(pointSensor, indexGrid);
355 ul[0] =
static_cast<long>(std::floor(indexGrid[0]));
356 ul[1] =
static_cast<long>(std::floor(indexGrid[1]));
357 if (ul[0] < leftLargest.GetIndex()[0])
358 ul[0] = leftLargest.GetIndex()[0];
359 if (ul[1] < leftLargest.GetIndex()[1])
360 ul[1] = leftLargest.GetIndex()[1];
361 if (ul[0] >
static_cast<IndexValueType>(leftLargest.GetIndex()[0] + leftLargest.GetSize()[0] - 2))
362 ul[0] = (leftLargest.GetIndex()[0] + leftLargest.GetSize()[0] - 2);
363 if (ul[1] >
static_cast<IndexValueType>(leftLargest.GetIndex()[1] + leftLargest.GetSize()[1] - 2))
364 ul[1] = (leftLargest.GetIndex()[1] + leftLargest.GetSize()[1] - 2);
374 double rx = indexGrid[0] -
static_cast<double>(ul[0]);
375 double ry = indexGrid[1] -
static_cast<double>(ul[1]);
378 pointEpi[0] += (1. - ry) * ((1. - rx) * leftGrid->GetPixel(ul)[0] + rx * leftGrid->GetPixel(ur)[0]) +
379 ry * ((1. - rx) * leftGrid->GetPixel(ll)[0] + rx * leftGrid->GetPixel(lr)[0]);
380 pointEpi[1] += (1. - ry) * ((1. - rx) * leftGrid->GetPixel(ul)[1] + rx * leftGrid->GetPixel(ur)[1]) +
381 ry * ((1. - rx) * leftGrid->GetPixel(ll)[1] + rx * leftGrid->GetPixel(lr)[1]);
383 itk::ContinuousIndex<double, 2> indexEpi;
384 horizIn->TransformPhysicalPointToContinuousIndex(pointEpi, indexEpi);
387 ul[0] =
static_cast<long>(std::floor(indexEpi[0]));
388 ul[1] =
static_cast<long>(std::floor(indexEpi[1]));
389 if (ul[0] < buffered.GetIndex()[0])
390 ul[0] = buffered.GetIndex()[0];
391 if (ul[1] < buffered.GetIndex()[1])
392 ul[1] = buffered.GetIndex()[1];
393 if (ul[0] >
static_cast<IndexValueType>(buffered.GetIndex()[0] + buffered.GetSize()[0] - 2))
394 ul[0] = (buffered.GetIndex()[0] + buffered.GetSize()[0] - 2);
395 if (ul[1] >
static_cast<IndexValueType>(buffered.GetIndex()[1] + buffered.GetSize()[1] - 2))
396 ul[1] = (buffered.GetIndex()[1] + buffered.GetSize()[1] - 2);
407 if (!maskIn || (maskIn && maskIn->GetPixel(ul) > 0 && maskIn->GetPixel(ur) > 0 && maskIn->GetPixel(ll) > 0 && maskIn->GetPixel(lr) > 0))
409 rx = indexEpi[0] -
static_cast<double>(ul[0]);
410 ry = indexEpi[1] -
static_cast<double>(ul[1]);
412 itk::ContinuousIndex<double, 2> indexRight(indexEpi);
414 indexRight[0] += (1. - ry) * ((1. - rx) * horizIn->GetPixel(ul) + rx * horizIn->GetPixel(ur)) +
415 ry * ((1. - rx) * horizIn->GetPixel(ll) + rx * horizIn->GetPixel(lr));
418 indexRight[1] += (1. - ry) * ((1. - rx) * vertiIn->GetPixel(ul) + rx * vertiIn->GetPixel(ur)) +
419 ry * ((1. - rx) * vertiIn->GetPixel(ll) + rx * vertiIn->GetPixel(lr));
423 horizIn->TransformContinuousIndexToPhysicalPoint(indexRight, pointRight);
425 itk::ContinuousIndex<double, 2> indexGridRight;
426 rightGrid->TransformPhysicalPointToContinuousIndex(pointRight, indexGridRight);
429 ul[0] =
static_cast<long>(std::floor(indexGridRight[0]));
430 ul[1] =
static_cast<long>(std::floor(indexGridRight[1]));
431 if (ul[0] < rightLargest.GetIndex()[0])
432 ul[0] = rightLargest.GetIndex()[0];
433 if (ul[1] < rightLargest.GetIndex()[1])
434 ul[1] = rightLargest.GetIndex()[1];
435 if (ul[0] >
static_cast<IndexValueType>(rightLargest.GetIndex()[0] + rightLargest.GetSize()[0] - 2))
436 ul[0] = (rightLargest.GetIndex()[0] + rightLargest.GetSize()[0] - 2);
437 if (ul[1] >
static_cast<IndexValueType>(rightLargest.GetIndex()[1] + rightLargest.GetSize()[1] - 2))
438 ul[1] = (rightLargest.GetIndex()[1] + rightLargest.GetSize()[1] - 2);
448 rx = indexGridRight[0] -
static_cast<double>(ul[0]);
449 ry = indexGridRight[1] -
static_cast<double>(ul[1]);
453 pointSensorRight[0] += (1. - ry) * ((1. - rx) * rightGrid->GetPixel(ul)[0] + rx * rightGrid->GetPixel(ur)[0]) +
454 ry * ((1. - rx) * rightGrid->GetPixel(ll)[0] + rx * rightGrid->GetPixel(lr)[0]);
455 pointSensorRight[1] += (1. - ry) * ((1. - rx) * rightGrid->GetPixel(ul)[1] + rx * rightGrid->GetPixel(ur)[1]) +
456 ry * ((1. - rx) * rightGrid->GetPixel(ll)[1] + rx * rightGrid->GetPixel(lr)[1]);
458 horizIter.Set(pointSensorRight[0] - pointSensor[0]);
459 vertiIter.Set(pointSensorRight[1] - pointSensor[1]);
463 horizIter.Set(m_NoDataValue);
464 vertiIter.Set(m_NoDataValue);
469 horizIter.Set(m_NoDataValue);
470 vertiIter.Set(m_NoDataValue);