otbSARDopplerCentroidFreqImageFilter.txx

/*
 * Copyright (C) 2005-2018 Centre National d'Etudes Spatiales (CNES)
 *
 * This file is part of Orfeo Toolbox
 *
 *     https://www.orfeo-toolbox.org/
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef otbSARDopplerCentroidFreqImageFilter_txx
#define otbSARDopplerCentroidFreqImageFilter_txx

#include "otbSARDopplerCentroidFreqImageFilter.h"

#include "itkImageScanlineConstIterator.h"
#include "itkImageScanlineIterator.h"
#include "itkProgressReporter.h"
#include "itkNumericTraitsPointPixel.h"

#include "otbSarSensorModelAdapter.h"

#include <cmath>
#include <algorithm>
#include <omp.h>

#if defined(USE_BOOST_TIME)
using boost::posix_time::microseconds;
using boost::posix_time::seconds;
#else
using ossimplugins::time::microseconds;
using ossimplugins::time::seconds;
#endif

namespace otb
{
  /** 
   * Constructor with default initialization
   */
  template <class TImage> 
  SARDopplerCentroidFreqImageFilter< TImage >::SARDopplerCentroidFreqImageFilter()
    :  m_FirstAziTime(0), m_FirstRangeTime(0), m_MidAziTime(0),
       m_VSatAtMidAziTime(0), m_Ks(0), m_AziTimeInt(0),m_RangeSamplingRate(0), m_FM_C0(0), m_FM_C1(0), 
       m_FM_C2(0), m_FM_Tau0(0), m_DCF_C0(0), m_DCF_C1(0), m_DCF_C2(0), m_DCF_Tau0(0), m_RefTime0(0), 
       m_FirstEstimation(true)
  {
  }
    
  /** 
   * Destructor
   */
  template <class TImage> 
  SARDopplerCentroidFreqImageFilter< TImage >::~SARDopplerCentroidFreqImageFilter()
  {
   
  }

  /**
   * Print
   */
  template<class TImage>
  void
  SARDopplerCentroidFreqImageFilter< TImage >
  ::PrintSelf(std::ostream & os, itk::Indent indent) const
  {
    Superclass::PrintSelf(os, indent);
  }

  template<class TImage>
  void
  SARDopplerCentroidFreqImageFilter< TImage >
  ::getAllCoefs(ImageKeywordlist  const& kwl, std::vector<FMRateRecordType> & FMRateRecords)
  {
    char fmRatePrefix_[1024];
    std::size_t nbLists(0);
    
    nbLists = std::stoi(kwl.GetMetadataByKey("azimuthFmRate.azi_fm_rate_coef_nb_list"));

    std::string FM_PREFIX = "azimuthFmRate.azi_fm_rate_coef_list";
    
    for (std::size_t listId=0; listId!= nbLists ; ++listId) 
      {
	const int pos = sprintf(fmRatePrefix_, "%s%zu.", FM_PREFIX.c_str(), (listId+1));
	assert(pos > 0 && pos < sizeof(fmRatePrefix_));
	const std::string FMPrefix(fmRatePrefix_, pos);

	FMRateRecordType fmRateRecord;
	fmRateRecord.azimuthFMRateTime = ossimplugins::time::toModifiedJulianDate(kwl.GetMetadataByKey(FMPrefix+ 
										      "azi_fm_rate_coef_time"));
	fmRateRecord.coef0FMRate = std::stod(kwl.GetMetadataByKey(FMPrefix + "1.azi_fm_rate_coef"));
	fmRateRecord.coef1FMRate = std::stod(kwl.GetMetadataByKey(FMPrefix + "2.azi_fm_rate_coef"));
	fmRateRecord.coef2FMRate = std::stod(kwl.GetMetadataByKey(FMPrefix + "3.azi_fm_rate_coef"));
	fmRateRecord.tau0FMRate = std::stod(kwl.GetMetadataByKey(FMPrefix + "slant_range_time"));

	FMRateRecords.push_back(fmRateRecord);
      }
  }

  template<class TImage>
  void
  SARDopplerCentroidFreqImageFilter< TImage >
  ::getAllCoefs(ImageKeywordlist  const& kwl, std::vector<DCFRecordType> & DCFRecords)
  {
     char dcfPrefix_[1024];
    std::size_t nbLists(0);
    
    nbLists = std::stoi(kwl.GetMetadataByKey("dopplerCentroid.dop_coef_nb_list"));

    std::string DCF_PREFIX = "dopplerCentroid.dop_coef_list";
    
    for (std::size_t listId=0; listId!= nbLists ; ++listId) 
      {
	const int pos = sprintf(dcfPrefix_, "%s%zu.", DCF_PREFIX.c_str(), (listId+1));
	assert(pos > 0 && pos < sizeof(dcfPrefix_));
	const std::string DCFPrefix(dcfPrefix_, pos);

	DCFRecordType dcfRecord;
	dcfRecord.azimuthDCFTime = ossimplugins::time::toModifiedJulianDate(kwl.GetMetadataByKey(DCFPrefix + 
									      "dop_coef_time"));
	dcfRecord.coef0DCF = std::stod(kwl.GetMetadataByKey(DCFPrefix + "1.dop_coef"));
	dcfRecord.coef1DCF = std::stod(kwl.GetMetadataByKey(DCFPrefix + "2.dop_coef"));
	dcfRecord.coef2DCF = std::stod(kwl.GetMetadataByKey(DCFPrefix + "3.dop_coef"));
	dcfRecord.tau0DCF = std::stod(kwl.GetMetadataByKey(DCFPrefix + "slant_range_time"));

	 DCFRecords.push_back(dcfRecord);
      }
  }

  /**
   * Method selectFMRateCoef
   */
  template<class TImage>
  bool 
  SARDopplerCentroidFreqImageFilter< TImage >
  ::selectFMRateCoef()
  {
    // Get input
    ImagePointer inputPtr = const_cast< ImageType * >( this->GetInput() );

     // Retrieve all polynomials
    std::vector<FMRateRecordType> FMRateRecords;

    ImageKeywordlist inputKWL = inputPtr->GetImageKeywordlist();
    
    this->getAllCoefs(inputKWL, FMRateRecords);

    DurationType diffAziTimeMin;
    unsigned int polySelected = 0;
    
    // Select one polynomial (with m_MidAziTime)
    for (unsigned int i = 0; i < FMRateRecords.size(); i++)
      {
	// Difference between midAziTime and aziTime of the current polynomial
	DurationType diffAziTime(0);
	if (m_MidAziTime > FMRateRecords[i].azimuthFMRateTime)
	  {
	    diffAziTime = DurationType(m_MidAziTime - FMRateRecords[i].azimuthFMRateTime);
	  }
	else
	  {
	    diffAziTime = DurationType(FMRateRecords[i].azimuthFMRateTime - m_MidAziTime);
	  }
	
	if (diffAziTime < diffAziTimeMin || i == 0)
	  {
	    diffAziTimeMin = diffAziTime;
	    polySelected = i;
	  }
      }

    // Assign m_FM_C0, m_FM_C1, m_FM_C2, m_FM_Tau0
    m_FM_C0 = FMRateRecords[polySelected].coef0FMRate;
    m_FM_C1 = FMRateRecords[polySelected].coef1FMRate;
    m_FM_C2 = FMRateRecords[polySelected].coef2FMRate;
    m_FM_Tau0 = FMRateRecords[polySelected].tau0FMRate;

    std::cout << "m_FM_C0 = " << m_FM_C0 << std::endl;
    std::cout << "m_FM_C1 = " << m_FM_C1 << std::endl;
    std::cout << "m_FM_C2 = " << m_FM_C2 << std::endl;
    std::cout << "m_FM_Tau0 = " << m_FM_Tau0 << std::endl;
    
    return true;
  }

 /**
   * Method selectDCFCoef
   */
  template<class TImage>
  bool 
  SARDopplerCentroidFreqImageFilter< TImage >
  ::selectDCFCoef()
  {
     // Get input
    ImagePointer inputPtr = const_cast< ImageType * >( this->GetInput() );

     // Retrieve all polynomials
    std::vector<DCFRecordType> DCFRecords;
    
    ImageKeywordlist inputKWL = inputPtr->GetImageKeywordlist();
    
    this->getAllCoefs(inputKWL, DCFRecords);

    DurationType diffAziTimeMin;
    unsigned int polySelected = 0;
    
    // Select one polynomial (with m_MidAziTime)
    for (unsigned int i = 0; i < DCFRecords.size(); i++)
      {
	// Difference between midAziTime and aziTime of the current polynomial
	DurationType diffAziTime(0);
	if (m_MidAziTime > DCFRecords[i].azimuthDCFTime)
	  {
	    diffAziTime = DurationType(m_MidAziTime - DCFRecords[i].azimuthDCFTime);
	  }
	else
	  {
	    diffAziTime = DurationType(DCFRecords[i].azimuthDCFTime - m_MidAziTime);
	  }
	
	if (diffAziTime < diffAziTimeMin || i == 0)
	  {
	    diffAziTimeMin = diffAziTime;
	    polySelected = i;
	  }
      }

    // Assign m_FM_C0, m_FM_C1, m_FM_C2, m_FM_Tau0
    m_DCF_C0 = DCFRecords[polySelected].coef0DCF;
    m_DCF_C1 = DCFRecords[polySelected].coef1DCF;
    m_DCF_C2 = DCFRecords[polySelected].coef2DCF;
    m_DCF_Tau0 = DCFRecords[polySelected].tau0DCF;

    std::cout << "m_DCF_C0 = " << m_DCF_C0 << std::endl;
    std::cout << "m_DCF_C1 = " << m_DCF_C1 << std::endl;
    std::cout << "m_DCF_C2 = " << m_DCF_C2 << std::endl;
    std::cout << "m_DCF_Tau0 = " << m_DCF_Tau0 << std::endl;
    
    return true;
  }

  /**
   * Apply FM Rate coefficients Method
   */
  template<class TImage>
  long double
  SARDopplerCentroidFreqImageFilter<TImage>
  ::applyFMRateCoefs(double index_sample)
  {
    double slant_range_time = m_FirstRangeTime + (index_sample / m_RangeSamplingRate);
    
    return (m_FM_C0 + m_FM_C1*(slant_range_time - m_FM_Tau0) + m_FM_C2*(slant_range_time - m_FM_Tau0)*
	    (slant_range_time - m_FM_Tau0));
  }

  /**
   * Apply doppler centroid Frequency coefficients Method
   */
  template<class TImage>
  long double
  SARDopplerCentroidFreqImageFilter<TImage>
  ::applyDCFCoefs(double index_sample)
  {
    double slant_range_time = m_FirstRangeTime + (index_sample / m_RangeSamplingRate);
    
    return (m_DCF_C0 + m_DCF_C1*(slant_range_time - m_DCF_Tau0) + m_DCF_C2*(slant_range_time - m_DCF_Tau0)*
	    (slant_range_time - m_DCF_Tau0));
  }
  
  /** 
   * Method GenerateInputRequestedRegion
   */
  template<class TImage>
  void
  SARDopplerCentroidFreqImageFilter< TImage >
  ::GenerateInputRequestedRegion()
  {
    // call the superclass' implementation of this method
    Superclass::GenerateInputRequestedRegion();
    
    // Get Output requested region
    ImageRegionType outputRequestedRegion = this->GetOutput()->GetRequestedRegion();

     ///////////// For Input image same region  /////////////
    ImagePointer  inputPtr = const_cast< ImageType * >( this->GetInput() );
    inputPtr->SetRequestedRegion(outputRequestedRegion);
  }
  

  /**
   * Method ThreadedGenerateData
   */
  template<class TImage>
  void
  SARDopplerCentroidFreqImageFilter<TImage>
  ::BeforeThreadedGenerateData()
  {
    // Estimates general parameters for the current burst, if m_FirstEstimation == true
    if (m_FirstEstimation)
      {
	m_FirstEstimation = false;

	// Get input
	ImagePointer inputPtr = const_cast< ImageType * >( this->GetInput() );
	// Choose KeyWordList
	ImageKeywordlist inputKWL = inputPtr->GetImageKeywordlist();
	
	// Check version of header/kwl (at least 3)
	int headerVersion = std::stoi(inputKWL.GetMetadataByKey("header.version"));

	if (headerVersion < 3)
	  {
	    itkExceptionMacro(<<"Header version is inferior to 3. Please Upgrade your geom file");
	  }

	// Get some metadata
	double aziSteeringRate = std::stod(inputKWL.GetMetadataByKey("support_data.azimuth_steering_rate"));
	// Conversion to radians per seconds
	aziSteeringRate *= (M_PI/180); 

	std::cout << "aziSteeringRate = " << aziSteeringRate << std::endl; 

	m_FirstAziTime = ossimplugins::time::toModifiedJulianDate((inputKWL.GetMetadataByKey("support_data.first_line_time")));
	m_FirstRangeTime = std::stod(inputKWL.GetMetadataByKey("support_data.slant_range_to_first_pixel"));

	m_AziTimeInt = std::stod(inputKWL.GetMetadataByKey("support_data.line_time_interval"));
	m_RangeSamplingRate = std::stod(inputKWL.GetMetadataByKey("support_data.range_sampling_rate"));

	double radarFrequency = std::stod(inputKWL.GetMetadataByKey("support_data.radar_frequency"));
	
	int nbLineBurst = std::stod(inputKWL.GetMetadataByKey("support_data.geom.bursts.number_lines_per_burst"));
	int nbSampleBurst = std::stod(inputKWL.GetMetadataByKey("support_data.geom.bursts.number_samples_per_burst"));
	// Estimation m_Ks
	m_LineAtMidBurst = nbLineBurst/2.;
	m_MidAziTime = m_FirstAziTime + seconds(m_AziTimeInt * m_LineAtMidBurst);
	
	// Try to create a SarSensorModelAdapter
	SarSensorModelAdapter::Pointer sarSensorModel = SarSensorModelAdapter::New();
	bool loadOk = sarSensorModel->LoadState(inputKWL);

	if(!loadOk || !sarSensorModel->IsValidSensorModel())
	  itkExceptionMacro(<<"Input image does not contain a valid SAR sensor model.");

	Point3DType satpos, satvel;
 
	bool lineToSatPosAndVelOk = sarSensorModel->LineToSatPositionAndVelocity(m_LineAtMidBurst, satpos, satvel);

	if (!lineToSatPosAndVelOk)
	  itkExceptionMacro(<<"Failed to estimate satellite position and velocity.");
  
	m_VSatAtMidAziTime = std::sqrt(satvel[0]*satvel[0] + satvel[1]*satvel[1] + satvel[2]*satvel[2]);

	m_Ks = (2*m_VSatAtMidAziTime/C) * radarFrequency * aziSteeringRate;
	
	std::cout << "m_FirstAziTime = " << m_FirstAziTime << std::endl;
	std::cout << "m_MidAziTime = " << m_MidAziTime << std::endl;

	DurationType diffTime = m_MidAziTime - m_FirstAziTime;

	std::cout << "diffTime = " << diffTime << std::endl;

	m_MidRanTime = m_FirstRangeTime + (nbSampleBurst / (2*m_RangeSamplingRate));

	// Polynomial selection (FM Rate and Doppler Centroid Frequency)
	this->selectFMRateCoef();
	this->selectDCFCoef();

	// Estimate Reference time at first sample
	m_RefTime0 = - (this->applyDCFCoefs(0) / this->applyFMRateCoefs(0));

	m_RefTimeMid =  - (this->applyDCFCoefs(nbSampleBurst / 2) / this->applyFMRateCoefs(nbSampleBurst / 2));

	std::cout << "m_RefTime0 = " << m_RefTime0 << std::endl;
      }
  }
   

  /**
   * Method ThreadedGenerateData
   */
  template<class TImage>
  void
  SARDopplerCentroidFreqImageFilter<TImage>
  ::ThreadedGenerateData(const ImageRegionType & outputRegionForThread,
			 itk::ThreadIdType /*threadId*/)
  {
    // Compute corresponding input region for master and slave cartesian mean
    ImageRegionType inputRegionForThread = outputRegionForThread;

    // Iterator on output
    OutputIterator OutIt(this->GetOutput(), outputRegionForThread);
    OutIt.GoToBegin();

    // Iterator on input 
    InputIterator  InIt(this->GetInput(), inputRegionForThread);
    InIt.GoToBegin();

    // For each line
    while (!OutIt.IsAtEnd() && !InIt.IsAtEnd())
      {
	OutIt.GoToBeginOfLine();
	InIt.GoToBeginOfLine();


	// For each colunm
	while (!OutIt.IsAtEndOfLine() && !InIt.IsAtEndOfLine()) 
	  {
	    // Input index
	    ImageIndexType currentInputIndex = InIt.GetIndex();
	    Point2DType currentInputPoint;
	    this->GetInput()->TransformIndexToPhysicalPoint(currentInputIndex,currentInputPoint);
	    
	    double indL = currentInputPoint[1] - 0.5;
	    double indC = currentInputPoint[0] - 0.5;

	    // Zero Doppler azimuth Time
	    TimeType aziDur = m_FirstAziTime + seconds(m_AziTimeInt*indL);

	    // Reference Time
	    TimeType refDur = m_MidAziTime + (seconds( - (this->applyDCFCoefs(indC) / this->applyFMRateCoefs(indC)) - m_RefTimeMid));
	    // Kt
	    double Kt = (this->applyFMRateCoefs(indC) * m_Ks) / (this->applyFMRateCoefs(indC) - m_Ks);

	    double diffTime = DurationType(aziDur - refDur).total_seconds();
	    
	    // Shift for Doppler centroid
	    ImagePixelType dopCentroid_shift = static_cast<ImagePixelType>(this->applyDCFCoefs(indC) + 
									   Kt * diffTime);
   
	    //////////// Assign Output ////////////
	    OutIt.Set(dopCentroid_shift);

	    // Increment iterators
	    ++OutIt;
	    ++InIt;
	  } // End colunms (ouput)

	// Next Line
	OutIt.NextLine();
	InIt.NextLine();
      } // End lines (ouput)
  }


} /*namespace otb*/

#endif