<commit_before>/*
    Copyright 2002-2015 CEA LIST

    This file is part of LIMA.

    LIMA 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.

    LIMA 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 LIMA.  If not, see <http://www.gnu.org/licenses/>
*/
#include "EnhancedAnalysisDictionary.h"
#include "EnhancedAnalysisDictionaryIterator.h"
#include "AbstractAccessResource.h"
#include "common/XMLConfigurationFiles/xmlConfigurationFileExceptions.h"
#include "common/AbstractFactoryPattern/SimpleFactory.h"
#include "common/MediaticData/mediaticData.h"
#include "linguisticProcessing/core/LinguisticResources/LinguisticResources.h"

#include <iostream>
#include <QFileSystemWatcher>
#include <QReadWriteLock>
#include <QWriteLocker>
#include <QReadLocker>

using namespace Lima::Common;
using namespace Lima::Common::XMLConfigurationFiles;
using namespace Lima::Common::MediaticData;
using namespace std;

namespace Lima
{

namespace LinguisticProcessing
{

namespace AnalysisDict
{

SimpleFactory<AbstractResource,EnhancedAnalysisDictionary> enhancedAnalysisDictionaryFactory(ENHANCEDANALYSISDICTIONARY_CLASSID);

class EnhancedAnalysisDictionaryPrivate
{
  friend class EnhancedAnalysisDictionary;

  EnhancedAnalysisDictionaryPrivate();

  EnhancedAnalysisDictionaryPrivate(
    FsaStringsPool* sp,
    Lima::Common::AbstractAccessByString* access,
    const std::string& dataFile);

  ~EnhancedAnalysisDictionaryPrivate();

  Lima::Common::AbstractAccessByString* m_access;
  DictionaryData* m_dicoData;
  Lima::FsaStringsPool* m_sp;
  bool m_isMainKeys;
  // A read/write locker to prevent accessing m_dicoData during its loading
  QReadWriteLock m_lock;
};

EnhancedAnalysisDictionaryPrivate::EnhancedAnalysisDictionaryPrivate() :
    m_access(0),
    m_dicoData(new DictionaryData()),
    m_sp(0)
{
}

EnhancedAnalysisDictionaryPrivate::EnhancedAnalysisDictionaryPrivate(
      FsaStringsPool* sp,
      Lima::Common::AbstractAccessByString* access,
      const std::string& dataFile) :
   m_access(access),
   m_dicoData(new DictionaryData()),
   m_sp(sp)
{
  QWriteLocker locker(&m_lock);
  m_dicoData->loadBinaryFile(dataFile);
}

EnhancedAnalysisDictionaryPrivate::~EnhancedAnalysisDictionaryPrivate()
{
}

EnhancedAnalysisDictionary::EnhancedAnalysisDictionary(const QString& dataFilePath)
    : AbstractAnalysisDictionary(),
    m_d(new EnhancedAnalysisDictionaryPrivate())
{
  ANALYSISDICTLOGINIT;
//   LDEBUG  << "EnhancedAnalysisDictionary::EnhancedAnalysisDictionary";
  connect(this,SIGNAL(resourceFileChanged(QString)),this,SLOT(dictionaryFileChanged(QString)));

  if (!dataFilePath.isEmpty())
  {
    resourceFileWatcher().addPath(dataFilePath);
    QWriteLocker locker(&m_d->m_lock);
    m_d->m_dicoData->loadBinaryFile(dataFilePath.toUtf8().constData());
  }
}

EnhancedAnalysisDictionary::EnhancedAnalysisDictionary(
      FsaStringsPool* sp,
      Lima::Common::AbstractAccessByString* access,
      const std::string& dataFile) :
   m_d(new EnhancedAnalysisDictionaryPrivate(sp, access, dataFile))
{
  ANALYSISDICTLOGINIT;
//   LDEBUG  << "EnhancedAnalysisDictionary::EnhancedAnalysisDictionary" << dataFile.c_str();
  connect(this,SIGNAL(resourceFileChanged(QString)),this,SLOT(dictionaryFileChanged(QString)));
  resourceFileWatcher().addPath(QString::fromUtf8(dataFile.c_str()));
  QWriteLocker locker(&m_d->m_lock);
  m_d->m_dicoData->loadBinaryFile(dataFile);
}

EnhancedAnalysisDictionary::~EnhancedAnalysisDictionary()
{
  delete m_d;
}

void EnhancedAnalysisDictionary::init(
  Common::XMLConfigurationFiles::GroupConfigurationStructure& unitConfiguration,
  Manager* manager)
{
  ANALYSISDICTLOGINIT;
//   LDEBUG  << "EnhancedAnalysisDictionary::init";
  MediaId language=manager->getInitializationParameters().language;
  m_d->m_sp=&Common::MediaticData::MediaticData::changeable().stringsPool(language);
  try
  {
    string accessId=unitConfiguration.getParamsValueAtKey("accessKeys");
    const AbstractResource* res=LinguisticResources::single().getResource(language,accessId);
    const AbstractAccessResource* aar=static_cast<const AbstractAccessResource*>(res);
    m_d->m_isMainKeys=aar->isMainKeys();
    m_d->m_access=aar->getAccessByString();
  }
  catch (NoSuchParam& )
  {
    LERROR << "no param 'accessKeys' in EnhancedAnalysisDictionary group for language " << (int) language;
    throw InvalidConfiguration();
  }
  try
  {
    std::string binaryFilePath = Common::MediaticData::MediaticData::single().getResourcesPath() + "/" + unitConfiguration.getParamsValueAtKey("dictionaryValuesFile");
    resourceFileWatcher().addPath(QString::fromUtf8(binaryFilePath.c_str()));
    QWriteLocker locker(&m_d->m_lock);
    m_d->m_dicoData->loadBinaryFile(binaryFilePath);
  }
  catch (NoSuchList& )
  {
    LERROR << "no list 'dictionaryValues' in EnhancedAnalysisDictionary group for language " << (int) language;
    throw InvalidConfiguration();
  }

}

void EnhancedAnalysisDictionary::dictionaryFileChanged ( const QString & path )
{
  ANALYSISDICTLOGINIT;
  LINFO << "EnhancedAnalysisDictionary::dictionaryFileChanged" << path;
  QWriteLocker locker(&m_d->m_lock);
  if (m_d->m_dicoData != 0)
    delete m_d->m_dicoData;
  m_d->m_dicoData = new DictionaryData();
  m_d->m_dicoData->loadBinaryFile(path.toUtf8().constData());
}

uint64_t EnhancedAnalysisDictionary::getSize() const
{
  QReadLocker locker(&m_d->m_lock);
  return m_d->m_dicoData->getSize();
}

DictionaryEntry EnhancedAnalysisDictionary::getEntry(const StringsPoolIndex wordId, const Lima::LimaString& word) const
{
  if (m_d->m_isMainKeys) return getEntryData(wordId);
  return getEntryData(static_cast<StringsPoolIndex>(m_d->m_access->getIndex(word)));
}

DictionaryEntry EnhancedAnalysisDictionary::getEntry(const Lima::LimaString& word) const
{
  return getEntryData( static_cast<StringsPoolIndex>(m_d->m_access->getIndex(word)) );
}

DictionaryEntry EnhancedAnalysisDictionary::getEntry(const StringsPoolIndex wordId) const
{
  if (m_d->m_isMainKeys) return getEntryData(wordId);
  return getEntryData( static_cast<StringsPoolIndex>(m_d->m_access->getIndex((*m_d->m_sp)[wordId])) );
}

DictionaryEntry EnhancedAnalysisDictionary::getEntryData(const StringsPoolIndex wordId) const
{
//  ANALYSISDICTLOGINIT;
//  LDEBUG << "getEntry " << wordId;
  QReadLocker locker(&m_d->m_lock);
  if (wordId >= m_d->m_dicoData->getSize())
  {
//    LDEBUG << "return empty : index out of range";
    return DictionaryEntry(new EnhancedAnalysisDictionaryEntry(static_cast<StringsPoolIndex>(0),false,true,false,false,false,0,0,m_d->m_dicoData,m_d->m_isMainKeys,m_d->m_access,m_d->m_sp));
  }

  StringsPoolIndex strId=wordId;
  if (!m_d->m_isMainKeys) {
    strId = (*m_d->m_sp)[m_d->m_access->getSpelling(wordId)];
  }

  unsigned char* p=m_d->m_dicoData->getEntryAddr(wordId);
  uint64_t read=DictionaryData::readCodedInt(p);
  bool final=false;
  if (read == 1)
  {
    final=true;
    read=DictionaryData::readCodedInt(p);
  }
  if (read == 0)
  {
//    LDEBUG << "return empty entry";
    return DictionaryEntry(new EnhancedAnalysisDictionaryEntry(strId,final,true,false,false,false,p,p,m_d->m_dicoData,m_d->m_isMainKeys,m_d->m_access,m_d->m_sp));
  }
  unsigned char* start=p;
  unsigned char* end=p+read;
  bool hasLing=false;
  bool hasAccented=false;
  bool hasConcat=false;
  read=DictionaryData::readCodedInt(p);
  hasLing=(read != 0);
//  LDEBUG << "info length = " << read;
  p+=read;
  if (p!=end)
  {
    read=DictionaryData::readCodedInt(p);
//    LDEBUG << "accented length = " << read;
    hasAccented=(read != 0);
    p+=read;
    if (p!=end)
    {
      read=DictionaryData::readCodedInt(p);
//      LDEBUG << "concat length = " << read;
      hasConcat=(read != 0);
      p+=read;
    }
  }
  Q_ASSERT(p==end);
//  LDEBUG << "return entry " << (uint64_t)start << " , " << (uint64_t)end;
  return DictionaryEntry(new EnhancedAnalysisDictionaryEntry(strId,final,false,hasLing,hasConcat,hasAccented,start,end,m_d->m_dicoData,m_d->m_isMainKeys,m_d->m_access,m_d->m_sp));
}

std::pair< DictionarySubWordIterator, DictionarySubWordIterator > EnhancedAnalysisDictionary::getSubWordEntries(const int offset, const LimaString& key) const
{
  std::pair<AccessSubWordIterator,AccessSubWordIterator> accessItrs=
    m_d->m_access->getSubWords(offset,key);
  return std::pair< DictionarySubWordIterator, DictionarySubWordIterator >(
           DictionarySubWordIterator(new EnhancedAnalysisDictionarySubWordIterator(accessItrs.first,*this)),
           DictionarySubWordIterator(new EnhancedAnalysisDictionarySubWordIterator(accessItrs.second,*this)));

}

std::pair< DictionarySuperWordIterator, DictionarySuperWordIterator > EnhancedAnalysisDictionary::getSuperWordEntries(const LimaString& key) const
{
  std::pair<AccessSuperWordIterator,AccessSuperWordIterator> accessItrs=
    m_d->m_access->getSuperWords(key);
  return std::pair< DictionarySuperWordIterator, DictionarySuperWordIterator >(
           DictionarySuperWordIterator(new EnhancedAnalysisDictionarySuperWordIterator(accessItrs.first)),
           DictionarySuperWordIterator(new EnhancedAnalysisDictionarySuperWordIterator(accessItrs.second)));
}

} // namespace
} // namespace
} // namespace
<commit_msg>Do not try to reload a deleted resource file<commit_after>/*
    Copyright 2002-2015 CEA LIST

    This file is part of LIMA.

    LIMA 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.

    LIMA 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 LIMA.  If not, see <http://www.gnu.org/licenses/>
*/
#include "EnhancedAnalysisDictionary.h"
#include "EnhancedAnalysisDictionaryIterator.h"
#include "AbstractAccessResource.h"
#include "common/XMLConfigurationFiles/xmlConfigurationFileExceptions.h"
#include "common/AbstractFactoryPattern/SimpleFactory.h"
#include "common/MediaticData/mediaticData.h"
#include "linguisticProcessing/core/LinguisticResources/LinguisticResources.h"

#include <iostream>
#include <QReadWriteLock>
#include <QWriteLocker>
#include <QReadLocker>
#include <QFileInfo>

using namespace Lima;
using namespace Lima::Common;
using namespace Lima::Common::XMLConfigurationFiles;
using namespace Lima::Common::MediaticData;
using namespace std;

namespace Lima
{

namespace LinguisticProcessing
{

namespace AnalysisDict
{

SimpleFactory<AbstractResource,EnhancedAnalysisDictionary> enhancedAnalysisDictionaryFactory(ENHANCEDANALYSISDICTIONARY_CLASSID);

class EnhancedAnalysisDictionaryPrivate
{
  friend class EnhancedAnalysisDictionary;

  EnhancedAnalysisDictionaryPrivate();

  EnhancedAnalysisDictionaryPrivate(
    FsaStringsPool* sp,
    Lima::Common::AbstractAccessByString* access,
    const std::string& dataFile);

  ~EnhancedAnalysisDictionaryPrivate();

  Lima::Common::AbstractAccessByString* m_access;
  DictionaryData* m_dicoData;
  Lima::FsaStringsPool* m_sp;
  bool m_isMainKeys;
  // A read/write locker to prevent accessing m_dicoData during its loading
  QReadWriteLock m_lock;
};

EnhancedAnalysisDictionaryPrivate::EnhancedAnalysisDictionaryPrivate() :
    m_access(0),
    m_dicoData(new DictionaryData()),
    m_sp(0)
{
}

EnhancedAnalysisDictionaryPrivate::EnhancedAnalysisDictionaryPrivate(
      FsaStringsPool* sp,
      Lima::Common::AbstractAccessByString* access,
      const std::string& dataFile) :
   m_access(access),
   m_dicoData(new DictionaryData()),
   m_sp(sp)
{
  QWriteLocker locker(&m_lock);
  m_dicoData->loadBinaryFile(dataFile);
}

EnhancedAnalysisDictionaryPrivate::~EnhancedAnalysisDictionaryPrivate()
{
}

EnhancedAnalysisDictionary::EnhancedAnalysisDictionary(const QString& dataFilePath)
    : AbstractAnalysisDictionary(),
    m_d(new EnhancedAnalysisDictionaryPrivate())
{
  ANALYSISDICTLOGINIT;
//   LDEBUG  << "EnhancedAnalysisDictionary::EnhancedAnalysisDictionary";
  connect(this,SIGNAL(resourceFileChanged(QString)),this,SLOT(dictionaryFileChanged(QString)));

  if (!dataFilePath.isEmpty())
  {
    resourceFileWatcher().addPath(dataFilePath);
    QWriteLocker locker(&m_d->m_lock);
    m_d->m_dicoData->loadBinaryFile(dataFilePath.toUtf8().constData());
  }
}

EnhancedAnalysisDictionary::EnhancedAnalysisDictionary(
      FsaStringsPool* sp,
      Lima::Common::AbstractAccessByString* access,
      const std::string& dataFile) :
   m_d(new EnhancedAnalysisDictionaryPrivate(sp, access, dataFile))
{
  ANALYSISDICTLOGINIT;
//   LDEBUG  << "EnhancedAnalysisDictionary::EnhancedAnalysisDictionary" << dataFile.c_str();
  connect(this,SIGNAL(resourceFileChanged(QString)),this,SLOT(dictionaryFileChanged(QString)));
  resourceFileWatcher().addPath(QString::fromUtf8(dataFile.c_str()));
  QWriteLocker locker(&m_d->m_lock);
  m_d->m_dicoData->loadBinaryFile(dataFile);
}

EnhancedAnalysisDictionary::~EnhancedAnalysisDictionary()
{
  delete m_d;
}

void EnhancedAnalysisDictionary::init(
  Common::XMLConfigurationFiles::GroupConfigurationStructure& unitConfiguration,
  Manager* manager)
{
  ANALYSISDICTLOGINIT;
//   LDEBUG  << "EnhancedAnalysisDictionary::init";
  MediaId language=manager->getInitializationParameters().language;
  m_d->m_sp=&Common::MediaticData::MediaticData::changeable().stringsPool(language);
  try
  {
    string accessId=unitConfiguration.getParamsValueAtKey("accessKeys");
    const AbstractResource* res=LinguisticResources::single().getResource(language,accessId);
    const AbstractAccessResource* aar=static_cast<const AbstractAccessResource*>(res);
    m_d->m_isMainKeys=aar->isMainKeys();
    m_d->m_access=aar->getAccessByString();
  }
  catch (NoSuchParam& )
  {
    LERROR << "no param 'accessKeys' in EnhancedAnalysisDictionary group for language " << (int) language;
    throw InvalidConfiguration();
  }
  try
  {
    std::string binaryFilePath = Common::MediaticData::MediaticData::single().getResourcesPath() + "/" + unitConfiguration.getParamsValueAtKey("dictionaryValuesFile");
    resourceFileWatcher().addPath(QString::fromUtf8(binaryFilePath.c_str()));
    QWriteLocker locker(&m_d->m_lock);
    m_d->m_dicoData->loadBinaryFile(binaryFilePath);
  }
  catch (NoSuchList& )
  {
    LERROR << "no list 'dictionaryValues' in EnhancedAnalysisDictionary group for language " << (int) language;
    throw InvalidConfiguration();
  }

}

void EnhancedAnalysisDictionary::dictionaryFileChanged ( const QString & path )
{
  ANALYSISDICTLOGINIT;
  // Check if the file exists as, when a file is replaced, dictionaryFileChanged can be triggered
  // two times, when it is first suppressed and when the new version is available. One should not
  // try to load the missing file
  if (QFileInfo(path).exists())
  {
    LINFO << "EnhancedAnalysisDictionary::dictionaryFileChanged reload" << path;
    QWriteLocker locker(&m_d->m_lock);
    if (m_d->m_dicoData != 0)
      delete m_d->m_dicoData;
    m_d->m_dicoData = new DictionaryData();
    m_d->m_dicoData->loadBinaryFile(path.toUtf8().constData());
  }
  else
  {
    LINFO << "EnhancedAnalysisDictionary::dictionaryFileChanged deleted, ignoring" << path;
  }
}

uint64_t EnhancedAnalysisDictionary::getSize() const
{
  QReadLocker locker(&m_d->m_lock);
  return m_d->m_dicoData->getSize();
}

DictionaryEntry EnhancedAnalysisDictionary::getEntry(const StringsPoolIndex wordId, const Lima::LimaString& word) const
{
  if (m_d->m_isMainKeys) return getEntryData(wordId);
  return getEntryData(static_cast<StringsPoolIndex>(m_d->m_access->getIndex(word)));
}

DictionaryEntry EnhancedAnalysisDictionary::getEntry(const Lima::LimaString& word) const
{
  return getEntryData( static_cast<StringsPoolIndex>(m_d->m_access->getIndex(word)) );
}

DictionaryEntry EnhancedAnalysisDictionary::getEntry(const StringsPoolIndex wordId) const
{
  if (m_d->m_isMainKeys) return getEntryData(wordId);
  return getEntryData( static_cast<StringsPoolIndex>(m_d->m_access->getIndex((*m_d->m_sp)[wordId])) );
}

DictionaryEntry EnhancedAnalysisDictionary::getEntryData(const StringsPoolIndex wordId) const
{
//  ANALYSISDICTLOGINIT;
//  LDEBUG << "getEntry " << wordId;
  QReadLocker locker(&m_d->m_lock);
  if (wordId >= m_d->m_dicoData->getSize())
  {
//    LDEBUG << "return empty : index out of range";
    return DictionaryEntry(new EnhancedAnalysisDictionaryEntry(static_cast<StringsPoolIndex>(0),false,true,false,false,false,0,0,m_d->m_dicoData,m_d->m_isMainKeys,m_d->m_access,m_d->m_sp));
  }

  StringsPoolIndex strId=wordId;
  if (!m_d->m_isMainKeys) {
    strId = (*m_d->m_sp)[m_d->m_access->getSpelling(wordId)];
  }

  unsigned char* p=m_d->m_dicoData->getEntryAddr(wordId);
  uint64_t read=DictionaryData::readCodedInt(p);
  bool final=false;
  if (read == 1)
  {
    final=true;
    read=DictionaryData::readCodedInt(p);
  }
  if (read == 0)
  {
//    LDEBUG << "return empty entry";
    return DictionaryEntry(new EnhancedAnalysisDictionaryEntry(strId,final,true,false,false,false,p,p,m_d->m_dicoData,m_d->m_isMainKeys,m_d->m_access,m_d->m_sp));
  }
  unsigned char* start=p;
  unsigned char* end=p+read;
  bool hasLing=false;
  bool hasAccented=false;
  bool hasConcat=false;
  read=DictionaryData::readCodedInt(p);
  hasLing=(read != 0);
//  LDEBUG << "info length = " << read;
  p+=read;
  if (p!=end)
  {
    read=DictionaryData::readCodedInt(p);
//    LDEBUG << "accented length = " << read;
    hasAccented=(read != 0);
    p+=read;
    if (p!=end)
    {
      read=DictionaryData::readCodedInt(p);
//      LDEBUG << "concat length = " << read;
      hasConcat=(read != 0);
      p+=read;
    }
  }
  Q_ASSERT(p==end);
//  LDEBUG << "return entry " << (uint64_t)start << " , " << (uint64_t)end;
  return DictionaryEntry(new EnhancedAnalysisDictionaryEntry(strId,final,false,hasLing,hasConcat,hasAccented,start,end,m_d->m_dicoData,m_d->m_isMainKeys,m_d->m_access,m_d->m_sp));
}

std::pair< DictionarySubWordIterator, DictionarySubWordIterator > EnhancedAnalysisDictionary::getSubWordEntries(const int offset, const LimaString& key) const
{
  std::pair<AccessSubWordIterator,AccessSubWordIterator> accessItrs=
    m_d->m_access->getSubWords(offset,key);
  return std::pair< DictionarySubWordIterator, DictionarySubWordIterator >(
           DictionarySubWordIterator(new EnhancedAnalysisDictionarySubWordIterator(accessItrs.first,*this)),
           DictionarySubWordIterator(new EnhancedAnalysisDictionarySubWordIterator(accessItrs.second,*this)));

}

std::pair< DictionarySuperWordIterator, DictionarySuperWordIterator > EnhancedAnalysisDictionary::getSuperWordEntries(const LimaString& key) const
{
  std::pair<AccessSuperWordIterator,AccessSuperWordIterator> accessItrs=
    m_d->m_access->getSuperWords(key);
  return std::pair< DictionarySuperWordIterator, DictionarySuperWordIterator >(
           DictionarySuperWordIterator(new EnhancedAnalysisDictionarySuperWordIterator(accessItrs.first)),
           DictionarySuperWordIterator(new EnhancedAnalysisDictionarySuperWordIterator(accessItrs.second)));
}

} // namespace
} // namespace
} // namespace
<|endoftext|>


<commit_before>/*======================================================================

  This file is part of the elastix software.

  Copyright (c) University Medical Center Utrecht. All rights reserved.
  See src/CopyrightElastix.txt or http://elastix.isi.uu.nl/legal.php for
  details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE. See the above copyright notices for more information.

======================================================================*/

#ifndef __elxAdvancedMattesMutualInformationMetric_HXX__
#define __elxAdvancedMattesMutualInformationMetric_HXX__

#include "elxAdvancedMattesMutualInformationMetric.h"
#include "itkHardLimiterFunction.h"
#include "itkExponentialLimiterFunction.h"
#include <string>
#include "vnl/vnl_math.h"

namespace elastix
{
using namespace itk;

  /**
   * ****************** Constructor ***********************
   */

  template <class TElastix>
    AdvancedMattesMutualInformationMetric<TElastix>
    ::AdvancedMattesMutualInformationMetric()
  {
    this->m_Param_c = 1.0;
    this->m_Param_gamma = 0.101;
    this->SetUseDerivative( true );

  } // end Constructor()

  /**
   * ******************* Initialize ***********************
   */

  template <class TElastix>
    void AdvancedMattesMutualInformationMetric<TElastix>
    ::Initialize( void ) throw (ExceptionObject)
  {
    TimerPointer timer = TimerType::New();
    timer->StartTimer();
    this->Superclass1::Initialize();
    timer->StopTimer();
    elxout << "Initialization of AdvancedMattesMutualInformation metric took: "
      << static_cast<long>( timer->GetElapsedClockSec() * 1000 ) << " ms." << std::endl;

  } // end Initialize()

  /**
   * ***************** BeforeEachResolution ***********************
   */

  template <class TElastix>
    void AdvancedMattesMutualInformationMetric<TElastix>
    ::BeforeEachResolution( void )
  {
    /** Get the current resolution level. */
    unsigned int level =
      ( this->m_Registration->GetAsITKBaseType() )->GetCurrentLevel();

    /** Get and set the number of histogram bins. */
    unsigned int numberOfHistogramBins = 32;
    this->GetConfiguration()->ReadParameter( numberOfHistogramBins,
      "NumberOfHistogramBins", this->GetComponentLabel(), level, 0 );
    this->SetNumberOfFixedHistogramBins( numberOfHistogramBins );
    this->SetNumberOfMovingHistogramBins( numberOfHistogramBins );

    unsigned int numberOfFixedHistogramBins = numberOfHistogramBins;
    unsigned int numberOfMovingHistogramBins = numberOfHistogramBins;
    this->GetConfiguration()->ReadParameter( numberOfFixedHistogramBins,
      "NumberOfFixedHistogramBins", this->GetComponentLabel(), level, 0 );
    this->GetConfiguration()->ReadParameter( numberOfMovingHistogramBins,
      "NumberOfMovingHistogramBins", this->GetComponentLabel(), level, 0 );
    this->SetNumberOfFixedHistogramBins( numberOfFixedHistogramBins );
    this->SetNumberOfMovingHistogramBins( numberOfMovingHistogramBins );

    /** Set limiters. */
    typedef HardLimiterFunction< RealType, FixedImageDimension > FixedLimiterType;
    typedef ExponentialLimiterFunction< RealType, MovingImageDimension > MovingLimiterType;
    this->SetFixedImageLimiter( FixedLimiterType::New() );
    this->SetMovingImageLimiter( MovingLimiterType::New() );

    /** Get and set the limit range ratios. */
    double fixedLimitRangeRatio = 0.01;
    double movingLimitRangeRatio = 0.01;
    this->GetConfiguration()->ReadParameter( fixedLimitRangeRatio,
      "FixedLimitRangeRatio", this->GetComponentLabel(), level, 0 );
    this->GetConfiguration()->ReadParameter( movingLimitRangeRatio,
      "MovingLimitRangeRatio", this->GetComponentLabel(), level, 0 );
    this->SetFixedLimitRangeRatio( fixedLimitRangeRatio );
    this->SetMovingLimitRangeRatio( movingLimitRangeRatio );

    /** Set B-spline Parzen kernel orders. */
    unsigned int fixedKernelBSplineOrder = 0;
    unsigned int movingKernelBSplineOrder = 3;
    this->GetConfiguration()->ReadParameter( fixedKernelBSplineOrder,
      "FixedKernelBSplineOrder", this->GetComponentLabel(), level, 0 );
    this->GetConfiguration()->ReadParameter( movingKernelBSplineOrder,
      "MovingKernelBSplineOrder", this->GetComponentLabel(), level, 0 );
    this->SetFixedKernelBSplineOrder( fixedKernelBSplineOrder );
    this->SetMovingKernelBSplineOrder( movingKernelBSplineOrder );

    /** Set whether a low memory consumption should be used. */
    bool useExplicitPDFDerivatives = true;
    this->GetConfiguration()->ReadParameter( useExplicitPDFDerivatives,
      "UseExplicitPDFDerivatives", this->GetComponentLabel(), level, 0 );
    this->SetUseExplicitPDFDerivatives( useExplicitPDFDerivatives );

    /** Set whether a finite difference derivative should be used. */
    bool useFiniteDifferenceDerivative = false;
    this->GetConfiguration()->ReadParameter( useFiniteDifferenceDerivative,
      "FiniteDifferenceDerivative", this->GetComponentLabel(), level, 0 );
    this->SetUseFiniteDifferenceDerivative( useFiniteDifferenceDerivative );

    /** Prepare for computing the perturbation gain c_k. */
    this->SetCurrentIteration( 0 );
    if ( useFiniteDifferenceDerivative )
    {
      double c = 1.0;
      double gamma = 0.101;
      this->GetConfiguration()->ReadParameter(c, "SP_c", this->GetComponentLabel(), level, 0 );
      this->GetConfiguration()->ReadParameter(gamma, "SP_gamma", this->GetComponentLabel(), level, 0 );
      this->SetParam_c( c );
      this->SetParam_gamma( gamma );
      this->SetFiniteDifferencePerturbation( this->Compute_c( 0 ) );
    }

    this->SetUseMovingImageDerivativeScales( false );
    MovingImageDerivativeScalesType movingImageDerivativeScales;
    int usescales = 0;
    for ( unsigned int i = 0; i < MovingImageDimension; ++i )
    {
      usescales |= this->GetConfiguration()->ReadParameter( movingImageDerivativeScales[i],
        "MovingImageDerivativeScales", this->GetComponentLabel(), i, -1, true );
    }
    if ( usescales == 0 )
    {
      this->SetUseMovingImageDerivativeScales( true );
      this->SetMovingImageDerivativeScales( movingImageDerivativeScales );
      elxout << "Multiplying moving image derivatives by: " << movingImageDerivativeScales << std::endl;
    }

  } // end BeforeEachResolution()

  /**
   * ***************** AfterEachIteration ***********************
   */

  template <class TElastix>
    void AdvancedMattesMutualInformationMetric<TElastix>
    ::AfterEachIteration(void)
  {
    if ( this->GetUseFiniteDifferenceDerivative() )
    {
      this->m_CurrentIteration++;
      this->SetFiniteDifferencePerturbation(
        this->Compute_c( this->m_CurrentIteration )  );
    }
  }  // end AfterEachIteration()

  /**
   * ************************** Compute_c *************************
   *
   * This function computes the parameter a at iteration k, as
   * in the finite difference optimizer.
   */

  template <class TElastix>
    double AdvancedMattesMutualInformationMetric<TElastix>
    ::Compute_c( unsigned long k ) const
  {
    return static_cast<double>(
      this->m_Param_c / vcl_pow( k + 1, this->m_Param_gamma ) );

  } // end Compute_c()

} // end namespace elastix

#endif // end #ifndef __elxAdvancedMattesMutualInformationMetric_HXX__
<commit_msg>MS:<commit_after>/*======================================================================

  This file is part of the elastix software.

  Copyright (c) University Medical Center Utrecht. All rights reserved.
  See src/CopyrightElastix.txt or http://elastix.isi.uu.nl/legal.php for
  details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE. See the above copyright notices for more information.

======================================================================*/

#ifndef __elxAdvancedMattesMutualInformationMetric_HXX__
#define __elxAdvancedMattesMutualInformationMetric_HXX__

#include "elxAdvancedMattesMutualInformationMetric.h"
#include "itkHardLimiterFunction.h"
#include "itkExponentialLimiterFunction.h"
#include <string>
#include "vnl/vnl_math.h"

namespace elastix
{
using namespace itk;

  /**
   * ****************** Constructor ***********************
   */

  template <class TElastix>
    AdvancedMattesMutualInformationMetric<TElastix>
    ::AdvancedMattesMutualInformationMetric()
  {
    this->m_Param_c = 1.0;
    this->m_Param_gamma = 0.101;
    this->SetUseDerivative( true );

  } // end Constructor()

  /**
   * ******************* Initialize ***********************
   */

  template <class TElastix>
    void AdvancedMattesMutualInformationMetric<TElastix>
    ::Initialize( void ) throw (ExceptionObject)
  {
    TimerPointer timer = TimerType::New();
    timer->StartTimer();
    this->Superclass1::Initialize();
    timer->StopTimer();
    elxout << "Initialization of AdvancedMattesMutualInformation metric took: "
      << static_cast<long>( timer->GetElapsedClockSec() * 1000 ) << " ms." << std::endl;

  } // end Initialize()

  /**
   * ***************** BeforeEachResolution ***********************
   */

  template <class TElastix>
    void AdvancedMattesMutualInformationMetric<TElastix>
    ::BeforeEachResolution( void )
  {
    /** Get the current resolution level. */
    unsigned int level =
      ( this->m_Registration->GetAsITKBaseType() )->GetCurrentLevel();

    /** Get and set the number of histogram bins. */
    unsigned int numberOfHistogramBins = 32;
    this->GetConfiguration()->ReadParameter( numberOfHistogramBins,
      "NumberOfHistogramBins", this->GetComponentLabel(), level, 0 );
    this->SetNumberOfFixedHistogramBins( numberOfHistogramBins );
    this->SetNumberOfMovingHistogramBins( numberOfHistogramBins );

    unsigned int numberOfFixedHistogramBins = numberOfHistogramBins;
    unsigned int numberOfMovingHistogramBins = numberOfHistogramBins;
    this->GetConfiguration()->ReadParameter( numberOfFixedHistogramBins,
      "NumberOfFixedHistogramBins", this->GetComponentLabel(), level, 0 );
    this->GetConfiguration()->ReadParameter( numberOfMovingHistogramBins,
      "NumberOfMovingHistogramBins", this->GetComponentLabel(), level, 0 );
    this->SetNumberOfFixedHistogramBins( numberOfFixedHistogramBins );
    this->SetNumberOfMovingHistogramBins( numberOfMovingHistogramBins );

    /** Set limiters. */
    typedef HardLimiterFunction< RealType, FixedImageDimension > FixedLimiterType;
    typedef ExponentialLimiterFunction< RealType, MovingImageDimension > MovingLimiterType;
    this->SetFixedImageLimiter( FixedLimiterType::New() );
    this->SetMovingImageLimiter( MovingLimiterType::New() );

    /** Get and set the limit range ratios. */
    double fixedLimitRangeRatio = 0.01;
    double movingLimitRangeRatio = 0.01;
    this->GetConfiguration()->ReadParameter( fixedLimitRangeRatio,
      "FixedLimitRangeRatio", this->GetComponentLabel(), level, 0 );
    this->GetConfiguration()->ReadParameter( movingLimitRangeRatio,
      "MovingLimitRangeRatio", this->GetComponentLabel(), level, 0 );
    this->SetFixedLimitRangeRatio( fixedLimitRangeRatio );
    this->SetMovingLimitRangeRatio( movingLimitRangeRatio );

    /** Set B-spline Parzen kernel orders. */
    unsigned int fixedKernelBSplineOrder = 0;
    unsigned int movingKernelBSplineOrder = 3;
    this->GetConfiguration()->ReadParameter( fixedKernelBSplineOrder,
      "FixedKernelBSplineOrder", this->GetComponentLabel(), level, 0 );
    this->GetConfiguration()->ReadParameter( movingKernelBSplineOrder,
      "MovingKernelBSplineOrder", this->GetComponentLabel(), level, 0 );
    this->SetFixedKernelBSplineOrder( fixedKernelBSplineOrder );
    this->SetMovingKernelBSplineOrder( movingKernelBSplineOrder );

    /** Set whether a low memory consumption should be used. */
    bool useExplicitPDFDerivatives = true;
    this->GetConfiguration()->ReadParameter( useExplicitPDFDerivatives,
      "UseFastAndLowMemoryVersion", this->GetComponentLabel(), level, 0 );
    this->SetUseExplicitPDFDerivatives( useExplicitPDFDerivatives );

    /** Set whether a finite difference derivative should be used. */
    bool useFiniteDifferenceDerivative = false;
    this->GetConfiguration()->ReadParameter( useFiniteDifferenceDerivative,
      "FiniteDifferenceDerivative", this->GetComponentLabel(), level, 0 );
    this->SetUseFiniteDifferenceDerivative( useFiniteDifferenceDerivative );

    /** Prepare for computing the perturbation gain c_k. */
    this->SetCurrentIteration( 0 );
    if ( useFiniteDifferenceDerivative )
    {
      double c = 1.0;
      double gamma = 0.101;
      this->GetConfiguration()->ReadParameter(c, "SP_c", this->GetComponentLabel(), level, 0 );
      this->GetConfiguration()->ReadParameter(gamma, "SP_gamma", this->GetComponentLabel(), level, 0 );
      this->SetParam_c( c );
      this->SetParam_gamma( gamma );
      this->SetFiniteDifferencePerturbation( this->Compute_c( 0 ) );
    }

    this->SetUseMovingImageDerivativeScales( false );
    MovingImageDerivativeScalesType movingImageDerivativeScales;
    int usescales = 0;
    for ( unsigned int i = 0; i < MovingImageDimension; ++i )
    {
      usescales |= this->GetConfiguration()->ReadParameter( movingImageDerivativeScales[i],
        "MovingImageDerivativeScales", this->GetComponentLabel(), i, -1, true );
    }
    if ( usescales == 0 )
    {
      this->SetUseMovingImageDerivativeScales( true );
      this->SetMovingImageDerivativeScales( movingImageDerivativeScales );
      elxout << "Multiplying moving image derivatives by: " << movingImageDerivativeScales << std::endl;
    }

  } // end BeforeEachResolution()

  /**
   * ***************** AfterEachIteration ***********************
   */

  template <class TElastix>
    void AdvancedMattesMutualInformationMetric<TElastix>
    ::AfterEachIteration(void)
  {
    if ( this->GetUseFiniteDifferenceDerivative() )
    {
      this->m_CurrentIteration++;
      this->SetFiniteDifferencePerturbation(
        this->Compute_c( this->m_CurrentIteration )  );
    }
  }  // end AfterEachIteration()

  /**
   * ************************** Compute_c *************************
   *
   * This function computes the parameter a at iteration k, as
   * in the finite difference optimizer.
   */

  template <class TElastix>
    double AdvancedMattesMutualInformationMetric<TElastix>
    ::Compute_c( unsigned long k ) const
  {
    return static_cast<double>(
      this->m_Param_c / vcl_pow( k + 1, this->m_Param_gamma ) );

  } // end Compute_c()

} // end namespace elastix

#endif // end #ifndef __elxAdvancedMattesMutualInformationMetric_HXX__
<|endoftext|>


<commit_before>/*
 * 				Copyright <SWGEmu>
		See file COPYING for copying conditions. */

#include "DroidMaintenanceModuleDataComponent.h"
#include "server/zone/ZoneServer.h"
#include "server/zone/objects/tangible/component/droid/DroidComponent.h"
#include "server/zone/packets/object/ObjectMenuResponse.h"
#include "server/zone/objects/player/sui/listbox/SuiListBox.h"
#include "server/zone/objects/player/sui/callbacks/RemoveDroidStructureSuiCallback.h"
#include "server/zone/objects/player/sessions/DroidMaintenanceSession.h"
#include "server/zone/Zone.h"

DroidMaintenanceModuleDataComponent::DroidMaintenanceModuleDataComponent() {
	setLoggingName("DroidMaintenanceModule");
	moduleRating = 0;
	maxStructures = 3;
	assignedStructures.removeAll();
}

DroidMaintenanceModuleDataComponent::~DroidMaintenanceModuleDataComponent() {
	// No op
}

String DroidMaintenanceModuleDataComponent::getModuleName() {
	return String("maintenance_module");
}

void DroidMaintenanceModuleDataComponent::initializeTransientMembers() {

	// Pull module stat from parent sceno
	DroidComponent* droidComponent = cast<DroidComponent*>(getParent());
	if (droidComponent == NULL) {
		info("droidComponent was null");
		return;
	}

	if( droidComponent->hasKey( "struct_module") ){
		moduleRating = droidComponent->getAttributeValue( "struct_module");
	}
}

void DroidMaintenanceModuleDataComponent::updateCraftingValues(CraftingValues* values, bool firstUpdate) {
	moduleRating = values->getCurrentValue("struct_module");
}

void DroidMaintenanceModuleDataComponent::fillAttributeList(AttributeListMessage* alm, CreatureObject* droid) {
	alm->insertAttribute( "struct_module", moduleRating ); // Structure Maintenance Rating
}

void DroidMaintenanceModuleDataComponent::fillObjectMenuResponse(SceneObject* droidObject, ObjectMenuResponse* menuResponse, CreatureObject* player) {

	menuResponse->addRadialMenuItem(MAINT_MODULE_ROOT, 3, "@pet/droid_modules:struct_maint_root" ); // Structure Maintenance
	menuResponse->addRadialMenuItemToRadialID( MAINT_MODULE_ROOT, MAINT_MODULE_PERFORM, 3, "@pet/droid_modules:struct_maint_perform" ); // Perform Maintenance Run
	menuResponse->addRadialMenuItemToRadialID( MAINT_MODULE_ROOT, MAINT_MODULE_EDIT, 3, "@pet/droid_modules:struct_maint_edit" ); // Edit Maintenance List

}

int DroidMaintenanceModuleDataComponent::handleObjectMenuSelect(CreatureObject* player, byte selectedID, PetControlDevice* controller) {

	ManagedReference<DroidObject*> droid = getDroidObject();
	if( droid == NULL ){
		info( "Droid is null");
		return 0;
	}

	Locker dlock( droid, player );

	// Perform Maintenance Run
	if( selectedID == MAINT_MODULE_PERFORM ){

		// Init Maintenance Run Session
		//droid->getCooldownTimerMap()->updateToCurrentAndAddMili("Droid_Cooldown",1800000);
		// Interplanetary Time + 1 hour 40 minutes, 33 seconds
		// Local time: distance < 432 == 15 mins + (1s per 3 meters to next structure for that planet @ < 432m from first structure)
		// Local time: distance > 432 == 48 minutes 20 seconds
		// Start a session for the player and this module!
		ManagedReference<DroidMaintenanceSession*> session = new DroidMaintenanceSession(this, player);
		session->initialize();
		player->addActiveSession(SessionFacadeType::DROIDMAINTENANCERUN, session);
		session->sendMaintanceRunBox();
		return 0;
	}
	// Edit Maintenance List
	else if( selectedID == MAINT_MODULE_EDIT ){

		if( assignedStructures.isEmpty() ){
			player->sendSystemMessage( "@pet/droid_modules:droid_maint_empty_list" ); // The maintenance list for this droid is empty. Go to one of your structures and assign this droid to maintain it.
			return 0;
		}

		// Build List of Assigned Structures
		ManagedReference<SuiListBox*> box = new SuiListBox(player, SuiWindowType::DROID_EDIT_ASSIGNED_STRUCTURES, SuiListBox::HANDLETWOBUTTON);
		box->setCallback(new RemoveDroidStructureSuiCallback(player->getZoneServer(), this));
		box->setPromptTitle("@pet/droid_modules:droid_maint_edit_title"); // Edit Maintenance List
		box->setPromptText("@pet/droid_modules:droid_maint_edit_prompt"); // Please select a structure to remove from this droid's maintenance list.

		box->setOkButton(true, "@delete");
		box->setCancelButton(true, "@cancel");

		for (int i = 0; i < assignedStructures.size(); i++) {

			uint64 objectId = assignedStructures.elementAt(i);
			ManagedReference<SceneObject*> obj = player->getZoneServer()->getObject(objectId);
			if( obj != NULL && obj->isStructureObject() ){
				StructureObject* structureObject = cast<StructureObject*>(obj.get());
				if( structureObject != NULL ){
					box->addMenuItem( structureObject->getDisplayedName(), objectId );
				}
			}
		}

		box->setUsingObject(droid);
		player->getPlayerObject()->addSuiBox(box);
		player->sendMessage(box->generateMessage());

	}

	return 0;
}

int DroidMaintenanceModuleDataComponent::getBatteryDrain() {
	return 0;  // No constant drain, but each maintenance run will use power
}
void DroidMaintenanceModuleDataComponent::getStructureList(Vector<ManagedReference<StructureObject* > >* list,ZoneServer* srv, uint32 planet) {
	for (int i = 0; i < assignedStructures.size(); i++) {
		uint64 objectId = assignedStructures.elementAt(i);
		ManagedReference<SceneObject*> obj = srv->getObject(objectId);
		if( obj != NULL && obj->isStructureObject() ){
			StructureObject* structureObject = cast<StructureObject*>(obj.get());
			if( structureObject != NULL ){
				// is it on this planet?
				if (structureObject->getZone()->getZoneCRC() == planet) {
					list->add(structureObject);
				} else {
					if (moduleRating >= 12) // ratign 12 min for interplanetary
						list->add(structureObject);
				}
			}
		}
	}
}

void DroidMaintenanceModuleDataComponent::copy(BaseDroidModuleComponent* other){

	DroidMaintenanceModuleDataComponent* otherModule = cast<DroidMaintenanceModuleDataComponent*>(other);
	if( otherModule == NULL )
		return;

	moduleRating = otherModule->moduleRating;

	// Save stat in parent sceno
	DroidComponent* droidComponent = cast<DroidComponent*>(getParent());
	if (droidComponent == NULL)
		return;

	droidComponent->addProperty("struct_module", moduleRating, 0, "exp_effectiveness");

	if (moduleRating < 6) {
		maxStructures = 3;
	} else if (moduleRating < 12) {
		maxStructures = 6;
	} else if (moduleRating < 15) {
		maxStructures = 9;
	} else {
		maxStructures = 10;
	}
}

void DroidMaintenanceModuleDataComponent::addToStack(BaseDroidModuleComponent* other){

	DroidMaintenanceModuleDataComponent* otherModule = cast<DroidMaintenanceModuleDataComponent*>(other);
	if( otherModule == NULL )
		return;

	// Maintenance modules do not stack.  Just keep the highest rated module
	if( otherModule->moduleRating > moduleRating )
		moduleRating = otherModule->moduleRating;

	// Save stat in parent sceno
	DroidComponent* droidComponent = cast<DroidComponent*>(getParent());
	if (droidComponent == NULL)
		return;

	// Attribute should have already been created in copy method
	if( !droidComponent->changeAttributeValue( "struct_module", moduleRating) ){
		info( "addToStack updateAttributeValue failed");
		return;
	}

	if (moduleRating < 6) {
		maxStructures = 3;
	} else if (moduleRating < 12) {
		maxStructures = 6;
	} else if (moduleRating < 15) {
		maxStructures = 9;
	} else {
		maxStructures = 10;
	}

}

String DroidMaintenanceModuleDataComponent::toString(){
	StringBuffer str;
	str << getModuleName() << "\n";
	str << "Number of Assigned Structures: " << assignedStructures.size() << "\n";
	for (int i = 0; i < assignedStructures.size(); i++) {
		uint64 objectID = assignedStructures.elementAt(i);
		str << "\tStructure: " << objectID << "\n";
	}
	return str.toString();
}
bool DroidMaintenanceModuleDataComponent::isAssignedTo(uint64 structure) {
	return assignedStructures.contains(structure);
}
bool DroidMaintenanceModuleDataComponent::assignStructure( uint64 objectID ){

	if( !assignedStructures.contains( objectID ) && assignedStructures.size() <= maxStructures) {
		assignedStructures.add( objectID );
		return true;
	} else {
		return false;
	}

}

void DroidMaintenanceModuleDataComponent::unassignStructure( uint64 objectID ){

	if( assignedStructures.contains( objectID ) )
		assignedStructures.removeElement( objectID );

}

void DroidMaintenanceModuleDataComponent::onCall(){
	validateStructures();
}

void DroidMaintenanceModuleDataComponent::validateStructures(){

	Vector<uint64> structures;
	assignedStructures.clone( structures );

	for (int i = 0; i < structures.size(); i++) {

		uint64 objectID = structures.elementAt(i);
		if( !isValidStructure( objectID ) ){
			assignedStructures.removeElement( objectID );
		}

	}

}
void DroidMaintenanceModuleDataComponent::payStructures(CreatureObject* player, VectorMap<unsigned long long, int> assignments) {
	// we know each struct to pay and any fees applied.
	ManagedReference<DroidObject*> droid = getDroidObject();

	for(int i=0;i< assignments.size();i++) {
		uint64 objectID = assignments.elementAt(i).getKey();
		int maintToPay = assignments.elementAt(i).getValue();
		ManagedReference<SceneObject*> obj = player->getZoneServer()->getObject(objectID);
		StructureObject* structureObject = cast<StructureObject*>(obj.get());
		if (structureObject != NULL)
			Locker sLock(obj,player);
			structureObject->payMaintenance(maintToPay,player,true);
	}
}
long DroidMaintenanceModuleDataComponent::calculateRunTime(VectorMap<unsigned long long, int> assignments) {
	long duration = 0;
	ManagedReference<DroidObject*> droid = getDroidObject();
	String localPlanet = droid->getZone()->getZoneName();
	ManagedReference<SceneObject*> primeStructure;
	for(int i=0;i< assignments.size();i++) {
		uint64 objectID = assignments.elementAt(i).getKey();
		if(isValidStructure(objectID)) {
			ManagedReference<SceneObject*> obj = droid->getZoneServer()->getObject(objectID);
			if (obj->getZone()->getZoneName() != localPlanet) {
				duration += 6033000; // 1 hr 40 minutes and 33 seconds
			}else {
				// same planet
				if (primeStructure == NULL) {
					primeStructure = obj;
				}
				if (obj == primeStructure) {
					if (droid->getDistanceTo(obj) > 432) {
						duration += 2900000; // 48 minutes and 20 seconds
					} else {
						duration += 900000; // 15 minutes
					}
				} else {
					float distance = primeStructure->getDistanceTo(obj);
					if (distance > 432) {
						duration += 2900000; // 48 minutes and 20 seconds
						primeStructure = obj; // new prime structure
					} else {
						// this struct is within 432 of prime
						float timeAdded = (distance/3) * 1000; // add 1 sec for every 3 meters apart
						duration += (timeAdded + 900000);
						primeStructure = obj; // we now grab this structure as our prime going down the list.
					}
				}
			}
		}
	}
	return duration;
}
bool DroidMaintenanceModuleDataComponent::isValidStructure(uint64 objectID){

	ManagedReference<DroidObject*> droid = getDroidObject();
	if( droid == NULL ) {
		return false;
	}

	ManagedReference<SceneObject*> obj = droid->getZoneServer()->getObject(objectID);
	if( obj == NULL ) {
		return false;
	}

	if( !obj->isStructureObject() ) {
		return false;
	}
	// we need to check for more than structure we need to check for factories and harvestors as well.
	StructureObject* structureObject = cast<StructureObject*>(obj.get());
	if( structureObject == NULL ) {
		return false;
	}

	ManagedWeakReference< CreatureObject*> linkedCreature = droid->getLinkedCreature();
	if( linkedCreature == NULL ) {
		return false;
	}

	if( !structureObject->isOnAdminList( linkedCreature.get() ) ) {
		return false;
	}

	if( structureObject->getZone() == NULL ) {
		return false;
	}

	// All checks passed
	return true;

}

bool DroidMaintenanceModuleDataComponent::toBinaryStream(ObjectOutputStream* stream) {

	int _currentOffset = stream->getOffset();
	stream->writeShort(0);
	int _varCount = writeObjectMembers(stream);
	stream->writeShort(_currentOffset, _varCount);
	return true;
}

int DroidMaintenanceModuleDataComponent::writeObjectMembers(ObjectOutputStream* stream) {

	String _name;
	int _offset;
	uint32 _totalSize;

	_name = "assignedStructures";
	_name.toBinaryStream(stream);
	_offset = stream->getOffset();
	stream->writeInt(0);
	TypeInfo< Vector<unsigned long long> >::toBinaryStream(&assignedStructures, stream);
	_totalSize = (uint32) (stream->getOffset() - (_offset + 4));
	stream->writeInt(_offset, _totalSize);

	return 1;
}

bool DroidMaintenanceModuleDataComponent::readObjectMember(ObjectInputStream* stream, const String& name) {

	if (name == "assignedStructures") {
		TypeInfo< Vector<unsigned long long> >::parseFromBinaryStream(&assignedStructures, stream);
		return true;
	}

	return false;
}

bool DroidMaintenanceModuleDataComponent::parseFromBinaryStream(ObjectInputStream* stream) {

	uint16 _varCount = stream->readShort();

	for (int i = 0; i < _varCount; ++i) {
		String _name;
		_name.parseFromBinaryStream(stream);
		uint32 _varSize = stream->readInt();
		int _currentOffset = stream->getOffset();
		if(readObjectMember(stream, _name)) {
		}
		stream->setOffset(_currentOffset + _varSize);
	}
	return true;
}
<commit_msg>[Fixed] stability issue<commit_after>/*
 * 				Copyright <SWGEmu>
		See file COPYING for copying conditions. */

#include "DroidMaintenanceModuleDataComponent.h"
#include "server/zone/ZoneServer.h"
#include "server/zone/objects/tangible/component/droid/DroidComponent.h"
#include "server/zone/packets/object/ObjectMenuResponse.h"
#include "server/zone/objects/player/sui/listbox/SuiListBox.h"
#include "server/zone/objects/player/sui/callbacks/RemoveDroidStructureSuiCallback.h"
#include "server/zone/objects/player/sessions/DroidMaintenanceSession.h"
#include "server/zone/Zone.h"

DroidMaintenanceModuleDataComponent::DroidMaintenanceModuleDataComponent() {
	setLoggingName("DroidMaintenanceModule");
	moduleRating = 0;
	maxStructures = 3;
	assignedStructures.removeAll();
}

DroidMaintenanceModuleDataComponent::~DroidMaintenanceModuleDataComponent() {
	// No op
}

String DroidMaintenanceModuleDataComponent::getModuleName() {
	return String("maintenance_module");
}

void DroidMaintenanceModuleDataComponent::initializeTransientMembers() {

	// Pull module stat from parent sceno
	DroidComponent* droidComponent = cast<DroidComponent*>(getParent());
	if (droidComponent == NULL) {
		info("droidComponent was null");
		return;
	}

	if( droidComponent->hasKey( "struct_module") ){
		moduleRating = droidComponent->getAttributeValue( "struct_module");
	}
}

void DroidMaintenanceModuleDataComponent::updateCraftingValues(CraftingValues* values, bool firstUpdate) {
	moduleRating = values->getCurrentValue("struct_module");
}

void DroidMaintenanceModuleDataComponent::fillAttributeList(AttributeListMessage* alm, CreatureObject* droid) {
	alm->insertAttribute( "struct_module", moduleRating ); // Structure Maintenance Rating
}

void DroidMaintenanceModuleDataComponent::fillObjectMenuResponse(SceneObject* droidObject, ObjectMenuResponse* menuResponse, CreatureObject* player) {

	menuResponse->addRadialMenuItem(MAINT_MODULE_ROOT, 3, "@pet/droid_modules:struct_maint_root" ); // Structure Maintenance
	menuResponse->addRadialMenuItemToRadialID( MAINT_MODULE_ROOT, MAINT_MODULE_PERFORM, 3, "@pet/droid_modules:struct_maint_perform" ); // Perform Maintenance Run
	menuResponse->addRadialMenuItemToRadialID( MAINT_MODULE_ROOT, MAINT_MODULE_EDIT, 3, "@pet/droid_modules:struct_maint_edit" ); // Edit Maintenance List

}

int DroidMaintenanceModuleDataComponent::handleObjectMenuSelect(CreatureObject* player, byte selectedID, PetControlDevice* controller) {

	ManagedReference<DroidObject*> droid = getDroidObject();
	if( droid == NULL ){
		info( "Droid is null");
		return 0;
	}

	Locker dlock( droid, player );

	// Perform Maintenance Run
	if( selectedID == MAINT_MODULE_PERFORM ){

		// Init Maintenance Run Session
		//droid->getCooldownTimerMap()->updateToCurrentAndAddMili("Droid_Cooldown",1800000);
		// Interplanetary Time + 1 hour 40 minutes, 33 seconds
		// Local time: distance < 432 == 15 mins + (1s per 3 meters to next structure for that planet @ < 432m from first structure)
		// Local time: distance > 432 == 48 minutes 20 seconds
		// Start a session for the player and this module!
		ManagedReference<DroidMaintenanceSession*> session = new DroidMaintenanceSession(this, player);
		session->initialize();
		player->addActiveSession(SessionFacadeType::DROIDMAINTENANCERUN, session);
		session->sendMaintanceRunBox();
		return 0;
	}
	// Edit Maintenance List
	else if( selectedID == MAINT_MODULE_EDIT ){

		if( assignedStructures.isEmpty() ){
			player->sendSystemMessage( "@pet/droid_modules:droid_maint_empty_list" ); // The maintenance list for this droid is empty. Go to one of your structures and assign this droid to maintain it.
			return 0;
		}

		// Build List of Assigned Structures
		ManagedReference<SuiListBox*> box = new SuiListBox(player, SuiWindowType::DROID_EDIT_ASSIGNED_STRUCTURES, SuiListBox::HANDLETWOBUTTON);
		box->setCallback(new RemoveDroidStructureSuiCallback(player->getZoneServer(), this));
		box->setPromptTitle("@pet/droid_modules:droid_maint_edit_title"); // Edit Maintenance List
		box->setPromptText("@pet/droid_modules:droid_maint_edit_prompt"); // Please select a structure to remove from this droid's maintenance list.

		box->setOkButton(true, "@delete");
		box->setCancelButton(true, "@cancel");

		for (int i = 0; i < assignedStructures.size(); i++) {

			uint64 objectId = assignedStructures.elementAt(i);
			ManagedReference<SceneObject*> obj = player->getZoneServer()->getObject(objectId);
			if( obj != NULL && obj->isStructureObject() ){
				StructureObject* structureObject = cast<StructureObject*>(obj.get());
				if( structureObject != NULL ){
					box->addMenuItem( structureObject->getDisplayedName(), objectId );
				}
			}
		}

		box->setUsingObject(droid);
		player->getPlayerObject()->addSuiBox(box);
		player->sendMessage(box->generateMessage());

	}

	return 0;
}

int DroidMaintenanceModuleDataComponent::getBatteryDrain() {
	return 0;  // No constant drain, but each maintenance run will use power
}
void DroidMaintenanceModuleDataComponent::getStructureList(Vector<ManagedReference<StructureObject* > >* list,ZoneServer* srv, uint32 planet) {
	for (int i = 0; i < assignedStructures.size(); i++) {
		uint64 objectId = assignedStructures.elementAt(i);
		ManagedReference<SceneObject*> obj = srv->getObject(objectId);
		if( obj != NULL && obj->isStructureObject() ){
			StructureObject* structureObject = cast<StructureObject*>(obj.get());
			if( structureObject != NULL ){
				// is it on this planet?
				if (structureObject->getZone()->getZoneCRC() == planet) {
					list->add(structureObject);
				} else {
					if (moduleRating >= 12) // ratign 12 min for interplanetary
						list->add(structureObject);
				}
			}
		}
	}
}

void DroidMaintenanceModuleDataComponent::copy(BaseDroidModuleComponent* other){

	DroidMaintenanceModuleDataComponent* otherModule = cast<DroidMaintenanceModuleDataComponent*>(other);
	if( otherModule == NULL )
		return;

	moduleRating = otherModule->moduleRating;

	// Save stat in parent sceno
	DroidComponent* droidComponent = cast<DroidComponent*>(getParent());
	if (droidComponent == NULL)
		return;

	droidComponent->addProperty("struct_module", moduleRating, 0, "exp_effectiveness");

	if (moduleRating < 6) {
		maxStructures = 3;
	} else if (moduleRating < 12) {
		maxStructures = 6;
	} else if (moduleRating < 15) {
		maxStructures = 9;
	} else {
		maxStructures = 10;
	}
}

void DroidMaintenanceModuleDataComponent::addToStack(BaseDroidModuleComponent* other){

	DroidMaintenanceModuleDataComponent* otherModule = cast<DroidMaintenanceModuleDataComponent*>(other);
	if( otherModule == NULL )
		return;

	// Maintenance modules do not stack.  Just keep the highest rated module
	if( otherModule->moduleRating > moduleRating )
		moduleRating = otherModule->moduleRating;

	// Save stat in parent sceno
	DroidComponent* droidComponent = cast<DroidComponent*>(getParent());
	if (droidComponent == NULL)
		return;

	// Attribute should have already been created in copy method
	if( !droidComponent->changeAttributeValue( "struct_module", moduleRating) ){
		info( "addToStack updateAttributeValue failed");
		return;
	}

	if (moduleRating < 6) {
		maxStructures = 3;
	} else if (moduleRating < 12) {
		maxStructures = 6;
	} else if (moduleRating < 15) {
		maxStructures = 9;
	} else {
		maxStructures = 10;
	}

}

String DroidMaintenanceModuleDataComponent::toString(){
	StringBuffer str;
	str << getModuleName() << "\n";
	str << "Number of Assigned Structures: " << assignedStructures.size() << "\n";
	for (int i = 0; i < assignedStructures.size(); i++) {
		uint64 objectID = assignedStructures.elementAt(i);
		str << "\tStructure: " << objectID << "\n";
	}
	return str.toString();
}
bool DroidMaintenanceModuleDataComponent::isAssignedTo(uint64 structure) {
	return assignedStructures.contains(structure);
}
bool DroidMaintenanceModuleDataComponent::assignStructure( uint64 objectID ){

	if( !assignedStructures.contains( objectID ) && assignedStructures.size() <= maxStructures) {
		assignedStructures.add( objectID );
		return true;
	} else {
		return false;
	}

}

void DroidMaintenanceModuleDataComponent::unassignStructure( uint64 objectID ){

	if( assignedStructures.contains( objectID ) )
		assignedStructures.removeElement( objectID );

}

void DroidMaintenanceModuleDataComponent::onCall(){
	validateStructures();
}

void DroidMaintenanceModuleDataComponent::validateStructures(){

	Vector<uint64> structures;
	assignedStructures.clone( structures );

	for (int i = 0; i < structures.size(); i++) {

		uint64 objectID = structures.elementAt(i);
		if( !isValidStructure( objectID ) ){
			assignedStructures.removeElement( objectID );
		}

	}

}
void DroidMaintenanceModuleDataComponent::payStructures(CreatureObject* player, VectorMap<unsigned long long, int> assignments) {
	// we know each struct to pay and any fees applied.
	ManagedReference<DroidObject*> droid = getDroidObject();

	for(int i=0;i< assignments.size();i++) {
		uint64 objectID = assignments.elementAt(i).getKey();
		int maintToPay = assignments.elementAt(i).getValue();
		ManagedReference<SceneObject*> obj = player->getZoneServer()->getObject(objectID);
		StructureObject* structureObject = cast<StructureObject*>(obj.get());
		if (structureObject != NULL) {
			Locker sLock(obj,player);
			structureObject->payMaintenance(maintToPay,player,true);
		}
	}
}
long DroidMaintenanceModuleDataComponent::calculateRunTime(VectorMap<unsigned long long, int> assignments) {
	long duration = 0;
	ManagedReference<DroidObject*> droid = getDroidObject();
	String localPlanet = droid->getZone()->getZoneName();
	ManagedReference<SceneObject*> primeStructure;
	for(int i=0;i< assignments.size();i++) {
		uint64 objectID = assignments.elementAt(i).getKey();
		if(isValidStructure(objectID)) {
			ManagedReference<SceneObject*> obj = droid->getZoneServer()->getObject(objectID);
			if (obj->getZone()->getZoneName() != localPlanet) {
				duration += 6033000; // 1 hr 40 minutes and 33 seconds
			}else {
				// same planet
				if (primeStructure == NULL) {
					primeStructure = obj;
				}
				if (obj == primeStructure) {
					if (droid->getDistanceTo(obj) > 432) {
						duration += 2900000; // 48 minutes and 20 seconds
					} else {
						duration += 900000; // 15 minutes
					}
				} else {
					float distance = primeStructure->getDistanceTo(obj);
					if (distance > 432) {
						duration += 2900000; // 48 minutes and 20 seconds
						primeStructure = obj; // new prime structure
					} else {
						// this struct is within 432 of prime
						float timeAdded = (distance/3) * 1000; // add 1 sec for every 3 meters apart
						duration += (timeAdded + 900000);
						primeStructure = obj; // we now grab this structure as our prime going down the list.
					}
				}
			}
		}
	}
	return duration;
}
bool DroidMaintenanceModuleDataComponent::isValidStructure(uint64 objectID){

	ManagedReference<DroidObject*> droid = getDroidObject();
	if( droid == NULL ) {
		return false;
	}

	ManagedReference<SceneObject*> obj = droid->getZoneServer()->getObject(objectID);
	if( obj == NULL ) {
		return false;
	}

	if( !obj->isStructureObject() ) {
		return false;
	}
	// we need to check for more than structure we need to check for factories and harvestors as well.
	StructureObject* structureObject = cast<StructureObject*>(obj.get());
	if( structureObject == NULL ) {
		return false;
	}

	ManagedWeakReference< CreatureObject*> linkedCreature = droid->getLinkedCreature();
	if( linkedCreature == NULL ) {
		return false;
	}

	if( !structureObject->isOnAdminList( linkedCreature.get() ) ) {
		return false;
	}

	if( structureObject->getZone() == NULL ) {
		return false;
	}

	// All checks passed
	return true;

}

bool DroidMaintenanceModuleDataComponent::toBinaryStream(ObjectOutputStream* stream) {

	int _currentOffset = stream->getOffset();
	stream->writeShort(0);
	int _varCount = writeObjectMembers(stream);
	stream->writeShort(_currentOffset, _varCount);
	return true;
}

int DroidMaintenanceModuleDataComponent::writeObjectMembers(ObjectOutputStream* stream) {

	String _name;
	int _offset;
	uint32 _totalSize;

	_name = "assignedStructures";
	_name.toBinaryStream(stream);
	_offset = stream->getOffset();
	stream->writeInt(0);
	TypeInfo< Vector<unsigned long long> >::toBinaryStream(&assignedStructures, stream);
	_totalSize = (uint32) (stream->getOffset() - (_offset + 4));
	stream->writeInt(_offset, _totalSize);

	return 1;
}

bool DroidMaintenanceModuleDataComponent::readObjectMember(ObjectInputStream* stream, const String& name) {

	if (name == "assignedStructures") {
		TypeInfo< Vector<unsigned long long> >::parseFromBinaryStream(&assignedStructures, stream);
		return true;
	}

	return false;
}

bool DroidMaintenanceModuleDataComponent::parseFromBinaryStream(ObjectInputStream* stream) {

	uint16 _varCount = stream->readShort();

	for (int i = 0; i < _varCount; ++i) {
		String _name;
		_name.parseFromBinaryStream(stream);
		uint32 _varSize = stream->readInt();
		int _currentOffset = stream->getOffset();
		if(readObjectMember(stream, _name)) {
		}
		stream->setOffset(_currentOffset + _varSize);
	}
	return true;
}
<|endoftext|>


<commit_before>#include "CamMatErrorFunctionV3.h"
#include <Messages/Representations.pb.h>
#include <google/protobuf/io/zero_copy_stream_impl.h>
#include <fstream>
#include "Tools/DataConversion.h"

void CamMatErrorFunctionV3::actual_plotting(const Parameter &p, naoth::CameraInfo::CameraID cameraID)
{
    Eigen::Matrix<double, 11, 1> parameter;
    if(bounds != nullptr){
        parameter = bounds->unbound(p);
    } else {
        parameter = p;
    }

    Vector2d offsetBody(parameter(0),parameter(1));
    Vector3d offsetHead(parameter(2), parameter(3),parameter(4));
    Vector3d offsetCam[naoth::CameraInfo::numOfCamera];
    offsetCam[naoth::CameraInfo::Top]    = Vector3d(parameter(5), parameter(6),parameter(7));
    offsetCam[naoth::CameraInfo::Bottom] = Vector3d(parameter(8), parameter(9),parameter(10));

    for(CalibrationData::const_iterator sample = calibrationData.begin(); sample != calibrationData.end(); ++sample)
    {
            CameraMatrix tmpCM = CameraGeometry::calculateCameraMatrixFromChestPose(
                        sample->chestPose,
                        NaoInfo::robotDimensions.cameraTransform[cameraID].offset,
                        NaoInfo::robotDimensions.cameraTransform[cameraID].rotationY,
                        offsetBody,
                        offsetHead,
                        offsetCam[cameraID],
                        sample->headYaw,
                        sample->headPitch,
                        sample->orientation
                        );

            tmpCM.translation += Vector3d(global_position->x, global_position->y, 0); // move around on field
            tmpCM.rotation = RotationMatrix::getRotationZ(*global_orientation) * tmpCM.rotation;

            std::vector<Vector2d> edgelProjections;
            edgelProjections.resize(getEdgelsInImage(sample,cameraID).size());

            // draw projected edgels to field
            for(size_t i = 0; i < getEdgelsInImage(sample,cameraID).size(); i++)
            {
                const Vector2d& edgelOne = getEdgelsInImage(sample,cameraID)[i];

                CameraGeometry::imagePixelToFieldCoord(
                            tmpCM, getCameraInfo(cameraID),
                            edgelOne.x,
                            edgelOne.y,
                            0.0,
                            edgelProjections[i]);

                DEBUG_REQUEST("CamMatErrorFunctionV3:debug_drawings:draw_projected_edgels",
                        FIELD_DRAWING_CONTEXT;
                        PEN("000000", 10);
                        CIRCLE(edgelProjections[i].x, edgelProjections[i].y, 20);
                );
            }

            DEBUG_REQUEST("CamMatErrorFunctionV3:debug_drawings:draw_projected_edgels",
                    FIELD_DRAWING_CONTEXT;
                    PEN("FF0000", 10);
                    ROBOT(0,0,0);
            );

            DEBUG_REQUEST("CamMatErrorFunctionV3:debug_drawings:draw_matching_global",
            // determine distance to nearst field line and the total aberration
                for(std::vector<Vector2d>::const_iterator iter = edgelProjections.begin(); iter != edgelProjections.end(); ++iter)
                {

                    const Vector2d& seen_point_relative = *iter;

                    Pose2D   robotPose;
                    Vector2d seen_point_global = robotPose * seen_point_relative;

                    LinesTable::NamedPoint line_point_global = theFieldInfo.fieldLinesTable.get_closest_point(seen_point_global, LinesTable::all_lines);

                    // there is no such line
                    if(line_point_global.id == -1) {
                        continue;
                    }

                    FIELD_DRAWING_CONTEXT;
                    PEN("000000", 10);

                    CIRCLE(seen_point_global.x, seen_point_global.y, 20);
                    LINE(line_point_global.position.x,line_point_global.position.y,seen_point_global.x, seen_point_global.y);
                }

                PEN("FF0000", 10);
                ROBOT(0,0,0);
            );
    }
}

Eigen::VectorXd CamMatErrorFunctionV3::operator()(const Parameter& p) const
{
    Eigen::VectorXd r(numberOfResudials);

    Eigen::Matrix<double, 11, 1> parameter;
    if(bounds != nullptr){
        parameter = bounds->unbound(p);
    } else {
        parameter = p;
    }

    Vector2d offsetBody(parameter(0),parameter(1));
    Vector3d offsetHead(parameter(2), parameter(3),parameter(4));
    Vector3d offsetCam[naoth::CameraInfo::numOfCamera];
    offsetCam[naoth::CameraInfo::Top]    = Vector3d(parameter(5), parameter(6),parameter(7));
    offsetCam[naoth::CameraInfo::Bottom] = Vector3d(parameter(8), parameter(9),parameter(10));

    size_t idx = 0;
    size_t empty = 0;
    for(int cameraID = 0; cameraID < naoth::CameraInfo::numOfCamera; cameraID++)
    {
        for(CalibrationData::const_iterator sample = calibrationData.begin(); sample != calibrationData.end(); ++sample)
        {
            if (getEdgelsInImage(sample,cameraID).empty()) {
                ++empty;
                continue;
            }

            CameraMatrix tmpCM = CameraGeometry::calculateCameraMatrixFromChestPose(
                        //sample->kinematicChain,
                        sample->chestPose,
                        NaoInfo::robotDimensions.cameraTransform[cameraID].offset,
                        NaoInfo::robotDimensions.cameraTransform[cameraID].rotationY,
                        offsetBody,
                        offsetHead,
                        offsetCam[cameraID],
                        sample->headYaw,
                        sample->headPitch,
                        sample->orientation
                        );

            tmpCM.translation += Vector3d(global_position->x, global_position->y, 0); // move around on field
            tmpCM.rotation = RotationMatrix::getRotationZ(*global_orientation) * tmpCM.rotation;

            std::vector<Vector2d> edgelProjections;
            edgelProjections.resize(getEdgelsInImage(sample,cameraID).size());

            // project edgels pairs to field
            for(size_t i = 0; i < getEdgelsInImage(sample,cameraID).size(); i++)
            {
                const Vector2d& edgelOne = getEdgelsInImage(sample,cameraID)[i];

                CameraGeometry::imagePixelToFieldCoord(
                            tmpCM, getCameraInfo(cameraID),
                            edgelOne.x,
                            edgelOne.y,
                            0.0,
                            edgelProjections[i]);
            }

            double total_sum = 0;
            // determine distance to nearst field line and the total aberration
            for(std::vector<Vector2d>::const_iterator seen_point_relative = edgelProjections.begin(); seen_point_relative != edgelProjections.end(); ++seen_point_relative){
                Pose2D   robotPose;
                Vector2d seen_point_global = robotPose * (*seen_point_relative);

                LinesTable::NamedPoint line_point_global = theFieldInfo.fieldLinesTable.get_closest_point(seen_point_global, LinesTable::all_lines);

                // there is no such line or point
                if(line_point_global.id == -1 /*&& corner.id == -1*/) {
                    continue;
                }

                double dist = (seen_point_global - line_point_global.position).abs();

                total_sum += dist;
            }

            r(idx) = -total_sum; // the resudial is target - actual, i.e. 0 - total_sum
            ++idx;
        }
    }

    ASSERT(empty+idx == calibrationData.size()*2);
    ASSERT(idx == numberOfResudials);
    return r;
}

void CamMatErrorFunctionV3::write_calibration_data_to_file(){
    std::ofstream out;
    out.open("/tmp/calibration_data", std::ofstream::binary);

    naothmessages::CalibrationDataCMC message;

    message.set_numberofresudials(numberOfResudials);
    for(CalibrationData::const_iterator iter = calibrationData.begin(); iter != calibrationData.end(); ++iter){
        naothmessages::CalibrationDataCMC::CalibrationDataSampleV3 *sample = message.add_calibrationdata();

        naoth::DataConversion::toMessage(iter->chestPose, *sample->mutable_chestpose());

        for(std::vector<Vector2d>::const_iterator iter2 = iter->edgelsInImage.begin(); iter2 != iter->edgelsInImage.end(); ++iter2){
            naothmessages::DoubleVector2 *edgel = sample->add_edgelsinimage();
            naoth::DataConversion::toMessage(*iter2,*edgel);
        }

        for(std::vector<Vector2d>::const_iterator iter2 = iter->edgelsInImageTop.begin(); iter2 != iter->edgelsInImageTop.end(); ++iter2){
            naothmessages::DoubleVector2 *edgel = sample->add_edgelsinimagetop();
            naoth::DataConversion::toMessage(*iter2,*edgel);
        }

        naoth::DataConversion::toMessage(iter->orientation, *sample->mutable_orientation());
        sample->set_headpitch(iter->headPitch);
        sample->set_headyaw(iter->headYaw);
    }

    message.SerializeToOstream(&out);

    out.close();
}

void CamMatErrorFunctionV3::read_calibration_data_from_file(){
    std::ifstream in;
    in.open("/tmp/calibration_data", std::ifstream::binary);

    naothmessages::CalibrationDataCMC message;
    message.ParseFromIstream(&in);

    size_t size = static_cast<size_t>(message.calibrationdata_size());
    calibrationData.resize(size);
    for(size_t i = 0; i < size; ++i){
       naothmessages::CalibrationDataCMC::CalibrationDataSampleV3 *msg_sample = message.mutable_calibrationdata(static_cast<int>(i));
       CalibrationDataSample& sample = calibrationData[i];

       naoth::DataConversion::fromMessage(*(msg_sample->mutable_chestpose()), sample.chestPose);

       sample.edgelsInImage.resize(static_cast<size_t>(msg_sample->edgelsinimage_size()));
       for(int i = 0; i < msg_sample->edgelsinimage_size(); ++i){
           naoth::DataConversion::fromMessage(msg_sample->edgelsinimage(i), sample.edgelsInImage[static_cast<size_t>(i)]);
       }

       sample.edgelsInImageTop.resize(static_cast<size_t>(msg_sample->edgelsinimagetop_size()));
       for(int i = 0; i < msg_sample->edgelsinimagetop_size(); ++i){
           naoth::DataConversion::fromMessage(msg_sample->edgelsinimagetop(i), sample.edgelsInImageTop[static_cast<size_t>(i)]);
       }

       naoth::DataConversion::fromMessage(*(msg_sample->mutable_orientation()), sample.orientation);
       sample.headPitch = msg_sample->headpitch();
       sample.headYaw   = msg_sample->headyaw();
    }

    in.close();
}
<commit_msg>fix: determine numberOfResudials during reading calibration data<commit_after>#include "CamMatErrorFunctionV3.h"
#include <Messages/Representations.pb.h>
#include <google/protobuf/io/zero_copy_stream_impl.h>
#include <fstream>
#include "Tools/DataConversion.h"

void CamMatErrorFunctionV3::actual_plotting(const Parameter &p, naoth::CameraInfo::CameraID cameraID)
{
    Eigen::Matrix<double, 11, 1> parameter;
    if(bounds != nullptr){
        parameter = bounds->unbound(p);
    } else {
        parameter = p;
    }

    Vector2d offsetBody(parameter(0),parameter(1));
    Vector3d offsetHead(parameter(2), parameter(3),parameter(4));
    Vector3d offsetCam[naoth::CameraInfo::numOfCamera];
    offsetCam[naoth::CameraInfo::Top]    = Vector3d(parameter(5), parameter(6),parameter(7));
    offsetCam[naoth::CameraInfo::Bottom] = Vector3d(parameter(8), parameter(9),parameter(10));

    for(CalibrationData::const_iterator sample = calibrationData.begin(); sample != calibrationData.end(); ++sample)
    {
            CameraMatrix tmpCM = CameraGeometry::calculateCameraMatrixFromChestPose(
                        sample->chestPose,
                        NaoInfo::robotDimensions.cameraTransform[cameraID].offset,
                        NaoInfo::robotDimensions.cameraTransform[cameraID].rotationY,
                        offsetBody,
                        offsetHead,
                        offsetCam[cameraID],
                        sample->headYaw,
                        sample->headPitch,
                        sample->orientation
                        );

            tmpCM.translation += Vector3d(global_position->x, global_position->y, 0); // move around on field
            tmpCM.rotation = RotationMatrix::getRotationZ(*global_orientation) * tmpCM.rotation;

            std::vector<Vector2d> edgelProjections;
            edgelProjections.resize(getEdgelsInImage(sample,cameraID).size());

            // draw projected edgels to field
            for(size_t i = 0; i < getEdgelsInImage(sample,cameraID).size(); i++)
            {
                const Vector2d& edgelOne = getEdgelsInImage(sample,cameraID)[i];

                CameraGeometry::imagePixelToFieldCoord(
                            tmpCM, getCameraInfo(cameraID),
                            edgelOne.x,
                            edgelOne.y,
                            0.0,
                            edgelProjections[i]);

                DEBUG_REQUEST("CamMatErrorFunctionV3:debug_drawings:draw_projected_edgels",
                        FIELD_DRAWING_CONTEXT;
                        PEN("000000", 10);
                        CIRCLE(edgelProjections[i].x, edgelProjections[i].y, 20);
                );
            }

            DEBUG_REQUEST("CamMatErrorFunctionV3:debug_drawings:draw_projected_edgels",
                    FIELD_DRAWING_CONTEXT;
                    PEN("FF0000", 10);
                    ROBOT(0,0,0);
            );

            DEBUG_REQUEST("CamMatErrorFunctionV3:debug_drawings:draw_matching_global",
            // determine distance to nearst field line and the total aberration
                for(std::vector<Vector2d>::const_iterator iter = edgelProjections.begin(); iter != edgelProjections.end(); ++iter)
                {

                    const Vector2d& seen_point_relative = *iter;

                    Pose2D   robotPose;
                    Vector2d seen_point_global = robotPose * seen_point_relative;

                    LinesTable::NamedPoint line_point_global = theFieldInfo.fieldLinesTable.get_closest_point(seen_point_global, LinesTable::all_lines);

                    // there is no such line
                    if(line_point_global.id == -1) {
                        continue;
                    }

                    FIELD_DRAWING_CONTEXT;
                    PEN("000000", 10);

                    CIRCLE(seen_point_global.x, seen_point_global.y, 20);
                    LINE(line_point_global.position.x,line_point_global.position.y,seen_point_global.x, seen_point_global.y);
                }

                PEN("FF0000", 10);
                ROBOT(0,0,0);
            );
    }
}

Eigen::VectorXd CamMatErrorFunctionV3::operator()(const Parameter& p) const
{
    Eigen::VectorXd r(numberOfResudials);

    Eigen::Matrix<double, 11, 1> parameter;
    if(bounds != nullptr){
        parameter = bounds->unbound(p);
    } else {
        parameter = p;
    }

    Vector2d offsetBody(parameter(0),parameter(1));
    Vector3d offsetHead(parameter(2), parameter(3),parameter(4));
    Vector3d offsetCam[naoth::CameraInfo::numOfCamera];
    offsetCam[naoth::CameraInfo::Top]    = Vector3d(parameter(5), parameter(6),parameter(7));
    offsetCam[naoth::CameraInfo::Bottom] = Vector3d(parameter(8), parameter(9),parameter(10));

    size_t idx = 0;
    size_t empty = 0;
    for(int cameraID = 0; cameraID < naoth::CameraInfo::numOfCamera; cameraID++)
    {
        for(CalibrationData::const_iterator sample = calibrationData.begin(); sample != calibrationData.end(); ++sample)
        {
            if (getEdgelsInImage(sample,cameraID).empty()) {
                ++empty;
                continue;
            }

            CameraMatrix tmpCM = CameraGeometry::calculateCameraMatrixFromChestPose(
                        //sample->kinematicChain,
                        sample->chestPose,
                        NaoInfo::robotDimensions.cameraTransform[cameraID].offset,
                        NaoInfo::robotDimensions.cameraTransform[cameraID].rotationY,
                        offsetBody,
                        offsetHead,
                        offsetCam[cameraID],
                        sample->headYaw,
                        sample->headPitch,
                        sample->orientation
                        );

            tmpCM.translation += Vector3d(global_position->x, global_position->y, 0); // move around on field
            tmpCM.rotation = RotationMatrix::getRotationZ(*global_orientation) * tmpCM.rotation;

            std::vector<Vector2d> edgelProjections;
            edgelProjections.resize(getEdgelsInImage(sample,cameraID).size());

            // project edgels pairs to field
            for(size_t i = 0; i < getEdgelsInImage(sample,cameraID).size(); i++)
            {
                const Vector2d& edgelOne = getEdgelsInImage(sample,cameraID)[i];

                CameraGeometry::imagePixelToFieldCoord(
                            tmpCM, getCameraInfo(cameraID),
                            edgelOne.x,
                            edgelOne.y,
                            0.0,
                            edgelProjections[i]);
            }

            double total_sum = 0;
            // determine distance to nearst field line and the total aberration
            for(std::vector<Vector2d>::const_iterator seen_point_relative = edgelProjections.begin(); seen_point_relative != edgelProjections.end(); ++seen_point_relative){
                Pose2D   robotPose;
                Vector2d seen_point_global = robotPose * (*seen_point_relative);

                LinesTable::NamedPoint line_point_global = theFieldInfo.fieldLinesTable.get_closest_point(seen_point_global, LinesTable::all_lines);

                // there is no such line or point
                if(line_point_global.id == -1 /*&& corner.id == -1*/) {
                    continue;
                }

                double dist = (seen_point_global - line_point_global.position).abs();

                total_sum += dist;
            }

            r(idx) = -total_sum; // the resudial is target - actual, i.e. 0 - total_sum
            ++idx;
        }
    }

    ASSERT(empty+idx == calibrationData.size()*2);
    ASSERT(idx == numberOfResudials);
    return r;
}

void CamMatErrorFunctionV3::write_calibration_data_to_file(){
    std::ofstream out;
    out.open("/tmp/calibration_data", std::ofstream::binary);

    naothmessages::CalibrationDataCMC message;

    message.set_numberofresudials(numberOfResudials);
    for(CalibrationData::const_iterator iter = calibrationData.begin(); iter != calibrationData.end(); ++iter){
        naothmessages::CalibrationDataCMC::CalibrationDataSampleV3 *sample = message.add_calibrationdata();

        naoth::DataConversion::toMessage(iter->chestPose, *sample->mutable_chestpose());

        for(std::vector<Vector2d>::const_iterator iter2 = iter->edgelsInImage.begin(); iter2 != iter->edgelsInImage.end(); ++iter2){
            naothmessages::DoubleVector2 *edgel = sample->add_edgelsinimage();
            naoth::DataConversion::toMessage(*iter2,*edgel);
        }

        for(std::vector<Vector2d>::const_iterator iter2 = iter->edgelsInImageTop.begin(); iter2 != iter->edgelsInImageTop.end(); ++iter2){
            naothmessages::DoubleVector2 *edgel = sample->add_edgelsinimagetop();
            naoth::DataConversion::toMessage(*iter2,*edgel);
        }

        naoth::DataConversion::toMessage(iter->orientation, *sample->mutable_orientation());
        sample->set_headpitch(iter->headPitch);
        sample->set_headyaw(iter->headYaw);
    }

    message.SerializeToOstream(&out);

    out.close();
}

void CamMatErrorFunctionV3::read_calibration_data_from_file(){
    std::ifstream in;
    in.open("/tmp/calibration_data", std::ifstream::binary);

    naothmessages::CalibrationDataCMC message;
    message.ParseFromIstream(&in);

    numberOfResudials = 0;
    size_t size = static_cast<size_t>(message.calibrationdata_size());
    calibrationData.resize(size);
    for(size_t i = 0; i < size; ++i){
       naothmessages::CalibrationDataCMC::CalibrationDataSampleV3 *msg_sample = message.mutable_calibrationdata(static_cast<int>(i));
       CalibrationDataSample& sample = calibrationData[i];

       naoth::DataConversion::fromMessage(*(msg_sample->mutable_chestpose()), sample.chestPose);

       sample.edgelsInImage.resize(static_cast<size_t>(msg_sample->edgelsinimage_size()));
       for(int i = 0; i < msg_sample->edgelsinimage_size(); ++i){
           naoth::DataConversion::fromMessage(msg_sample->edgelsinimage(i), sample.edgelsInImage[static_cast<size_t>(i)]);
       }
       if(sample.edgelsInImage.size() > 0){
           ++numberOfResudials;
       }

       sample.edgelsInImageTop.resize(static_cast<size_t>(msg_sample->edgelsinimagetop_size()));
       for(int i = 0; i < msg_sample->edgelsinimagetop_size(); ++i){
           naoth::DataConversion::fromMessage(msg_sample->edgelsinimagetop(i), sample.edgelsInImageTop[static_cast<size_t>(i)]);
       }
       if(sample.edgelsInImageTop.size() > 0){
           ++numberOfResudials;
       }

       naoth::DataConversion::fromMessage(*(msg_sample->mutable_orientation()), sample.orientation);
       sample.headPitch = msg_sample->headpitch();
       sample.headYaw   = msg_sample->headyaw();
    }

    in.close();
}
<|endoftext|>


<commit_before>/**
* @file CameraMatrixProvider.cpp
*
* @author Heinrich Mellmann
* Implementation of class CameraMatrixProvider
*/

#include "CameraMatrixCorrector.h"

// debug
#include "Tools/Debug/DebugModify.h"
#include "Tools/Debug/DebugRequest.h"
#include "Tools/Debug/DebugDrawings3D.h"
#include "Tools/CameraGeometry.h"
#include "Tools/Math/Line.h"

#include "Motion/MorphologyProcessor/ForwardKinematics.h"
#include "Motion/CameraMatrixCalculator/CameraMatrixCalculator.h"

#include "PlatformInterface/Platform.h"

CameraMatrixCorrector::CameraMatrixCorrector()
{
  udpateTime = getFrameInfo().getTime();

  // this is a HACK!!!!!!!!!!
  if(!getKinematicChain().is_initialized())
  {
    getKinematicChain().init(getSensorJointData());
  }

  DEBUG_REQUEST_REGISTER("CameraMatrix:calibrate_camera_matrix",
    "calculates the roll and tilt offset of the camera using the goal (it. shoult be exactely 3000mm in front of the robot)",
    false);

  DEBUG_REQUEST_REGISTER("CameraMatrix:reset_calibration", "set the calibration offsets of the CM to 0", false);

  DEBUG_REQUEST_REGISTER("3DViewer:Robot:Camera", "Show the robot body in the 3D viewer.", false);
}

CameraMatrixCorrector::~CameraMatrixCorrector()
{
}

void CameraMatrixCorrector::execute()
{
  double deltaTime = ( getFrameInfo().getTime() - udpateTime ) * 0.001;
  udpateTime = getFrameInfo().getTime();

  DEBUG_REQUEST("CameraMatrix:calibrate_camera_matrix", calibrate(); );
  DEBUG_REQUEST_ON_DEACTIVE("CameraMatrix:calibrate_camera_matrix", getCameraInfoParameter().saveToConfig(); );

  DEBUG_REQUEST("CameraMatrix:reset_calibration", reset_calibration(); );
  DEBUG_REQUEST_ON_DEACTIVE("CameraMatrix:reset_calibration", getCameraInfoParameter().saveToConfig(); );

  // calculate the kinematic chain
  Kinematics::ForwardKinematics::calculateKinematicChainAll(
    getAccelerometerData(),
    getInertialModel().orientation,
    getKinematicChain(),
    theFSRPos,
    deltaTime);

  getKinematicChain().updateCoM();

  /* TODO: this doesn't work properly
  getCameraMatrix()
       .rotateY(getCameraMatrixOffset().offset.y)
       .rotateX(getCameraMatrixOffset().offset.x);
  */

  DEBUG_REQUEST("3DViewer:Robot:Camera",
    const CameraInfo& ci = getCameraInfoParameter();
      DebugDrawings3D::getInstance().addCamera(getCameraMatrix(),ci.getFocalLength(), ci.resolutionWidth, ci.resolutionHeight););

}//end execute

void CameraMatrixCorrector::reset_calibration()
{
  CameraInfoParameter& cameraInfo = getCameraInfoParameter();
  cameraInfo.cameraRollOffset = 0.0;
  cameraInfo.cameraTiltOffset = 0.0;
}

void CameraMatrixCorrector::calibrate()
{
  // calibrate the camera matrix
  // currently it is in test-mode, the correction parameter
  // are stored as static members of CameraMatrixCalculator

  if (getGoalPercept().getNumberOfSeenPosts() < 2
      || !getGoalPercept().getPost(0).positionReliable
      || !getGoalPercept().getPost(1).positionReliable)
    return;

  Vector2<double> offset;
  double epsylon = 1e-8;

  // make only one step
  for (int i = 0; i < 1; i++)
  {
    // approximate the derivative Dg(x)=(dg1, dg2) of g at point x=(y,p)
    double dg11 = projectionError(offset.x + epsylon, offset.y);
    double dg12 = projectionError(offset.x - epsylon, offset.y);
    double dg1 = (dg11 - dg12) / (2 * epsylon);

    double dg21 = projectionError(offset.x, offset.y + epsylon);
    double dg22 = projectionError(offset.x, offset.y - epsylon);
    double dg2 = (dg21 - dg22) / (2 * epsylon);

    // Dg(x)^T*Dg(x)
    Vector2<double> dg(dg1, dg2);

    if (dg.abs() < epsylon) break;

    // g(x) - target
    double w = projectionError(offset.x, offset.y);

    //Vector2<double> z_GN = (-((Dg.transpose()*Dg).invert()*Dg.transpose()*w));
    Vector2<double> z_GN = dg * (-w / (dg * dg));
    offset += z_GN;
  }//end for

  // apply changes
  CameraInfoParameter& cameraInfo = getCameraInfoParameter();

  double lambda = 0.01;
  if (offset.abs() > lambda)
    offset.normalize(lambda);

  cameraInfo.cameraRollOffset += offset.x;
  cameraInfo.cameraTiltOffset += offset.y;

  double maxValue = Math::fromDegrees(10.0); // maximal correction
  cameraInfo.cameraRollOffset = Math::clamp(cameraInfo.cameraRollOffset, -maxValue, maxValue);
  cameraInfo.cameraTiltOffset = Math::clamp(cameraInfo.cameraTiltOffset, -maxValue, maxValue);

  // until now we only changed the parameters, change the pure CameraInfo as well
  getCameraInfo() = cameraInfo;
}//end calibrate

double CameraMatrixCorrector::projectionError(double offsetX, double offsetY)
{
  CameraMatrix tmpCM(getCameraMatrix());

  tmpCM.rotateY(offsetY)
       .rotateX(offsetX);

  // project the goal posts
  const CameraInfoParameter& cameraInfo = getCameraInfoParameter();

  Vector2<double> leftPosition;
  CameraGeometry::imagePixelToFieldCoord(
      tmpCM,
      cameraInfo,
      getGoalPercept().getPost(0).basePoint.x,
      getGoalPercept().getPost(0).basePoint.y,
      0.0,
      leftPosition);

  Vector2<double> rightPosition;
  CameraGeometry::imagePixelToFieldCoord(
      tmpCM,
      cameraInfo,
      getGoalPercept().getPost(1).basePoint.x,
      getGoalPercept().getPost(1).basePoint.y,
      0.0,
      rightPosition);

  double error =
    (getFieldInfo().opponentGoalPostLeft  - leftPosition).abs() +
    (getFieldInfo().opponentGoalPostRight - rightPosition).abs();

  /*
  double goal_width = (getFieldInfo().opponentGoalPostLeft-getFieldInfo().opponentGoalPostRight).abs2();
  double seen_width = (leftPosition-rightPosition).abs2();
  double error = Math::sqr(goal_width - seen_width);
  */

  return error;
}//end projectionError
<commit_msg>don't limit the absolute offset, only the offset of the single iteration<commit_after>/**
* @file CameraMatrixProvider.cpp
*
* @author Heinrich Mellmann
* Implementation of class CameraMatrixProvider
*/

#include "CameraMatrixCorrector.h"

// debug
#include "Tools/Debug/DebugModify.h"
#include "Tools/Debug/DebugRequest.h"
#include "Tools/Debug/DebugDrawings3D.h"
#include "Tools/CameraGeometry.h"
#include "Tools/Math/Line.h"

#include "Motion/MorphologyProcessor/ForwardKinematics.h"
#include "Motion/CameraMatrixCalculator/CameraMatrixCalculator.h"

#include "PlatformInterface/Platform.h"

CameraMatrixCorrector::CameraMatrixCorrector()
{
  udpateTime = getFrameInfo().getTime();

  // this is a HACK!!!!!!!!!!
  if(!getKinematicChain().is_initialized())
  {
    getKinematicChain().init(getSensorJointData());
  }

  DEBUG_REQUEST_REGISTER("CameraMatrix:calibrate_camera_matrix",
    "calculates the roll and tilt offset of the camera using the goal (it. shoult be exactely 3000mm in front of the robot)",
    false);

  DEBUG_REQUEST_REGISTER("CameraMatrix:reset_calibration", "set the calibration offsets of the CM to 0", false);

  DEBUG_REQUEST_REGISTER("3DViewer:Robot:Camera", "Show the robot body in the 3D viewer.", false);
}

CameraMatrixCorrector::~CameraMatrixCorrector()
{
}

void CameraMatrixCorrector::execute()
{
  double deltaTime = ( getFrameInfo().getTime() - udpateTime ) * 0.001;
  udpateTime = getFrameInfo().getTime();

  DEBUG_REQUEST("CameraMatrix:calibrate_camera_matrix", calibrate(); );
  DEBUG_REQUEST_ON_DEACTIVE("CameraMatrix:calibrate_camera_matrix", getCameraInfoParameter().saveToConfig(); );

  DEBUG_REQUEST("CameraMatrix:reset_calibration", reset_calibration(); );
  DEBUG_REQUEST_ON_DEACTIVE("CameraMatrix:reset_calibration", getCameraInfoParameter().saveToConfig(); );

  // calculate the kinematic chain
  Kinematics::ForwardKinematics::calculateKinematicChainAll(
    getAccelerometerData(),
    getInertialModel().orientation,
    getKinematicChain(),
    theFSRPos,
    deltaTime);

  getKinematicChain().updateCoM();

  /* TODO: this doesn't work properly
  getCameraMatrix()
       .rotateY(getCameraMatrixOffset().offset.y)
       .rotateX(getCameraMatrixOffset().offset.x);
  */

  DEBUG_REQUEST("3DViewer:Robot:Camera",
    const CameraInfo& ci = getCameraInfoParameter();
      DebugDrawings3D::getInstance().addCamera(getCameraMatrix(),ci.getFocalLength(), ci.resolutionWidth, ci.resolutionHeight););

}//end execute

void CameraMatrixCorrector::reset_calibration()
{
  CameraInfoParameter& cameraInfo = getCameraInfoParameter();
  cameraInfo.cameraRollOffset = 0.0;
  cameraInfo.cameraTiltOffset = 0.0;
}

void CameraMatrixCorrector::calibrate()
{
  // calibrate the camera matrix
  // currently it is in test-mode, the correction parameter
  // are stored as static members of CameraMatrixCalculator

  if (getGoalPercept().getNumberOfSeenPosts() < 2
      || !getGoalPercept().getPost(0).positionReliable
      || !getGoalPercept().getPost(1).positionReliable)
    return;

  Vector2<double> offset;
  double epsylon = 1e-8;

  // make only one step
  for (int i = 0; i < 1; i++)
  {
    // approximate the derivative Dg(x)=(dg1, dg2) of g at point x=(y,p)
    double dg11 = projectionError(offset.x + epsylon, offset.y);
    double dg12 = projectionError(offset.x - epsylon, offset.y);
    double dg1 = (dg11 - dg12) / (2 * epsylon);

    double dg21 = projectionError(offset.x, offset.y + epsylon);
    double dg22 = projectionError(offset.x, offset.y - epsylon);
    double dg2 = (dg21 - dg22) / (2 * epsylon);

    // Dg(x)^T*Dg(x)
    Vector2<double> dg(dg1, dg2);

    if (dg.abs() < epsylon) break;

    // g(x) - target
    double w = projectionError(offset.x, offset.y);

    //Vector2<double> z_GN = (-((Dg.transpose()*Dg).invert()*Dg.transpose()*w));
    Vector2<double> z_GN = dg * (-w / (dg * dg));
    offset += z_GN;
  }//end for

  // apply changes
  CameraInfoParameter& cameraInfo = getCameraInfoParameter();

  double lambda = 0.01;
  if (offset.abs() > lambda)
    offset.normalize(lambda);

  double maxValue = Math::fromDegrees(10.0); // maximal correction offset
  offset.x = Math::clamp(offset.x, -maxValue, maxValue);
  offset.y = Math::clamp(offset.y, -maxValue, maxValue);

  cameraInfo.cameraRollOffset += offset.x;
  cameraInfo.cameraTiltOffset += offset.y;

  // until now we only changed the parameters, change the pure CameraInfo as well
  getCameraInfo() = cameraInfo;
}//end calibrate

double CameraMatrixCorrector::projectionError(double offsetX, double offsetY)
{
  CameraMatrix tmpCM(getCameraMatrix());

  tmpCM.rotateY(offsetY)
       .rotateX(offsetX);

  // project the goal posts
  const CameraInfoParameter& cameraInfo = getCameraInfoParameter();

  Vector2<double> leftPosition;
  CameraGeometry::imagePixelToFieldCoord(
      tmpCM,
      cameraInfo,
      getGoalPercept().getPost(0).basePoint.x,
      getGoalPercept().getPost(0).basePoint.y,
      0.0,
      leftPosition);

  Vector2<double> rightPosition;
  CameraGeometry::imagePixelToFieldCoord(
      tmpCM,
      cameraInfo,
      getGoalPercept().getPost(1).basePoint.x,
      getGoalPercept().getPost(1).basePoint.y,
      0.0,
      rightPosition);

  double error =
    (getFieldInfo().opponentGoalPostLeft  - leftPosition).abs() +
    (getFieldInfo().opponentGoalPostRight - rightPosition).abs();

  /*
  double goal_width = (getFieldInfo().opponentGoalPostLeft-getFieldInfo().opponentGoalPostRight).abs2();
  double seen_width = (leftPosition-rightPosition).abs2();
  double error = Math::sqr(goal_width - seen_width);
  */

  return error;
}//end projectionError
<|endoftext|>