Orfeo Toolbox  4.0
otbStereoSensorModelToElevationMapFilter.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 __otbStereoSensorModelToElevationMapFilter_txx
19 #define __otbStereoSensorModelToElevationMapFilter_txx
20 
22 
24 #include "itkProgressReporter.h"
28 #include "otbDEMHandler.h"
29 
30 namespace otb
31 {
32 template <class TInputImage, class TOutputHeight>
35 {
36  // Filter has two inputs
37  this->SetNumberOfRequiredInputs(2);
38  this->SetNumberOfRequiredOutputs(2);
39  this->SetNthOutput(1, OutputImageType::New());
40 
41  // Default interpolator
43 
44  // Default correlation radius
45  m_Radius.Fill(3);
46 
47  // Height exploration setup
48  m_LowerElevation = -20;
49  m_HigherElevation = 20;
50  m_ElevationStep = 1.;
51  m_CorrelationThreshold = 0.7;
52  m_VarianceThreshold = 4;
53  m_SubtractInitialElevation = false;
54 
55  // DEM handling (deprecated)
56  m_UseDEM = false;
57 }
58 
59 template <class TInputImage, class TOutputHeight>
62 {}
63 
64 template <class TInputImage, class TOutputHeight>
65 void
67 ::SetMasterInput(const TInputImage * image)
68 {
69  this->SetNthInput(0, const_cast<TInputImage *>( image ));
70 }
71 
72 template <class TInputImage, class TOutputHeight>
73 void
75 ::SetSlaveInput(const TInputImage * image)
76 {
77  this->SetNthInput(1, const_cast<TInputImage *>( image ));
78 }
79 
80 template <class TInputImage, class TOutputHeight>
81 const TInputImage *
84 {
85  if(this->GetNumberOfInputs() < 1)
86  {
87  return 0;
88  }
89  return static_cast<const TInputImage *>(this->itk::ProcessObject::GetInput(0));
90 }
91 
92 template <class TInputImage, class TOutputHeight>
93 const TInputImage *
96 {
97  if(this->GetNumberOfInputs()<2)
98  {
99  return 0;
100  }
101  return static_cast<const TInputImage *>(this->itk::ProcessObject::GetInput(1));
102 }
103 
104 template <class TInputImage, class TOutputHeight>
105 TOutputHeight *
108 {
109  if(this->GetNumberOfOutputs()<2)
110  {
111  return 0;
112  }
113  return static_cast<TOutputHeight *>(this->itk::ProcessObject::GetOutput(1));
114 }
115 
116 template <class TInputImage, class TOutputHeight>
119 {
120  // Call the superclass implementation
121  Superclass::GenerateInputRequestedRegion();
122 
123  // Retrieve pointers
124  InputImageType * masterPtr = const_cast<InputImageType *>(this->GetMasterInput());
125  InputImageType * slavePtr = const_cast<InputImageType*>(this->GetSlaveInput());
126  OutputImageType * outputPtr = this->GetOutput();
127 
128  if ( !masterPtr || !slavePtr || !outputPtr )
129  {
130  return;
131  }
132 
133  // get a copy of the fixed requested region (should equal the output
134  // requested region)
135  typename InputImageType::RegionType masterRequestedRegion, slaveRequestedRegion;
136  masterRequestedRegion = outputPtr->GetRequestedRegion();
137 
138  // pad the master requested region by the operator radius
139  masterRequestedRegion.PadByRadius( m_Radius );
140 
141  // Find corners of the master requested region
142  typename InputImageType::IndexType mul, mur, mll, mlr;
143  mul = masterRequestedRegion.GetIndex();
144  mur = masterRequestedRegion.GetIndex();
145  mur[0] += masterRequestedRegion.GetSize()[0]-1;
146  mll = masterRequestedRegion.GetIndex();
147  mll[1] += masterRequestedRegion.GetSize()[1]-1;
148  mlr = masterRequestedRegion.GetIndex();
149  mlr[0] += masterRequestedRegion.GetSize()[0]-1;
150  mlr[1] += masterRequestedRegion.GetSize()[1]-1;
151 
152  // Transform to physical space
153  typename InputImageType::PointType mpul, mpur, mpll, mplr;
154  masterPtr->TransformIndexToPhysicalPoint(mul, mpul);
155  masterPtr->TransformIndexToPhysicalPoint(mur, mpur);
156  masterPtr->TransformIndexToPhysicalPoint(mll, mpll);
157  masterPtr->TransformIndexToPhysicalPoint(mlr, mplr);
158 
159  // Build the transform to switch from the master to the slave image
160  typename GenericRSTransformType::Pointer transform = GenericRSTransformType::New();
161  transform->SetInputKeywordList(masterPtr->GetImageKeywordlist());
162  transform->SetOutputKeywordList(slavePtr->GetImageKeywordlist());
163 
164  transform->InstanciateTransform();
165 
166  typename GenericRSTransformType::ParametersType params(1);
167 
168  // Build minimum height and maximum height points corresponding to
169  // corners of the master requested region
170  typename InputImageType::PointType sMinPul, sMinPur, sMinPll, sMinPlr, sMaxPul, sMaxPur, sMaxPll, sMaxPlr;
171 
172  // Lower case
173  params[0] = m_LowerElevation;
174  transform->SetParameters(params);
175 
176  sMinPul = transform->TransformPoint(mpul);
177  sMinPur = transform->TransformPoint(mpur);
178  sMinPll = transform->TransformPoint(mpll);
179  sMinPlr = transform->TransformPoint(mplr);
180 
181  // Higher case
182  params[0] = m_HigherElevation;
183  transform->SetParameters(params);
184 
185  sMaxPul = transform->TransformPoint(mpul);
186  sMaxPur = transform->TransformPoint(mpur);
187  sMaxPll = transform->TransformPoint(mpll);
188  sMaxPlr = transform->TransformPoint(mplr);
189 
190  // Transform to index
191  typename InputImageType::IndexType sMinul, sMinur, sMinll, sMinlr, sMaxul, sMaxur, sMaxll, sMaxlr;
192 
193  slavePtr->TransformPhysicalPointToIndex(sMinPul, sMinul);
194  slavePtr->TransformPhysicalPointToIndex(sMaxPul, sMaxul);
195  slavePtr->TransformPhysicalPointToIndex(sMinPur, sMinur);
196  slavePtr->TransformPhysicalPointToIndex(sMaxPur, sMaxur);
197  slavePtr->TransformPhysicalPointToIndex(sMinPll, sMinll);
198  slavePtr->TransformPhysicalPointToIndex(sMaxPll, sMaxll);
199  slavePtr->TransformPhysicalPointToIndex(sMinPlr, sMinlr);
200  slavePtr->TransformPhysicalPointToIndex(sMaxPlr, sMaxlr);
201 
202  // Find the corresponding bounding box
203  typename InputImageType::IndexType sul, slr;
204 
205  sul[0] = std::min(std::min(std::min(sMinul[0], sMaxul[0]), std::min(sMinur[0], sMaxur[0])),
206  std::min(std::min(sMinll[0], sMaxll[0]), std::min(sMinlr[0], sMaxlr[0])));
207  sul[1] = std::min(std::min(std::min(sMinul[1], sMaxul[1]), std::min(sMinur[1], sMaxur[1])),
208  std::min(std::min(sMinll[1], sMaxll[1]), std::min(sMinlr[1], sMaxlr[1])));
209  slr[0] = std::max(std::max(std::max(sMinul[0], sMaxul[0]), std::max(sMinur[0], sMaxur[0])),
210  std::max(std::max(sMinll[0], sMaxll[0]), std::max(sMinlr[0], sMaxlr[0])));
211  slr[1] = std::max(std::max(std::max(sMinul[1], sMaxul[1]), std::max(sMinur[1], sMaxur[1])),
212  std::max(std::max(sMinll[1], sMaxll[1]), std::max(sMinlr[1], sMaxlr[1])));
213 
214  // Build the slave requested region
215  slaveRequestedRegion.SetIndex(sul);
216  typename InputImageType::SizeType ssize;
217  ssize[0] = slr[0] - sul[0]+1;
218  ssize[1] = slr[1] - sul[1]+1;
219  slaveRequestedRegion.SetSize(ssize);
220 
221  // crop the master region at the master's largest possible region
222  if ( masterRequestedRegion.Crop(masterPtr->GetLargestPossibleRegion()))
223  {
224  masterPtr->SetRequestedRegion( masterRequestedRegion );
225  }
226  else
227  {
228  // Couldn't crop the region (requested region is outside the largest
229  // possible region). Throw an exception.
230  // store what we tried to request (prior to trying to crop)
231  masterPtr->SetRequestedRegion( masterRequestedRegion );
232 
233  // build an exception
234  itk::InvalidRequestedRegionError e(__FILE__, __LINE__);
235  std::ostringstream msg;
236  msg << this->GetNameOfClass()
237  << "::GenerateInputRequestedRegion()";
238  e.SetLocation(msg.str().c_str());
239  e.SetDescription("Requested region is (at least partially) outside the largest possible region of image 1.");
240  e.SetDataObject(masterPtr);
241  throw e;
242  }
243 
244  // crop the slave region at the slave's largest possible region
245  if ( slaveRequestedRegion.Crop(slavePtr->GetLargestPossibleRegion()))
246  {
247  slavePtr->SetRequestedRegion( slaveRequestedRegion );
248  }
249  else
250  {
251  // Couldn't crop the region (requested region is outside the largest
252  // possible region). This case might happen so we do not throw any exception but
253  // request a null region instead
254  typename InputImageType::SizeType slaveSize;
255  slaveSize.Fill(0);
256  slaveRequestedRegion.SetSize(slaveSize);
257  typename InputImageType::IndexType slaveIndex;
258  slaveIndex.Fill(0);
259  slaveRequestedRegion.SetIndex(slaveIndex);
260 
261  // store what we tried to request (prior to trying to crop)
262  slavePtr->SetRequestedRegion(slaveRequestedRegion);
263  }
264  return;
265 }
266 
267 template <class TInputImage, class TOutputHeight>
268 void
271 {
272  // Wire the interpolator
273  m_Interpolator->SetInputImage(this->GetSlaveInput());
274  this->GetCorrelationOutput()->FillBuffer(0.);
275 
276  // Initialize with average elevation
277  this->GetOutput()->FillBuffer(otb::DEMHandler::Instance()->GetDefaultHeightAboveEllipsoid());
278 
279  // Initialize with DEM elevation (not threadable because of some
280  // mutex in ossim)
281  OutputImageType * outputPtr = this->GetOutput();
282 
283  typename GenericRSTransformType::Pointer rsTransform = GenericRSTransformType::New();
284  rsTransform->SetInputKeywordList(outputPtr->GetImageKeywordlist());
285  rsTransform->InstanciateTransform();
286 
287  // Fill ouptut
288  itk::ImageRegionIteratorWithIndex<OutputImageType> outputIt(outputPtr, outputPtr->GetBufferedRegion());
289 
290  for(outputIt.GoToBegin(); !outputIt.IsAtEnd(); ++outputIt)
291  {
292  // Retrieve physical point
293  OutputPointType outputPoint, geoPoint;
294  outputPtr->TransformIndexToPhysicalPoint(outputIt.GetIndex(), outputPoint);
295 
296  // Transform to geo point
297  geoPoint = rsTransform->TransformPoint(outputPoint);
298  outputIt.Set(otb::DEMHandler::Instance()->GetHeightAboveEllipsoid(geoPoint));
299  }
300 
301 }
302 
303 template <class TInputImage, class TOutputHeight>
304 void
306 ::ThreadedGenerateData(const OutputRegionType & outputRegionForThread, itk::ThreadIdType threadId)
307 {
308  // Retrieve pointers
309  const InputImageType* masterPtr = this->GetMasterInput();
310  const InputImageType* slavePtr = this->GetSlaveInput();
311  OutputImageType* outputPtr = this->GetOutput();
312  OutputImageType* correlPtr = this->GetCorrelationOutput();
313 
314  // Set-up the forward-inverse sensor model transform
315  typename GenericRSTransform3DType::Pointer transform = GenericRSTransform3DType::New();
316  transform->SetInputKeywordList(masterPtr->GetImageKeywordlist());
317  transform->SetOutputKeywordList(slavePtr->GetImageKeywordlist());
318 
319 
320  // TODO: Uncomment when model optimization pushed
321  // // GCP refinement
322  // unsigned int gcpCount1 = this->GetMasterInput()->GetGCPCount();
323  // unsigned int gcpCount2 = this->GetSlaveInput()->GetGCPCount();
324 
325 
326  // transform->ClearInputTiePoints();
327  // transform->ClearOutputTiePoints();
328 
329  // for(unsigned int i = 0; i < gcpCount1; ++i)
330  // {
331  // typename GenericRSTransformType::InputPointType imagePoint, groundPoint, groundPointWGS84;
332  // // Transform GCP ground part to WGS84
333  // groundPoint[0] = this->GetMasterInput()->GetGCPs(i).m_GCPX;
334  // groundPoint[1] = this->GetMasterInput()->GetGCPs(i).m_GCPY;
335 
336  // typename GenericRSTransformType::Pointer gcpTransform = GenericRSTransformType::New();
337  // gcpTransform->SetInputProjectionRef(this->GetInput()->GetGCPProjection());
338  // gcpTransform->InstanciateTransform();
339 
340  // groundPointWGS84 = gcpTransform->TransformPoint(groundPoint);
341 
342  // imagePoint[0] = this->GetMasterInput()->GetGCPs(i).m_GCPCol;
343  // imagePoint[1] = this->GetMasterInput()->GetGCPs(i).m_GCPRow;
344 
345  // transform->AddInputTiePoint(imagePoint, groundPointWGS84);
346  // transform->OptimizeInputTransformOn();
347  // }
348 
349  // for(unsigned int i = 0; i < gcpCount2; ++i)
350  // {
351  // typename GenericRSTransformType::InputPointType imagePoint, groundPoint, groundPointWGS84;
352  // // Transform GCP ground part to WGS84
353  // groundPoint[0] = this->GetSlaveInput()->GetGCPs(i).m_GCPX;
354  // groundPoint[1] = this->GetSlaveInput()->GetGCPs(i).m_GCPY;
355 
356  // typename GenericRSTransformType::Pointer gcpTransform = GenericRSTransformType::New();
357  // gcpTransform->SetInputProjectionRef(this->GetInput()->GetGCPProjection());
358  // gcpTransform->InstanciateTransform();
359 
360  // groundPointWGS84 = gcpTransform->TransformPoint(groundPoint);
361 
362  // imagePoint[0] = this->GetSlaveInput()->GetGCPs(i).m_GCPCol;
363  // imagePoint[1] = this->GetSlaveInput()->GetGCPs(i).m_GCPRow;
364 
365  // transform->AddOutputTiePoint(imagePoint, groundPointWGS84);
366  // transform->OptimizeOutputTransformOn();
367  // }
368 
369 
370  // support progress methods/callbacks
371  itk::ProgressReporter progress(this, threadId, outputRegionForThread.GetNumberOfPixels());
372 
373  // Define an iterator on the output elevation map
374  itk::ConstNeighborhoodIterator<InputImageType> inputIt(m_Radius, masterPtr, outputRegionForThread);
375  itk::ImageRegionIterator<OutputImageType> outputIt(outputPtr, outputRegionForThread);
376  itk::ImageRegionIteratorWithIndex<OutputImageType> correlIt(correlPtr, outputRegionForThread);
377 
378  // Transform elevation set-up
379  transform->InstanciateTransform();
380 
381  // Start visiting buffer again
382  outputIt.GoToBegin();
383  correlIt.GoToBegin();
384  inputIt.GoToBegin();
385 
386 
387  // This will hold the master patch
388  std::vector<double> master;
389  master.reserve(inputIt.Size());
390  master = std::vector<double>(inputIt.Size(), 0);
391 
392  // And this will hold the slave patch
393  std::vector<double> slave;
394  slave.reserve(inputIt.Size());
395  slave = std::vector<double>(inputIt.Size(), 0);
396 
397 
398  // Walk the output map
399  while(!outputIt.IsAtEnd() && !inputIt.IsAtEnd())
400  {
401  // Define some loop variables
402  typename InputImageType::PointType inPoint, outPoint, currentPoint, optimalPoint;
403  typename GenericRSTransform3DType::InputPointType in3DPoint, out3DPoint;
404  typename InputImageType::IndexType index;
405 
406  // Retrieve intial height
407  double initHeight = outputIt.Get();
408  double optimalHeight = initHeight;
409  double optimalCorrelation = 0;
410 
411  // Check if there is an height info
412  if(initHeight != -32768)
413  {
414  // These are used to estimate master patch variance
415  double masterSum = 0;
416  double masterVariance = 0;
417 
418  // Fill master patch
419  for(unsigned int i = 0; i < inputIt.Size(); ++i)
420  {
421  // Add to the master vector
422  double value = inputIt.GetPixel(i);
423  master[i] = value;
424 
425  // Cumulate for mean
426  masterSum += value;
427  }
428 
429  // Finalize mean
430  masterSum /= inputIt.Size();
431 
432  // Complete pooled variance computation
433  for(unsigned int i = 0; i < inputIt.Size(); ++i)
434  {
435  masterVariance += (master[i] - masterSum) * (master[i] - masterSum);
436  }
437  masterVariance /= (inputIt.Size()-1);
438 
439  // Check for high enough variance so that correlation will be reliable
440  if(masterVariance > m_VarianceThreshold)
441  {
442  // Explore candidate heights
443  for(double height = initHeight + m_LowerElevation;
444  height < initHeight + m_HigherElevation;
445  height += m_ElevationStep)
446  {
447 
448  // Interpolate slave patch
449  for(unsigned int i = 0; i < inputIt.Size(); ++i)
450  {
451  // Retrieve the current index
452  index = inputIt.GetIndex(i);
453 
454  // Retrieve master physical point
455  masterPtr->TransformIndexToPhysicalPoint(index, inPoint);
456  in3DPoint[0] = inPoint[0];
457  in3DPoint[1] = inPoint[1];
458  in3DPoint[2] = height;
459 
460  // Transform to slave
461  out3DPoint = transform->TransformPoint(in3DPoint);
462  outPoint[0] = out3DPoint[0];
463  outPoint[1] = out3DPoint[1];
464 
465  // Interpolate
466  if(m_Interpolator->IsInsideBuffer(outPoint))
467  {
468  // And fill slave vector
469  slave[i] = m_Interpolator->Evaluate(outPoint);
470  }
471  else
472  {
473  // If out of buffer, fill with 0.
474  slave[i] = 0.;
475  }
476  }
477 
478  // Now that we have the master and slave patches, call the
479  // correlation function
480  double correlationValue = this->Correlation(master, slave);
481 
482  // Check if a better correlation was found
483  if(correlationValue > optimalCorrelation)
484  {
485  // If found, update values
486  optimalCorrelation = correlationValue;
487  optimalHeight = height;
488  }
489  }
490  }
491  // TODO: refinenement step ?
492  }
493 
494  // Final check on best correlation value
495  double finalOffset = 0.;
496 
497  // Switch to subtract initial elevation mode
498  if(m_SubtractInitialElevation)
499  {
500  finalOffset = initHeight;
501  }
502 
503  if(optimalCorrelation > m_CorrelationThreshold)
504  {
505  outputIt.Set(optimalHeight-finalOffset);
506  correlIt.Set(optimalCorrelation);
507  }
508  else
509  {
510  outputIt.Set(initHeight - finalOffset);
511  correlIt.Set(0);
512  }
513 
514  // Update progress
515  progress.CompletedPixel();
516 
517  // And iterators
518  ++inputIt;
519  ++outputIt;
520  ++correlIt;
521  }
522 }
523 
524 template <class TInputImage, class TOutputHeight>
525 double
527 ::Correlation(const std::vector<double>& master, const std::vector<double>& slave) const
528 {
529  double meanSlave = 0;
530  double meanMaster = 0;
531  double correlationValue = 0;
532 
533  // Compute means
534  for(unsigned int i = 0; i < master.size(); ++i)
535  {
536  meanSlave += slave[i];
537  meanMaster += master[i];
538  }
539  meanSlave /= slave.size();
540  meanMaster /= master.size();
541 
542  double crossProd = 0.;
543  double squareSumMaster = 0.;
544  double squareSumSlave = 0.;
545 
546 
547  // Compute correlation
548  for(unsigned int i = 0; i < master.size(); ++i)
549  {
550  crossProd += (slave[i]-meanSlave) * (master[i]-meanMaster);
551  squareSumSlave += (slave[i]-meanSlave) * (slave[i]-meanSlave);
552  squareSumMaster += (master[i]-meanMaster) * (master[i]-meanMaster);
553  }
554 
555  correlationValue = vcl_abs(crossProd/(vcl_sqrt(squareSumSlave)*vcl_sqrt(squareSumMaster)));
556 
557  return correlationValue;
558 }
559 
560 } // end namespace otb
561 
562 #endif

Generated at Sat Mar 8 2014 16:19:32 for Orfeo Toolbox with doxygen 1.8.3.1