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

Generated at Sat Mar 8 2014 15:53:57 for Orfeo Toolbox with doxygen 1.8.3.1