OTB  9.0.0
Orfeo Toolbox
otbDisparityMapTo3DFilter.hxx
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2005-2022 Centre National d'Etudes Spatiales (CNES)
3  *
4  * This file is part of Orfeo Toolbox
5  *
6  * https://www.orfeo-toolbox.org/
7  *
8  * Licensed under the Apache License, Version 2.0 (the "License");
9  * you may not use this file except in compliance with the License.
10  * You may obtain a copy of the License at
11  *
12  * http://www.apache.org/licenses/LICENSE-2.0
13  *
14  * Unless required by applicable law or agreed to in writing, software
15  * distributed under the License is distributed on an "AS IS" BASIS,
16  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
17  * See the License for the specific language governing permissions and
18  * limitations under the License.
19  */
20 
21 #ifndef otbDisparityMapTo3DFilter_hxx
22 #define otbDisparityMapTo3DFilter_hxx
23 
25 #include "itkImageRegionConstIteratorWithIndex.h"
26 #include "itkImageRegionIterator.h"
27 
28 namespace otb
29 {
30 
31 template <class TDisparityImage, class TOutputImage, class TEpipolarGridImage, class TMaskImage>
33 {
34  // Set the number of inputs
35  this->SetNumberOfRequiredInputs(5);
36  this->SetNumberOfRequiredInputs(1);
37 
38  // Set the outputs
39  this->SetNumberOfRequiredOutputs(1);
40  this->SetNthOutput(0, TOutputImage::New());
41 }
42 
43 template <class TDisparityImage, class TOutputImage, class TEpipolarGridImage, class TMaskImage>
45 {
46  // Process object is not const-correct so the const casting is required.
47  this->SetNthInput(0, const_cast<TDisparityImage*>(hmap));
48 }
49 
50 template <class TDisparityImage, class TOutputImage, class TEpipolarGridImage, class TMaskImage>
52 {
53  // Process object is not const-correct so the const casting is required.
54  this->SetNthInput(1, const_cast<TDisparityImage*>(vmap));
55 }
56 
57 template <class TDisparityImage, class TOutputImage, class TEpipolarGridImage, class TMaskImage>
59 {
60  // Process object is not const-correct so the const casting is required.
61  this->SetNthInput(2, const_cast<TEpipolarGridImage*>(grid));
62 }
63 
64 template <class TDisparityImage, class TOutputImage, class TEpipolarGridImage, class TMaskImage>
66 {
67  // Process object is not const-correct so the const casting is required.
68  this->SetNthInput(3, const_cast<TEpipolarGridImage*>(grid));
69 }
70 
71 template <class TDisparityImage, class TOutputImage, class TEpipolarGridImage, class TMaskImage>
73 {
74  // Process object is not const-correct so the const casting is required.
75  this->SetNthInput(4, const_cast<TMaskImage*>(mask));
76 }
77 
78 template <class TDisparityImage, class TOutputImage, class TEpipolarGridImage, class TMaskImage>
80 {
81  if (this->GetNumberOfInputs() < 1)
82  {
83  return nullptr;
84  }
85  return static_cast<const TDisparityImage*>(this->itk::ProcessObject::GetInput(0));
86 }
87 
88 template <class TDisparityImage, class TOutputImage, class TEpipolarGridImage, class TMaskImage>
90 {
91  if (this->GetNumberOfInputs() < 2)
92  {
93  return nullptr;
94  }
95  return static_cast<const TDisparityImage*>(this->itk::ProcessObject::GetInput(1));
96 }
97 
98 template <class TDisparityImage, class TOutputImage, class TEpipolarGridImage, class TMaskImage>
100 {
101  if (this->GetNumberOfInputs() < 3)
102  {
103  return nullptr;
104  }
105  return static_cast<const TEpipolarGridImage*>(this->itk::ProcessObject::GetInput(2));
106 }
107 
108 template <class TDisparityImage, class TOutputImage, class TEpipolarGridImage, class TMaskImage>
110 {
111  if (this->GetNumberOfInputs() < 4)
112  {
113  return nullptr;
114  }
115  return static_cast<const TEpipolarGridImage*>(this->itk::ProcessObject::GetInput(3));
116 }
117 
118 template <class TDisparityImage, class TOutputImage, class TEpipolarGridImage, class TMaskImage>
120 {
121  if (this->GetNumberOfInputs() < 5)
122  {
123  return nullptr;
124  }
125  return static_cast<const TMaskImage*>(this->itk::ProcessObject::GetInput(4));
126 }
127 
128 template <class TDisparityImage, class TOutputImage, class TEpipolarGridImage, class TMaskImage>
130 {
131  const TDisparityImage* horizDisp = this->GetHorizontalDisparityMapInput();
132  TOutputImage* outputPtr = this->GetOutput();
133 
134  outputPtr->SetLargestPossibleRegion(horizDisp->GetLargestPossibleRegion());
135  outputPtr->SetNumberOfComponentsPerPixel(3);
136 
137  // copy also origin and spacing
138  outputPtr->SetOrigin(horizDisp->GetOrigin());
139  outputPtr->SetSignedSpacing(horizDisp->GetSignedSpacing());
140 }
141 
142 template <class TDisparityImage, class TOutputImage, class TEpipolarGridImage, class TMaskImage>
144 {
145  // For the epi grid : generate full buffer here !
146  TEpipolarGridImage* leftGrid = const_cast<TEpipolarGridImage*>(this->GetLeftEpipolarGridInput());
147  TEpipolarGridImage* rightGrid = const_cast<TEpipolarGridImage*>(this->GetRightEpipolarGridInput());
148 
149  leftGrid->SetRequestedRegionToLargestPossibleRegion();
150  rightGrid->SetRequestedRegionToLargestPossibleRegion();
151 
152  TOutputImage* outputDEM = this->GetOutput();
153 
154  TDisparityImage* horizDisp = const_cast<TDisparityImage*>(this->GetHorizontalDisparityMapInput());
155  TDisparityImage* vertiDisp = const_cast<TDisparityImage*>(this->GetVerticalDisparityMapInput());
156  TMaskImage* maskDisp = const_cast<TMaskImage*>(this->GetDisparityMaskInput());
157 
158  // We impose that both disparity map inputs have the same size
159  if (vertiDisp && horizDisp->GetLargestPossibleRegion() != vertiDisp->GetLargestPossibleRegion())
160  {
161  itkExceptionMacro(<< "Horizontal and vertical disparity maps do not have the same size ! Horizontal largest region: "
162  << horizDisp->GetLargestPossibleRegion() << ", vertical largest region: " << vertiDisp->GetLargestPossibleRegion());
163  }
164 
165 
166  if (maskDisp && horizDisp->GetLargestPossibleRegion() != maskDisp->GetLargestPossibleRegion())
167  {
168  itkExceptionMacro(<< "Disparity map and mask do not have the same size ! Map region : " << horizDisp->GetLargestPossibleRegion()
169  << ", mask region : " << maskDisp->GetLargestPossibleRegion());
170  }
171 
172  horizDisp->SetRequestedRegion(outputDEM->GetRequestedRegion());
173 
174  if (vertiDisp)
175  {
176  vertiDisp->SetRequestedRegion(outputDEM->GetRequestedRegion());
177  }
178 
179  if (maskDisp)
180  {
181  maskDisp->SetRequestedRegion(outputDEM->GetRequestedRegion());
182  }
183 
184  // Check that the keywordlists are not empty
185  if (!m_LeftImageMetadata->HasSensorGeometry() || !m_RightImageMetadata->HasSensorGeometry())
186  {
187  itkExceptionMacro(<< "At least one of the image keywordlist is empty : can't instantiate corresponding projection");
188  }
189 }
190 
191 template <class TDisparityImage, class TOutputImage, class TEpipolarGridImage, class TMaskImage>
193 {
194  // Instantiate transforms
195  m_LeftToGroundTransform = RSTransformType::New();
196  m_RightToGroundTransform = RSTransformType::New();
197 
198  m_LeftToGroundTransform->SetInputImageMetadata(m_LeftImageMetadata);
199  m_RightToGroundTransform->SetInputImageMetadata(m_RightImageMetadata);
200 
201  m_LeftToGroundTransform->InstantiateTransform();
202  m_RightToGroundTransform->InstantiateTransform();
203 }
204 
205 template <class TDisparityImage, class TOutputImage, class TEpipolarGridImage, class TMaskImage>
207  const RegionType& itkNotUsed(outputRegionForThread), itk::ThreadIdType itkNotUsed(threadId))
208 {
209  const TDisparityImage* horizDisp = this->GetHorizontalDisparityMapInput();
210  const TDisparityImage* vertiDisp = this->GetVerticalDisparityMapInput();
211 
212  const TMaskImage* disparityMask = this->GetDisparityMaskInput();
213 
214  TOutputImage* outputDEM = this->GetOutput();
215 
216  // Get epipolar grids
217  const TEpipolarGridImage* leftGrid = this->GetLeftEpipolarGridInput();
218  const TEpipolarGridImage* rightGrid = this->GetRightEpipolarGridInput();
219 
220  typename TEpipolarGridImage::RegionType gridRegion = leftGrid->GetLargestPossibleRegion();
221 
222  typename TOutputImage::RegionType outputRequestedRegion = outputDEM->GetRequestedRegion();
223 
224  itk::ImageRegionIterator<OutputImageType> demIt(outputDEM, outputRequestedRegion);
225  itk::ImageRegionConstIteratorWithIndex<DisparityMapType> horizIt(horizDisp, outputRequestedRegion);
226 
227  demIt.GoToBegin();
228  horizIt.GoToBegin();
229 
230  bool useVerti = false;
231  itk::ImageRegionConstIteratorWithIndex<DisparityMapType> vertiIt;
232  if (vertiDisp)
233  {
234  useVerti = true;
235  vertiIt = itk::ImageRegionConstIteratorWithIndex<DisparityMapType>(vertiDisp, outputRequestedRegion);
236  vertiIt.GoToBegin();
237  }
238 
239  bool useMask = false;
240  itk::ImageRegionConstIterator<MaskImageType> maskIt;
241  if (disparityMask)
242  {
243  useMask = true;
244  maskIt = itk::ImageRegionConstIterator<MaskImageType>(disparityMask, outputRequestedRegion);
245  maskIt.GoToBegin();
246  }
247 
248  double elevationMin = 0.0;
249  double elevationMax = 300.0;
250 
251  typename OptimizerType::Pointer optimizer = OptimizerType::New();
252 
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);
262 
263  typename GridImageType::PointType ulPoint;
264  typename GridImageType::PointType urPoint;
265  typename GridImageType::PointType lrPoint;
266  typename GridImageType::PointType llPoint;
267 
268  TDPointType sensorPoint;
269  TDPointType leftGroundHmin;
270  TDPointType leftGroundHmax;
271  TDPointType rightGroundHmin;
272  TDPointType rightGroundHmax;
273 
274  while (!demIt.IsAtEnd() && !horizIt.IsAtEnd())
275  {
276  // check mask value if any
277  if (useMask)
278  {
279  if (!(maskIt.Get() > 0))
280  {
281  // TODO : what to do when masked ? put a no-data value ?
282  typename OutputImageType::PixelType pixel3D(3);
283  pixel3D.Fill(0);
284  demIt.Set(pixel3D);
285 
286  ++demIt;
287  ++horizIt;
288  if (useVerti)
289  ++vertiIt;
290  ++maskIt;
291  continue;
292  }
293  }
294 
295  // compute left ray
296  horizDisp->TransformIndexToPhysicalPoint(horizIt.GetIndex(), epiPoint);
297  leftGrid->TransformPhysicalPointToContinuousIndex(epiPoint, gridIndexConti);
298 
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))
306  {
307  ulIndex[0] = gridRegion.GetIndex(0) + gridRegion.GetSize(0) - 2;
308  }
309  if (ulIndex[1] > (gridRegion.GetIndex(1) + static_cast<int>(gridRegion.GetSize(1)) - 2))
310  {
311  ulIndex[1] = gridRegion.GetIndex(1) + gridRegion.GetSize(1) - 2;
312  }
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]);
321 
322  leftGrid->TransformIndexToPhysicalPoint(ulIndex, ulPoint);
323  leftGrid->TransformIndexToPhysicalPoint(urIndex, urPoint);
324  leftGrid->TransformIndexToPhysicalPoint(lrIndex, lrPoint);
325  leftGrid->TransformIndexToPhysicalPoint(llIndex, llPoint);
326 
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];
337 
338  sensorPoint[0] = cPixel[0];
339  sensorPoint[1] = cPixel[1];
340  sensorPoint[2] = elevationMin;
341  leftGroundHmin = m_LeftToGroundTransform->TransformPoint(sensorPoint);
342 
343  sensorPoint[2] = elevationMax;
344  leftGroundHmax = m_LeftToGroundTransform->TransformPoint(sensorPoint);
345 
346  // compute right ray
347  itk::ContinuousIndex<double, 2> rightIndexEstimate;
348  rightIndexEstimate[0] = static_cast<double>((horizIt.GetIndex())[0]) + static_cast<double>(horizIt.Get());
349 
350  double verticalShift = 0;
351  if (useVerti)
352  verticalShift = static_cast<double>(vertiIt.Get());
353  rightIndexEstimate[1] = static_cast<double>((horizIt.GetIndex())[1]) + verticalShift;
354 
355  horizDisp->TransformContinuousIndexToPhysicalPoint(rightIndexEstimate, epiPoint);
356  rightGrid->TransformPhysicalPointToContinuousIndex(epiPoint, gridIndexConti);
357 
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))
365  {
366  ulIndex[0] = gridRegion.GetIndex(0) + gridRegion.GetSize(0) - 2;
367  }
368  if (ulIndex[1] > (gridRegion.GetIndex(1) + static_cast<int>(gridRegion.GetSize(1)) - 2))
369  {
370  ulIndex[1] = gridRegion.GetIndex(1) + gridRegion.GetSize(1) - 2;
371  }
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]);
380 
381  rightGrid->TransformIndexToPhysicalPoint(ulIndex, ulPoint);
382  rightGrid->TransformIndexToPhysicalPoint(urIndex, urPoint);
383  rightGrid->TransformIndexToPhysicalPoint(lrIndex, lrPoint);
384  rightGrid->TransformIndexToPhysicalPoint(llIndex, llPoint);
385 
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];
396 
397  sensorPoint[0] = cPixel[0];
398  sensorPoint[1] = cPixel[1];
399  sensorPoint[2] = elevationMin;
400  rightGroundHmin = m_RightToGroundTransform->TransformPoint(sensorPoint);
401 
402  sensorPoint[2] = elevationMax;
403  rightGroundHmax = m_RightToGroundTransform->TransformPoint(sensorPoint);
404 
405  // Compute ray intersection with the generic line of sight optimizer
406  typename PointSetType::Pointer pointSetA = PointSetType::New();
407  typename PointSetType::Pointer pointSetB = PointSetType::New();
408 
409  pointSetA->SetPoint(0, leftGroundHmax);
410  pointSetA->SetPoint(1, rightGroundHmax);
411  pointSetA->SetPointData(0, 0);
412  pointSetA->SetPointData(1, 1);
413 
414  pointSetB->SetPoint(0, leftGroundHmin);
415  pointSetB->SetPoint(1, rightGroundHmin);
416  pointSetB->SetPointData(0, 0);
417  pointSetB->SetPointData(1, 1);
418 
419  TDPointType midPoint3D = optimizer->Compute(pointSetA, pointSetB);
420 
421  // record 3D point
422  typename OutputImageType::PixelType pixel3D(3);
423  pixel3D[0] = midPoint3D[0];
424  pixel3D[1] = midPoint3D[1];
425  pixel3D[2] = midPoint3D[2];
426  demIt.Set(pixel3D);
427 
428  ++demIt;
429  ++horizIt;
430 
431  if (useVerti)
432  ++vertiIt;
433  if (useMask)
434  ++maskIt;
435  }
436 }
437 }
438 
439 #endif
otb::DisparityMapTo3DFilter::SetLeftEpipolarGridInput
void SetLeftEpipolarGridInput(const TEpipolarGridImage *grid)
Definition: otbDisparityMapTo3DFilter.hxx:58
otb::LineOfSightOptimizer::Pointer
itk::SmartPointer< Self > Pointer
Definition: otbLineOfSightOptimizer.h:48
otb::DisparityMapTo3DFilter::ThreadedGenerateData
void ThreadedGenerateData(const RegionType &outputRegionForThread, itk::ThreadIdType threadId) override
Definition: otbDisparityMapTo3DFilter.hxx:206
otbDisparityMapTo3DFilter.h
otb::DisparityMapTo3DFilter::SetHorizontalDisparityMapInput
void SetHorizontalDisparityMapInput(const TDisparityImage *hmap)
Definition: otbDisparityMapTo3DFilter.hxx:44
otb::DisparityMapTo3DFilter::TDPointType
RSTransformType::InputPointType TDPointType
Definition: otbDisparityMapTo3DFilter.h:83
otb::DisparityMapTo3DFilter::GenerateInputRequestedRegion
void GenerateInputRequestedRegion() override
Definition: otbDisparityMapTo3DFilter.hxx:143
otb::DisparityMapTo3DFilter::GenerateOutputInformation
void GenerateOutputInformation() override
Definition: otbDisparityMapTo3DFilter.hxx:129
otb
The "otb" namespace contains all Orfeo Toolbox (OTB) classes.
Definition: otbJoinContainer.h:32
otb::DisparityMapTo3DFilter::GetVerticalDisparityMapInput
const TDisparityImage * GetVerticalDisparityMapInput() const
Definition: otbDisparityMapTo3DFilter.hxx:89
otb::DisparityMapTo3DFilter::RegionType
OutputImageType::RegionType RegionType
Definition: otbDisparityMapTo3DFilter.h:74
otb::DisparityMapTo3DFilter::GetRightEpipolarGridInput
const TEpipolarGridImage * GetRightEpipolarGridInput() const
Definition: otbDisparityMapTo3DFilter.hxx:109
otb::DisparityMapTo3DFilter::SetDisparityMaskInput
void SetDisparityMaskInput(const TMaskImage *mask)
Definition: otbDisparityMapTo3DFilter.hxx:72
otb::DisparityMapTo3DFilter::SetVerticalDisparityMapInput
void SetVerticalDisparityMapInput(const TDisparityImage *vmap)
Definition: otbDisparityMapTo3DFilter.hxx:51
otb::DisparityMapTo3DFilter::SetRightEpipolarGridInput
void SetRightEpipolarGridInput(const TEpipolarGridImage *grid)
Definition: otbDisparityMapTo3DFilter.hxx:65
otb::DisparityMapTo3DFilter::DisparityMapTo3DFilter
DisparityMapTo3DFilter()
Definition: otbDisparityMapTo3DFilter.hxx:32
otb::DisparityMapTo3DFilter::GetLeftEpipolarGridInput
const TEpipolarGridImage * GetLeftEpipolarGridInput() const
Definition: otbDisparityMapTo3DFilter.hxx:99
otb::DisparityMapTo3DFilter::BeforeThreadedGenerateData
void BeforeThreadedGenerateData() override
Definition: otbDisparityMapTo3DFilter.hxx:192
otb::DisparityMapTo3DFilter::GetHorizontalDisparityMapInput
const TDisparityImage * GetHorizontalDisparityMapInput() const
Definition: otbDisparityMapTo3DFilter.hxx:79
otb::DisparityMapTo3DFilter::GetDisparityMaskInput
const TMaskImage * GetDisparityMaskInput() const
Definition: otbDisparityMapTo3DFilter.hxx:119