text: /*=========================================================================
   *
   *  Copyright Insight Software Consortium
   *
   *  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.txt
   *
   *  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 itkExpectationBasedPointSetToPointSetMetricv4_hxx
  #define itkExpectationBasedPointSetToPointSetMetricv4_hxx
  
  #include "itkExpectationBasedPointSetToPointSetMetricv4.h"
  #include "itkArray.h"
  
  namespace itk {
  
  /** Constructor */
  template<typename TFixedPointSet, typename TMovingPointSet, class TInternalComputationValueType>
  ExpectationBasedPointSetToPointSetMetricv4<TFixedPointSet, TMovingPointSet, TInternalComputationValueType>
  ::ExpectationBasedPointSetToPointSetMetricv4() :
    m_PointSetSigma( 1.0 ),
    m_PreFactor( 0.0 ),
    m_Denominator( 0.0 )
  
  {
  }
  
  /** Destructor */
  template<typename TFixedPointSet, typename TMovingPointSet, class TInternalComputationValueType>
  ExpectationBasedPointSetToPointSetMetricv4<TFixedPointSet, TMovingPointSet, TInternalComputationValueType>
  ::~ExpectationBasedPointSetToPointSetMetricv4() = default;
  
  template<typename TFixedPointSet, typename TMovingPointSet, class TInternalComputationValueType>
  void
  ExpectationBasedPointSetToPointSetMetricv4<TFixedPointSet, TMovingPointSet, TInternalComputationValueType>
  ::Initialize()
  {
    Superclass::Initialize();
  
    if( this->m_PointSetSigma <= NumericTraits<CoordRepType>::epsilon() )
      {
      itkExceptionMacro("m_PointSetSigma is too small. <= epsilon");
      }
    this->m_PreFactor = 1.0 / ( std::sqrt( 2 * itk::Math::pi ) * this->m_PointSetSigma );
    this->m_Denominator = 2.0 * itk::Math::sqr( this->m_PointSetSigma );
  }
  
  template<typename TFixedPointSet, typename TMovingPointSet, class TInternalComputationValueType>
  typename ExpectationBasedPointSetToPointSetMetricv4<TFixedPointSet, TMovingPointSet, TInternalComputationValueType>
  ::MeasureType
  ExpectationBasedPointSetToPointSetMetricv4<TFixedPointSet, TMovingPointSet, TInternalComputationValueType>
  ::GetLocalNeighborhoodValue( const PointType & point, const PixelType & itkNotUsed( pixel ) ) const
  {
    MeasureType localValue = NumericTraits<MeasureType>::ZeroValue();
  
    NeighborsIdentifierType neighborhood;
    this->m_MovingTransformedPointsLocator->FindClosestNPoints( point, this->m_EvaluationKNeighborhood, neighborhood );
  
    for( NeighborsIterator it = neighborhood.begin(); it != neighborhood.end(); ++it )
      {
      PointType neighbor = this->m_MovingTransformedPointSet->GetPoint( *it );
      const MeasureType distance = point.SquaredEuclideanDistanceTo( neighbor );
      localValue -= this->m_PreFactor * std::exp( -distance / this->m_Denominator );
      }
  
    return localValue;
  }
  
  template<typename TFixedPointSet, typename TMovingPointSet, class TInternalComputationValueType>
  void
  ExpectationBasedPointSetToPointSetMetricv4<TFixedPointSet, TMovingPointSet, TInternalComputationValueType>
  ::GetLocalNeighborhoodValueAndDerivative( const PointType & point,
    MeasureType &measure, LocalDerivativeType &localDerivative, const PixelType & itkNotUsed( pixel ) ) const
  {
    Array<MeasureType> measureValues;
    measureValues.SetSize( this->m_EvaluationKNeighborhood );
    measureValues.Fill( 0.0 );
  
    measure = NumericTraits< MeasureType >::ZeroValue();
  
    localDerivative.Fill( 0.0 );
  
    PointType weightedPoint;
    weightedPoint.Fill( 0.0 );
  
    NeighborsIdentifierType neighborhood;
  
    this->m_MovingTransformedPointsLocator->FindClosestNPoints( point, this->m_EvaluationKNeighborhood, neighborhood );
  
    for( NeighborsIterator it = neighborhood.begin(); it != neighborhood.end(); ++it )
      {
      PointType neighbor = this->m_MovingTransformedPointSet->GetPoint( *it );
      const MeasureType distance = point.SquaredEuclideanDistanceTo( neighbor );
      measureValues[it - neighborhood.begin()] = -this->m_PreFactor * std::exp( -distance / this->m_Denominator );
      measure += measureValues[it - neighborhood.begin()];
      }
  
    if ( std::fabs(measure) <= NumericTraits<MeasureType>::epsilon() )
      {
      return;
      }
  
    for( NeighborsIterator it = neighborhood.begin(); it != neighborhood.end(); ++it )
      {
      PointType neighbor = this->m_MovingTransformedPointSet->GetPoint( *it );
      VectorType neighborVector = neighbor.GetVectorFromOrigin();
      weightedPoint += ( neighborVector * measureValues[it - neighborhood.begin()] / measure );
      }
  
    const MeasureType distance = point.SquaredEuclideanDistanceTo( weightedPoint );
  
    const MeasureType weight = this->m_PreFactor * std::exp( -distance / this->m_Denominator ) / -measure;
  
    VectorType force = ( weightedPoint - point ) * weight;
  
    for( unsigned int d = 0; d < localDerivative.Size(); d++ )
      {
      localDerivative[d] = force[d];
      }
  }
  
  template<typename TFixedPointSet, typename TMovingPointSet, class TInternalComputationValueType>
  typename LightObject::Pointer
  ExpectationBasedPointSetToPointSetMetricv4<TFixedPointSet, TMovingPointSet, TInternalComputationValueType>
  ::InternalClone() const
  {
    typename Self::Pointer rval = Self::New();
    rval->SetMovingPointSet( this->m_MovingPointSet );
    rval->SetFixedPointSet( this->m_FixedPointSet );
    rval->SetPointSetSigma( this->m_PointSetSigma );
    rval->SetEvaluationKNeighborhood( this->m_EvaluationKNeighborhood );
  
    return rval.GetPointer();
  }
  
  template<typename TFixedPointSet, typename TMovingPointSet, class TInternalComputationValueType>
  void
  ExpectationBasedPointSetToPointSetMetricv4<TFixedPointSet, TMovingPointSet, TInternalComputationValueType>
  ::PrintSelf( std::ostream& os, Indent indent ) const
  {
    Superclass::PrintSelf( os, indent );
  
    os << indent << "PointSetSigma: " << this->m_PointSetSigma << std::endl;
    os << indent << "EvaluateKNeighborhood: " << this->m_EvaluationKNeighborhood << std::endl;
  }
  } // end namespace itk
  
  
  #endif
  