21 #ifndef otbDisparityMapTo3DFilter_hxx
22 #define otbDisparityMapTo3DFilter_hxx
25 #include "itkImageRegionConstIteratorWithIndex.h"
26 #include "itkImageRegionIterator.h"
31 template <
class TDisparityImage,
class TOutputImage,
class TEpipolarGr
idImage,
class TMaskImage>
35 this->SetNumberOfRequiredInputs(5);
36 this->SetNumberOfRequiredInputs(1);
39 this->SetNumberOfRequiredOutputs(1);
40 this->SetNthOutput(0, TOutputImage::New());
43 template <
class TDisparityImage,
class TOutputImage,
class TEpipolarGr
idImage,
class TMaskImage>
47 this->SetNthInput(0,
const_cast<TDisparityImage*
>(hmap));
50 template <
class TDisparityImage,
class TOutputImage,
class TEpipolarGr
idImage,
class TMaskImage>
54 this->SetNthInput(1,
const_cast<TDisparityImage*
>(vmap));
57 template <
class TDisparityImage,
class TOutputImage,
class TEpipolarGr
idImage,
class TMaskImage>
61 this->SetNthInput(2,
const_cast<TEpipolarGridImage*
>(grid));
64 template <
class TDisparityImage,
class TOutputImage,
class TEpipolarGr
idImage,
class TMaskImage>
68 this->SetNthInput(3,
const_cast<TEpipolarGridImage*
>(grid));
71 template <
class TDisparityImage,
class TOutputImage,
class TEpipolarGr
idImage,
class TMaskImage>
75 this->SetNthInput(4,
const_cast<TMaskImage*
>(mask));
78 template <
class TDisparityImage,
class TOutputImage,
class TEpipolarGr
idImage,
class TMaskImage>
81 if (this->GetNumberOfInputs() < 1)
85 return static_cast<const TDisparityImage*
>(this->itk::ProcessObject::GetInput(0));
88 template <
class TDisparityImage,
class TOutputImage,
class TEpipolarGr
idImage,
class TMaskImage>
91 if (this->GetNumberOfInputs() < 2)
95 return static_cast<const TDisparityImage*
>(this->itk::ProcessObject::GetInput(1));
98 template <
class TDisparityImage,
class TOutputImage,
class TEpipolarGr
idImage,
class TMaskImage>
101 if (this->GetNumberOfInputs() < 3)
105 return static_cast<const TEpipolarGridImage*
>(this->itk::ProcessObject::GetInput(2));
108 template <
class TDisparityImage,
class TOutputImage,
class TEpipolarGr
idImage,
class TMaskImage>
111 if (this->GetNumberOfInputs() < 4)
115 return static_cast<const TEpipolarGridImage*
>(this->itk::ProcessObject::GetInput(3));
118 template <
class TDisparityImage,
class TOutputImage,
class TEpipolarGr
idImage,
class TMaskImage>
121 if (this->GetNumberOfInputs() < 5)
125 return static_cast<const TMaskImage*
>(this->itk::ProcessObject::GetInput(4));
128 template <
class TDisparityImage,
class TOutputImage,
class TEpipolarGr
idImage,
class TMaskImage>
131 const TDisparityImage* horizDisp = this->GetHorizontalDisparityMapInput();
132 TOutputImage* outputPtr = this->GetOutput();
134 outputPtr->SetLargestPossibleRegion(horizDisp->GetLargestPossibleRegion());
135 outputPtr->SetNumberOfComponentsPerPixel(3);
138 outputPtr->SetOrigin(horizDisp->GetOrigin());
139 outputPtr->SetSignedSpacing(horizDisp->GetSignedSpacing());
142 template <
class TDisparityImage,
class TOutputImage,
class TEpipolarGr
idImage,
class TMaskImage>
146 TEpipolarGridImage* leftGrid =
const_cast<TEpipolarGridImage*
>(this->GetLeftEpipolarGridInput());
147 TEpipolarGridImage* rightGrid =
const_cast<TEpipolarGridImage*
>(this->GetRightEpipolarGridInput());
149 leftGrid->SetRequestedRegionToLargestPossibleRegion();
150 rightGrid->SetRequestedRegionToLargestPossibleRegion();
152 TOutputImage* outputDEM = this->GetOutput();
154 TDisparityImage* horizDisp =
const_cast<TDisparityImage*
>(this->GetHorizontalDisparityMapInput());
155 TDisparityImage* vertiDisp =
const_cast<TDisparityImage*
>(this->GetVerticalDisparityMapInput());
156 TMaskImage* maskDisp =
const_cast<TMaskImage*
>(this->GetDisparityMaskInput());
159 if (vertiDisp && horizDisp->GetLargestPossibleRegion() != vertiDisp->GetLargestPossibleRegion())
161 itkExceptionMacro(<<
"Horizontal and vertical disparity maps do not have the same size ! Horizontal largest region: "
162 << horizDisp->GetLargestPossibleRegion() <<
", vertical largest region: " << vertiDisp->GetLargestPossibleRegion());
166 if (maskDisp && horizDisp->GetLargestPossibleRegion() != maskDisp->GetLargestPossibleRegion())
168 itkExceptionMacro(<<
"Disparity map and mask do not have the same size ! Map region : " << horizDisp->GetLargestPossibleRegion()
169 <<
", mask region : " << maskDisp->GetLargestPossibleRegion());
172 horizDisp->SetRequestedRegion(outputDEM->GetRequestedRegion());
176 vertiDisp->SetRequestedRegion(outputDEM->GetRequestedRegion());
181 maskDisp->SetRequestedRegion(outputDEM->GetRequestedRegion());
185 if (!m_LeftImageMetadata->HasSensorGeometry() || !m_RightImageMetadata->HasSensorGeometry())
187 itkExceptionMacro(<<
"At least one of the image keywordlist is empty : can't instantiate corresponding projection");
191 template <
class TDisparityImage,
class TOutputImage,
class TEpipolarGr
idImage,
class TMaskImage>
195 m_LeftToGroundTransform = RSTransformType::New();
196 m_RightToGroundTransform = RSTransformType::New();
198 m_LeftToGroundTransform->SetInputImageMetadata(m_LeftImageMetadata);
199 m_RightToGroundTransform->SetInputImageMetadata(m_RightImageMetadata);
201 m_LeftToGroundTransform->InstantiateTransform();
202 m_RightToGroundTransform->InstantiateTransform();
205 template <
class TDisparityImage,
class TOutputImage,
class TEpipolarGr
idImage,
class TMaskImage>
207 const RegionType& itkNotUsed(outputRegionForThread), itk::ThreadIdType itkNotUsed(threadId))
209 const TDisparityImage* horizDisp = this->GetHorizontalDisparityMapInput();
210 const TDisparityImage* vertiDisp = this->GetVerticalDisparityMapInput();
212 const TMaskImage* disparityMask = this->GetDisparityMaskInput();
214 TOutputImage* outputDEM = this->GetOutput();
217 const TEpipolarGridImage* leftGrid = this->GetLeftEpipolarGridInput();
218 const TEpipolarGridImage* rightGrid = this->GetRightEpipolarGridInput();
220 typename TEpipolarGridImage::RegionType gridRegion = leftGrid->GetLargestPossibleRegion();
222 typename TOutputImage::RegionType outputRequestedRegion = outputDEM->GetRequestedRegion();
224 itk::ImageRegionIterator<OutputImageType> demIt(outputDEM, outputRequestedRegion);
225 itk::ImageRegionConstIteratorWithIndex<DisparityMapType> horizIt(horizDisp, outputRequestedRegion);
230 bool useVerti =
false;
231 itk::ImageRegionConstIteratorWithIndex<DisparityMapType> vertiIt;
235 vertiIt = itk::ImageRegionConstIteratorWithIndex<DisparityMapType>(vertiDisp, outputRequestedRegion);
239 bool useMask =
false;
240 itk::ImageRegionConstIterator<MaskImageType> maskIt;
244 maskIt = itk::ImageRegionConstIterator<MaskImageType>(disparityMask, outputRequestedRegion);
248 double elevationMin = 0.0;
249 double elevationMax = 300.0;
253 typename TDisparityImage::PointType epiPoint;
254 itk::ContinuousIndex<double, 2> gridIndexConti;
255 double subPixIndex[2];
256 typename GridImageType::IndexType ulIndex, urIndex, lrIndex, llIndex;
257 typename GridImageType::PixelType ulPixel(2);
258 typename GridImageType::PixelType urPixel(2);
259 typename GridImageType::PixelType lrPixel(2);
260 typename GridImageType::PixelType llPixel(2);
261 typename GridImageType::PixelType cPixel(2);
263 typename GridImageType::PointType ulPoint;
264 typename GridImageType::PointType urPoint;
265 typename GridImageType::PointType lrPoint;
266 typename GridImageType::PointType llPoint;
274 while (!demIt.IsAtEnd() && !horizIt.IsAtEnd())
279 if (!(maskIt.Get() > 0))
282 typename OutputImageType::PixelType pixel3D(3);
296 horizDisp->TransformIndexToPhysicalPoint(horizIt.GetIndex(), epiPoint);
297 leftGrid->TransformPhysicalPointToContinuousIndex(epiPoint, gridIndexConti);
299 ulIndex[0] =
static_cast<int>(std::floor(gridIndexConti[0]));
300 ulIndex[1] =
static_cast<int>(std::floor(gridIndexConti[1]));
301 if (ulIndex[0] < gridRegion.GetIndex(0))
302 ulIndex[0] = gridRegion.GetIndex(0);
303 if (ulIndex[1] < gridRegion.GetIndex(1))
304 ulIndex[1] = gridRegion.GetIndex(1);
305 if (ulIndex[0] > (gridRegion.GetIndex(0) +
static_cast<int>(gridRegion.GetSize(0)) - 2))
307 ulIndex[0] = gridRegion.GetIndex(0) + gridRegion.GetSize(0) - 2;
309 if (ulIndex[1] > (gridRegion.GetIndex(1) +
static_cast<int>(gridRegion.GetSize(1)) - 2))
311 ulIndex[1] = gridRegion.GetIndex(1) + gridRegion.GetSize(1) - 2;
313 urIndex[0] = ulIndex[0] + 1;
314 urIndex[1] = ulIndex[1];
315 lrIndex[0] = ulIndex[0] + 1;
316 lrIndex[1] = ulIndex[1] + 1;
317 llIndex[0] = ulIndex[0];
318 llIndex[1] = ulIndex[1] + 1;
319 subPixIndex[0] = gridIndexConti[0] -
static_cast<double>(ulIndex[0]);
320 subPixIndex[1] = gridIndexConti[1] -
static_cast<double>(ulIndex[1]);
322 leftGrid->TransformIndexToPhysicalPoint(ulIndex, ulPoint);
323 leftGrid->TransformIndexToPhysicalPoint(urIndex, urPoint);
324 leftGrid->TransformIndexToPhysicalPoint(lrIndex, lrPoint);
325 leftGrid->TransformIndexToPhysicalPoint(llIndex, llPoint);
327 ulPixel[0] = (leftGrid->GetPixel(ulIndex))[0] + ulPoint[0];
328 ulPixel[1] = (leftGrid->GetPixel(ulIndex))[1] + ulPoint[1];
329 urPixel[0] = (leftGrid->GetPixel(urIndex))[0] + urPoint[0];
330 urPixel[1] = (leftGrid->GetPixel(urIndex))[1] + urPoint[1];
331 lrPixel[0] = (leftGrid->GetPixel(lrIndex))[0] + lrPoint[0];
332 lrPixel[1] = (leftGrid->GetPixel(lrIndex))[1] + lrPoint[1];
333 llPixel[0] = (leftGrid->GetPixel(llIndex))[0] + llPoint[0];
334 llPixel[1] = (leftGrid->GetPixel(llIndex))[1] + llPoint[1];
335 cPixel = (ulPixel * (1.0 - subPixIndex[0]) + urPixel * subPixIndex[0]) * (1.0 - subPixIndex[1]) +
336 (llPixel * (1.0 - subPixIndex[0]) + lrPixel * subPixIndex[0]) * subPixIndex[1];
338 sensorPoint[0] = cPixel[0];
339 sensorPoint[1] = cPixel[1];
340 sensorPoint[2] = elevationMin;
341 leftGroundHmin = m_LeftToGroundTransform->TransformPoint(sensorPoint);
343 sensorPoint[2] = elevationMax;
344 leftGroundHmax = m_LeftToGroundTransform->TransformPoint(sensorPoint);
347 itk::ContinuousIndex<double, 2> rightIndexEstimate;
348 rightIndexEstimate[0] =
static_cast<double>((horizIt.GetIndex())[0]) +
static_cast<double>(horizIt.Get());
350 double verticalShift = 0;
352 verticalShift =
static_cast<double>(vertiIt.Get());
353 rightIndexEstimate[1] =
static_cast<double>((horizIt.GetIndex())[1]) + verticalShift;
355 horizDisp->TransformContinuousIndexToPhysicalPoint(rightIndexEstimate, epiPoint);
356 rightGrid->TransformPhysicalPointToContinuousIndex(epiPoint, gridIndexConti);
358 ulIndex[0] =
static_cast<int>(std::floor(gridIndexConti[0]));
359 ulIndex[1] =
static_cast<int>(std::floor(gridIndexConti[1]));
360 if (ulIndex[0] < gridRegion.GetIndex(0))
361 ulIndex[0] = gridRegion.GetIndex(0);
362 if (ulIndex[1] < gridRegion.GetIndex(1))
363 ulIndex[1] = gridRegion.GetIndex(1);
364 if (ulIndex[0] > (gridRegion.GetIndex(0) +
static_cast<int>(gridRegion.GetSize(0)) - 2))
366 ulIndex[0] = gridRegion.GetIndex(0) + gridRegion.GetSize(0) - 2;
368 if (ulIndex[1] > (gridRegion.GetIndex(1) +
static_cast<int>(gridRegion.GetSize(1)) - 2))
370 ulIndex[1] = gridRegion.GetIndex(1) + gridRegion.GetSize(1) - 2;
372 urIndex[0] = ulIndex[0] + 1;
373 urIndex[1] = ulIndex[1];
374 lrIndex[0] = ulIndex[0] + 1;
375 lrIndex[1] = ulIndex[1] + 1;
376 llIndex[0] = ulIndex[0];
377 llIndex[1] = ulIndex[1] + 1;
378 subPixIndex[0] = gridIndexConti[0] -
static_cast<double>(ulIndex[0]);
379 subPixIndex[1] = gridIndexConti[1] -
static_cast<double>(ulIndex[1]);
381 rightGrid->TransformIndexToPhysicalPoint(ulIndex, ulPoint);
382 rightGrid->TransformIndexToPhysicalPoint(urIndex, urPoint);
383 rightGrid->TransformIndexToPhysicalPoint(lrIndex, lrPoint);
384 rightGrid->TransformIndexToPhysicalPoint(llIndex, llPoint);
386 ulPixel[0] = (rightGrid->GetPixel(ulIndex))[0] + ulPoint[0];
387 ulPixel[1] = (rightGrid->GetPixel(ulIndex))[1] + ulPoint[1];
388 urPixel[0] = (rightGrid->GetPixel(urIndex))[0] + urPoint[0];
389 urPixel[1] = (rightGrid->GetPixel(urIndex))[1] + urPoint[1];
390 lrPixel[0] = (rightGrid->GetPixel(lrIndex))[0] + lrPoint[0];
391 lrPixel[1] = (rightGrid->GetPixel(lrIndex))[1] + lrPoint[1];
392 llPixel[0] = (rightGrid->GetPixel(llIndex))[0] + llPoint[0];
393 llPixel[1] = (rightGrid->GetPixel(llIndex))[1] + llPoint[1];
394 cPixel = (ulPixel * (1.0 - subPixIndex[0]) + urPixel * subPixIndex[0]) * (1.0 - subPixIndex[1]) +
395 (llPixel * (1.0 - subPixIndex[0]) + lrPixel * subPixIndex[0]) * subPixIndex[1];
397 sensorPoint[0] = cPixel[0];
398 sensorPoint[1] = cPixel[1];
399 sensorPoint[2] = elevationMin;
400 rightGroundHmin = m_RightToGroundTransform->TransformPoint(sensorPoint);
402 sensorPoint[2] = elevationMax;
403 rightGroundHmax = m_RightToGroundTransform->TransformPoint(sensorPoint);
406 typename PointSetType::Pointer pointSetA = PointSetType::New();
407 typename PointSetType::Pointer pointSetB = PointSetType::New();
409 pointSetA->SetPoint(0, leftGroundHmax);
410 pointSetA->SetPoint(1, rightGroundHmax);
411 pointSetA->SetPointData(0, 0);
412 pointSetA->SetPointData(1, 1);
414 pointSetB->SetPoint(0, leftGroundHmin);
415 pointSetB->SetPoint(1, rightGroundHmin);
416 pointSetB->SetPointData(0, 0);
417 pointSetB->SetPointData(1, 1);
419 TDPointType midPoint3D = optimizer->Compute(pointSetA, pointSetB);
422 typename OutputImageType::PixelType pixel3D(3);
423 pixel3D[0] = midPoint3D[0];
424 pixel3D[1] = midPoint3D[1];
425 pixel3D[2] = midPoint3D[2];