OTB  6.7.0
Orfeo Toolbox
otbDisparityMapTo3DFilter.hxx
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2005-2019 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 
26 #include "itkImageRegionIterator.h"
27 
28 namespace otb
29 {
30 
31 template <class TDisparityImage, class TOutputImage,
32 class TEpipolarGridImage, class TMaskImage>
35 {
36  // Set the number of inputs
37  this->SetNumberOfRequiredInputs(5);
38  this->SetNumberOfRequiredInputs(1);
39 
40  // Set the outputs
41  this->SetNumberOfRequiredOutputs(1);
42  this->SetNthOutput(0,TOutputImage::New());
43 }
44 
45 template <class TDisparityImage, class TOutputImage,
46 class TEpipolarGridImage, class TMaskImage>
49 {}
50 
51 template <class TDisparityImage, class TOutputImage,
52 class TEpipolarGridImage, class TMaskImage>
53 void
55 ::SetHorizontalDisparityMapInput( const TDisparityImage * hmap)
56 {
57  // Process object is not const-correct so the const casting is required.
58  this->SetNthInput(0, const_cast<TDisparityImage *>( hmap ));
59 }
60 
61 template <class TDisparityImage, class TOutputImage,
62 class TEpipolarGridImage, class TMaskImage>
63 void
65 ::SetVerticalDisparityMapInput( const TDisparityImage * vmap)
66 {
67  // Process object is not const-correct so the const casting is required.
68  this->SetNthInput(1, const_cast<TDisparityImage *>( vmap ));
69 }
70 
71 template <class TDisparityImage, class TOutputImage,
72 class TEpipolarGridImage, class TMaskImage>
73 void
75 ::SetLeftEpipolarGridInput( const TEpipolarGridImage * grid)
76 {
77  // Process object is not const-correct so the const casting is required.
78  this->SetNthInput(2, const_cast<TEpipolarGridImage *>( grid ));
79 }
80 
81 template <class TDisparityImage, class TOutputImage,
82 class TEpipolarGridImage, class TMaskImage>
83 void
85 ::SetRightEpipolarGridInput( const TEpipolarGridImage * grid)
86 {
87  // Process object is not const-correct so the const casting is required.
88  this->SetNthInput(3, const_cast<TEpipolarGridImage *>( grid ));
89 }
90 
91 template <class TDisparityImage, class TOutputImage,
92 class TEpipolarGridImage, class TMaskImage>
93 void
95 ::SetDisparityMaskInput( const TMaskImage * mask)
96 {
97  // Process object is not const-correct so the const casting is required.
98  this->SetNthInput(4, const_cast<TMaskImage *>( mask ));
99 }
100 
101 template <class TDisparityImage, class TOutputImage,
102 class TEpipolarGridImage, class TMaskImage>
103 const TDisparityImage *
106 {
107  if(this->GetNumberOfInputs()<1)
108  {
109  return nullptr;
110  }
111  return static_cast<const TDisparityImage *>(this->itk::ProcessObject::GetInput(0));
112 }
113 
114 template <class TDisparityImage, class TOutputImage,
115 class TEpipolarGridImage, class TMaskImage>
116 const TDisparityImage *
119 {
120  if(this->GetNumberOfInputs()<2)
121  {
122  return nullptr;
123  }
124  return static_cast<const TDisparityImage *>(this->itk::ProcessObject::GetInput(1));
125 }
126 
127 template <class TDisparityImage, class TOutputImage,
128 class TEpipolarGridImage, class TMaskImage>
129 const TEpipolarGridImage *
132 {
133  if(this->GetNumberOfInputs()<3)
134  {
135  return nullptr;
136  }
137  return static_cast<const TEpipolarGridImage *>(this->itk::ProcessObject::GetInput(2));
138 }
139 
140 template <class TDisparityImage, class TOutputImage,
141 class TEpipolarGridImage, class TMaskImage>
142 const TEpipolarGridImage *
145 {
146  if(this->GetNumberOfInputs()<4)
147  {
148  return nullptr;
149  }
150  return static_cast<const TEpipolarGridImage *>(this->itk::ProcessObject::GetInput(3));
151 }
152 
153 template <class TDisparityImage, class TOutputImage,
154 class TEpipolarGridImage, class TMaskImage>
155 const TMaskImage *
158 {
159  if(this->GetNumberOfInputs()<5)
160  {
161  return nullptr;
162  }
163  return static_cast<const TMaskImage *>(this->itk::ProcessObject::GetInput(4));
164 }
165 
166 template <class TDisparityImage, class TOutputImage,
167 class TEpipolarGridImage, class TMaskImage>
168 void
171 {
172  const TDisparityImage * horizDisp = this->GetHorizontalDisparityMapInput();
173  TOutputImage * outputPtr = this->GetOutput();
174 
175  outputPtr->SetLargestPossibleRegion(horizDisp->GetLargestPossibleRegion());
176  outputPtr->SetNumberOfComponentsPerPixel(3);
177 
178  // copy also origin and spacing
179  outputPtr->SetOrigin(horizDisp->GetOrigin());
180  outputPtr->SetSignedSpacing(horizDisp->GetSignedSpacing());
181 }
182 
183 template <class TDisparityImage, class TOutputImage,
184 class TEpipolarGridImage, class TMaskImage>
185 void
188 {
189  // For the epi grid : generate full buffer here !
190  TEpipolarGridImage * leftGrid = const_cast<TEpipolarGridImage*>(this->GetLeftEpipolarGridInput());
191  TEpipolarGridImage * rightGrid = const_cast<TEpipolarGridImage*>(this->GetRightEpipolarGridInput());
192 
193  leftGrid->SetRequestedRegionToLargestPossibleRegion();
194  rightGrid->SetRequestedRegionToLargestPossibleRegion();
195 
196  TOutputImage * outputDEM = this->GetOutput();
197 
198  TDisparityImage * horizDisp = const_cast<TDisparityImage*>(this->GetHorizontalDisparityMapInput());
199  TDisparityImage * vertiDisp = const_cast<TDisparityImage*>(this->GetVerticalDisparityMapInput());
200  TMaskImage * maskDisp = const_cast<TMaskImage*>(this->GetDisparityMaskInput());
201 
202  // We impose that both disparity map inputs have the same size
203  if(vertiDisp &&
204  horizDisp->GetLargestPossibleRegion() != vertiDisp->GetLargestPossibleRegion())
205  {
206  itkExceptionMacro(<<"Horizontal and vertical disparity maps do not have the same size ! Horizontal largest region: "
207  <<horizDisp->GetLargestPossibleRegion()<<", vertical largest region: "<<vertiDisp->GetLargestPossibleRegion());
208  }
209 
210 
211  if (maskDisp && horizDisp->GetLargestPossibleRegion() != maskDisp->GetLargestPossibleRegion())
212  {
213  itkExceptionMacro(<<"Disparity map and mask do not have the same size ! Map region : "
214  <<horizDisp->GetLargestPossibleRegion()<<", mask region : "<<maskDisp->GetLargestPossibleRegion());
215  }
216 
217  horizDisp->SetRequestedRegion( outputDEM->GetRequestedRegion() );
218 
219  if (vertiDisp)
220  {
221  vertiDisp->SetRequestedRegion( outputDEM->GetRequestedRegion() );
222  }
223 
224  if (maskDisp)
225  {
226  maskDisp->SetRequestedRegion( outputDEM->GetRequestedRegion() );
227  }
228 
229  // Check that the keywordlists are not empty
230  if (m_LeftKeywordList.GetSize() == 0 || m_RightKeywordList.GetSize() == 0)
231  {
232  itkExceptionMacro(<<"At least one of the image keywordlist is empty : can't instantiate corresponding projection");
233  }
234 }
235 
236 template <class TDisparityImage, class TOutputImage,
237 class TEpipolarGridImage, class TMaskImage>
238 void
241 {
242  // Instantiate transforms
243  m_LeftToGroundTransform = RSTransformType::New();
244  m_RightToGroundTransform = RSTransformType::New();
245 
246  m_LeftToGroundTransform->SetInputKeywordList(m_LeftKeywordList);
247  m_RightToGroundTransform->SetInputKeywordList(m_RightKeywordList);
248 
249  m_LeftToGroundTransform->InstantiateTransform();
250  m_RightToGroundTransform->InstantiateTransform();
251 }
252 
253 template <class TDisparityImage, class TOutputImage,
254 class TEpipolarGridImage, class TMaskImage>
255 void
257 ::ThreadedGenerateData(const RegionType & itkNotUsed(outputRegionForThread), itk::ThreadIdType itkNotUsed(threadId))
258 {
259  const TDisparityImage * horizDisp = this->GetHorizontalDisparityMapInput();
260  const TDisparityImage * vertiDisp = this->GetVerticalDisparityMapInput();
261 
262  const TMaskImage * disparityMask = this->GetDisparityMaskInput();
263 
264  TOutputImage * outputDEM = this->GetOutput();
265 
266  // Get epipolar grids
267  const TEpipolarGridImage * leftGrid = this->GetLeftEpipolarGridInput();
268  const TEpipolarGridImage * rightGrid = this->GetRightEpipolarGridInput();
269 
270  typename TEpipolarGridImage::RegionType gridRegion = leftGrid->GetLargestPossibleRegion();
271 
272  typename TOutputImage::RegionType outputRequestedRegion = outputDEM->GetRequestedRegion();
273 
274  itk::ImageRegionIterator<OutputImageType> demIt(outputDEM,outputRequestedRegion);
275  itk::ImageRegionConstIteratorWithIndex<DisparityMapType> horizIt(horizDisp,outputRequestedRegion);
276 
277  demIt.GoToBegin();
278  horizIt.GoToBegin();
279 
280  bool useVerti = false;
282  if (vertiDisp)
283  {
284  useVerti = true;
285  vertiIt = itk::ImageRegionConstIteratorWithIndex<DisparityMapType>(vertiDisp,outputRequestedRegion);
286  vertiIt.GoToBegin();
287  }
288 
289  bool useMask = false;
291  if (disparityMask)
292  {
293  useMask = true;
294  maskIt = itk::ImageRegionConstIterator<MaskImageType>(disparityMask,outputRequestedRegion);
295  maskIt.GoToBegin();
296  }
297 
298  double elevationMin = 0.0;
299  double elevationMax = 300.0;
300 
301  typename OptimizerType::Pointer optimizer = OptimizerType::New();
302 
303  typename TDisparityImage::PointType epiPoint;
304  itk::ContinuousIndex<double,2> gridIndexConti;
305  double subPixIndex[2];
306  typename GridImageType::IndexType ulIndex, urIndex, lrIndex, llIndex;
307  typename GridImageType::PixelType ulPixel(2);
308  typename GridImageType::PixelType urPixel(2);
309  typename GridImageType::PixelType lrPixel(2);
310  typename GridImageType::PixelType llPixel(2);
311  typename GridImageType::PixelType cPixel(2);
312 
313  typename GridImageType::PointType ulPoint;
314  typename GridImageType::PointType urPoint;
315  typename GridImageType::PointType lrPoint;
316  typename GridImageType::PointType llPoint;
317 
318  TDPointType sensorPoint;
319  TDPointType leftGroundHmin;
320  TDPointType leftGroundHmax;
321  TDPointType rightGroundHmin;
322  TDPointType rightGroundHmax;
323 
324  while (!demIt.IsAtEnd() && !horizIt.IsAtEnd())
325  {
326  // check mask value if any
327  if (useMask)
328  {
329  if (!(maskIt.Get() > 0))
330  {
331  // TODO : what to do when masked ? put a no-data value ?
332  typename OutputImageType::PixelType pixel3D(3);
333  pixel3D.Fill(0);
334  demIt.Set(pixel3D);
335 
336  ++demIt;
337  ++horizIt;
338  if (useVerti) ++vertiIt;
339  ++maskIt;
340  continue;
341  }
342  }
343 
344  // compute left ray
345  horizDisp->TransformIndexToPhysicalPoint(horizIt.GetIndex(),epiPoint);
346  leftGrid->TransformPhysicalPointToContinuousIndex(epiPoint,gridIndexConti);
347 
348  ulIndex[0] = static_cast<int>(std::floor(gridIndexConti[0]));
349  ulIndex[1] = static_cast<int>(std::floor(gridIndexConti[1]));
350  if (ulIndex[0] < gridRegion.GetIndex(0)) ulIndex[0] = gridRegion.GetIndex(0);
351  if (ulIndex[1] < gridRegion.GetIndex(1)) ulIndex[1] = gridRegion.GetIndex(1);
352  if (ulIndex[0] > (gridRegion.GetIndex(0) + static_cast<int>(gridRegion.GetSize(0)) - 2))
353  {
354  ulIndex[0] = gridRegion.GetIndex(0) + gridRegion.GetSize(0) - 2;
355  }
356  if (ulIndex[1] > (gridRegion.GetIndex(1) + static_cast<int>(gridRegion.GetSize(1)) - 2))
357  {
358  ulIndex[1] = gridRegion.GetIndex(1) + gridRegion.GetSize(1) - 2;
359  }
360  urIndex[0] = ulIndex[0] + 1;
361  urIndex[1] = ulIndex[1];
362  lrIndex[0] = ulIndex[0] + 1;
363  lrIndex[1] = ulIndex[1] + 1;
364  llIndex[0] = ulIndex[0];
365  llIndex[1] = ulIndex[1] + 1;
366  subPixIndex[0] = gridIndexConti[0] - static_cast<double>(ulIndex[0]);
367  subPixIndex[1] = gridIndexConti[1] - static_cast<double>(ulIndex[1]);
368 
369  leftGrid->TransformIndexToPhysicalPoint(ulIndex, ulPoint);
370  leftGrid->TransformIndexToPhysicalPoint(urIndex, urPoint);
371  leftGrid->TransformIndexToPhysicalPoint(lrIndex, lrPoint);
372  leftGrid->TransformIndexToPhysicalPoint(llIndex, llPoint);
373 
374  ulPixel[0] = (leftGrid->GetPixel(ulIndex))[0] + ulPoint[0];
375  ulPixel[1] = (leftGrid->GetPixel(ulIndex))[1] + ulPoint[1];
376  urPixel[0] = (leftGrid->GetPixel(urIndex))[0] + urPoint[0];
377  urPixel[1] = (leftGrid->GetPixel(urIndex))[1] + urPoint[1];
378  lrPixel[0] = (leftGrid->GetPixel(lrIndex))[0] + lrPoint[0];
379  lrPixel[1] = (leftGrid->GetPixel(lrIndex))[1] + lrPoint[1];
380  llPixel[0] = (leftGrid->GetPixel(llIndex))[0] + llPoint[0];
381  llPixel[1] = (leftGrid->GetPixel(llIndex))[1] + llPoint[1];
382  cPixel = (ulPixel * (1.0 - subPixIndex[0]) + urPixel * subPixIndex[0]) * (1.0 - subPixIndex[1]) +
383  (llPixel * (1.0 - subPixIndex[0]) + lrPixel * subPixIndex[0]) * subPixIndex[1];
384 
385  sensorPoint[0] = cPixel[0];
386  sensorPoint[1] = cPixel[1];
387  sensorPoint[2] = elevationMin;
388  leftGroundHmin = m_LeftToGroundTransform->TransformPoint(sensorPoint);
389 
390  sensorPoint[2] = elevationMax;
391  leftGroundHmax = m_LeftToGroundTransform->TransformPoint(sensorPoint);
392 
393  // compute right ray
394  itk::ContinuousIndex<double,2> rightIndexEstimate;
395  rightIndexEstimate[0] = static_cast<double>((horizIt.GetIndex())[0]) + static_cast<double>(horizIt.Get());
396 
397  double verticalShift = 0;
398  if (useVerti) verticalShift = static_cast<double>(vertiIt.Get());
399  rightIndexEstimate[1] = static_cast<double>((horizIt.GetIndex())[1]) + verticalShift;
400 
401  horizDisp->TransformContinuousIndexToPhysicalPoint(rightIndexEstimate,epiPoint);
402  rightGrid->TransformPhysicalPointToContinuousIndex(epiPoint,gridIndexConti);
403 
404  ulIndex[0] = static_cast<int>(std::floor(gridIndexConti[0]));
405  ulIndex[1] = static_cast<int>(std::floor(gridIndexConti[1]));
406  if (ulIndex[0] < gridRegion.GetIndex(0)) ulIndex[0] = gridRegion.GetIndex(0);
407  if (ulIndex[1] < gridRegion.GetIndex(1)) ulIndex[1] = gridRegion.GetIndex(1);
408  if (ulIndex[0] > (gridRegion.GetIndex(0) + static_cast<int>(gridRegion.GetSize(0)) - 2))
409  {
410  ulIndex[0] = gridRegion.GetIndex(0) + gridRegion.GetSize(0) - 2;
411  }
412  if (ulIndex[1] > (gridRegion.GetIndex(1) + static_cast<int>(gridRegion.GetSize(1)) - 2))
413  {
414  ulIndex[1] = gridRegion.GetIndex(1) + gridRegion.GetSize(1) - 2;
415  }
416  urIndex[0] = ulIndex[0] + 1;
417  urIndex[1] = ulIndex[1];
418  lrIndex[0] = ulIndex[0] + 1;
419  lrIndex[1] = ulIndex[1] + 1;
420  llIndex[0] = ulIndex[0];
421  llIndex[1] = ulIndex[1] + 1;
422  subPixIndex[0] = gridIndexConti[0] - static_cast<double>(ulIndex[0]);
423  subPixIndex[1] = gridIndexConti[1] - static_cast<double>(ulIndex[1]);
424 
425  rightGrid->TransformIndexToPhysicalPoint(ulIndex, ulPoint);
426  rightGrid->TransformIndexToPhysicalPoint(urIndex, urPoint);
427  rightGrid->TransformIndexToPhysicalPoint(lrIndex, lrPoint);
428  rightGrid->TransformIndexToPhysicalPoint(llIndex, llPoint);
429 
430  ulPixel[0] = (rightGrid->GetPixel(ulIndex))[0] + ulPoint[0];
431  ulPixel[1] = (rightGrid->GetPixel(ulIndex))[1] + ulPoint[1];
432  urPixel[0] = (rightGrid->GetPixel(urIndex))[0] + urPoint[0];
433  urPixel[1] = (rightGrid->GetPixel(urIndex))[1] + urPoint[1];
434  lrPixel[0] = (rightGrid->GetPixel(lrIndex))[0] + lrPoint[0];
435  lrPixel[1] = (rightGrid->GetPixel(lrIndex))[1] + lrPoint[1];
436  llPixel[0] = (rightGrid->GetPixel(llIndex))[0] + llPoint[0];
437  llPixel[1] = (rightGrid->GetPixel(llIndex))[1] + llPoint[1];
438  cPixel = (ulPixel * (1.0 - subPixIndex[0]) + urPixel * subPixIndex[0]) * (1.0 - subPixIndex[1]) +
439  (llPixel * (1.0 - subPixIndex[0]) + lrPixel * subPixIndex[0]) * subPixIndex[1];
440 
441  sensorPoint[0] = cPixel[0];
442  sensorPoint[1] = cPixel[1];
443  sensorPoint[2] = elevationMin;
444  rightGroundHmin = m_RightToGroundTransform->TransformPoint(sensorPoint);
445 
446  sensorPoint[2] = elevationMax;
447  rightGroundHmax = m_RightToGroundTransform->TransformPoint(sensorPoint);
448 
449  // Compute ray intersection with the generic line of sight optimizer
450  typename PointSetType::Pointer pointSetA = PointSetType::New();
451  typename PointSetType::Pointer pointSetB = PointSetType::New();
452 
453  pointSetA->SetPoint(0,leftGroundHmax);
454  pointSetA->SetPoint(1,rightGroundHmax);
455  pointSetA->SetPointData(0,0);
456  pointSetA->SetPointData(1,1);
457 
458  pointSetB->SetPoint(0,leftGroundHmin);
459  pointSetB->SetPoint(1,rightGroundHmin);
460  pointSetB->SetPointData(0,0);
461  pointSetB->SetPointData(1,1);
462 
463  TDPointType midPoint3D = optimizer->Compute(pointSetA,pointSetB);
464 
465  // record 3D point
466  typename OutputImageType::PixelType pixel3D(3);
467  pixel3D[0] = midPoint3D[0];
468  pixel3D[1] = midPoint3D[1];
469  pixel3D[2] = midPoint3D[2];
470  demIt.Set(pixel3D);
471 
472  ++demIt;
473  ++horizIt;
474 
475  if (useVerti) ++vertiIt;
476  if (useMask) ++maskIt;
477 
478  }
479 
480 }
481 
482 }
483 
484 #endif
void SetDisparityMaskInput(const TMaskImage *mask)
const TMaskImage * GetDisparityMaskInput() const
PixelType Get(void) const
void Set(const PixelType &value) const
const IndexType GetIndex() const
void SetHorizontalDisparityMapInput(const TDisparityImage *hmap)
void ThreadedGenerateData(const RegionType &outputRegionForThread, itk::ThreadIdType threadId) override
const IndexType & GetIndex() const
const TEpipolarGridImage * GetRightEpipolarGridInput() const
const TDisparityImage * GetHorizontalDisparityMapInput() const
itk::Index< Monteverdi_DIMENSION > IndexType
Definition: mvdTypes.h:133
void SetLeftEpipolarGridInput(const TEpipolarGridImage *grid)
unsigned int ThreadIdType
void SetRightEpipolarGridInput(const TEpipolarGridImage *grid)
DataObject * GetInput(const DataObjectIdentifierType &key)
OutputImageType::RegionType RegionType
bool IsAtEnd(void) const
const TEpipolarGridImage * GetLeftEpipolarGridInput() const
PixelType Get(void) const
VectorImageType::PointType PointType
Definition: mvdTypes.h:189
void SetVerticalDisparityMapInput(const TDisparityImage *vmap)
const TDisparityImage * GetVerticalDisparityMapInput() const