OTB  7.2.0
Orfeo Toolbox
otbDisparityMapToDEMFilter.hxx
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2005-2020 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 otbDisparityMapToDEMFilter_hxx
22 #define otbDisparityMapToDEMFilter_hxx
23 
25 #include "itkImageRegionConstIteratorWithIndex.h"
26 #include "itkImageRegionIterator.h"
27 
28 namespace otb
29 {
30 
31 template <class TDisparityImage, class TInputImage, class TOutputDEMImage, class TEpipolarGridImage, class TMaskImage>
33 {
34  // Set the number of inputs
35  this->SetNumberOfRequiredInputs(7);
36  this->SetNumberOfRequiredInputs(1);
37 
38  // Set the outputs
39  this->SetNumberOfRequiredOutputs(1);
40  this->SetNthOutput(0, TOutputDEMImage::New());
41 
42  // Default DEM reconstruction parameters
43  m_ElevationMin = 0.0;
44  m_ElevationMax = 100.0;
45  m_DEMGridStep = 10.0;
46 
47  m_InputSplitter = SplitterType::New();
48  m_UsedInputSplits = 1;
49 }
50 
51 template <class TDisparityImage, class TInputImage, class TOutputDEMImage, class TEpipolarGridImage, class TMaskImage>
53 {
54 }
55 
56 template <class TDisparityImage, class TInputImage, class TOutputDEMImage, class TEpipolarGridImage, class TMaskImage>
58  const TDisparityImage* hmap)
59 {
60  // Process object is not const-correct so the const casting is required.
61  this->SetNthInput(0, const_cast<TDisparityImage*>(hmap));
62 }
63 
64 template <class TDisparityImage, class TInputImage, class TOutputDEMImage, class TEpipolarGridImage, class TMaskImage>
66  const TDisparityImage* vmap)
67 {
68  // Process object is not const-correct so the const casting is required.
69  this->SetNthInput(1, const_cast<TDisparityImage*>(vmap));
70 }
71 
72 template <class TDisparityImage, class TInputImage, class TOutputDEMImage, class TEpipolarGridImage, class TMaskImage>
74 {
75  // Process object is not const-correct so the const casting is required.
76  this->SetNthInput(2, const_cast<TInputImage*>(image));
77 }
78 
79 template <class TDisparityImage, class TInputImage, class TOutputDEMImage, class TEpipolarGridImage, class TMaskImage>
81 {
82  // Process object is not const-correct so the const casting is required.
83  this->SetNthInput(3, const_cast<TInputImage*>(image));
84 }
85 
86 template <class TDisparityImage, class TInputImage, class TOutputDEMImage, class TEpipolarGridImage, class TMaskImage>
88  const TEpipolarGridImage* grid)
89 {
90  // Process object is not const-correct so the const casting is required.
91  this->SetNthInput(4, const_cast<TEpipolarGridImage*>(grid));
92 }
93 
94 template <class TDisparityImage, class TInputImage, class TOutputDEMImage, class TEpipolarGridImage, class TMaskImage>
96  const TEpipolarGridImage* grid)
97 {
98  // Process object is not const-correct so the const casting is required.
99  this->SetNthInput(5, const_cast<TEpipolarGridImage*>(grid));
100 }
101 
102 template <class TDisparityImage, class TInputImage, class TOutputDEMImage, class TEpipolarGridImage, class TMaskImage>
104 {
105  // Process object is not const-correct so the const casting is required.
106  this->SetNthInput(6, const_cast<TMaskImage*>(mask));
107 }
108 
109 template <class TDisparityImage, class TInputImage, class TOutputDEMImage, class TEpipolarGridImage, class TMaskImage>
110 const TDisparityImage*
112 {
113  if (this->GetNumberOfInputs() < 1)
114  {
115  return nullptr;
116  }
117  return static_cast<const TDisparityImage*>(this->itk::ProcessObject::GetInput(0));
118 }
119 
120 template <class TDisparityImage, class TInputImage, class TOutputDEMImage, class TEpipolarGridImage, class TMaskImage>
121 const TDisparityImage*
123 {
124  if (this->GetNumberOfInputs() < 2)
125  {
126  return nullptr;
127  }
128  return static_cast<const TDisparityImage*>(this->itk::ProcessObject::GetInput(1));
129 }
130 
131 template <class TDisparityImage, class TInputImage, class TOutputDEMImage, class TEpipolarGridImage, class TMaskImage>
133 {
134  if (this->GetNumberOfInputs() < 3)
135  {
136  return nullptr;
137  }
138  return static_cast<const TInputImage*>(this->itk::ProcessObject::GetInput(2));
139 }
140 
141 template <class TDisparityImage, class TInputImage, class TOutputDEMImage, class TEpipolarGridImage, class TMaskImage>
143 {
144  if (this->GetNumberOfInputs() < 4)
145  {
146  return nullptr;
147  }
148  return static_cast<const TInputImage*>(this->itk::ProcessObject::GetInput(3));
149 }
150 
151 template <class TDisparityImage, class TInputImage, class TOutputDEMImage, class TEpipolarGridImage, class TMaskImage>
152 const TEpipolarGridImage*
154 {
155  if (this->GetNumberOfInputs() < 5)
156  {
157  return nullptr;
158  }
159  return static_cast<const TEpipolarGridImage*>(this->itk::ProcessObject::GetInput(4));
160 }
161 
162 template <class TDisparityImage, class TInputImage, class TOutputDEMImage, class TEpipolarGridImage, class TMaskImage>
163 const TEpipolarGridImage*
165 {
166  if (this->GetNumberOfInputs() < 6)
167  {
168  return nullptr;
169  }
170  return static_cast<const TEpipolarGridImage*>(this->itk::ProcessObject::GetInput(5));
171 }
172 
173 template <class TDisparityImage, class TInputImage, class TOutputDEMImage, class TEpipolarGridImage, class TMaskImage>
175 {
176  if (this->GetNumberOfInputs() < 7)
177  {
178  return nullptr;
179  }
180  return static_cast<const TMaskImage*>(this->itk::ProcessObject::GetInput(6));
181 }
182 
183 template <class TDisparityImage, class TInputImage, class TOutputDEMImage, class TEpipolarGridImage, class TMaskImage>
185 {
186  if (this->GetNumberOfOutputs() < 1)
187  {
188  return 0;
189  }
190  return static_cast<const TOutputDEMImage*>(this->itk::ProcessObject::GetOutput(0));
191 }
192 
193 template <class TDisparityImage, class TInputImage, class TOutputDEMImage, class TEpipolarGridImage, class TMaskImage>
195 {
196  if (this->GetNumberOfOutputs() < 1)
197  {
198  return nullptr;
199  }
200  return static_cast<TOutputDEMImage*>(this->itk::ProcessObject::GetOutput(0));
201 }
202 
203 template <class TDisparityImage, class TInputImage, class TOutputDEMImage, class TEpipolarGridImage, class TMaskImage>
205 {
206  // Get common area between images
207  const TInputImage* leftImgPtr = this->GetLeftInput();
208  const TInputImage* rightImgPtr = this->GetRightInput();
209  TOutputDEMImage* outputPtr = this->GetDEMOutput();
210 
211  // Set-up a transform to use the DEMHandler
212  typedef otb::GenericRSTransform<> RSTransform2DType;
213  RSTransform2DType::Pointer leftToGroundTransform = RSTransform2DType::New();
214  leftToGroundTransform->SetInputKeywordList(leftImgPtr->GetImageKeywordlist());
215  leftToGroundTransform->InstantiateTransform();
216 
217  RSTransform2DType::Pointer rightToGroundTransform = RSTransform2DType::New();
218  rightToGroundTransform->SetInputKeywordList(rightImgPtr->GetImageKeywordlist());
219  rightToGroundTransform->InstantiateTransform();
220 
221  // left image
222  typename SensorImageType::SizeType inputSize = leftImgPtr->GetLargestPossibleRegion().GetSize();
223  typename SensorImageType::PointType ulp, urp, llp, lrp;
224  itk::ContinuousIndex<double, 2> ul(leftImgPtr->GetLargestPossibleRegion().GetIndex());
225  ul[0] += -0.5;
226  ul[1] += -0.5;
227 
228  itk::ContinuousIndex<double, 2> ur(ul);
229  itk::ContinuousIndex<double, 2> lr(ul);
230  itk::ContinuousIndex<double, 2> ll(ul);
231  ur[0] += static_cast<double>(inputSize[0]);
232  lr[0] += static_cast<double>(inputSize[0]);
233  lr[1] += static_cast<double>(inputSize[1]);
234  ll[1] += static_cast<double>(inputSize[1]);
235 
236  leftImgPtr->TransformContinuousIndexToPhysicalPoint(ul, ulp);
237  leftImgPtr->TransformContinuousIndexToPhysicalPoint(ur, urp);
238  leftImgPtr->TransformContinuousIndexToPhysicalPoint(ll, llp);
239  leftImgPtr->TransformContinuousIndexToPhysicalPoint(lr, lrp);
240 
241  RSTransform2DType::OutputPointType left_ul, left_ur, left_ll, left_lr;
242  left_ul = leftToGroundTransform->TransformPoint(ulp);
243  left_ur = leftToGroundTransform->TransformPoint(urp);
244  left_ll = leftToGroundTransform->TransformPoint(llp);
245  left_lr = leftToGroundTransform->TransformPoint(lrp);
246 
247  // right image
248  inputSize = rightImgPtr->GetLargestPossibleRegion().GetSize();
249  ul = rightImgPtr->GetLargestPossibleRegion().GetIndex();
250  ul[0] += -0.5;
251  ul[1] += -0.5;
252  ur = ul;
253  lr = ul;
254  ll = ul;
255  ur[0] += static_cast<double>(inputSize[0]);
256  lr[0] += static_cast<double>(inputSize[0]);
257  lr[1] += static_cast<double>(inputSize[1]);
258  ll[1] += static_cast<double>(inputSize[1]);
259 
260  rightImgPtr->TransformContinuousIndexToPhysicalPoint(ul, ulp);
261  rightImgPtr->TransformContinuousIndexToPhysicalPoint(ur, urp);
262  rightImgPtr->TransformContinuousIndexToPhysicalPoint(ll, llp);
263  rightImgPtr->TransformContinuousIndexToPhysicalPoint(lr, lrp);
264 
265  RSTransform2DType::OutputPointType right_ul, right_ur, right_lr, right_ll;
266  right_ul = rightToGroundTransform->TransformPoint(ulp);
267  right_ur = rightToGroundTransform->TransformPoint(urp);
268  right_ll = rightToGroundTransform->TransformPoint(llp);
269  right_lr = rightToGroundTransform->TransformPoint(lrp);
270 
271  double left_xmin = std::min(std::min(std::min(left_ul[0], left_ur[0]), left_lr[0]), left_ll[0]);
272  double left_xmax = std::max(std::max(std::max(left_ul[0], left_ur[0]), left_lr[0]), left_ll[0]);
273  double left_ymin = std::min(std::min(std::min(left_ul[1], left_ur[1]), left_lr[1]), left_ll[1]);
274  double left_ymax = std::max(std::max(std::max(left_ul[1], left_ur[1]), left_lr[1]), left_ll[1]);
275 
276  double right_xmin = std::min(std::min(std::min(right_ul[0], right_ur[0]), right_lr[0]), right_ll[0]);
277  double right_xmax = std::max(std::max(std::max(right_ul[0], right_ur[0]), right_lr[0]), right_ll[0]);
278  double right_ymin = std::min(std::min(std::min(right_ul[1], right_ur[1]), right_lr[1]), right_ll[1]);
279  double right_ymax = std::max(std::max(std::max(right_ul[1], right_ur[1]), right_lr[1]), right_ll[1]);
280 
281  double box_xmin = std::max(left_xmin, right_xmin);
282  double box_xmax = std::min(left_xmax, right_xmax);
283  double box_ymin = std::max(left_ymin, right_ymin);
284  double box_ymax = std::min(left_ymax, right_ymax);
285 
286  if (box_xmin >= box_xmax || box_ymin >= box_ymax)
287  {
288  itkExceptionMacro(<< "Wrong reconstruction area, images don't overlap, check image corners");
289  }
290 
291  // Compute step :
292  // TODO : use a clean RS transform instead
293  typename TOutputDEMImage::SpacingType outSpacing;
294  outSpacing[0] = 57.295779513 * m_DEMGridStep / (6378137.0 * std::cos((box_ymin + box_ymax) * 0.5 * 0.01745329251));
295  outSpacing[1] = -57.295779513 * m_DEMGridStep / 6378137.0;
296  outputPtr->SetSignedSpacing(outSpacing);
297 
298  // Choose origin
299  typename TOutputDEMImage::PointType outOrigin;
300  outOrigin[0] = box_xmin + 0.5 * outSpacing[0];
301  outOrigin[1] = box_ymax + 0.5 * outSpacing[1];
302  outputPtr->SetOrigin(outOrigin);
303 
304  // Compute output size
305  typename DEMImageType::RegionType outRegion;
306  outRegion.SetIndex(0, 0);
307  outRegion.SetIndex(1, 0);
308  outRegion.SetSize(0, static_cast<unsigned int>((box_xmax - box_xmin) / std::abs(outSpacing[0])));
309  outRegion.SetSize(1, static_cast<unsigned int>((box_ymax - box_ymin) / std::abs(outSpacing[1])));
310 
311  outputPtr->SetLargestPossibleRegion(outRegion);
312  outputPtr->SetNumberOfComponentsPerPixel(1);
313 }
314 
315 template <class TDisparityImage, class TInputImage, class TOutputDEMImage, class TEpipolarGridImage, class TMaskImage>
317 {
318  // For the epi grid : generate full buffer here !
319  TEpipolarGridImage* leftGrid = const_cast<TEpipolarGridImage*>(this->GetLeftEpipolarGridInput());
320  TEpipolarGridImage* rightGrid = const_cast<TEpipolarGridImage*>(this->GetRightEpipolarGridInput());
321 
322  leftGrid->SetRequestedRegionToLargestPossibleRegion();
323  rightGrid->SetRequestedRegionToLargestPossibleRegion();
324 
325  leftGrid->UpdateOutputData();
326  rightGrid->UpdateOutputData();
327 
328  // For the input left-right images
329  // -> No need, only use metadata and keywordlist
330  const TInputImage* leftSensor = this->GetLeftInput();
331  TOutputDEMImage* outputDEM = this->GetDEMOutput();
332 
333  typename DEMImageType::RegionType outRegion = outputDEM->GetRequestedRegion();
334  typename DEMImageType::PointType outOrigin = outputDEM->GetOrigin();
335  typename DEMImageType::SpacingType outSpacing = outputDEM->GetSignedSpacing();
336 
337  RSTransformType::Pointer groundToLeftTransform = RSTransformType::New();
338  groundToLeftTransform->SetOutputKeywordList(leftSensor->GetImageKeywordlist());
339  groundToLeftTransform->InstantiateTransform();
340 
341  // For the disparity maps and mask
342  // Iterate over OutputRequestedRegion corners for elevation min and max
343  TDisparityImage* horizDisp = const_cast<TDisparityImage*>(this->GetHorizontalDisparityMapInput());
344  TDisparityImage* vertiDisp = const_cast<TDisparityImage*>(this->GetVerticalDisparityMapInput());
345  TMaskImage* maskDisp = const_cast<TMaskImage*>(this->GetDisparityMaskInput());
346 
347  // We impose that both disparity map inputs have the same size
348  if (horizDisp->GetLargestPossibleRegion() != vertiDisp->GetLargestPossibleRegion())
349  {
350  itkExceptionMacro(<< "Horizontal and vertical disparity maps do not have the same size ! Horizontal largest region: "
351  << horizDisp->GetLargestPossibleRegion() << ", vertical largest region: " << vertiDisp->GetLargestPossibleRegion());
352  }
353 
354  if (maskDisp && horizDisp->GetLargestPossibleRegion() != maskDisp->GetLargestPossibleRegion())
355  {
356  itkExceptionMacro(<< "Disparity map and mask do not have the same size ! Map region : " << horizDisp->GetLargestPossibleRegion()
357  << ", mask region : " << maskDisp->GetLargestPossibleRegion());
358  }
359 
360  // up left at elevation min
361  TDPointType corners[8];
362  corners[0][0] = outOrigin[0] + outSpacing[0] * (-0.5 + static_cast<double>(outRegion.GetIndex(0)));
363  corners[0][1] = outOrigin[1] + outSpacing[1] * (-0.5 + static_cast<double>(outRegion.GetIndex(1)));
364  corners[0][2] = m_ElevationMin;
365  // up left at elevation max
366  corners[1][0] = corners[0][0];
367  corners[1][1] = corners[0][1];
368  corners[1][2] = m_ElevationMax;
369  // up right at elevation min
370  corners[2][0] = corners[0][0] + outSpacing[0] * static_cast<double>(outRegion.GetSize(0));
371  corners[2][1] = corners[0][1];
372  corners[2][2] = m_ElevationMin;
373  // up right at elevation max
374  corners[3][0] = corners[2][0];
375  corners[3][1] = corners[2][1];
376  corners[3][2] = m_ElevationMax;
377  // low right at elevation min
378  corners[4][0] = corners[0][0] + outSpacing[0] * static_cast<double>(outRegion.GetSize(0));
379  corners[4][1] = corners[0][1] + outSpacing[1] * static_cast<double>(outRegion.GetSize(1));
380  corners[4][2] = m_ElevationMin;
381  // low right at elevation max
382  corners[5][0] = corners[4][0];
383  corners[5][1] = corners[4][1];
384  corners[5][2] = m_ElevationMax;
385  // low left at elevation min
386  corners[6][0] = corners[0][0];
387  corners[6][1] = corners[0][1] + outSpacing[1] * static_cast<double>(outRegion.GetSize(1));
388  corners[6][2] = m_ElevationMin;
389  // low left at elevation max
390  corners[7][0] = corners[6][0];
391  corners[7][1] = corners[6][1];
392  corners[7][2] = m_ElevationMax;
393 
394  double x_loc, y_loc;
395  double a_grid, b_grid;
396  double u_loc[2];
397  double v_loc[2];
398  double det;
399  typename GridImageType::IndexType gridIndex;
400  typename GridImageType::IndexType gridIndexU;
401  typename GridImageType::IndexType gridIndexV;
402  typename GridImageType::PointType gridPoint;
403  typename GridImageType::PointType gridPointU;
404  typename GridImageType::PointType gridPointV;
405  typename GridImageType::PixelType origLoc(2);
406  typename GridImageType::PixelType uUnitLoc(2);
407  typename GridImageType::PixelType vUnitLoc(2);
408 
409  typename GridImageType::RegionType gridRegion = leftGrid->GetLargestPossibleRegion();
410  itk::ContinuousIndex<double, 2> gridContiIndex;
411  typename DisparityMapType::PointType epiPosition;
412  itk::ContinuousIndex<double, 2> epiContiIndex;
413  double epiIndexMin[2];
414  double epiIndexMax[2];
415  int maxGridIndex[2];
416  int minGridIndex[2];
417 
418  epiIndexMin[0] = itk::NumericTraits<double>::Zero;
419  epiIndexMin[1] = itk::NumericTraits<double>::Zero;
420 
421  epiIndexMax[0] = itk::NumericTraits<double>::Zero;
422  epiIndexMax[1] = itk::NumericTraits<double>::Zero;
423 
424  maxGridIndex[0] = static_cast<int>(gridRegion.GetIndex(0) + gridRegion.GetSize(0)) - 2;
425  maxGridIndex[1] = static_cast<int>(gridRegion.GetIndex(1) + gridRegion.GetSize(1)) - 2;
426  minGridIndex[0] = static_cast<int>(gridRegion.GetIndex(0));
427  minGridIndex[1] = static_cast<int>(gridRegion.GetIndex(1));
428 
429  for (unsigned int k = 0; k < 8; k++)
430  {
431  // compute left image coordinate
432  TDPointType tmpSensor = groundToLeftTransform->TransformPoint(corners[k]);
433 
434  // compute epipolar position
435  gridIndex[0] = minGridIndex[0];
436  gridIndex[1] = minGridIndex[1];
437 
438  // we assume 3 iterations are enough to find the 4 surrounding pixels
439  for (unsigned int s = 0; s < 3; s++)
440  {
441  gridIndexU[0] = gridIndex[0] + 1;
442  gridIndexU[1] = gridIndex[1];
443 
444  gridIndexV[0] = gridIndex[0];
445  gridIndexV[1] = gridIndex[1] + 1;
446 
447  leftGrid->TransformIndexToPhysicalPoint(gridIndex, gridPoint);
448  leftGrid->TransformIndexToPhysicalPoint(gridIndexU, gridPointU);
449  leftGrid->TransformIndexToPhysicalPoint(gridIndexV, gridPointV);
450 
451  origLoc[0] = (leftGrid->GetPixel(gridIndex))[0] + gridPoint[0];
452  origLoc[1] = (leftGrid->GetPixel(gridIndex))[1] + gridPoint[1];
453  uUnitLoc[0] = (leftGrid->GetPixel(gridIndexU))[0] + gridPointU[0];
454  uUnitLoc[1] = (leftGrid->GetPixel(gridIndexU))[1] + gridPointU[1];
455  vUnitLoc[0] = (leftGrid->GetPixel(gridIndexV))[0] + gridPointV[0];
456  vUnitLoc[1] = (leftGrid->GetPixel(gridIndexV))[1] + gridPointV[1];
457 
458  u_loc[0] = static_cast<double>(uUnitLoc[0] - origLoc[0]);
459  u_loc[1] = static_cast<double>(uUnitLoc[1] - origLoc[1]);
460 
461  v_loc[0] = static_cast<double>(vUnitLoc[0] - origLoc[0]);
462  v_loc[1] = static_cast<double>(vUnitLoc[1] - origLoc[1]);
463 
464  det = u_loc[0] * v_loc[1] - v_loc[0] * u_loc[1];
465 
466  x_loc = static_cast<double>(tmpSensor[0]) - static_cast<double>(origLoc[0]);
467  y_loc = static_cast<double>(tmpSensor[1]) - static_cast<double>(origLoc[1]);
468 
469  a_grid = (x_loc * v_loc[1] - y_loc * v_loc[0]) / det;
470  b_grid = (y_loc * u_loc[0] - x_loc * u_loc[1]) / det;
471 
472  gridContiIndex[0] = static_cast<double>(gridIndex[0]) + a_grid;
473  gridContiIndex[1] = static_cast<double>(gridIndex[1]) + b_grid;
474 
475  leftGrid->TransformContinuousIndexToPhysicalPoint(gridContiIndex, epiPosition);
476 
477  horizDisp->TransformPhysicalPointToContinuousIndex(epiPosition, epiContiIndex);
478 
479  if (0.0 < a_grid && a_grid < 1.0 && 0.0 < b_grid && b_grid < 1.0)
480  {
481  // The four nearest positions in epipolar grid have been found : stop search
482  break;
483  }
484  else
485  {
486  // Shift the gridIndex
487  int a_grid_int = static_cast<int>(gridIndex[0]) + static_cast<int>(std::floor(a_grid));
488  int b_grid_int = static_cast<int>(gridIndex[1]) + static_cast<int>(std::floor(b_grid));
489 
490  if (a_grid_int < minGridIndex[0])
491  a_grid_int = minGridIndex[0];
492  if (a_grid_int > maxGridIndex[0])
493  a_grid_int = maxGridIndex[0];
494  if (b_grid_int < minGridIndex[1])
495  b_grid_int = minGridIndex[1];
496  if (b_grid_int > maxGridIndex[1])
497  b_grid_int = maxGridIndex[1];
498 
499  gridIndex[0] = a_grid_int;
500  gridIndex[1] = b_grid_int;
501  }
502  }
503 
504  if (k == 0)
505  {
506  epiIndexMin[0] = epiContiIndex[0];
507  epiIndexMin[1] = epiContiIndex[1];
508  epiIndexMax[0] = epiContiIndex[0];
509  epiIndexMax[1] = epiContiIndex[1];
510  }
511  else
512  {
513  if (epiContiIndex[0] < epiIndexMin[0])
514  epiIndexMin[0] = epiContiIndex[0];
515  if (epiContiIndex[1] < epiIndexMin[1])
516  epiIndexMin[1] = epiContiIndex[1];
517  if (epiIndexMax[0] < epiContiIndex[0])
518  epiIndexMax[0] = epiContiIndex[0];
519  if (epiIndexMax[1] < epiContiIndex[1])
520  epiIndexMax[1] = epiContiIndex[1];
521  }
522  }
523 
524  typename DisparityMapType::RegionType inputDisparityRegion;
525  inputDisparityRegion.SetIndex(0, static_cast<int>(std::floor(epiIndexMin[0] + 0.5)));
526  inputDisparityRegion.SetIndex(1, static_cast<int>(std::floor(epiIndexMin[1] + 0.5)));
527  inputDisparityRegion.SetSize(0, 1 + static_cast<unsigned int>(std::floor(epiIndexMax[0] - epiIndexMin[0] + 0.5)));
528  inputDisparityRegion.SetSize(1, 1 + static_cast<unsigned int>(std::floor(epiIndexMax[1] - epiIndexMin[1] + 0.5)));
529 
530  // crop the disparity region at the largest possible region
531  if (inputDisparityRegion.Crop(horizDisp->GetLargestPossibleRegion()))
532  {
533  horizDisp->SetRequestedRegion(inputDisparityRegion);
534  vertiDisp->SetRequestedRegion(inputDisparityRegion);
535 
536  if (maskDisp)
537  {
538  maskDisp->SetRequestedRegion(inputDisparityRegion);
539  }
540  }
541  else
542  {
543  // use empty region
544  typename DisparityMapType::RegionType emptyRegion;
545 
546  emptyRegion.SetIndex(horizDisp->GetLargestPossibleRegion().GetIndex());
547  emptyRegion.SetSize(0, 0);
548  emptyRegion.SetSize(1, 0);
549 
550  horizDisp->SetRequestedRegion(emptyRegion);
551  vertiDisp->SetRequestedRegion(emptyRegion);
552  if (maskDisp)
553  {
554  maskDisp->SetRequestedRegion(emptyRegion);
555  }
556 
557  // no exception : it can happen in this filter
558  if (0)
559  {
560  // Couldn't crop the region (requested region is outside the largest
561  // possible region). Throw an exception.
562  // store what we tried to request (prior to trying to crop)
563  horizDisp->SetRequestedRegion(inputDisparityRegion);
564  vertiDisp->SetRequestedRegion(inputDisparityRegion);
565 
566  // build an exception
567  itk::InvalidRequestedRegionError e(__FILE__, __LINE__);
568  std::ostringstream msg;
569  msg << this->GetNameOfClass() << "::GenerateInputRequestedRegion()";
570  e.SetLocation(msg.str());
571  e.SetDescription("Requested region is (at least partially) outside the largest possible region of disparity map.");
572  e.SetDataObject(horizDisp);
573  throw e;
574  }
575  }
576 }
577 
578 template <class TDisparityImage, class TInputImage, class TOutputDEMImage, class TEpipolarGridImage, class TMaskImage>
580 {
581  const TDisparityImage* horizDisp = this->GetHorizontalDisparityMapInput();
582  const TInputImage* leftSensor = this->GetLeftInput();
583  const TInputImage* rightSensor = this->GetRightInput();
584 
585  const TOutputDEMImage* outputDEM = this->GetDEMOutput();
586 
587  typename DisparityMapType::RegionType requestedRegion = horizDisp->GetRequestedRegion();
588 
589  m_UsedInputSplits = m_InputSplitter->GetNumberOfSplits(requestedRegion, this->GetNumberOfThreads());
590 
591  m_LeftToGroundTransform = RSTransformType::New();
592  m_RightToGroundTransform = RSTransformType::New();
593 
594  m_LeftToGroundTransform->SetInputKeywordList(leftSensor->GetImageKeywordlist());
595  m_RightToGroundTransform->SetInputKeywordList(rightSensor->GetImageKeywordlist());
596 
597  m_LeftToGroundTransform->InstantiateTransform();
598  m_RightToGroundTransform->InstantiateTransform();
599 
600  // ensure empty regions are not processed
601  if (requestedRegion.GetSize(0) == 0 && requestedRegion.GetSize(1) == 0)
602  {
603  m_UsedInputSplits = 0;
604  }
605 
606  if (m_UsedInputSplits <= static_cast<unsigned int>(this->GetNumberOfThreads()))
607  {
608  m_TempDEMRegions.clear();
609 
610  for (unsigned int i = 0; i < m_UsedInputSplits; i++)
611  {
612  typename DEMImageType::Pointer tmpImg = TOutputDEMImage::New();
613  tmpImg->SetNumberOfComponentsPerPixel(1);
614  tmpImg->SetRegions(outputDEM->GetRequestedRegion());
615  tmpImg->Allocate();
616 
617  typename DEMImageType::PixelType minElevation = static_cast<typename DEMImageType::PixelType>(m_ElevationMin);
618  tmpImg->FillBuffer(minElevation);
619 
620  m_TempDEMRegions.push_back(tmpImg);
621  }
622  }
623  else
624  {
625  itkExceptionMacro(<< "Wrong number of splits for input multithreading : " << m_UsedInputSplits);
626  }
627 }
628 
629 template <class TDisparityImage, class TInputImage, class TOutputDEMImage, class TEpipolarGridImage, class TMaskImage>
631  const RegionType& itkNotUsed(outputRegionForThread), itk::ThreadIdType threadId)
632 {
633  const TDisparityImage* horizDisp = this->GetHorizontalDisparityMapInput();
634  const TDisparityImage* vertiDisp = this->GetVerticalDisparityMapInput();
635 
636  const TMaskImage* disparityMask = this->GetDisparityMaskInput();
637 
638  const TOutputDEMImage* outputDEM = this->GetDEMOutput();
639 
640  const TEpipolarGridImage* leftGrid = this->GetLeftEpipolarGridInput();
641  const TEpipolarGridImage* rightGrid = this->GetRightEpipolarGridInput();
642 
643  typename TEpipolarGridImage::RegionType gridRegion = leftGrid->GetLargestPossibleRegion();
644 
645  TOutputDEMImage* tmpDEM = nullptr;
646  typename TOutputDEMImage::RegionType outputRequestedRegion = outputDEM->GetRequestedRegion();
647 
648  typename TDisparityImage::RegionType disparityRegion;
649  if (static_cast<unsigned int>(threadId) < m_UsedInputSplits)
650  {
651  disparityRegion = m_InputSplitter->GetSplit(threadId, m_UsedInputSplits, horizDisp->GetRequestedRegion());
652  tmpDEM = m_TempDEMRegions[threadId];
653  }
654  else
655  {
656  return;
657  }
658 
659  itk::ImageRegionConstIteratorWithIndex<DisparityMapType> horizIt(horizDisp, disparityRegion);
660  itk::ImageRegionConstIteratorWithIndex<DisparityMapType> vertiIt(vertiDisp, disparityRegion);
661 
662  horizIt.GoToBegin();
663  vertiIt.GoToBegin();
664 
665  bool useMask = false;
666  itk::ImageRegionConstIterator<MaskImageType> maskIt;
667  if (disparityMask)
668  {
669  useMask = true;
670  maskIt = itk::ImageRegionConstIterator<MaskImageType>(disparityMask, disparityRegion);
671  maskIt.GoToBegin();
672  }
673 
674  typename TDisparityImage::PointType epiPoint;
675  itk::ContinuousIndex<double, 2> gridIndexConti;
676  double subPixIndex[2];
677  typename GridImageType::IndexType ulIndex, urIndex, lrIndex, llIndex;
678  typename GridImageType::PixelType ulPixel(2);
679  typename GridImageType::PixelType urPixel(2);
680  typename GridImageType::PixelType lrPixel(2);
681  typename GridImageType::PixelType llPixel(2);
682  typename GridImageType::PixelType cPixel(2);
683 
684  typename GridImageType::PointType ulPoint;
685  typename GridImageType::PointType urPoint;
686  typename GridImageType::PointType lrPoint;
687  typename GridImageType::PointType llPoint;
688 
689  TDPointType sensorPoint;
690  TDPointType leftGroundHmin;
691  TDPointType leftGroundHmax;
692  TDPointType rightGroundHmin;
693  TDPointType rightGroundHmax;
694 
695  while (!horizIt.IsAtEnd() && !vertiIt.IsAtEnd())
696  {
697  // check mask value if any
698  if (useMask)
699  {
700  if (!(maskIt.Get() > 0))
701  {
702  ++horizIt;
703  ++vertiIt;
704  ++maskIt;
705  continue;
706  }
707  }
708 
709  // compute left ray
710  horizDisp->TransformIndexToPhysicalPoint(horizIt.GetIndex(), epiPoint);
711  leftGrid->TransformPhysicalPointToContinuousIndex(epiPoint, gridIndexConti);
712 
713  ulIndex[0] = static_cast<int>(std::floor(gridIndexConti[0]));
714  ulIndex[1] = static_cast<int>(std::floor(gridIndexConti[1]));
715  if (ulIndex[0] < gridRegion.GetIndex(0))
716  ulIndex[0] = gridRegion.GetIndex(0);
717  if (ulIndex[1] < gridRegion.GetIndex(1))
718  ulIndex[1] = gridRegion.GetIndex(1);
719  if (ulIndex[0] > (gridRegion.GetIndex(0) + static_cast<int>(gridRegion.GetSize(0)) - 2))
720  {
721  ulIndex[0] = gridRegion.GetIndex(0) + gridRegion.GetSize(0) - 2;
722  }
723  if (ulIndex[1] > (gridRegion.GetIndex(1) + static_cast<int>(gridRegion.GetSize(1)) - 2))
724  {
725  ulIndex[1] = gridRegion.GetIndex(1) + gridRegion.GetSize(1) - 2;
726  }
727  urIndex[0] = ulIndex[0] + 1;
728  urIndex[1] = ulIndex[1];
729  lrIndex[0] = ulIndex[0] + 1;
730  lrIndex[1] = ulIndex[1] + 1;
731  llIndex[0] = ulIndex[0];
732  llIndex[1] = ulIndex[1] + 1;
733  subPixIndex[0] = gridIndexConti[0] - static_cast<double>(ulIndex[0]);
734  subPixIndex[1] = gridIndexConti[1] - static_cast<double>(ulIndex[1]);
735 
736  leftGrid->TransformIndexToPhysicalPoint(ulIndex, ulPoint);
737  leftGrid->TransformIndexToPhysicalPoint(urIndex, urPoint);
738  leftGrid->TransformIndexToPhysicalPoint(lrIndex, lrPoint);
739  leftGrid->TransformIndexToPhysicalPoint(llIndex, llPoint);
740 
741  ulPixel[0] = (leftGrid->GetPixel(ulIndex))[0] + ulPoint[0];
742  ulPixel[1] = (leftGrid->GetPixel(ulIndex))[1] + ulPoint[1];
743  urPixel[0] = (leftGrid->GetPixel(urIndex))[0] + urPoint[0];
744  urPixel[1] = (leftGrid->GetPixel(urIndex))[1] + urPoint[1];
745  lrPixel[0] = (leftGrid->GetPixel(lrIndex))[0] + lrPoint[0];
746  lrPixel[1] = (leftGrid->GetPixel(lrIndex))[1] + lrPoint[1];
747  llPixel[0] = (leftGrid->GetPixel(llIndex))[0] + llPoint[0];
748  llPixel[1] = (leftGrid->GetPixel(llIndex))[1] + llPoint[1];
749  cPixel = (ulPixel * (1.0 - subPixIndex[0]) + urPixel * subPixIndex[0]) * (1.0 - subPixIndex[1]) +
750  (llPixel * (1.0 - subPixIndex[0]) + lrPixel * subPixIndex[0]) * subPixIndex[1];
751 
752  sensorPoint[0] = cPixel[0];
753  sensorPoint[1] = cPixel[1];
754  sensorPoint[2] = m_ElevationMin;
755  leftGroundHmin = m_LeftToGroundTransform->TransformPoint(sensorPoint);
756 
757  sensorPoint[2] = m_ElevationMax;
758  leftGroundHmax = m_LeftToGroundTransform->TransformPoint(sensorPoint);
759 
760  // compute right ray
761  itk::ContinuousIndex<double, 2> rightIndexEstimate;
762  rightIndexEstimate[0] = static_cast<double>((horizIt.GetIndex())[0]) + static_cast<double>(horizIt.Get());
763  rightIndexEstimate[1] = static_cast<double>((horizIt.GetIndex())[1]) + static_cast<double>(vertiIt.Get());
764 
765  horizDisp->TransformContinuousIndexToPhysicalPoint(rightIndexEstimate, epiPoint);
766  rightGrid->TransformPhysicalPointToContinuousIndex(epiPoint, gridIndexConti);
767 
768  ulIndex[0] = static_cast<int>(std::floor(gridIndexConti[0]));
769  ulIndex[1] = static_cast<int>(std::floor(gridIndexConti[1]));
770  if (ulIndex[0] < gridRegion.GetIndex(0))
771  ulIndex[0] = gridRegion.GetIndex(0);
772  if (ulIndex[1] < gridRegion.GetIndex(1))
773  ulIndex[1] = gridRegion.GetIndex(1);
774  if (ulIndex[0] > (gridRegion.GetIndex(0) + static_cast<int>(gridRegion.GetSize(0)) - 2))
775  {
776  ulIndex[0] = gridRegion.GetIndex(0) + gridRegion.GetSize(0) - 2;
777  }
778  if (ulIndex[1] > (gridRegion.GetIndex(1) + static_cast<int>(gridRegion.GetSize(1)) - 2))
779  {
780  ulIndex[1] = gridRegion.GetIndex(1) + gridRegion.GetSize(1) - 2;
781  }
782  urIndex[0] = ulIndex[0] + 1;
783  urIndex[1] = ulIndex[1];
784  lrIndex[0] = ulIndex[0] + 1;
785  lrIndex[1] = ulIndex[1] + 1;
786  llIndex[0] = ulIndex[0];
787  llIndex[1] = ulIndex[1] + 1;
788  subPixIndex[0] = gridIndexConti[0] - static_cast<double>(ulIndex[0]);
789  subPixIndex[1] = gridIndexConti[1] - static_cast<double>(ulIndex[1]);
790 
791  rightGrid->TransformIndexToPhysicalPoint(ulIndex, ulPoint);
792  rightGrid->TransformIndexToPhysicalPoint(urIndex, urPoint);
793  rightGrid->TransformIndexToPhysicalPoint(lrIndex, lrPoint);
794  rightGrid->TransformIndexToPhysicalPoint(llIndex, llPoint);
795 
796  ulPixel[0] = (rightGrid->GetPixel(ulIndex))[0] + ulPoint[0];
797  ulPixel[1] = (rightGrid->GetPixel(ulIndex))[1] + ulPoint[1];
798  urPixel[0] = (rightGrid->GetPixel(urIndex))[0] + urPoint[0];
799  urPixel[1] = (rightGrid->GetPixel(urIndex))[1] + urPoint[1];
800  lrPixel[0] = (rightGrid->GetPixel(lrIndex))[0] + lrPoint[0];
801  lrPixel[1] = (rightGrid->GetPixel(lrIndex))[1] + lrPoint[1];
802  llPixel[0] = (rightGrid->GetPixel(llIndex))[0] + llPoint[0];
803  llPixel[1] = (rightGrid->GetPixel(llIndex))[1] + llPoint[1];
804  cPixel = (ulPixel * (1.0 - subPixIndex[0]) + urPixel * subPixIndex[0]) * (1.0 - subPixIndex[1]) +
805  (llPixel * (1.0 - subPixIndex[0]) + lrPixel * subPixIndex[0]) * subPixIndex[1];
806 
807  sensorPoint[0] = cPixel[0];
808  sensorPoint[1] = cPixel[1];
809  sensorPoint[2] = m_ElevationMin;
810  rightGroundHmin = m_RightToGroundTransform->TransformPoint(sensorPoint);
811 
812  sensorPoint[2] = m_ElevationMax;
813  rightGroundHmax = m_RightToGroundTransform->TransformPoint(sensorPoint);
814 
815  // Compute ray intersection (mid-point method), TODO : implement non-iterative method from Hartley & Sturm
816  double a = (leftGroundHmax[0] - leftGroundHmin[0]) * (leftGroundHmax[0] - leftGroundHmin[0]) +
817  (leftGroundHmax[1] - leftGroundHmin[1]) * (leftGroundHmax[1] - leftGroundHmin[1]) +
818  (leftGroundHmax[2] - leftGroundHmin[2]) * (leftGroundHmax[2] - leftGroundHmin[2]);
819  double b = (rightGroundHmax[0] - rightGroundHmin[0]) * (rightGroundHmax[0] - rightGroundHmin[0]) +
820  (rightGroundHmax[1] - rightGroundHmin[1]) * (rightGroundHmax[1] - rightGroundHmin[1]) +
821  (rightGroundHmax[2] - rightGroundHmin[2]) * (rightGroundHmax[2] - rightGroundHmin[2]);
822  double c = -(leftGroundHmax[0] - leftGroundHmin[0]) * (rightGroundHmax[0] - rightGroundHmin[0]) -
823  (leftGroundHmax[1] - leftGroundHmin[1]) * (rightGroundHmax[1] - rightGroundHmin[1]) -
824  (leftGroundHmax[2] - leftGroundHmin[2]) * (rightGroundHmax[2] - rightGroundHmin[2]);
825  double g = (leftGroundHmax[0] - leftGroundHmin[0]) * (rightGroundHmin[0] - leftGroundHmin[0]) +
826  (leftGroundHmax[1] - leftGroundHmin[1]) * (rightGroundHmin[1] - leftGroundHmin[1]) +
827  (leftGroundHmax[2] - leftGroundHmin[2]) * (rightGroundHmin[2] - leftGroundHmin[2]);
828  double h = -(rightGroundHmax[0] - rightGroundHmin[0]) * (rightGroundHmin[0] - leftGroundHmin[0]) -
829  (rightGroundHmax[1] - rightGroundHmin[1]) * (rightGroundHmin[1] - leftGroundHmin[1]) -
830  (rightGroundHmax[2] - rightGroundHmin[2]) * (rightGroundHmin[2] - leftGroundHmin[2]);
831 
832  double rLeft = (b * g - c * h) / (a * b - c * c);
833  double rRight = (a * h - c * g) / (a * b - c * c);
834 
835  TDPointType leftFoot;
836  leftFoot.SetToBarycentricCombination(leftGroundHmax, leftGroundHmin, rLeft);
837 
838  TDPointType rightFoot;
839  rightFoot.SetToBarycentricCombination(rightGroundHmax, rightGroundHmin, rRight);
840 
841  TDPointType midPoint3D;
842  midPoint3D.SetToMidPoint(leftFoot, rightFoot);
843 
844  // Is point inside DEM area ?
845  typename DEMImageType::PointType midPoint2D;
846  midPoint2D[0] = midPoint3D[0];
847  midPoint2D[1] = midPoint3D[1];
848  itk::ContinuousIndex<double, 2> midIndex;
849  outputDEM->TransformPhysicalPointToContinuousIndex(midPoint2D, midIndex);
850  typename DEMImageType::IndexType cellIndex;
851 
852  // TODO JGT check if cellIndex should be calculed from the center of the pixel
853  // TransformContinuousIndexToPhysicalPoint with index [0,0] returns Origin of image
854  // TransformContinuousIndexToPhysicalPoint with index [0.5,0.5] returns a slight difference from Origin of image
855  cellIndex[0] = static_cast<int>(std::floor(midIndex[0] + 0.5));
856  cellIndex[1] = static_cast<int>(std::floor(midIndex[1] + 0.5));
857 
858  if (outputRequestedRegion.IsInside(cellIndex))
859  {
860  // Estimate local reference elevation (average, DEM or geoid) => NO NEED, ALREADY HAVE 3D RAYS
861  // double localElevation = demHandler->GetHeightAboveEllipsoid(midPoint2D);
862 
863  // Add point to its corresponding cell (keep maximum)
864  DEMPixelType cellHeight = static_cast<DEMPixelType>(midPoint3D[2]);
865  if (cellHeight > tmpDEM->GetPixel(cellIndex) && cellHeight < static_cast<DEMPixelType>(m_ElevationMax))
866  {
867  tmpDEM->SetPixel(cellIndex, cellHeight);
868  }
869  }
870 
871  ++horizIt;
872  ++vertiIt;
873 
874  if (useMask)
875  ++maskIt;
876  }
877 }
878 
879 template <class TDisparityImage, class TInputImage, class TOutputDEMImage, class TEpipolarGridImage, class TMaskImage>
881 {
882  TOutputDEMImage* outputDEM = this->GetDEMOutput();
883 
884  if (m_TempDEMRegions.size() < 1)
885  {
886  outputDEM->FillBuffer(m_ElevationMin);
887  return;
888  }
889 
890  itk::ImageRegionIterator<DEMImageType> outputDEMIt(outputDEM, outputDEM->GetRequestedRegion());
891  itk::ImageRegionIterator<DEMImageType> firstDEMIt(m_TempDEMRegions[0], outputDEM->GetRequestedRegion());
892 
893  outputDEMIt.GoToBegin();
894  firstDEMIt.GoToBegin();
895 
896  // Copy first DEM
897  while (!outputDEMIt.IsAtEnd() && !firstDEMIt.IsAtEnd())
898  {
899  outputDEMIt.Set(firstDEMIt.Get());
900  ++outputDEMIt;
901  ++firstDEMIt;
902  }
903 
904  // Check DEMs from other threads and keep the maximum elevation
905  for (unsigned int i = 1; i < m_TempDEMRegions.size(); i++)
906  {
907  itk::ImageRegionIterator<DEMImageType> tmpDEMIt(m_TempDEMRegions[i], outputDEM->GetRequestedRegion());
908 
909  outputDEMIt.GoToBegin();
910  tmpDEMIt.GoToBegin();
911 
912  while (!outputDEMIt.IsAtEnd() && !tmpDEMIt.IsAtEnd())
913  {
914  if (tmpDEMIt.Get() > outputDEMIt.Get())
915  {
916  outputDEMIt.Set(tmpDEMIt.Get());
917  }
918  ++outputDEMIt;
919  ++tmpDEMIt;
920  }
921  }
922 }
923 }
924 
925 #endif
itk::Size< Monteverdi_DIMENSION > SizeType
Definition: mvdTypes.h:137
const TInputImage * GetLeftInput() const
itk::Index< Monteverdi_DIMENSION > IndexType
Definition: mvdTypes.h:133
const TDisparityImage * GetVerticalDisparityMapInput() const
itk::SmartPointer< Self > Pointer
void SetHorizontalDisparityMapInput(const TDisparityImage *hmap)
void SetLeftInput(const TInputImage *image)
const TEpipolarGridImage * GetRightEpipolarGridInput() const
void SetVerticalDisparityMapInput(const TDisparityImage *vmap)
void SetRightEpipolarGridInput(const TEpipolarGridImage *grid)
void SetRightInput(const TInputImage *image)
RSTransformType::InputPointType TDPointType
const TEpipolarGridImage * GetLeftEpipolarGridInput() const
The "otb" namespace contains all Orfeo Toolbox (OTB) classes.
const TInputImage * GetRightInput() const
This is the class to handle generic remote sensing transform.
const TDisparityImage * GetHorizontalDisparityMapInput() const
const TMaskImage * GetDisparityMaskInput() const
void SetDisparityMaskInput(const TMaskImage *mask)
void SetLeftEpipolarGridInput(const TEpipolarGridImage *grid)
VectorImageType::SpacingType SpacingType
Definition: mvdTypes.h:175
void ThreadedGenerateData(const RegionType &outputRegionForThread, itk::ThreadIdType threadId) override
VectorImageType::PointType PointType
Definition: mvdTypes.h:183
const TOutputDEMImage * GetDEMOutput() const