OTB  6.7.0
Orfeo Toolbox
otbStereoSensorModelToElevationMapFilter.hxx
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2005-2019 Centre National d'Etudes Spatiales (CNES)
3  *
4  * This file is part of Orfeo Toolbox
5  *
6  * https://www.orfeo-toolbox.org/
7  *
8  * Licensed under the Apache License, Version 2.0 (the "License");
9  * you may not use this file except in compliance with the License.
10  * You may obtain a copy of the License at
11  *
12  * http://www.apache.org/licenses/LICENSE-2.0
13  *
14  * Unless required by applicable law or agreed to in writing, software
15  * distributed under the License is distributed on an "AS IS" BASIS,
16  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
17  * See the License for the specific language governing permissions and
18  * limitations under the License.
19  */
20 
21 #ifndef otbStereoSensorModelToElevationMapFilter_hxx
22 #define otbStereoSensorModelToElevationMapFilter_hxx
23 
25 
27 #include "itkProgressReporter.h"
31 #include "otbDEMHandler.h"
32 
33 namespace otb
34 {
35 template <class TInputImage, class TOutputHeight>
38 {
39  // Filter has two inputs
40  this->SetNumberOfRequiredInputs(2);
41  this->SetNumberOfRequiredOutputs(2);
42  this->SetNthOutput(1, OutputImageType::New());
43 
44  // Default interpolator
46 
47  // Default correlation radius
48  m_Radius.Fill(3);
49 
50  // Height exploration setup
51  m_LowerElevation = -20;
52  m_HigherElevation = 20;
53  m_ElevationStep = 1.;
54  m_CorrelationThreshold = 0.7;
55  m_VarianceThreshold = 4;
56  m_SubtractInitialElevation = 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 nullptr;
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 nullptr;
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 nullptr;
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->InstantiateTransform();
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());
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->InstantiateTransform();
286 
287  // Fill output
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  const InputImageType* masterPtr = this->GetMasterInput();
302  const InputImageType* slavePtr = this->GetSlaveInput();
303 
304  // Set-up the forward-inverse sensor model transform
305  m_MasterToSlave = GenericRSTransform3DType::New();
306  m_MasterToSlave->SetInputKeywordList(masterPtr->GetImageKeywordlist());
307  m_MasterToSlave->SetOutputKeywordList(slavePtr->GetImageKeywordlist());
308  m_MasterToSlave->InstantiateTransform();
309 
310 }
311 
312 template <class TInputImage, class TOutputHeight>
313 void
315 ::ThreadedGenerateData(const OutputRegionType & outputRegionForThread, itk::ThreadIdType threadId)
316 {
317  // Retrieve pointers
318  const InputImageType* masterPtr = this->GetMasterInput();
319  OutputImageType* outputPtr = this->GetOutput();
320  OutputImageType* correlPtr = this->GetCorrelationOutput();
321 
322  // TODO: Uncomment when model optimization pushed
323  // // GCP refinement
324  // unsigned int gcpCount1 = this->GetMasterInput()->GetGCPCount();
325  // unsigned int gcpCount2 = this->GetSlaveInput()->GetGCPCount();
326 
327 
328  // transform->ClearInputTiePoints();
329  // transform->ClearOutputTiePoints();
330 
331  // for(unsigned int i = 0; i < gcpCount1; ++i)
332  // {
333  // typename GenericRSTransformType::InputPointType imagePoint, groundPoint, groundPointWGS84;
334  // // Transform GCP ground part to WGS84
335  // groundPoint[0] = this->GetMasterInput()->GetGCPs(i).m_GCPX;
336  // groundPoint[1] = this->GetMasterInput()->GetGCPs(i).m_GCPY;
337 
338  // typename GenericRSTransformType::Pointer gcpTransform = GenericRSTransformType::New();
339  // gcpTransform->SetInputProjectionRef(this->GetInput()->GetGCPProjection());
340  // gcpTransform->InstantiateTransform();
341 
342  // groundPointWGS84 = gcpTransform->TransformPoint(groundPoint);
343 
344  // imagePoint[0] = this->GetMasterInput()->GetGCPs(i).m_GCPCol;
345  // imagePoint[1] = this->GetMasterInput()->GetGCPs(i).m_GCPRow;
346 
347  // transform->AddInputTiePoint(imagePoint, groundPointWGS84);
348  // transform->OptimizeInputTransformOn();
349  // }
350 
351  // for(unsigned int i = 0; i < gcpCount2; ++i)
352  // {
353  // typename GenericRSTransformType::InputPointType imagePoint, groundPoint, groundPointWGS84;
354  // // Transform GCP ground part to WGS84
355  // groundPoint[0] = this->GetSlaveInput()->GetGCPs(i).m_GCPX;
356  // groundPoint[1] = this->GetSlaveInput()->GetGCPs(i).m_GCPY;
357 
358  // typename GenericRSTransformType::Pointer gcpTransform = GenericRSTransformType::New();
359  // gcpTransform->SetInputProjectionRef(this->GetInput()->GetGCPProjection());
360  // gcpTransform->InstantiateTransform();
361 
362  // groundPointWGS84 = gcpTransform->TransformPoint(groundPoint);
363 
364  // imagePoint[0] = this->GetSlaveInput()->GetGCPs(i).m_GCPCol;
365  // imagePoint[1] = this->GetSlaveInput()->GetGCPs(i).m_GCPRow;
366 
367  // transform->AddOutputTiePoint(imagePoint, groundPointWGS84);
368  // transform->OptimizeOutputTransformOn();
369  // }
370 
371 
372  // support progress methods/callbacks
373  itk::ProgressReporter progress(this, threadId, outputRegionForThread.GetNumberOfPixels());
374 
375  // Define an iterator on the output elevation map
376  itk::ConstNeighborhoodIterator<InputImageType> inputIt(m_Radius, masterPtr, outputRegionForThread);
377  itk::ImageRegionIterator<OutputImageType> outputIt(outputPtr, outputRegionForThread);
378  itk::ImageRegionIteratorWithIndex<OutputImageType> correlIt(correlPtr, outputRegionForThread);
379 
380  // Start visiting buffer again
381  outputIt.GoToBegin();
382  correlIt.GoToBegin();
383  inputIt.GoToBegin();
384 
385 
386  // This will hold the master patch
387  std::vector<double> master;
388  master.reserve(inputIt.Size());
389  master = std::vector<double>(inputIt.Size(), 0);
390 
391  // And this will hold the slave patch
392  std::vector<double> slave;
393  slave.reserve(inputIt.Size());
394  slave = std::vector<double>(inputIt.Size(), 0);
395 
396 
397  // Walk the output map
398  while(!outputIt.IsAtEnd() && !inputIt.IsAtEnd())
399  {
400  // Define some loop variables
401  typename InputImageType::PointType inPoint, outPoint, currentPoint, optimalPoint;
402  typename GenericRSTransform3DType::InputPointType in3DPoint, out3DPoint;
403  typename InputImageType::IndexType index;
404 
405  // Retrieve initial height
406  double initHeight = outputIt.Get();
407  double optimalHeight = initHeight;
408  double optimalCorrelation = 0;
409 
410  // Check if there is an height info
411  if(initHeight != -32768)
412  {
413  // These are used to estimate master patch variance
414  double masterSum = 0;
415  double masterVariance = 0;
416 
417  // Fill master patch
418  for(unsigned int i = 0; i < inputIt.Size(); ++i)
419  {
420  // Add to the master vector
421  double value = inputIt.GetPixel(i);
422  master[i] = value;
423 
424  // Cumulate for mean
425  masterSum += value;
426  }
427 
428  // Finalize mean
429  masterSum /= inputIt.Size();
430 
431  // Complete pooled variance computation
432  for(unsigned int i = 0; i < inputIt.Size(); ++i)
433  {
434  masterVariance += (master[i] - masterSum) * (master[i] - masterSum);
435  }
436  masterVariance /= (inputIt.Size()-1);
437 
438  // Check for high enough variance so that correlation will be reliable
439  if(masterVariance > m_VarianceThreshold)
440  {
441  // Explore candidate heights
442  for(double height = initHeight + m_LowerElevation;
443  height < initHeight + m_HigherElevation;
444  height += m_ElevationStep)
445  {
446 
447  // Interpolate slave patch
448  for(unsigned int i = 0; i < inputIt.Size(); ++i)
449  {
450  // Retrieve the current index
451  index = inputIt.GetIndex(i);
452 
453  // Retrieve master physical point
454  masterPtr->TransformIndexToPhysicalPoint(index, inPoint);
455  in3DPoint[0] = inPoint[0];
456  in3DPoint[1] = inPoint[1];
457  in3DPoint[2] = height;
458 
459  // Transform to slave
460  out3DPoint = m_MasterToSlave->TransformPoint(in3DPoint);
461  outPoint[0] = out3DPoint[0];
462  outPoint[1] = out3DPoint[1];
463 
464  // Interpolate
465  if(m_Interpolator->IsInsideBuffer(outPoint))
466  {
467  // And fill slave vector
468  slave[i] = m_Interpolator->Evaluate(outPoint);
469  }
470  else
471  {
472  // If out of buffer, fill with 0.
473  slave[i] = 0.;
474  }
475  }
476 
477  // Now that we have the master and slave patches, call the
478  // correlation function
479  double correlationValue = this->Correlation(master, slave);
480 
481  // Check if a better correlation was found
482  if(correlationValue > optimalCorrelation)
483  {
484  // If found, update values
485  optimalCorrelation = correlationValue;
486  optimalHeight = height;
487  }
488  }
489  }
490  // TODO: refinenement step ?
491  }
492 
493  // Final check on best correlation value
494  double finalOffset = 0.;
495 
496  // Switch to subtract initial elevation mode
497  if(m_SubtractInitialElevation)
498  {
499  finalOffset = initHeight;
500  }
501 
502  if(optimalCorrelation > m_CorrelationThreshold)
503  {
504  outputIt.Set(optimalHeight-finalOffset);
505  correlIt.Set(optimalCorrelation);
506  }
507  else
508  {
509  outputIt.Set(initHeight - finalOffset);
510  correlIt.Set(0);
511  }
512 
513  // Update progress
514  progress.CompletedPixel();
515 
516  // And iterators
517  ++inputIt;
518  ++outputIt;
519  ++correlIt;
520  }
521 }
522 
523 template <class TInputImage, class TOutputHeight>
524 double
526 ::Correlation(const std::vector<double>& master, const std::vector<double>& slave) const
527 {
528  double meanSlave = 0;
529  double meanMaster = 0;
530  double correlationValue = 0;
531 
532  // Compute means
533  for(unsigned int i = 0; i < master.size(); ++i)
534  {
535  meanSlave += slave[i];
536  meanMaster += master[i];
537  }
538  meanSlave /= slave.size();
539  meanMaster /= master.size();
540 
541  double crossProd = 0.;
542  double squareSumMaster = 0.;
543  double squareSumSlave = 0.;
544 
545 
546  // Compute correlation
547  for(unsigned int i = 0; i < master.size(); ++i)
548  {
549  crossProd += (slave[i]-meanSlave) * (master[i]-meanMaster);
550  squareSumSlave += (slave[i]-meanSlave) * (slave[i]-meanSlave);
551  squareSumMaster += (master[i]-meanMaster) * (master[i]-meanMaster);
552  }
553 
554  correlationValue = std::abs(crossProd/(std::sqrt(squareSumSlave)*std::sqrt(squareSumMaster)));
555 
556  return correlationValue;
557 }
558 
559 } // end namespace otb
560 
561 #endif
itk::Size< Monteverdi_DIMENSION > SizeType
Definition: mvdTypes.h:137
double Correlation(const std::vector< double > &master, const std::vector< double > &slave) const
void SetDataObject(DataObject *dobj)
virtual void SetDescription(const std::string &s)
virtual void SetLocation(const std::string &s)
void Set(const PixelType &value) const
void Set(const PixelType &value) const
void SetSize(const SizeValueType val[VDimension])
itk::Index< Monteverdi_DIMENSION > IndexType
Definition: mvdTypes.h:133
static Pointer Instance()
unsigned int ThreadIdType
DataObject * GetInput(const DataObjectIdentifierType &key)
bool IsAtEnd(void) const
TOutputImage OutputImageType
void ThreadedGenerateData(const OutputRegionType &outputRegionForThread, itk::ThreadIdType threadId) override
PixelType Get(void) const
VectorImageType::PointType PointType
Definition: mvdTypes.h:189
void Fill(SizeValueType value)
DataObject * GetOutput(const DataObjectIdentifierType &key)