<commit_before>/*=========================================================================
 *
 *  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 itkLabelOverlapMeasuresImageFilter_hxx
#define itkLabelOverlapMeasuresImageFilter_hxx

#include "itkLabelOverlapMeasuresImageFilter.h"

#include "itkImageRegionConstIterator.h"
#include "itkProgressReporter.h"

namespace itk {

template<typename TLabelImage>
LabelOverlapMeasuresImageFilter<TLabelImage>
::LabelOverlapMeasuresImageFilter()
{
  // this filter requires two input images
  this->SetNumberOfRequiredInputs( 2 );
}

template<typename TLabelImage>
void
LabelOverlapMeasuresImageFilter<TLabelImage>
::EnlargeOutputRequestedRegion( DataObject *data )
{
  Superclass::EnlargeOutputRequestedRegion( data );
  data->SetRequestedRegionToLargestPossibleRegion();
}

template<typename TLabelImage>
void
LabelOverlapMeasuresImageFilter<TLabelImage>
::AllocateOutputs()
{
  // Pass the source through as the output
  LabelImagePointer image =
    const_cast<TLabelImage *>( this->GetSourceImage() );
  this->GraftOutput(image);
}

template<typename TLabelImage>
void
LabelOverlapMeasuresImageFilter<TLabelImage>
::BeforeThreadedGenerateData()
{
  ThreadIdType numberOfThreads = this->GetNumberOfThreads();

  // Resize the thread temporaries
  this->m_LabelSetMeasuresPerThread.resize( numberOfThreads );

  // Initialize the temporaries
  for( ThreadIdType n = 0; n < numberOfThreads; n++ )
    {
    this->m_LabelSetMeasuresPerThread[n].clear();
    }

  // Initialize the final map
  this->m_LabelSetMeasures.clear();
}

template<typename TLabelImage>
void
LabelOverlapMeasuresImageFilter<TLabelImage>
::AfterThreadedGenerateData()
{
  // Run through the map for each thread and accumulate the set measures.
  for( ThreadIdType n = 0; n < this->GetNumberOfThreads(); n++ )
    {
    // iterate over the map for this thread
    for( MapConstIterator threadIt = this->m_LabelSetMeasuresPerThread[n].begin();
         threadIt != this->m_LabelSetMeasuresPerThread[n].end();
         ++threadIt )
      {
      // does this label exist in the cumulative structure yet?
      MapIterator mapIt = this->m_LabelSetMeasures.find( ( *threadIt ).first );
      if( mapIt == this->m_LabelSetMeasures.end() )
        {
        // create a new entry
        typedef typename MapType::value_type MapValueType;
        mapIt = this->m_LabelSetMeasures.insert( MapValueType(
                                                   (*threadIt).first, LabelSetMeasures() ) ).first;
        }

      // accumulate the information from this thread
      (*mapIt).second.m_Source += (*threadIt).second.m_Source;
      (*mapIt).second.m_Target += (*threadIt).second.m_Target;
      (*mapIt).second.m_Union += (*threadIt).second.m_Union;
      (*mapIt).second.m_Intersection +=
        (*threadIt).second.m_Intersection;
      (*mapIt).second.m_SourceComplement +=
        (*threadIt).second.m_SourceComplement;
      (*mapIt).second.m_TargetComplement +=
        (*threadIt).second.m_TargetComplement;
      } // end of thread map iterator loop
    } // end of thread loop
}

template<typename TLabelImage>
void
LabelOverlapMeasuresImageFilter<TLabelImage>
::ThreadedGenerateData( const RegionType& outputRegionForThread,
                        ThreadIdType threadId )
{
  ImageRegionConstIterator<LabelImageType> itS( this->GetSourceImage(),
                                                outputRegionForThread );
  ImageRegionConstIterator<LabelImageType> itT( this->GetTargetImage(),
                                                outputRegionForThread );

  // support progress methods/callbacks
  ProgressReporter progress( this, threadId,
                             2*outputRegionForThread.GetNumberOfPixels() );

  for( itS.GoToBegin(), itT.GoToBegin(); !itS.IsAtEnd(); ++itS, ++itT )
    {
    LabelType sourceLabel = itS.Get();
    LabelType targetLabel = itT.Get();

    // is the label already in this thread?
    MapIterator mapItS =
      this->m_LabelSetMeasuresPerThread[threadId].find( sourceLabel );
    MapIterator mapItT =
      this->m_LabelSetMeasuresPerThread[threadId].find( targetLabel );

    if( mapItS == this->m_LabelSetMeasuresPerThread[threadId].end() )
      {
      // create a new label set measures object
      typedef typename MapType::value_type MapValueType;
      mapItS = this->m_LabelSetMeasuresPerThread[threadId].insert(
        MapValueType( sourceLabel, LabelSetMeasures() ) ).first;
      }

    if( mapItT == this->m_LabelSetMeasuresPerThread[threadId].end() )
      {
      // create a new label set measures object
      typedef typename MapType::value_type MapValueType;
      mapItT = this->m_LabelSetMeasuresPerThread[threadId].insert(
        MapValueType( targetLabel, LabelSetMeasures() ) ).first;
      }

    (*mapItS).second.m_Source++;
    (*mapItT).second.m_Target++;

    if( sourceLabel == targetLabel )
      {
      (*mapItS).second.m_Intersection++;
      (*mapItS).second.m_Union++;
      }
    else
      {
      (*mapItS).second.m_Union++;
      (*mapItT).second.m_Union++;

      (*mapItS).second.m_SourceComplement++;
      (*mapItT).second.m_TargetComplement++;
      }

    progress.CompletedPixel();
    }
}

/**
 *  measures
 */
template<typename TLabelImage>
typename LabelOverlapMeasuresImageFilter<TLabelImage>::RealType
LabelOverlapMeasuresImageFilter<TLabelImage>
::GetTotalOverlap() const
{
  RealType numerator = 0.0;
  RealType denominator = 0.0;
  for( MapConstIterator mapIt = this->m_LabelSetMeasures.begin();
       mapIt != this->m_LabelSetMeasures.end(); ++mapIt )
    {
    // Do not include the background in the final value.
    if( (*mapIt).first == NumericTraits<LabelType>::ZeroValue() )
      {
      continue;
      }
    numerator += static_cast<RealType>( (*mapIt).second.m_Intersection );
    denominator += static_cast<RealType>( (*mapIt).second.m_Target );
    }

  if( Math::ExactlyEquals(denominator,0.0) )
    {
    return NumericTraits<RealType>::max();
    }
  else
    {
    return ( numerator / denominator );
    }
}

template<typename TLabelImage>
typename LabelOverlapMeasuresImageFilter<TLabelImage>::RealType
LabelOverlapMeasuresImageFilter<TLabelImage>
::GetTargetOverlap( LabelType label ) const
{
  MapConstIterator mapIt = this->m_LabelSetMeasures.find( label );
  if( mapIt == this->m_LabelSetMeasures.end() )
    {
    itkWarningMacro( "Label " << label << " not found." );
    return 0.0;
    }

  //std::cout << "GetTargetOverlap ::: 001" << std::endl;
  RealType value;

  if((*mapIt).second.m_Target == 0)
    {
    value = NumericTraits<RealType>::max();
    }
  else
    {
    value=
      static_cast<RealType>( (*mapIt).second.m_Intersection ) /
      static_cast<RealType>( (*mapIt).second.m_Target );
    }
  return value;
}

template<typename TLabelImage>
typename LabelOverlapMeasuresImageFilter<TLabelImage>::RealType
LabelOverlapMeasuresImageFilter<TLabelImage>
::GetUnionOverlap() const
{
  RealType numerator = 0.0;
  RealType denominator = 0.0;
  for( MapConstIterator mapIt = this->m_LabelSetMeasures.begin();
       mapIt != this->m_LabelSetMeasures.end(); ++mapIt )
    {
    // Do not include the background in the final value.
    if( (*mapIt).first == NumericTraits<LabelType>::ZeroValue() )
      {
      continue;
      }
    numerator += static_cast<RealType>( (*mapIt).second.m_Intersection );
    denominator += static_cast<RealType>( (*mapIt).second.m_Union );
    }

  if( Math::ExactlyEquals(denominator,0.0) )
    {
    return NumericTraits<RealType>::max();
    }
  else
    {
    return ( numerator / denominator );
    }
}

template<typename TLabelImage>
typename LabelOverlapMeasuresImageFilter<TLabelImage>::RealType
LabelOverlapMeasuresImageFilter<TLabelImage>
::GetUnionOverlap( LabelType label ) const
{
  MapConstIterator mapIt = this->m_LabelSetMeasures.find( label );
  if( mapIt == this->m_LabelSetMeasures.end() )
    {
    itkWarningMacro( "Label " << label << " not found." );
    return 0.0;
    }

  RealType value;
  if(Math::ExactlyEquals((*mapIt).second.m_Union, 0.0))
    {
    value = NumericTraits<RealType>::max();
    }
  else
    {
    value =
      static_cast<RealType>( (*mapIt).second.m_Intersection ) /
      static_cast<RealType>( (*mapIt).second.m_Union );
    }

  return value;
}

template<typename TLabelImage>
typename LabelOverlapMeasuresImageFilter<TLabelImage>::RealType
LabelOverlapMeasuresImageFilter<TLabelImage>
::GetMeanOverlap() const
{
  RealType uo = this->GetUnionOverlap();
  return ( 2.0 * uo / ( 1.0 + uo ) );
}

template<typename TLabelImage>
typename LabelOverlapMeasuresImageFilter<TLabelImage>::RealType
LabelOverlapMeasuresImageFilter<TLabelImage>
::GetMeanOverlap( LabelType label ) const
{
  RealType uo = this->GetUnionOverlap( label );
  return ( 2.0 * uo / ( 1.0 + uo ) );
}

template<typename TLabelImage>
typename LabelOverlapMeasuresImageFilter<TLabelImage>::RealType
LabelOverlapMeasuresImageFilter<TLabelImage>
::GetVolumeSimilarity() const
{
  RealType numerator = 0.0;
  RealType denominator = 0.0;
  for( MapConstIterator mapIt = this->m_LabelSetMeasures.begin();
       mapIt != this->m_LabelSetMeasures.end(); ++mapIt )
    {
    // Do not include the background in the final value.
    if( (*mapIt).first == NumericTraits<LabelType>::ZeroValue() )
      {
      continue;
      }
    numerator += ( ( static_cast<RealType>( (*mapIt).second.m_Source ) -
                     static_cast<RealType>( (*mapIt).second.m_Target ) ) );
    denominator += ( ( static_cast<RealType>( (*mapIt).second.m_Source ) +
                       static_cast<RealType>( (*mapIt).second.m_Target ) ) );
    }

  if( Math::ExactlyEquals(denominator,0.0) )
    {
    return NumericTraits<RealType>::max();
    }
  else
    {
    return ( 2.0 * numerator / denominator );
    }
}

template<typename TLabelImage>
typename LabelOverlapMeasuresImageFilter<TLabelImage>::RealType
LabelOverlapMeasuresImageFilter<TLabelImage>
::GetVolumeSimilarity( LabelType label ) const
{
  MapConstIterator mapIt = this->m_LabelSetMeasures.find( label );
  if( mapIt == this->m_LabelSetMeasures.end() )
    {
    itkWarningMacro( "Label " << label << " not found." );
    return 0.0;
    }
  RealType value = 2.0 *
    ( static_cast<RealType>( (*mapIt).second.m_Source ) -
      static_cast<RealType>( (*mapIt).second.m_Target ) ) /
    ( static_cast<RealType>( (*mapIt).second.m_Source ) +
      static_cast<RealType>( (*mapIt).second.m_Target ) );
  return value;
}

template<typename TLabelImage>
typename LabelOverlapMeasuresImageFilter<TLabelImage>::RealType
LabelOverlapMeasuresImageFilter<TLabelImage>
::GetFalseNegativeError() const
{
  RealType numerator = 0.0;
  RealType denominator = 0.0;
  for( MapConstIterator mapIt = this->m_LabelSetMeasures.begin();
       mapIt != this->m_LabelSetMeasures.end(); ++mapIt )
    {
    // Do not include the background in the final value.
    if( (*mapIt).first == NumericTraits<LabelType>::ZeroValue() )
      {
      continue;
      }
    numerator += static_cast<RealType>( (*mapIt).second.m_TargetComplement );
    denominator += static_cast<RealType>( (*mapIt).second.m_Target );
    }

  if( Math::ExactlyEquals(denominator,0.0) )
    {
    return NumericTraits<RealType>::max();
    }
  else
    {
    return ( numerator / denominator );
    }
}

template<typename TLabelImage>
typename LabelOverlapMeasuresImageFilter<TLabelImage>::RealType
LabelOverlapMeasuresImageFilter<TLabelImage>
::GetFalseNegativeError( LabelType label ) const
{
  MapConstIterator mapIt = this->m_LabelSetMeasures.find( label );
  if( mapIt == this->m_LabelSetMeasures.end() )
    {
    itkWarningMacro( "Label " << label << " not found." );
    return 0.0;
    }

  RealType value;
  if(Math::ExactlyEquals((*mapIt).second.m_Target, 0.0))
    {
    value = NumericTraits<RealType>::max();
    }
  else
    {
    value =
      static_cast<RealType>( (*mapIt).second.m_TargetComplement ) /
      static_cast<RealType>( (*mapIt).second.m_Target );
    }

  return value;
}

template<typename TLabelImage>
typename LabelOverlapMeasuresImageFilter<TLabelImage>::RealType
LabelOverlapMeasuresImageFilter<TLabelImage>
::GetFalsePositiveError() const
{
  RealType numerator = 0.0;
  RealType denominator = 0.0;
  for( MapConstIterator mapIt = this->m_LabelSetMeasures.begin();
       mapIt != this->m_LabelSetMeasures.end(); ++mapIt )
    {
    // Do not include the background in the final value.
    if( (*mapIt).first == NumericTraits<LabelType>::ZeroValue() )
      {
      continue;
      }
    numerator += static_cast<RealType>( (*mapIt).second.m_SourceComplement );
    denominator += static_cast<RealType>( (*mapIt).second.m_Source );
    }

  if( Math::ExactlyEquals(denominator,0.0) )
    {
    return NumericTraits<RealType>::max();
    }
  else
    {
    return ( numerator / denominator );
    }
}

template<typename TLabelImage>
typename LabelOverlapMeasuresImageFilter<TLabelImage>::RealType
LabelOverlapMeasuresImageFilter<TLabelImage>
::GetFalsePositiveError( LabelType label ) const
{
  MapConstIterator mapIt = this->m_LabelSetMeasures.find( label );
  if( mapIt == this->m_LabelSetMeasures.end() )
    {
    itkWarningMacro( "Label " << label << " not found." );
    return 0.0;
    }

  RealType value;
  if(Math::ExactlyEquals((*mapIt).second.m_Source, 0.0))
    {
    value = NumericTraits<RealType>::max();
    }
  else
    {
    value =
      static_cast<RealType>( (*mapIt).second.m_SourceComplement ) /
      static_cast<RealType>( (*mapIt).second.m_Source );
    }

  return value;
}

template<typename TLabelImage>
void
LabelOverlapMeasuresImageFilter<TLabelImage>
::PrintSelf( std::ostream& os, Indent indent ) const
{
  // todo!!!
  Superclass::PrintSelf( os, indent );
}

}// end namespace itk
#endif
<commit_msg>STYLE: Improve itkLabelOverlapMeasurementImageFilter style.<commit_after>/*=========================================================================
 *
 *  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 itkLabelOverlapMeasuresImageFilter_hxx
#define itkLabelOverlapMeasuresImageFilter_hxx

#include "itkLabelOverlapMeasuresImageFilter.h"

#include "itkImageRegionConstIterator.h"
#include "itkProgressReporter.h"

namespace itk {

template<typename TLabelImage>
LabelOverlapMeasuresImageFilter<TLabelImage>
::LabelOverlapMeasuresImageFilter()
{
  // This filter requires two input images
  this->SetNumberOfRequiredInputs( 2 );
}

template<typename TLabelImage>
void
LabelOverlapMeasuresImageFilter<TLabelImage>
::EnlargeOutputRequestedRegion( DataObject *data )
{
  Superclass::EnlargeOutputRequestedRegion( data );
  data->SetRequestedRegionToLargestPossibleRegion();
}

template<typename TLabelImage>
void
LabelOverlapMeasuresImageFilter<TLabelImage>
::AllocateOutputs()
{
  // Pass the source through as the output
  LabelImagePointer image =
    const_cast<TLabelImage *>( this->GetSourceImage() );
  this->GraftOutput(image);
}

template<typename TLabelImage>
void
LabelOverlapMeasuresImageFilter<TLabelImage>
::BeforeThreadedGenerateData()
{
  ThreadIdType numberOfThreads = this->GetNumberOfThreads();

  // Resize the thread temporaries
  this->m_LabelSetMeasuresPerThread.resize( numberOfThreads );

  // Initialize the temporaries
  for( ThreadIdType n = 0; n < numberOfThreads; n++ )
    {
    this->m_LabelSetMeasuresPerThread[n].clear();
    }

  // Initialize the final map
  this->m_LabelSetMeasures.clear();
}

template<typename TLabelImage>
void
LabelOverlapMeasuresImageFilter<TLabelImage>
::AfterThreadedGenerateData()
{
  // Run through the map for each thread and accumulate the set measures.
  for( ThreadIdType n = 0; n < this->GetNumberOfThreads(); n++ )
    {
    // Iterate over the map for this thread
    for( MapConstIterator threadIt = this->m_LabelSetMeasuresPerThread[n].begin();
         threadIt != this->m_LabelSetMeasuresPerThread[n].end();
         ++threadIt )
      {
      // Does this label exist in the cumulative structure yet?
      MapIterator mapIt = this->m_LabelSetMeasures.find( ( *threadIt ).first );
      if( mapIt == this->m_LabelSetMeasures.end() )
        {
        // Create a new entry
        typedef typename MapType::value_type MapValueType;
        mapIt = this->m_LabelSetMeasures.insert( MapValueType(
                                                   (*threadIt).first, LabelSetMeasures() ) ).first;
        }

      // Accumulate the information from this thread
      (*mapIt).second.m_Source += (*threadIt).second.m_Source;
      (*mapIt).second.m_Target += (*threadIt).second.m_Target;
      (*mapIt).second.m_Union += (*threadIt).second.m_Union;
      (*mapIt).second.m_Intersection +=
        (*threadIt).second.m_Intersection;
      (*mapIt).second.m_SourceComplement +=
        (*threadIt).second.m_SourceComplement;
      (*mapIt).second.m_TargetComplement +=
        (*threadIt).second.m_TargetComplement;
      } // end of thread map iterator loop
    } // end of thread loop
}

template<typename TLabelImage>
void
LabelOverlapMeasuresImageFilter<TLabelImage>
::ThreadedGenerateData( const RegionType& outputRegionForThread,
                        ThreadIdType threadId )
{
  ImageRegionConstIterator<LabelImageType> itS( this->GetSourceImage(),
                                                outputRegionForThread );
  ImageRegionConstIterator<LabelImageType> itT( this->GetTargetImage(),
                                                outputRegionForThread );

  // Support progress methods/callbacks
  ProgressReporter progress( this, threadId,
                             2*outputRegionForThread.GetNumberOfPixels() );

  for( itS.GoToBegin(), itT.GoToBegin(); !itS.IsAtEnd(); ++itS, ++itT )
    {
    LabelType sourceLabel = itS.Get();
    LabelType targetLabel = itT.Get();

    // Is the label already in this thread?
    MapIterator mapItS =
      this->m_LabelSetMeasuresPerThread[threadId].find( sourceLabel );
    MapIterator mapItT =
      this->m_LabelSetMeasuresPerThread[threadId].find( targetLabel );

    if( mapItS == this->m_LabelSetMeasuresPerThread[threadId].end() )
      {
      // Create a new label set measures object
      typedef typename MapType::value_type MapValueType;
      mapItS = this->m_LabelSetMeasuresPerThread[threadId].insert(
        MapValueType( sourceLabel, LabelSetMeasures() ) ).first;
      }

    if( mapItT == this->m_LabelSetMeasuresPerThread[threadId].end() )
      {
      // Create a new label set measures object
      typedef typename MapType::value_type MapValueType;
      mapItT = this->m_LabelSetMeasuresPerThread[threadId].insert(
        MapValueType( targetLabel, LabelSetMeasures() ) ).first;
      }

    (*mapItS).second.m_Source++;
    (*mapItT).second.m_Target++;

    if( sourceLabel == targetLabel )
      {
      (*mapItS).second.m_Intersection++;
      (*mapItS).second.m_Union++;
      }
    else
      {
      (*mapItS).second.m_Union++;
      (*mapItT).second.m_Union++;

      (*mapItS).second.m_SourceComplement++;
      (*mapItT).second.m_TargetComplement++;
      }

    progress.CompletedPixel();
    }
}

template<typename TLabelImage>
typename LabelOverlapMeasuresImageFilter<TLabelImage>::RealType
LabelOverlapMeasuresImageFilter<TLabelImage>
::GetTotalOverlap() const
{
  RealType numerator = 0.0;
  RealType denominator = 0.0;
  for( MapConstIterator mapIt = this->m_LabelSetMeasures.begin();
       mapIt != this->m_LabelSetMeasures.end(); ++mapIt )
    {
    // Do not include the background in the final value.
    if( (*mapIt).first == NumericTraits<LabelType>::ZeroValue() )
      {
      continue;
      }
    numerator += static_cast<RealType>( (*mapIt).second.m_Intersection );
    denominator += static_cast<RealType>( (*mapIt).second.m_Target );
    }

  if( Math::ExactlyEquals(denominator,0.0) )
    {
    return NumericTraits<RealType>::max();
    }
  else
    {
    return ( numerator / denominator );
    }
}

template<typename TLabelImage>
typename LabelOverlapMeasuresImageFilter<TLabelImage>::RealType
LabelOverlapMeasuresImageFilter<TLabelImage>
::GetTargetOverlap( LabelType label ) const
{
  MapConstIterator mapIt = this->m_LabelSetMeasures.find( label );
  if( mapIt == this->m_LabelSetMeasures.end() )
    {
    itkWarningMacro( "Label " << label << " not found." );
    return 0.0;
    }

  RealType value;

  if((*mapIt).second.m_Target == 0)
    {
    value = NumericTraits<RealType>::max();
    }
  else
    {
    value=
      static_cast<RealType>( (*mapIt).second.m_Intersection ) /
      static_cast<RealType>( (*mapIt).second.m_Target );
    }
  return value;
}

template<typename TLabelImage>
typename LabelOverlapMeasuresImageFilter<TLabelImage>::RealType
LabelOverlapMeasuresImageFilter<TLabelImage>
::GetUnionOverlap() const
{
  RealType numerator = 0.0;
  RealType denominator = 0.0;
  for( MapConstIterator mapIt = this->m_LabelSetMeasures.begin();
       mapIt != this->m_LabelSetMeasures.end(); ++mapIt )
    {
    // Do not include the background in the final value.
    if( (*mapIt).first == NumericTraits<LabelType>::ZeroValue() )
      {
      continue;
      }
    numerator += static_cast<RealType>( (*mapIt).second.m_Intersection );
    denominator += static_cast<RealType>( (*mapIt).second.m_Union );
    }

  if( Math::ExactlyEquals(denominator,0.0) )
    {
    return NumericTraits<RealType>::max();
    }
  else
    {
    return ( numerator / denominator );
    }
}

template<typename TLabelImage>
typename LabelOverlapMeasuresImageFilter<TLabelImage>::RealType
LabelOverlapMeasuresImageFilter<TLabelImage>
::GetUnionOverlap( LabelType label ) const
{
  MapConstIterator mapIt = this->m_LabelSetMeasures.find( label );
  if( mapIt == this->m_LabelSetMeasures.end() )
    {
    itkWarningMacro( "Label " << label << " not found." );
    return 0.0;
    }

  RealType value;
  if(Math::ExactlyEquals((*mapIt).second.m_Union, 0.0))
    {
    value = NumericTraits<RealType>::max();
    }
  else
    {
    value =
      static_cast<RealType>( (*mapIt).second.m_Intersection ) /
      static_cast<RealType>( (*mapIt).second.m_Union );
    }

  return value;
}

template<typename TLabelImage>
typename LabelOverlapMeasuresImageFilter<TLabelImage>::RealType
LabelOverlapMeasuresImageFilter<TLabelImage>
::GetMeanOverlap() const
{
  RealType uo = this->GetUnionOverlap();
  return ( 2.0 * uo / ( 1.0 + uo ) );
}

template<typename TLabelImage>
typename LabelOverlapMeasuresImageFilter<TLabelImage>::RealType
LabelOverlapMeasuresImageFilter<TLabelImage>
::GetMeanOverlap( LabelType label ) const
{
  RealType uo = this->GetUnionOverlap( label );
  return ( 2.0 * uo / ( 1.0 + uo ) );
}

template<typename TLabelImage>
typename LabelOverlapMeasuresImageFilter<TLabelImage>::RealType
LabelOverlapMeasuresImageFilter<TLabelImage>
::GetVolumeSimilarity() const
{
  RealType numerator = 0.0;
  RealType denominator = 0.0;
  for( MapConstIterator mapIt = this->m_LabelSetMeasures.begin();
       mapIt != this->m_LabelSetMeasures.end(); ++mapIt )
    {
    // Do not include the background in the final value.
    if( (*mapIt).first == NumericTraits<LabelType>::ZeroValue() )
      {
      continue;
      }
    numerator += ( ( static_cast<RealType>( (*mapIt).second.m_Source ) -
                     static_cast<RealType>( (*mapIt).second.m_Target ) ) );
    denominator += ( ( static_cast<RealType>( (*mapIt).second.m_Source ) +
                       static_cast<RealType>( (*mapIt).second.m_Target ) ) );
    }

  if( Math::ExactlyEquals(denominator,0.0) )
    {
    return NumericTraits<RealType>::max();
    }
  else
    {
    return ( 2.0 * numerator / denominator );
    }
}

template<typename TLabelImage>
typename LabelOverlapMeasuresImageFilter<TLabelImage>::RealType
LabelOverlapMeasuresImageFilter<TLabelImage>
::GetVolumeSimilarity( LabelType label ) const
{
  MapConstIterator mapIt = this->m_LabelSetMeasures.find( label );
  if( mapIt == this->m_LabelSetMeasures.end() )
    {
    itkWarningMacro( "Label " << label << " not found." );
    return 0.0;
    }
  RealType value = 2.0 *
    ( static_cast<RealType>( (*mapIt).second.m_Source ) -
      static_cast<RealType>( (*mapIt).second.m_Target ) ) /
    ( static_cast<RealType>( (*mapIt).second.m_Source ) +
      static_cast<RealType>( (*mapIt).second.m_Target ) );
  return value;
}

template<typename TLabelImage>
typename LabelOverlapMeasuresImageFilter<TLabelImage>::RealType
LabelOverlapMeasuresImageFilter<TLabelImage>
::GetFalseNegativeError() const
{
  RealType numerator = 0.0;
  RealType denominator = 0.0;
  for( MapConstIterator mapIt = this->m_LabelSetMeasures.begin();
       mapIt != this->m_LabelSetMeasures.end(); ++mapIt )
    {
    // Do not include the background in the final value.
    if( (*mapIt).first == NumericTraits<LabelType>::ZeroValue() )
      {
      continue;
      }
    numerator += static_cast<RealType>( (*mapIt).second.m_TargetComplement );
    denominator += static_cast<RealType>( (*mapIt).second.m_Target );
    }

  if( Math::ExactlyEquals(denominator,0.0) )
    {
    return NumericTraits<RealType>::max();
    }
  else
    {
    return ( numerator / denominator );
    }
}

template<typename TLabelImage>
typename LabelOverlapMeasuresImageFilter<TLabelImage>::RealType
LabelOverlapMeasuresImageFilter<TLabelImage>
::GetFalseNegativeError( LabelType label ) const
{
  MapConstIterator mapIt = this->m_LabelSetMeasures.find( label );
  if( mapIt == this->m_LabelSetMeasures.end() )
    {
    itkWarningMacro( "Label " << label << " not found." );
    return 0.0;
    }

  RealType value;
  if(Math::ExactlyEquals((*mapIt).second.m_Target, 0.0))
    {
    value = NumericTraits<RealType>::max();
    }
  else
    {
    value =
      static_cast<RealType>( (*mapIt).second.m_TargetComplement ) /
      static_cast<RealType>( (*mapIt).second.m_Target );
    }

  return value;
}

template<typename TLabelImage>
typename LabelOverlapMeasuresImageFilter<TLabelImage>::RealType
LabelOverlapMeasuresImageFilter<TLabelImage>
::GetFalsePositiveError() const
{
  RealType numerator = 0.0;
  RealType denominator = 0.0;
  for( MapConstIterator mapIt = this->m_LabelSetMeasures.begin();
       mapIt != this->m_LabelSetMeasures.end(); ++mapIt )
    {
    // Do not include the background in the final value.
    if( (*mapIt).first == NumericTraits<LabelType>::ZeroValue() )
      {
      continue;
      }
    numerator += static_cast<RealType>( (*mapIt).second.m_SourceComplement );
    denominator += static_cast<RealType>( (*mapIt).second.m_Source );
    }

  if( Math::ExactlyEquals(denominator,0.0) )
    {
    return NumericTraits<RealType>::max();
    }
  else
    {
    return ( numerator / denominator );
    }
}

template<typename TLabelImage>
typename LabelOverlapMeasuresImageFilter<TLabelImage>::RealType
LabelOverlapMeasuresImageFilter<TLabelImage>
::GetFalsePositiveError( LabelType label ) const
{
  MapConstIterator mapIt = this->m_LabelSetMeasures.find( label );
  if( mapIt == this->m_LabelSetMeasures.end() )
    {
    itkWarningMacro( "Label " << label << " not found." );
    return 0.0;
    }

  RealType value;
  if(Math::ExactlyEquals((*mapIt).second.m_Source, 0.0))
    {
    value = NumericTraits<RealType>::max();
    }
  else
    {
    value =
      static_cast<RealType>( (*mapIt).second.m_SourceComplement ) /
      static_cast<RealType>( (*mapIt).second.m_Source );
    }

  return value;
}

template<typename TLabelImage>
void
LabelOverlapMeasuresImageFilter<TLabelImage>
::PrintSelf( std::ostream& os, Indent indent ) const
{
  // todo!!!
  Superclass::PrintSelf( os, indent );
}

}// end namespace itk
#endif
<|endoftext|>


<commit_before>/*
 * File:   LEDSetter.cpp
 * Author: thomas
 *
 * Created on 22. April 2009, 16:55
 */

#include "LEDSetter.h"

LEDSetter::LEDSetter()
{

}

void LEDSetter::execute()
{
  getLEDData().change = false;
  if(getFrameRateCheckLEDRequest().ignore)
  {
    // head LEDs from behavior
    copyMonoLEDData(getBehaviorLEDRequest(), LEDData::EarRight0, LEDData::EarLeft324);
    copyMultiLEDData(getBehaviorLEDRequest(), LEDData::FaceRight0, LEDData::FaceLeft315);
  }
  else
  {
    // get all head LEDs from frame rate check
    copyMonoLEDData(getFrameRateCheckLEDRequest(), LEDData::EarRight0, LEDData::EarLeft324);
    copyMultiLEDData(getFrameRateCheckLEDRequest(), LEDData::FaceRight0, LEDData::FaceLeft315);
  }
  // feet and chest button from GameController
  copyMultiLEDData(getGameControllerLEDRequest(), LEDData::FootLeft, LEDData::ChestButton);

} // end execute

void LEDSetter::copyMultiLEDData(const LEDRequest &data, int from, int to)
{
  for(int i=from; i <= to; i++)
  {
    if(data.request.theMultiLED[i][LEDData::RED]
      != getLEDData().theMultiLED[i][LEDData::RED])
    {
      getLEDData().theMultiLED[i][LEDData::RED] =
       data.request.theMultiLED[i][LEDData::RED];
      getLEDData().change = true;
    }

    if(data.request.theMultiLED[i][LEDData::BLUE]
      != getLEDData().theMultiLED[i][LEDData::BLUE])
    {
      getLEDData().theMultiLED[i][LEDData::BLUE] =
        data.request.theMultiLED[i][LEDData::BLUE];
      getLEDData().change = true;
    }

    if(data.request.theMultiLED[i][LEDData::GREEN]
      != getLEDData().theMultiLED[i][LEDData::GREEN])
    {
      getLEDData().theMultiLED[i][LEDData::GREEN] =
        data.request.theMultiLED[i][LEDData::GREEN];
      getLEDData().change = true;
    }
  }

}

void LEDSetter::copyMonoLEDData(const LEDRequest &data, int from, int to)
{
  // head LEDs from behavior

  for(int i=0; i < LEDData::numOfMonoLED; i++)
  {
    if(data.request.theMonoLED[i] != getLEDData().theMonoLED[i])
    {
      getLEDData().theMonoLED[i] = data.request.theMonoLED[i];
      getLEDData().change = true;
    }
  }

}//end copyData

LEDSetter::~LEDSetter()
{
}
<commit_msg>very important commit<commit_after>/*
 * File:   LEDSetter.cpp
 * Author: thomas
 *
 * Created on 22. April 2009, 16:55
 */

#include "LEDSetter.h"

LEDSetter::LEDSetter()
{

}

void LEDSetter::execute()
{
  getLEDData().change = false;

  if(!getFrameRateCheckLEDRequest().ignore && (getFrameInfo().getFrameNumber() / 4) % 2 == 0)
  {
    // get all head LEDs from frame rate check
    copyMonoLEDData(getFrameRateCheckLEDRequest(), LEDData::EarRight0, LEDData::EarLeft324);
    copyMultiLEDData(getFrameRateCheckLEDRequest(), LEDData::FaceRight0, LEDData::FaceLeft315);
  }
  else
  {
    // head LEDs from behavior
    copyMonoLEDData(getBehaviorLEDRequest(), LEDData::EarRight0, LEDData::EarLeft324);
    copyMultiLEDData(getBehaviorLEDRequest(), LEDData::FaceRight0, LEDData::FaceLeft315);
  }
  // feet and chest button from GameController
  copyMultiLEDData(getGameControllerLEDRequest(), LEDData::FootLeft, LEDData::ChestButton);

} // end execute

void LEDSetter::copyMultiLEDData(const LEDRequest &data, int from, int to)
{
  for(int i=from; i <= to; i++)
  {
    if(data.request.theMultiLED[i][LEDData::RED]
      != getLEDData().theMultiLED[i][LEDData::RED])
    {
      getLEDData().theMultiLED[i][LEDData::RED] =
       data.request.theMultiLED[i][LEDData::RED];
      getLEDData().change = true;
    }

    if(data.request.theMultiLED[i][LEDData::BLUE]
      != getLEDData().theMultiLED[i][LEDData::BLUE])
    {
      getLEDData().theMultiLED[i][LEDData::BLUE] =
        data.request.theMultiLED[i][LEDData::BLUE];
      getLEDData().change = true;
    }

    if(data.request.theMultiLED[i][LEDData::GREEN]
      != getLEDData().theMultiLED[i][LEDData::GREEN])
    {
      getLEDData().theMultiLED[i][LEDData::GREEN] =
        data.request.theMultiLED[i][LEDData::GREEN];
      getLEDData().change = true;
    }
  }

}

void LEDSetter::copyMonoLEDData(const LEDRequest &data, int from, int to)
{
  // head LEDs from behavior

  for(int i=0; i < LEDData::numOfMonoLED; i++)
  {
    if(data.request.theMonoLED[i] != getLEDData().theMonoLED[i])
    {
      getLEDData().theMonoLED[i] = data.request.theMonoLED[i];
      getLEDData().change = true;
    }
  }

}//end copyData

LEDSetter::~LEDSetter()
{
}
<|endoftext|>


<commit_before><commit_msg>Fixed build<commit_after><|endoftext|>


<commit_before>#include "GifPacketView.h"
#include "win32/DeviceContext.h"
#include "../../GSHandler.h"
#include "../../GIF.h"
#include "../../uint128.h"
#include "string_format.h"

CGifPacketView::CGifPacketView(HWND parentWnd, const RECT& rect)
: CRegViewPage(parentWnd, rect)
{

}

CGifPacketView::~CGifPacketView()
{

}

std::string DumpPacked(uint8*& packet, const CGIF::TAG& tag, CGSHandler::RegisterWriteList& registerWrites)
{
	uint64 qtemp = 0;
	std::string result;

	for(unsigned int i = 0; i < tag.loops; i++)
	{
		for(unsigned int j = 0; j < tag.nreg; j++)
		{
			uint32 regDesc = static_cast<uint32>((tag.regs >> (j * 4)) & 0x0F);

			uint128 input = *reinterpret_cast<uint128*>(packet);
			packet += 0x10;

			switch(regDesc)
			{
			case 0x00:
				//PRIM
				registerWrites.push_back(CGSHandler::RegisterWrite(GS_REG_PRIM, input.nV0));
				break;
			case 0x01:
				//RGBA
				{
					uint64 temp	 = (input.nV[0] & 0xFF);
					temp		|= (input.nV[1] & 0xFF) << 8;
					temp		|= (input.nV[2] & 0xFF) << 16;
					temp		|= (input.nV[3] & 0xFF) << 24;
					temp		|= ((uint64)qtemp << 32);
					registerWrites.push_back(CGSHandler::RegisterWrite(GS_REG_RGBAQ, temp));
				}
				break;
			case 0x02:
				//ST
				qtemp = input.nV2;
				registerWrites.push_back(CGSHandler::RegisterWrite(GS_REG_ST, input.nD0));
				break;
			case 0x03:
				//UV
				{
					uint64 temp	 = (input.nV[0] & 0x7FFF);
					temp		|= (input.nV[1] & 0x7FFF) << 16;
					registerWrites.push_back(CGSHandler::RegisterWrite(GS_REG_UV, temp));
				}
				break;
			case 0x04:
				//XYZF2
				{
					uint64 temp	 = (input.nV[0] & 0xFFFF);
					temp		|= (input.nV[1] & 0xFFFF) << 16;
					temp		|= (uint64)(input.nV[2] & 0x0FFFFFF0) << 28;
					temp		|= (uint64)(input.nV[3] & 0x00000FF0) << 52;
					registerWrites.push_back(CGSHandler::RegisterWrite(((input.nV[3] & 0x8000) != 0) ? GS_REG_XYZF3 : GS_REG_XYZF2, temp));
				}
				break;
/*
			case 0x05:
				//XYZ2
				nTemp  = (nPacket.nV[0] & 0xFFFF);
				nTemp |= (nPacket.nV[1] & 0xFFFF) << 16;
				nTemp |= (uint64)(nPacket.nV[2] & 0xFFFFFFFF) << 32;
				if(nPacket.nV[3] & 0x8000)
				{
					writeList.push_back(CGSHandler::RegisterWrite(GS_REG_XYZ3, nTemp));
				}
				else
				{
					writeList.push_back(CGSHandler::RegisterWrite(GS_REG_XYZ2, nTemp));
				}
				break;
			case 0x06:
				//TEX0_1
				writeList.push_back(CGSHandler::RegisterWrite(GS_REG_TEX0_1, nPacket.nD0));
				break;
			case 0x07:
				//TEX0_2
				writeList.push_back(CGSHandler::RegisterWrite(GS_REG_TEX0_2, nPacket.nD0));
				break;
			case 0x08:
				//CLAMP_1
				writeList.push_back(CGSHandler::RegisterWrite(GS_REG_CLAMP_1, nPacket.nD0));
				break;
			case 0x0D:
				//XYZ3
				writeList.push_back(CGSHandler::RegisterWrite(GS_REG_XYZ3, nPacket.nD0));
				break;
			case 0x0E:
				//A + D
				if(m_gs != NULL)
				{
					writeList.push_back(CGSHandler::RegisterWrite(static_cast<uint8>(nPacket.nD1), nPacket.nD0));
				}
				break;
*/
			case 0x0F:
				//NOP
				break;
			default:
				assert(0);
				break;
			}
		}
	}

	return result;
}

void CGifPacketView::SetPacket(uint8* packet, uint32 packetSize)
{
	CGSHandler::RegisterWriteList registerWrites;

	while(1)
	{
		CGIF::TAG tag;
		memcpy(&tag, packet, sizeof(tag));
		packet += 0x10;

		uint32 tagSize = 0;
		switch(tag.cmd)
		{
		case 0:
			tagSize = tag.loops * tag.nreg * 0x10;
			break;
		case 1:
			tagSize = tag.loops * tag.nreg * 0x08;
			break;
		case 2:
			tagSize = tag.loops * 0x10;
			break;
		}

		if(tagSize == 0) break;
		if(tagSize > packetSize) break;

		if(tag.cmd == 0)
		{
			if(tag.pre)
			{
				registerWrites.push_back(CGSHandler::RegisterWrite(GS_REG_PRIM, tag.prim));
			}
		}

		packetSize -= tagSize;

		switch(tag.cmd)
		{
		case 0:
			DumpPacked(packet, tag, registerWrites);
			break;
		}
	}

	std::string result;
	for(unsigned int i = 0; i < registerWrites.size(); i++)
	{
		const auto& registerWrite(registerWrites[i]);
		result += string_format("%0.4X: %s\r\n", i, CGSHandler::DisassembleWrite(registerWrite.first, registerWrite.second).c_str());
	}

	SetDisplayText(result.c_str());
}
<commit_msg>Updated CGifPacketView to add A+D mode.<commit_after>#include "GifPacketView.h"
#include "win32/DeviceContext.h"
#include "../../GSHandler.h"
#include "../../GIF.h"
#include "../../uint128.h"
#include "string_format.h"

CGifPacketView::CGifPacketView(HWND parentWnd, const RECT& rect)
: CRegViewPage(parentWnd, rect)
{

}

CGifPacketView::~CGifPacketView()
{

}

std::string DumpPacked(uint8*& packet, const CGIF::TAG& tag, CGSHandler::RegisterWriteList& registerWrites)
{
	uint64 qtemp = 0;
	std::string result;

	for(unsigned int i = 0; i < tag.loops; i++)
	{
		for(unsigned int j = 0; j < tag.nreg; j++)
		{
			uint32 regDesc = static_cast<uint32>((tag.regs >> (j * 4)) & 0x0F);

			uint128 input = *reinterpret_cast<uint128*>(packet);
			packet += 0x10;

			switch(regDesc)
			{
			case 0x00:
				//PRIM
				registerWrites.push_back(CGSHandler::RegisterWrite(GS_REG_PRIM, input.nV0));
				break;
			case 0x01:
				//RGBA
				{
					uint64 temp	 = (input.nV[0] & 0xFF);
					temp		|= (input.nV[1] & 0xFF) << 8;
					temp		|= (input.nV[2] & 0xFF) << 16;
					temp		|= (input.nV[3] & 0xFF) << 24;
					temp		|= ((uint64)qtemp << 32);
					registerWrites.push_back(CGSHandler::RegisterWrite(GS_REG_RGBAQ, temp));
				}
				break;
			case 0x02:
				//ST
				qtemp = input.nV2;
				registerWrites.push_back(CGSHandler::RegisterWrite(GS_REG_ST, input.nD0));
				break;
			case 0x03:
				//UV
				{
					uint64 temp	 = (input.nV[0] & 0x7FFF);
					temp		|= (input.nV[1] & 0x7FFF) << 16;
					registerWrites.push_back(CGSHandler::RegisterWrite(GS_REG_UV, temp));
				}
				break;
			case 0x04:
				//XYZF2
				{
					uint64 temp	 = (input.nV[0] & 0xFFFF);
					temp		|= (input.nV[1] & 0xFFFF) << 16;
					temp		|= (uint64)(input.nV[2] & 0x0FFFFFF0) << 28;
					temp		|= (uint64)(input.nV[3] & 0x00000FF0) << 52;
					registerWrites.push_back(CGSHandler::RegisterWrite(((input.nV[3] & 0x8000) != 0) ? GS_REG_XYZF3 : GS_REG_XYZF2, temp));
				}
				break;
/*
			case 0x05:
				//XYZ2
				nTemp  = (nPacket.nV[0] & 0xFFFF);
				nTemp |= (nPacket.nV[1] & 0xFFFF) << 16;
				nTemp |= (uint64)(nPacket.nV[2] & 0xFFFFFFFF) << 32;
				if(nPacket.nV[3] & 0x8000)
				{
					writeList.push_back(CGSHandler::RegisterWrite(GS_REG_XYZ3, nTemp));
				}
				else
				{
					writeList.push_back(CGSHandler::RegisterWrite(GS_REG_XYZ2, nTemp));
				}
				break;
			case 0x06:
				//TEX0_1
				writeList.push_back(CGSHandler::RegisterWrite(GS_REG_TEX0_1, nPacket.nD0));
				break;
			case 0x07:
				//TEX0_2
				writeList.push_back(CGSHandler::RegisterWrite(GS_REG_TEX0_2, nPacket.nD0));
				break;
			case 0x08:
				//CLAMP_1
				writeList.push_back(CGSHandler::RegisterWrite(GS_REG_CLAMP_1, nPacket.nD0));
				break;
			case 0x0D:
				//XYZ3
				writeList.push_back(CGSHandler::RegisterWrite(GS_REG_XYZ3, nPacket.nD0));
				break;
*/
			case 0x0E:
				//A + D
				registerWrites.push_back(CGSHandler::RegisterWrite(static_cast<uint8>(input.nD1), input.nD0));
				break;
			case 0x0F:
				//NOP
				break;
			default:
				assert(0);
				break;
			}
		}
	}

	return result;
}

void CGifPacketView::SetPacket(uint8* packet, uint32 packetSize)
{
	CGSHandler::RegisterWriteList registerWrites;

	while(1)
	{
		CGIF::TAG tag;
		memcpy(&tag, packet, sizeof(tag));
		packet += 0x10;

		uint32 tagSize = 0;
		switch(tag.cmd)
		{
		case 0:
			tagSize = tag.loops * tag.nreg * 0x10;
			break;
		case 1:
			tagSize = tag.loops * tag.nreg * 0x08;
			break;
		case 2:
			tagSize = tag.loops * 0x10;
			break;
		}

		if(tagSize == 0) break;
		if(tagSize > packetSize) break;

		if(tag.cmd == 0)
		{
			if(tag.pre)
			{
				registerWrites.push_back(CGSHandler::RegisterWrite(GS_REG_PRIM, tag.prim));
			}
		}

		packetSize -= tagSize;

		switch(tag.cmd)
		{
		case 0:
			DumpPacked(packet, tag, registerWrites);
			break;
		}
	}

	std::string result;
	for(unsigned int i = 0; i < registerWrites.size(); i++)
	{
		const auto& registerWrite(registerWrites[i]);
		result += string_format("%0.4X: %s\r\n", i, CGSHandler::DisassembleWrite(registerWrite.first, registerWrite.second).c_str());
	}

	SetDisplayText(result.c_str());
}
<|endoftext|>


<commit_before>// Copyright 2015 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "flutter/lib/ui/painting/image_decoding.h"

#include "flutter/common/threads.h"
#include "flutter/flow/bitmap_image.h"
#include "flutter/flow/texture_image.h"
#include "flutter/glue/trace_event.h"
#include "flutter/lib/ui/painting/image.h"
#include "flutter/lib/ui/painting/resource_context.h"
#include "lib/ftl/functional/make_copyable.h"
#include "lib/tonic/dart_persistent_value.h"
#include "lib/tonic/dart_state.h"
#include "lib/tonic/logging/dart_invoke.h"
#include "lib/tonic/typed_data/uint8_list.h"
#include "third_party/skia/include/core/SkImageGenerator.h"

using tonic::DartInvoke;
using tonic::DartPersistentValue;
using tonic::ToDart;

namespace blink {
namespace {

sk_sp<SkImage> DecodeImage(std::vector<uint8_t> buffer) {
  TRACE_EVENT0("blink", "DecodeImage");

  if (buffer.empty())
    return nullptr;

  sk_sp<SkData> sk_data = SkData::MakeWithoutCopy(buffer.data(), buffer.size());

  if (sk_data == nullptr)
    return nullptr;

  std::unique_ptr<SkImageGenerator> generator(
      SkImageGenerator::NewFromEncoded(sk_data.get()));

  if (generator == nullptr)
    return nullptr;

  // First, try to create a texture image from the generator.
  GrContext* context = ResourceContext::Get();
  if (sk_sp<SkImage> image = flow::TextureImageCreate(context, *generator))
    return image;

  // Then, as a fallback, try to create a regular Skia managed image. These
  // don't require a context ready.
  return flow::BitmapImageCreate(*generator);
}

void InvokeImageCallback(sk_sp<SkImage> image,
                         std::unique_ptr<DartPersistentValue> callback) {
  tonic::DartState* dart_state = callback->dart_state().get();
  if (!dart_state)
    return;
  tonic::DartState::Scope scope(dart_state);
  if (!image) {
    DartInvoke(callback->value(), {Dart_Null()});
  } else {
    ftl::RefPtr<CanvasImage> resultImage = CanvasImage::Create();
    resultImage->set_image(std::move(image));
    DartInvoke(callback->value(), {ToDart(resultImage)});
  }
}

void DecodeImageAndInvokeImageCallback(
    std::unique_ptr<DartPersistentValue> callback,
    std::vector<uint8_t> buffer) {
  sk_sp<SkImage> image = DecodeImage(std::move(buffer));
  Threads::UI()->PostTask(
      ftl::MakeCopyable([ callback = std::move(callback), image ]() mutable {
        InvokeImageCallback(image, std::move(callback));
      }));
}

void DecodeImageFromList(Dart_NativeArguments args) {
  Dart_Handle exception = nullptr;

  tonic::Uint8List list =
      tonic::DartConverter<tonic::Uint8List>::FromArguments(args, 0, exception);
  if (exception) {
    Dart_ThrowException(exception);
    return;
  }

  Dart_Handle callback_handle = Dart_GetNativeArgument(args, 1);
  if (!Dart_IsClosure(callback_handle)) {
    Dart_ThrowException(ToDart("Callback must be a function"));
    return;
  }

  const uint8_t* bytes = reinterpret_cast<const uint8_t*>(list.data());
  std::vector<uint8_t> buffer(bytes, bytes + list.num_elements());

  Threads::IO()->PostTask(ftl::MakeCopyable([
    callback = std::make_unique<DartPersistentValue>(
        tonic::DartState::Current(), callback_handle),
    buffer = std::move(buffer)
  ]() mutable {
    DecodeImageAndInvokeImageCallback(std::move(callback), std::move(buffer));
  }));
}

}  // namespace

void ImageDecoding::RegisterNatives(tonic::DartLibraryNatives* natives) {
  natives->Register({
      {"decodeImageFromList", DecodeImageFromList, 2, true},
  });
}

}  // namespace blink
<commit_msg>Transfer ownership of the buffer in the image decoder bitmap fallback path (#3426)<commit_after>// Copyright 2015 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "flutter/lib/ui/painting/image_decoding.h"

#include "flutter/common/threads.h"
#include "flutter/flow/bitmap_image.h"
#include "flutter/flow/texture_image.h"
#include "flutter/glue/trace_event.h"
#include "flutter/lib/ui/painting/image.h"
#include "flutter/lib/ui/painting/resource_context.h"
#include "lib/ftl/functional/make_copyable.h"
#include "lib/tonic/dart_persistent_value.h"
#include "lib/tonic/dart_state.h"
#include "lib/tonic/logging/dart_invoke.h"
#include "lib/tonic/typed_data/uint8_list.h"
#include "third_party/skia/include/core/SkImageGenerator.h"

using tonic::DartInvoke;
using tonic::DartPersistentValue;
using tonic::ToDart;

namespace blink {
namespace {

void DeleteReleaseProc(const void* ptr, void* context) {
  delete static_cast<std::vector<uint8_t>*>(context);
}

sk_sp<SkImage> DecodeImage(std::vector<uint8_t> buffer) {
  TRACE_EVENT0("blink", "DecodeImage");

  if (buffer.empty())
    return nullptr;

  sk_sp<SkData> sk_data = SkData::MakeWithoutCopy(buffer.data(), buffer.size());

  if (sk_data == nullptr)
    return nullptr;

  std::unique_ptr<SkImageGenerator> generator(
      SkImageGenerator::NewFromEncoded(sk_data.get()));

  if (generator == nullptr)
    return nullptr;

  // First, try to create a texture image from the generator.
  GrContext* context = ResourceContext::Get();
  if (sk_sp<SkImage> image = flow::TextureImageCreate(context, *generator))
    return image;

  // Then, as a fallback, try to create a regular Skia managed image. These
  // don't require a context ready.
  std::vector<uint8_t>* data_buffer =
      new std::vector<uint8_t>(std::move(buffer));
  if (data_buffer == nullptr)
    return nullptr;
  sk_data = SkData::MakeWithProc(data_buffer->data(), data_buffer->size(),
                                 DeleteReleaseProc, data_buffer);
  if (sk_data == nullptr) {
    delete data_buffer;
    return nullptr;
  }
  return SkImage::MakeFromEncoded(sk_data);
}

void InvokeImageCallback(sk_sp<SkImage> image,
                         std::unique_ptr<DartPersistentValue> callback) {
  tonic::DartState* dart_state = callback->dart_state().get();
  if (!dart_state)
    return;
  tonic::DartState::Scope scope(dart_state);
  if (!image) {
    DartInvoke(callback->value(), {Dart_Null()});
  } else {
    ftl::RefPtr<CanvasImage> resultImage = CanvasImage::Create();
    resultImage->set_image(std::move(image));
    DartInvoke(callback->value(), {ToDart(resultImage)});
  }
}

void DecodeImageAndInvokeImageCallback(
    std::unique_ptr<DartPersistentValue> callback,
    std::vector<uint8_t> buffer) {
  sk_sp<SkImage> image = DecodeImage(std::move(buffer));
  Threads::UI()->PostTask(
      ftl::MakeCopyable([ callback = std::move(callback), image ]() mutable {
        InvokeImageCallback(image, std::move(callback));
      }));
}

void DecodeImageFromList(Dart_NativeArguments args) {
  Dart_Handle exception = nullptr;

  tonic::Uint8List list =
      tonic::DartConverter<tonic::Uint8List>::FromArguments(args, 0, exception);
  if (exception) {
    Dart_ThrowException(exception);
    return;
  }

  Dart_Handle callback_handle = Dart_GetNativeArgument(args, 1);
  if (!Dart_IsClosure(callback_handle)) {
    Dart_ThrowException(ToDart("Callback must be a function"));
    return;
  }

  const uint8_t* bytes = reinterpret_cast<const uint8_t*>(list.data());
  std::vector<uint8_t> buffer(bytes, bytes + list.num_elements());

  Threads::IO()->PostTask(ftl::MakeCopyable([
    callback = std::make_unique<DartPersistentValue>(
        tonic::DartState::Current(), callback_handle),
    buffer = std::move(buffer)
  ]() mutable {
    DecodeImageAndInvokeImageCallback(std::move(callback), std::move(buffer));
  }));
}

}  // namespace

void ImageDecoding::RegisterNatives(tonic::DartLibraryNatives* natives) {
  natives->Register({
      {"decodeImageFromList", DecodeImageFromList, 2, true},
  });
}

}  // namespace blink
<|endoftext|>


<commit_before><commit_msg>Added inverse design routines to preprocess [should be only temporary]<commit_after><|endoftext|>


<commit_before>/*
  Copyright (c) 2011, The Mineserver Project
  All rights reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions are met:
  * Redistributions of source code must retain the above copyright
    notice, this list of conditions and the following disclaimer.
  * Redistributions in binary form must reproduce the above copyright
    notice, this list of conditions and the following disclaimer in the
    documentation and/or other materials provided with the distribution.
  * Neither the name of the The Mineserver Project nor the
    names of its contributors may be used to endorse or promote products
    derived from this software without specific prior written permission.

  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
  DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
  ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include <mineserver/byteorder.h>
#include <mineserver/network/message/0x68.h>
#include <mineserver/network/protocol/notch/packet.h>
#include <mineserver/network/protocol/notch/packet/0x68.h>

int Mineserver::Network_Protocol_Notch_Packet_0x68::_read(Mineserver::Network_Protocol_Notch_PacketStream& ps, Mineserver::Network_Message** message)
{
  Mineserver::Network_Message_0x68* msg = new Mineserver::Network_Message_0x68;
  *message = msg;

  ps >> msg->mid >> msg->windowId >> msg->count;

  int16_t itemId, uses;
  int8_t count;

  for (int16_t i=0;i<count;++i) {
    itemId = uses = count = 0;

    ps >> itemId;

    if (itemId != -1) {
      ps >> count >> uses;
    }

    msg->slots.push_back(std::pair<int16_t, std::pair<int8_t, int16_t> >(itemId, std::pair<int8_t, int16_t>(count, uses)));
  }

  return STATE_GOOD;
}

int Mineserver::Network_Protocol_Notch_Packet_0x68::_write(Mineserver::Network_Protocol_Notch_PacketStream& ps, const Mineserver::Network_Message& message)
{
  const Mineserver::Network_Message_0x68* msg = static_cast<const Mineserver::Network_Message_0x68*>(&message);

  ps << msg->mid << msg->windowId << msg->count;

  int16_t itemId, uses;
  int8_t count;

  for (std::vector<std::pair<int16_t, std::pair<int8_t, int16_t> > >::const_iterator it=msg->slots.begin();it!=msg->slots.end();++it) {
    ps << it->first;

    if (it->first != -1) {
      ps << it->second.first << it->second.second;
    }
  }

  return STATE_GOOD;
}
<commit_msg>switched to properly named message header<commit_after>/*
  Copyright (c) 2011, The Mineserver Project
  All rights reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions are met:
  * Redistributions of source code must retain the above copyright
    notice, this list of conditions and the following disclaimer.
  * Redistributions in binary form must reproduce the above copyright
    notice, this list of conditions and the following disclaimer in the
    documentation and/or other materials provided with the distribution.
  * Neither the name of the The Mineserver Project nor the
    names of its contributors may be used to endorse or promote products
    derived from this software without specific prior written permission.

  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
  DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
  ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include <utility>

#include <mineserver/byteorder.h>
#include <mineserver/network/message/windowitems.h>
#include <mineserver/network/protocol/notch/packet.h>
#include <mineserver/network/protocol/notch/packet/0x68.h>

int Mineserver::Network_Protocol_Notch_Packet_0x68::_read(Mineserver::Network_Protocol_Notch_PacketStream& ps, Mineserver::Network_Message** message)
{
  Mineserver::Network_Message_WindowItems* msg = new Mineserver::Network_Message_WindowItems;
  *message = msg;

  ps >> msg->mid >> msg->windowId >> msg->count;

  int16_t itemId, uses;
  int8_t count;

  for (int16_t i=0;i<count;++i) {
    itemId = uses = count = 0;

    ps >> itemId;

    if (itemId != -1) {
      ps >> count >> uses;
    }

    msg->slots.push_back(std::make_pair(itemId, std::make_pair(count, uses)));
  }

  return STATE_GOOD;
}

int Mineserver::Network_Protocol_Notch_Packet_0x68::_write(Mineserver::Network_Protocol_Notch_PacketStream& ps, const Mineserver::Network_Message& message)
{
  const Mineserver::Network_Message_WindowItems* msg = static_cast<const Mineserver::Network_Message_WindowItems*>(&message);

  ps << msg->mid << msg->windowId << msg->count;

  for (std::vector<std::pair<int16_t, std::pair<int8_t, int16_t> > >::const_iterator it=msg->slots.begin();it!=msg->slots.end();++it) {
    ps << it->first;

    if (it->first != -1) {
      ps << it->second.first << it->second.second;
    }
  }

  return STATE_GOOD;
}
<|endoftext|>


<commit_before>#include "../BlockLocalPositionEstimator.hpp"
#include <systemlib/mavlink_log.h>
#include <matrix/math.hpp>

// mavlink pub
extern orb_advert_t mavlink_log_pub;

// required number of samples for sensor
// to initialize
static const uint32_t 		REQ_FLOW_INIT_COUNT = 10;
static const uint32_t 		FLOW_TIMEOUT = 1000000;	// 1 s

// minimum flow altitude
static const float flow_min_agl = 0.05;

void BlockLocalPositionEstimator::flowInit()
{
	// measure
	Vector<float, n_y_flow> y;

	if (flowMeasure(y) != OK) {
		_flowQStats.reset();
		return;
	}

	// if finished
	if (_flowQStats.getCount() > REQ_FLOW_INIT_COUNT) {
		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow init: "
					     "quality %d std %d",
					     int(_flowQStats.getMean()(0)),
					     int(_flowQStats.getStdDev()(0)));
		_flowInitialized = true;
		_flowFault = FAULT_NONE;
	}
}

void BlockLocalPositionEstimator::flowDeinit()
{
	_flowInitialized = false;
	_flowQStats.reset();
}

int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
{
	// check for sane pitch/roll
	if (_sub_att.get().roll > 0.5f || _sub_att.get().pitch > 0.5f) {
		return -1;
	}

	// check for agl
	if (agl() < flow_min_agl) {
		return -1;
	}

	// check quality
	float qual = _sub_flow.get().quality;

	if (qual < _flow_min_q.get()) {
		return -1;
	}

	// calculate range to center of image for flow
	if (!_validTZ) {
		return -1;
	}

	float d = agl() * cosf(_sub_att.get().roll) * cosf(_sub_att.get().pitch);

	// optical flow in x, y axis
	// TODO consider making flow scale a states of the kalman filter
	float flow_x_rad = _sub_flow.get().pixel_flow_x_integral * _flow_scale.get();
	float flow_y_rad = _sub_flow.get().pixel_flow_y_integral * _flow_scale.get();
	float dt_flow = _sub_flow.get().integration_timespan / 1.0e6f;

	if (dt_flow > 0.5f || dt_flow < 1.0e-6f) {
		return -1;
	}

	// angular rotation in x, y axis
	float gyro_x_rad = 0;
	float gyro_y_rad = 0;

	if (_flow_gyro_comp.get()) {
		gyro_x_rad = _flow_gyro_x_high_pass.update(
				     _sub_flow.get().gyro_x_rate_integral);
		gyro_y_rad = _flow_gyro_y_high_pass.update(
				     _sub_flow.get().gyro_y_rate_integral);
	}

	//warnx("flow x: %10.4f y: %10.4f gyro_x: %10.4f gyro_y: %10.4f d: %10.4f",
	//double(flow_x_rad), double(flow_y_rad), double(gyro_x_rad), double(gyro_y_rad), double(d));

	// compute velocities in camera frame using ground distance
	// assume camera frame is body frame
	Vector3f delta_b(
		-(flow_x_rad - gyro_x_rad)*d,
		-(flow_y_rad - gyro_y_rad)*d,
		0);

	// rotation of flow from body to nav frame
	Matrix3f R_nb(_sub_att.get().R);
	Vector3f delta_n = R_nb * delta_b;

	// imporant to timestamp flow even if distance is bad
	_time_last_flow = _timeStamp;

	// measurement
	y(Y_flow_vx) = delta_n(0) / dt_flow;
	y(Y_flow_vy) = delta_n(1) / dt_flow;

	_flowQStats.update(Scalarf(_sub_flow.get().quality));

	return OK;
}

void BlockLocalPositionEstimator::flowCorrect()
{
	// measure flow
	Vector<float, n_y_flow> y;

	if (flowMeasure(y) != OK) { return; }

	// flow measurement matrix and noise matrix
	Matrix<float, n_y_flow, n_x> C;
	C.setZero();
	C(Y_flow_vx, X_vx) = 1;
	C(Y_flow_vy, X_vy) = 1;

	SquareMatrix<float, n_y_flow> R;
	R.setZero();

	// polynomial noise model, found using least squares fit
	// h, h**2, v, v*h, v*h**2
	const float p[5] = {0.04005232f, -0.00656446f, -0.26265873f,  0.13686658f, -0.00397357f};

	// prevent extrapolation past end of polynomial fit by bounding independent variables
	float h = agl();
	float v = y.norm();
	const float h_min = 2.0f;
	const float h_max = 8.0f;
	const float v_min = 0.5f;
	const float v_max = 1.0f;

	if (h > h_max) {
		h = h_max;
	}

	if (h < h_min) {
		h = h_min;
	}

	if (v > v_max) {
		v = v_max;
	}

	if (v < v_min) {
		v = v_min;
	}

	// compute polynomial value
	float flow_vxy_stddev = p[0] * h + p[1] * h * h + p[2] * v + p[3] * v * h + p[4] * v * h * h;

	R(Y_flow_vx, Y_flow_vx) = flow_vxy_stddev * flow_vxy_stddev;
	R(Y_flow_vy, Y_flow_vy) = R(Y_flow_vx, Y_flow_vx);

	// residual
	Vector<float, 2> r = y - C * _x;
	_pub_innov.get().flow_innov[0] = r(0);
	_pub_innov.get().flow_innov[1] = r(1);
	_pub_innov.get().flow_innov_var[0] = R(0, 0);
	_pub_innov.get().flow_innov_var[1] = R(1, 1);

	// residual covariance, (inverse)
	Matrix<float, n_y_flow, n_y_flow> S_I =
		inv<float, n_y_flow>(C * _P * C.transpose() + R);

	// fault detection
	float beta = (r.transpose() * (S_I * r))(0, 0);

	if (beta > BETA_TABLE[n_y_flow]) {
		if (_flowFault < FAULT_MINOR) {
			//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow fault,  beta %5.2f", double(beta));
			_flowFault = FAULT_MINOR;
		}

	} else if (_flowFault) {
		_flowFault = FAULT_NONE;
		//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow OK");
	}

	if (_flowFault < fault_lvl_disable) {
		Matrix<float, n_x, n_y_flow> K =
			_P * C.transpose() * S_I;
		Vector<float, n_x> dx = K * r;
		correctionLogic(dx);
		_x += dx;
		_P -= K * C * _P;

	}

}

void BlockLocalPositionEstimator::flowCheckTimeout()
{
	if (_timeStamp - _time_last_flow > FLOW_TIMEOUT) {
		if (_flowInitialized) {
			flowDeinit();
			mavlink_log_critical(&mavlink_log_pub, "[lpe] flow timeout ");
		}
	}
}
<commit_msg>Increase min agl for flow from 5 to 30 cm to prevent drift on ground.<commit_after>#include "../BlockLocalPositionEstimator.hpp"
#include <systemlib/mavlink_log.h>
#include <matrix/math.hpp>

// mavlink pub
extern orb_advert_t mavlink_log_pub;

// required number of samples for sensor
// to initialize
static const uint32_t 		REQ_FLOW_INIT_COUNT = 10;
static const uint32_t 		FLOW_TIMEOUT = 1000000;	// 1 s

// minimum flow altitude
static const float flow_min_agl = 0.3;

void BlockLocalPositionEstimator::flowInit()
{
	// measure
	Vector<float, n_y_flow> y;

	if (flowMeasure(y) != OK) {
		_flowQStats.reset();
		return;
	}

	// if finished
	if (_flowQStats.getCount() > REQ_FLOW_INIT_COUNT) {
		mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow init: "
					     "quality %d std %d",
					     int(_flowQStats.getMean()(0)),
					     int(_flowQStats.getStdDev()(0)));
		_flowInitialized = true;
		_flowFault = FAULT_NONE;
	}
}

void BlockLocalPositionEstimator::flowDeinit()
{
	_flowInitialized = false;
	_flowQStats.reset();
}

int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
{
	// check for sane pitch/roll
	if (_sub_att.get().roll > 0.5f || _sub_att.get().pitch > 0.5f) {
		return -1;
	}

	// check for agl
	if (agl() < flow_min_agl) {
		return -1;
	}

	// check quality
	float qual = _sub_flow.get().quality;

	if (qual < _flow_min_q.get()) {
		return -1;
	}

	// calculate range to center of image for flow
	if (!_validTZ) {
		return -1;
	}

	float d = agl() * cosf(_sub_att.get().roll) * cosf(_sub_att.get().pitch);

	// optical flow in x, y axis
	// TODO consider making flow scale a states of the kalman filter
	float flow_x_rad = _sub_flow.get().pixel_flow_x_integral * _flow_scale.get();
	float flow_y_rad = _sub_flow.get().pixel_flow_y_integral * _flow_scale.get();
	float dt_flow = _sub_flow.get().integration_timespan / 1.0e6f;

	if (dt_flow > 0.5f || dt_flow < 1.0e-6f) {
		return -1;
	}

	// angular rotation in x, y axis
	float gyro_x_rad = 0;
	float gyro_y_rad = 0;

	if (_flow_gyro_comp.get()) {
		gyro_x_rad = _flow_gyro_x_high_pass.update(
				     _sub_flow.get().gyro_x_rate_integral);
		gyro_y_rad = _flow_gyro_y_high_pass.update(
				     _sub_flow.get().gyro_y_rate_integral);
	}

	//warnx("flow x: %10.4f y: %10.4f gyro_x: %10.4f gyro_y: %10.4f d: %10.4f",
	//double(flow_x_rad), double(flow_y_rad), double(gyro_x_rad), double(gyro_y_rad), double(d));

	// compute velocities in camera frame using ground distance
	// assume camera frame is body frame
	Vector3f delta_b(
		-(flow_x_rad - gyro_x_rad)*d,
		-(flow_y_rad - gyro_y_rad)*d,
		0);

	// rotation of flow from body to nav frame
	Matrix3f R_nb(_sub_att.get().R);
	Vector3f delta_n = R_nb * delta_b;

	// imporant to timestamp flow even if distance is bad
	_time_last_flow = _timeStamp;

	// measurement
	y(Y_flow_vx) = delta_n(0) / dt_flow;
	y(Y_flow_vy) = delta_n(1) / dt_flow;

	_flowQStats.update(Scalarf(_sub_flow.get().quality));

	return OK;
}

void BlockLocalPositionEstimator::flowCorrect()
{
	// measure flow
	Vector<float, n_y_flow> y;

	if (flowMeasure(y) != OK) { return; }

	// flow measurement matrix and noise matrix
	Matrix<float, n_y_flow, n_x> C;
	C.setZero();
	C(Y_flow_vx, X_vx) = 1;
	C(Y_flow_vy, X_vy) = 1;

	SquareMatrix<float, n_y_flow> R;
	R.setZero();

	// polynomial noise model, found using least squares fit
	// h, h**2, v, v*h, v*h**2
	const float p[5] = {0.04005232f, -0.00656446f, -0.26265873f,  0.13686658f, -0.00397357f};

	// prevent extrapolation past end of polynomial fit by bounding independent variables
	float h = agl();
	float v = y.norm();
	const float h_min = 2.0f;
	const float h_max = 8.0f;
	const float v_min = 0.5f;
	const float v_max = 1.0f;

	if (h > h_max) {
		h = h_max;
	}

	if (h < h_min) {
		h = h_min;
	}

	if (v > v_max) {
		v = v_max;
	}

	if (v < v_min) {
		v = v_min;
	}

	// compute polynomial value
	float flow_vxy_stddev = p[0] * h + p[1] * h * h + p[2] * v + p[3] * v * h + p[4] * v * h * h;

	R(Y_flow_vx, Y_flow_vx) = flow_vxy_stddev * flow_vxy_stddev;
	R(Y_flow_vy, Y_flow_vy) = R(Y_flow_vx, Y_flow_vx);

	// residual
	Vector<float, 2> r = y - C * _x;
	_pub_innov.get().flow_innov[0] = r(0);
	_pub_innov.get().flow_innov[1] = r(1);
	_pub_innov.get().flow_innov_var[0] = R(0, 0);
	_pub_innov.get().flow_innov_var[1] = R(1, 1);

	// residual covariance, (inverse)
	Matrix<float, n_y_flow, n_y_flow> S_I =
		inv<float, n_y_flow>(C * _P * C.transpose() + R);

	// fault detection
	float beta = (r.transpose() * (S_I * r))(0, 0);

	if (beta > BETA_TABLE[n_y_flow]) {
		if (_flowFault < FAULT_MINOR) {
			//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow fault,  beta %5.2f", double(beta));
			_flowFault = FAULT_MINOR;
		}

	} else if (_flowFault) {
		_flowFault = FAULT_NONE;
		//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow OK");
	}

	if (_flowFault < fault_lvl_disable) {
		Matrix<float, n_x, n_y_flow> K =
			_P * C.transpose() * S_I;
		Vector<float, n_x> dx = K * r;
		correctionLogic(dx);
		_x += dx;
		_P -= K * C * _P;

	}

}

void BlockLocalPositionEstimator::flowCheckTimeout()
{
	if (_timeStamp - _time_last_flow > FLOW_TIMEOUT) {
		if (_flowInitialized) {
			flowDeinit();
			mavlink_log_critical(&mavlink_log_pub, "[lpe] flow timeout ");
		}
	}
}
<|endoftext|>


<commit_before>/*
    Copyright (C) 2014-2017 Leosac

    This file is part of Leosac.

    Leosac is free software: you can redistribute it and/or modify
    it under the terms of the GNU Affero General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    Leosac is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
    GNU Affero General Public License for more details.

    You should have received a copy of the GNU Affero General Public License
    along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#pragma once

#include "api/MethodHandler.hpp"
#include <json.hpp>

namespace Leosac
{
namespace Module
{
namespace WebSockAPI
{
using json = nlohmann::json;

/**
 * Search hardware devices by name.
 *
 * Request:
 *     + 'partial_name': A part of the name we are looking for.
 *
 * Response:
 *     A list of {id,alias} for doors that match the partial name.
 *     [
 *       {id: $HARDWARE_DEVICE_ID,
 *       name: $HARDWARE_DEVICE_NAME,
 *       device-class: $HARDWARE_DEVICE_CLASS
 *       type: $HARDWARE_DEVICE_TYPE
 *       {...}
 *     ]
 *
 * @note Here, `$HARDWARE_DEVICE_TYPE` is the type of the model (eg
 * `pfdigital.gpio`) while `$HARDWARE_DEVICE_CLASS` is the
 * Leosac::Hardware::DeviceClass of a device.
 */
class HardwareSearch : public MethodHandler
{
  public:
    HardwareSearch(RequestContext ctx);

    static MethodHandlerUPtr create(RequestContext);

  protected:
    std::vector<ActionActionParam>
    required_permission(const json &req) const override;

  private:
    virtual json process_impl(const json &req) override;
};
}
}
}
<commit_msg>Trivial update of the typo in HardwareSearch.hpp<commit_after>/*
    Copyright (C) 2014-2017 Leosac

    This file is part of Leosac.

    Leosac is free software: you can redistribute it and/or modify
    it under the terms of the GNU Affero General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    Leosac is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
    GNU Affero General Public License for more details.

    You should have received a copy of the GNU Affero General Public License
    along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#pragma once

#include "api/MethodHandler.hpp"
#include <json.hpp>

namespace Leosac
{
namespace Module
{
namespace WebSockAPI
{
using json = nlohmann::json;

/**
 * Search hardware devices by name.
 *
 * Request:
 *     + 'partial_name': A part of the name we are looking for.
 *
 * Response:
 *     A list of {id, name, device_class, type} for device that match the partial name.
 *     [
 *       {id: $HARDWARE_DEVICE_ID,
 *       name: $HARDWARE_DEVICE_NAME,
 *       device-class: $HARDWARE_DEVICE_CLASS
 *       type: $HARDWARE_DEVICE_TYPE
 *       {...}
 *     ]
 *
 * @note Here, `$HARDWARE_DEVICE_TYPE` is the type of the model (eg
 * `pfdigital.gpio`) while `$HARDWARE_DEVICE_CLASS` is the
 * Leosac::Hardware::DeviceClass of a device.
 */
class HardwareSearch : public MethodHandler
{
  public:
    HardwareSearch(RequestContext ctx);

    static MethodHandlerUPtr create(RequestContext);

  protected:
    std::vector<ActionActionParam>
    required_permission(const json &req) const override;

  private:
    virtual json process_impl(const json &req) override;
};
}
}
}
<|endoftext|>


<commit_before><commit_msg>Fix crash in ppd parser.<commit_after><|endoftext|>


<commit_before>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: CIndexes.cxx,v $
 *
 *  $Revision: 1.17 $
 *
 *  last change: $Author: obo $ $Date: 2006-07-10 15:00:36 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/
#ifndef DBACCESS_INDEXES_HXX_
#include "CIndexes.hxx"
#endif
#ifndef _COM_SUN_STAR_SDBC_XROW_HPP_
#include <com/sun/star/sdbc/XRow.hpp>
#endif
#ifndef _COM_SUN_STAR_SDBC_XRESULTSET_HPP_
#include <com/sun/star/sdbc/XResultSet.hpp>
#endif
#ifndef _COM_SUN_STAR_SDBC_INDEXTYPE_HPP_
#include <com/sun/star/sdbc/IndexType.hpp>
#endif
#ifndef DBACCESS_SHARED_DBASTRINGS_HRC
#include "dbastrings.hrc"
#endif
#ifndef _CONNECTIVITY_DBTOOLS_HXX_
#include <connectivity/dbtools.hxx>
#endif
#ifndef _COMPHELPER_EXTRACT_HXX_
#include <comphelper/extract.hxx>
#endif
#ifndef _TOOLS_DEBUG_HXX
#include <tools/debug.hxx>
#endif

using namespace connectivity;
using namespace connectivity::sdbcx;
using namespace ::com::sun::star::uno;
using namespace ::com::sun::star::beans;
using namespace ::com::sun::star::sdbcx;
using namespace ::com::sun::star::sdbc;
using namespace ::com::sun::star::container;
using namespace ::com::sun::star::lang;
using namespace dbaccess;
using namespace cppu;

ObjectType OIndexes::createObject(const ::rtl::OUString& _rName)
{
    ObjectType xRet;
    if ( m_xIndexes.is() && m_xIndexes->hasByName(_rName) )
        xRet.set(m_xIndexes->getByName(_rName),UNO_QUERY);
    else
        xRet = OIndexesHelper::createObject(_rName);

    return xRet;
}
// -------------------------------------------------------------------------
Reference< XPropertySet > OIndexes::createDescriptor()
{
    Reference<XDataDescriptorFactory> xData( m_xIndexes,UNO_QUERY);
    if(xData.is())
        return xData->createDataDescriptor();
    else
        return OIndexesHelper::createDescriptor();
}
// -------------------------------------------------------------------------
// XAppend
ObjectType OIndexes::appendObject( const ::rtl::OUString& _rForName, const Reference< XPropertySet >& descriptor )
{
    Reference<XAppend> xData( m_xIndexes,UNO_QUERY);
    if ( !xData.is() )
        return OIndexesHelper::appendObject( _rForName, descriptor );

    xData->appendByDescriptor(descriptor);
    return createObject( _rForName );
}
// -------------------------------------------------------------------------
// XDrop
void OIndexes::dropObject(sal_Int32 _nPos,const ::rtl::OUString _sElementName)
{
    if ( m_xIndexes.is() )
    {
        Reference<XDrop> xData( m_xIndexes,UNO_QUERY);
        if ( xData.is() )
            xData->dropByName(_sElementName);
    }
    else
        OIndexesHelper::dropObject(_nPos,_sElementName);
}
// -------------------------------------------------------------------------
void SAL_CALL OIndexes::disposing(void)
{
    if ( m_xIndexes.is() )
        clear_NoDispose();
    else
        OIndexesHelper::disposing();
}
// -----------------------------------------------------------------------------

<commit_msg>INTEGRATION: CWS pchfix02 (1.17.36); FILE MERGED 2006/09/01 17:24:02 kaib 1.17.36.1: #i68856# Added header markers and pch files<commit_after>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: CIndexes.cxx,v $
 *
 *  $Revision: 1.18 $
 *
 *  last change: $Author: obo $ $Date: 2006-09-17 06:29:42 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/

// MARKER(update_precomp.py): autogen include statement, do not remove
#include "precompiled_dbaccess.hxx"
#ifndef DBACCESS_INDEXES_HXX_
#include "CIndexes.hxx"
#endif
#ifndef _COM_SUN_STAR_SDBC_XROW_HPP_
#include <com/sun/star/sdbc/XRow.hpp>
#endif
#ifndef _COM_SUN_STAR_SDBC_XRESULTSET_HPP_
#include <com/sun/star/sdbc/XResultSet.hpp>
#endif
#ifndef _COM_SUN_STAR_SDBC_INDEXTYPE_HPP_
#include <com/sun/star/sdbc/IndexType.hpp>
#endif
#ifndef DBACCESS_SHARED_DBASTRINGS_HRC
#include "dbastrings.hrc"
#endif
#ifndef _CONNECTIVITY_DBTOOLS_HXX_
#include <connectivity/dbtools.hxx>
#endif
#ifndef _COMPHELPER_EXTRACT_HXX_
#include <comphelper/extract.hxx>
#endif
#ifndef _TOOLS_DEBUG_HXX
#include <tools/debug.hxx>
#endif

using namespace connectivity;
using namespace connectivity::sdbcx;
using namespace ::com::sun::star::uno;
using namespace ::com::sun::star::beans;
using namespace ::com::sun::star::sdbcx;
using namespace ::com::sun::star::sdbc;
using namespace ::com::sun::star::container;
using namespace ::com::sun::star::lang;
using namespace dbaccess;
using namespace cppu;

ObjectType OIndexes::createObject(const ::rtl::OUString& _rName)
{
    ObjectType xRet;
    if ( m_xIndexes.is() && m_xIndexes->hasByName(_rName) )
        xRet.set(m_xIndexes->getByName(_rName),UNO_QUERY);
    else
        xRet = OIndexesHelper::createObject(_rName);

    return xRet;
}
// -------------------------------------------------------------------------
Reference< XPropertySet > OIndexes::createDescriptor()
{
    Reference<XDataDescriptorFactory> xData( m_xIndexes,UNO_QUERY);
    if(xData.is())
        return xData->createDataDescriptor();
    else
        return OIndexesHelper::createDescriptor();
}
// -------------------------------------------------------------------------
// XAppend
ObjectType OIndexes::appendObject( const ::rtl::OUString& _rForName, const Reference< XPropertySet >& descriptor )
{
    Reference<XAppend> xData( m_xIndexes,UNO_QUERY);
    if ( !xData.is() )
        return OIndexesHelper::appendObject( _rForName, descriptor );

    xData->appendByDescriptor(descriptor);
    return createObject( _rForName );
}
// -------------------------------------------------------------------------
// XDrop
void OIndexes::dropObject(sal_Int32 _nPos,const ::rtl::OUString _sElementName)
{
    if ( m_xIndexes.is() )
    {
        Reference<XDrop> xData( m_xIndexes,UNO_QUERY);
        if ( xData.is() )
            xData->dropByName(_sElementName);
    }
    else
        OIndexesHelper::dropObject(_nPos,_sElementName);
}
// -------------------------------------------------------------------------
void SAL_CALL OIndexes::disposing(void)
{
    if ( m_xIndexes.is() )
        clear_NoDispose();
    else
        OIndexesHelper::disposing();
}
// -----------------------------------------------------------------------------

<|endoftext|>


<commit_before><commit_msg>Minor<commit_after><|endoftext|>