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