<commit_before>/*=========================================================================
 *
 *  Copyright Insight Software Consortium
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *         http://www.apache.org/licenses/LICENSE-2.0.txt
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 *=========================================================================*/

#include "itkHDF5TransformIOFactory.h"
#include "itkHDF5TransformIO.h"
#include "itkTransformFileWriter.h"
#include "itkTransformFileReader.h"
#include "itkAffineTransform.h"
#include "itkTransformFactory.h"
#include "itkSimilarity2DTransform.h"
#include "itkBSplineTransform.h"
#include "itksys/SystemTools.hxx"
#include <iomanip>

//Transforms from Filtering/DisplacementField/include
#include "itkBSplineExponentialDiffeomorphicTransform.h"
#include "itkBSplineSmoothingOnUpdateDisplacementFieldTransform.h"
#include "itkConstantVelocityFieldTransform.h"
#include "itkDisplacementFieldTransform.h"
#include "itkGaussianExponentialDiffeomorphicTransform.h"
#include "itkGaussianSmoothingOnUpdateDisplacementFieldTransform.h"
#include "itkMath.h"

template<typename TParametersValueType, typename DisplacementTransformType >
static int ReadWriteTest(const char * const fileName, const bool isRealDisplacementField )
{
  // First make a DisplacementField wiht known values
  const double aNumberThatCanNotBeRepresentedInFloatingPoint = 1e-5 + 1e-7 + 1e-9 +1e-13;
  const double requiredSpacing = 1.2 + aNumberThatCanNotBeRepresentedInFloatingPoint;
  const double requiredOrigin = 23.0 + aNumberThatCanNotBeRepresentedInFloatingPoint;
  typename DisplacementTransformType::Pointer displacementTransform = DisplacementTransformType::New();
  typedef typename DisplacementTransformType::DisplacementFieldType FieldType;
  typename FieldType::Pointer knownField = FieldType::New(); //This is based on itk::Image
    {
    const int dimLength = 20;
    typename FieldType::SizeType size;
    size.Fill( dimLength );
    typename FieldType::IndexType start;
    start.Fill( 0 );
    typename FieldType::RegionType region;
    region.SetSize( size );
    region.SetIndex( start );
    knownField->SetRegions( region );

    typename FieldType::SpacingType spacing;
    spacing.Fill( requiredSpacing );
    knownField->SetSpacing( spacing );
    typename FieldType::PointType origin;
    origin.Fill( requiredOrigin );
    knownField->SetOrigin (origin);
    knownField->Allocate();

    typename DisplacementTransformType::OutputVectorType zeroVector;
    zeroVector.Fill( 0.0 );
    knownField->FillBuffer( zeroVector );

    displacementTransform->SetDisplacementField( knownField );
    }

  // Now test reading/writing many different transform types.

  typedef itk::TransformFileReaderTemplate<TParametersValueType> TransformReaderType;
  typename TransformReaderType::Pointer reader = TransformReaderType::New();
  reader->SetFileName( fileName );

  typedef itk::HDF5TransformIOTemplate<TParametersValueType> TransformIOType;
  typename TransformIOType::Pointer transformIO = TransformIOType::New();
  reader->SetTransformIO( transformIO );
  if( reader->GetTransformIO() != transformIO.GetPointer() )
    {
    std::cerr << "Set/Get TransformIO did not work correctly." << std::endl;
    return EXIT_FAILURE;
    }

  typedef itk::TransformFileWriterTemplate<TParametersValueType> TransformWriterType;
  typename TransformWriterType::Pointer writer = TransformWriterType::New();
  writer->SetFileName( fileName );
  writer->SetTransformIO( transformIO );
  if( writer->GetTransformIO() != transformIO.GetPointer() )
    {
    std::cerr << "Set/Get TransformIO did not work correctly." << std::endl;
    return EXIT_FAILURE;
    }

  try
    {
    writer->AddTransform( displacementTransform );
    writer->Update();
    }
  catch( itk::ExceptionObject & excp )
    {
    std::cerr << "Error while saving the transforms" << std::endl;
    std::cerr << excp << std::endl;
    std::cout << "[FAILED]" << std::endl;
    return EXIT_FAILURE;
    }

  try
    {
    reader->Update();
    }
  catch( itk::ExceptionObject & excp )
    {
    std::cerr << "Error while reading the transforms" << std::endl;
    std::cerr << excp << std::endl;
    std::cout << "[FAILED]" << std::endl;
    return EXIT_FAILURE;
    }

  if( isRealDisplacementField )
    {
    //New verify that the transform was read properly from disk without loss
    //Read first transform
    const typename itk::TransformFileReaderTemplate<TParametersValueType>::TransformListType *
    list = reader->GetTransformList();
    typename DisplacementTransformType::ConstPointer readDisplacementTransform =
    dynamic_cast<DisplacementTransformType *>( (*(list->begin())).GetPointer() );
    if (readDisplacementTransform.IsNull() )
      {
      std::cerr << " ERROR: dynamic_cast failed! " << std::endl;
      std::cerr << typeid(TParametersValueType).name() << std::endl;
      std::cerr << typeid(DisplacementTransformType).name() << std::endl;
      return EXIT_FAILURE;
      }
    typename DisplacementTransformType::DisplacementFieldType::ConstPointer
    readDisplacement = readDisplacementTransform->GetDisplacementField();
    if( readDisplacement.IsNull() )
      {
      std::cerr << " ERROR: GetDisplacementField failed! " << std::endl;
      std::cerr << typeid(TParametersValueType).name() << std::endl;
      std::cerr << typeid(DisplacementTransformType).name() << std::endl;
      std::cerr << "\n\n\n" << std::endl;

      (*list->begin())->Print ( std::cerr );
      return EXIT_FAILURE;
      }

    if ( ( readDisplacement->GetSpacing() != knownField->GetSpacing() ) )
      //  || ( readDisplacement->GetSpacing()[0] != requiredSpacing ) )
      {
      std::cerr << "Error invalid spacing restored from disk" << std::endl;
      std::cerr << std::setprecision(17) << "\n"
      << readDisplacement->GetSpacing() << " != "
      << knownField->GetSpacing() << "\n"
      << requiredSpacing
      << "It is likely going trough a float truncation " << (float)requiredSpacing << std::endl;
      return EXIT_FAILURE;
      }
    if ( ( readDisplacement->GetOrigin() != knownField->GetOrigin() )
        || ( itk::Math::NotExactlyEquals(readDisplacement->GetOrigin()[0], requiredOrigin) ) )
      {
      std::cerr << "Error invalid origin restored from disk" << std::endl;
      std::cerr << std::setprecision(17) << "\n"
      << readDisplacement->GetOrigin() << " != "
      << knownField->GetOrigin() << "\n"
      << requiredOrigin << std::endl;
      return EXIT_FAILURE;
      }
    }
  return EXIT_SUCCESS;
}

template<typename TParametersValueType>
static int oneTest(const char *const goodname,const char *const badname)
{
  typedef typename itk::AffineTransform<TParametersValueType,4>  AffineTransformType;
  typedef typename itk::AffineTransform<TParametersValueType,10> AffineTransformTypeNotRegistered;
  typename AffineTransformType::Pointer        affine = AffineTransformType::New();

  itk::ObjectFactoryBase::RegisterFactory(itk::HDF5TransformIOFactory::New() );

  // Set it's parameters
  {
  typename AffineTransformType::ParametersType p =
           affine->GetParameters();
  for ( unsigned int i = 0; i < p.GetSize(); i++ )
    {
    p[i] = i;
    }
  affine->SetParameters ( p );

  }
  {
  typename AffineTransformType::FixedParametersType p =
           affine->GetFixedParameters ();
  for ( unsigned int i = 0; i < p.GetSize(); i++ )
    {
    p[i] = i;
    }
  affine->SetFixedParameters ( p );
  }
  typename itk::TransformFileWriterTemplate<TParametersValueType>::Pointer
    writer = itk::TransformFileWriterTemplate<TParametersValueType>::New();
  typename itk::TransformFileReaderTemplate<TParametersValueType>::Pointer
    reader = itk::TransformFileReaderTemplate<TParametersValueType>::New();

  writer->AddTransform(affine);

  writer->SetFileName( goodname );
  reader->SetFileName( goodname );

  // Testing writing std::cout << "Testing write : ";
  affine->Print ( std::cout );
  try
    {
    writer->Update();
    std::cout << std::endl;
    std::cout << "Testing read : " << std::endl;
    reader->Update();
    }
  catch( itk::ExceptionObject & excp )
    {
    std::cerr << "Error while saving the transforms" << std::endl;
    std::cerr << excp << std::endl;
    std::cout << "[FAILED]" << std::endl;
    return EXIT_FAILURE;
    }

  try
    {
    const typename itk::TransformFileReaderTemplate<TParametersValueType>::TransformListType * list = reader->GetTransformList();
    typename itk::TransformFileReaderTemplate<TParametersValueType>::TransformListType::const_iterator lit = list->begin();
    while ( lit != list->end() )
      {
      (*lit)->Print ( std::cout );
      ++lit;
      }
    }
  catch( itk::ExceptionObject & excp )
    {
    std::cerr << "Error while saving the transforms" << std::endl;
    std::cerr << excp << std::endl;
    std::cout << "[FAILED]" << std::endl;
    return EXIT_FAILURE;
    }

  std::cout << "Creating bad writer" << std::endl;
  typename AffineTransformTypeNotRegistered::Pointer Bogus = AffineTransformTypeNotRegistered::New();

  // Set it's parameters
  {
  typename AffineTransformType::ParametersType p = Bogus->GetParameters();
  for ( unsigned int i = 0; i < p.GetSize(); i++ )
    {
    p[i] = i;
    }
  Bogus->SetParameters ( p );
   }
  {
  typename AffineTransformType::FixedParametersType p = Bogus->GetFixedParameters ();
  for ( unsigned int i = 0; i < p.GetSize(); i++ )
    {
    p[i] = i;
    }
  Bogus->SetFixedParameters ( p );
  }
  typename itk::TransformFileWriterTemplate<TParametersValueType>::Pointer
    badwriter = itk::TransformFileWriterTemplate<TParametersValueType>::New();
  typename itk::TransformFileReaderTemplate<TParametersValueType>::Pointer
    badreader = itk::TransformFileReaderTemplate<TParametersValueType>::New();
  badwriter->AddTransform(Bogus);
  badwriter->SetFileName(badname);
  badreader->SetFileName(badname);

  // Testing writing
  std::cout << "Testing write of non register transform : " << std::endl;
  std::cout << std::flush;
  try
    {
    badwriter->Update();
    }
  catch( itk::ExceptionObject & excp )
    {
    std::cerr << "Error while saving the transforms" << std::endl;
    std::cerr << excp << std::endl;
    std::cout << "[FAILED]" << std::endl;
    return EXIT_FAILURE;
    }

  // Testing writing
  std::cout << "Testing read of non register transform : " << std::endl;
  std::cout << std::flush;
  bool caught = false;
  try
    {
    badreader->Update();
    }
  catch( itk::ExceptionObject & excp )
    {
    caught = true;
    std::cout << "Caught exception as expected" << std::endl;
    std::cout << excp << std::endl;
    }
  if ( !caught )
    {
    std::cerr << "Did not catch non registered transform" << std::endl;
    std::cout << "[FAILED]" << std::endl;
    return EXIT_FAILURE;
    }

  int error_sum = 0;
  error_sum += ReadWriteTest< float, itk::BSplineSmoothingOnUpdateDisplacementFieldTransform<float,2> >(goodname, true);
  error_sum += ReadWriteTest< float, itk::BSplineSmoothingOnUpdateDisplacementFieldTransform<float,3> >(goodname, true);
  error_sum += ReadWriteTest< double, itk::BSplineSmoothingOnUpdateDisplacementFieldTransform<double,2> >(goodname, true);
  error_sum += ReadWriteTest< double, itk::BSplineSmoothingOnUpdateDisplacementFieldTransform<double,3> >(goodname, true);

  error_sum += ReadWriteTest< float, itk::DisplacementFieldTransform<float, 2> >(goodname, true);
  error_sum += ReadWriteTest< float, itk::DisplacementFieldTransform<float, 3> >(goodname, true);
  error_sum += ReadWriteTest< double, itk::DisplacementFieldTransform<double, 2> >(goodname, true);
  error_sum += ReadWriteTest< double, itk::DisplacementFieldTransform<double, 3> >(goodname, true);

  error_sum += ReadWriteTest< float, itk::BSplineSmoothingOnUpdateDisplacementFieldTransform<float,2> >(goodname, true);
  error_sum += ReadWriteTest< float, itk::BSplineSmoothingOnUpdateDisplacementFieldTransform<float,3> >(goodname, true);
  error_sum += ReadWriteTest< double, itk::BSplineSmoothingOnUpdateDisplacementFieldTransform<double,2> >(goodname, true);
  error_sum += ReadWriteTest< double, itk::BSplineSmoothingOnUpdateDisplacementFieldTransform<double,3> >(goodname, true);

  error_sum += ReadWriteTest< float, itk::GaussianSmoothingOnUpdateDisplacementFieldTransform<float,2> >(goodname, true);
  error_sum += ReadWriteTest< float, itk::GaussianSmoothingOnUpdateDisplacementFieldTransform<float,3> >(goodname, true);
  error_sum += ReadWriteTest< double, itk::GaussianSmoothingOnUpdateDisplacementFieldTransform<double,2> >(goodname, true);
  error_sum += ReadWriteTest< double, itk::GaussianSmoothingOnUpdateDisplacementFieldTransform<double,3> >(goodname, true);

   error_sum += ReadWriteTest< float, itk::ConstantVelocityFieldTransform<float,2> >(goodname, false);
   error_sum += ReadWriteTest< float, itk::ConstantVelocityFieldTransform<float,3> >(goodname, false);
   error_sum += ReadWriteTest< double, itk::ConstantVelocityFieldTransform<double,2> >(goodname, false);
   error_sum += ReadWriteTest< double, itk::ConstantVelocityFieldTransform<double,3> >(goodname, false);

   error_sum += ReadWriteTest< float, itk::GaussianExponentialDiffeomorphicTransform<float,2> >(goodname, false);
   error_sum += ReadWriteTest< float, itk::GaussianExponentialDiffeomorphicTransform<float,3> >(goodname, false);
   error_sum += ReadWriteTest< double, itk::GaussianExponentialDiffeomorphicTransform<double,2> >(goodname, false);
   error_sum += ReadWriteTest< double, itk::GaussianExponentialDiffeomorphicTransform<double,3> >(goodname, false);

  if( error_sum > 0 )
    {
    std::cerr << "Atleast 1 transform type could not be read/written " << error_sum << std::endl;
    std::cout << "[FAILED]" << std::endl;
    return EXIT_FAILURE;
    }

  std::cout << "[PASSED]" << std::endl;
  return EXIT_SUCCESS;
}

int itkIOTransformHDF5Test(int argc, char* argv[])
{
  if (argc > 1)
    {
    itksys::SystemTools::ChangeDirectory(argv[1]);
    }
  const int result1 =  oneTest<float>("Transforms_float.h5", "TransformsBad_float.h5" );
  const int result2 =  oneTest<double>("Transforms_double.hdf5", "TransformsBad_double.hdf5" );

  return ( !( result1 == EXIT_SUCCESS && result2 == EXIT_SUCCESS) );
}
<commit_msg>BUG: Do not use dynamic_cast for DisplacementFieldType in HDF5 test.<commit_after>/*=========================================================================
 *
 *  Copyright Insight Software Consortium
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *         http://www.apache.org/licenses/LICENSE-2.0.txt
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 *=========================================================================*/

#include "itkHDF5TransformIOFactory.h"
#include "itkHDF5TransformIO.h"
#include "itkTransformFileWriter.h"
#include "itkTransformFileReader.h"
#include "itkAffineTransform.h"
#include "itkTransformFactory.h"
#include "itkSimilarity2DTransform.h"
#include "itkBSplineTransform.h"
#include "itksys/SystemTools.hxx"
#include <iomanip>

//Transforms from Filtering/DisplacementField/include
#include "itkBSplineExponentialDiffeomorphicTransform.h"
#include "itkBSplineSmoothingOnUpdateDisplacementFieldTransform.h"
#include "itkConstantVelocityFieldTransform.h"
#include "itkDisplacementFieldTransform.h"
#include "itkGaussianExponentialDiffeomorphicTransform.h"
#include "itkGaussianSmoothingOnUpdateDisplacementFieldTransform.h"
#include "itkMath.h"

template<typename TParametersValueType, typename DisplacementTransformType >
static int ReadWriteTest(const char * const fileName, const bool isRealDisplacementField )
{
  // First make a DisplacementField wiht known values
  const double aNumberThatCanNotBeRepresentedInFloatingPoint = 1e-5 + 1e-7 + 1e-9 +1e-13;
  const double requiredSpacing = 1.2 + aNumberThatCanNotBeRepresentedInFloatingPoint;
  const double requiredOrigin = 23.0 + aNumberThatCanNotBeRepresentedInFloatingPoint;
  typename DisplacementTransformType::Pointer displacementTransform = DisplacementTransformType::New();
  typedef typename DisplacementTransformType::DisplacementFieldType FieldType;
  typename FieldType::Pointer knownField = FieldType::New(); //This is based on itk::Image
    {
    const int dimLength = 20;
    typename FieldType::SizeType size;
    size.Fill( dimLength );
    typename FieldType::IndexType start;
    start.Fill( 0 );
    typename FieldType::RegionType region;
    region.SetSize( size );
    region.SetIndex( start );
    knownField->SetRegions( region );

    typename FieldType::SpacingType spacing;
    spacing.Fill( requiredSpacing );
    knownField->SetSpacing( spacing );
    typename FieldType::PointType origin;
    origin.Fill( requiredOrigin );
    knownField->SetOrigin (origin);
    knownField->Allocate();

    typename DisplacementTransformType::OutputVectorType zeroVector;
    zeroVector.Fill( 0.0 );
    knownField->FillBuffer( zeroVector );

    displacementTransform->SetDisplacementField( knownField );
    }

  // Now test reading/writing many different transform types.

  typedef itk::TransformFileReaderTemplate<TParametersValueType> TransformReaderType;
  typename TransformReaderType::Pointer reader = TransformReaderType::New();
  reader->SetFileName( fileName );

  typedef itk::HDF5TransformIOTemplate<TParametersValueType> TransformIOType;
  typename TransformIOType::Pointer transformIO = TransformIOType::New();
  reader->SetTransformIO( transformIO );
  if( reader->GetTransformIO() != transformIO.GetPointer() )
    {
    std::cerr << "Set/Get TransformIO did not work correctly." << std::endl;
    return EXIT_FAILURE;
    }

  typedef itk::TransformFileWriterTemplate<TParametersValueType> TransformWriterType;
  typename TransformWriterType::Pointer writer = TransformWriterType::New();
  writer->SetFileName( fileName );
  writer->SetTransformIO( transformIO );
  if( writer->GetTransformIO() != transformIO.GetPointer() )
    {
    std::cerr << "Set/Get TransformIO did not work correctly." << std::endl;
    return EXIT_FAILURE;
    }

  try
    {
    writer->AddTransform( displacementTransform );
    writer->Update();
    }
  catch( itk::ExceptionObject & excp )
    {
    std::cerr << "Error while saving the transforms" << std::endl;
    std::cerr << excp << std::endl;
    std::cout << "[FAILED]" << std::endl;
    return EXIT_FAILURE;
    }

  try
    {
    reader->Update();
    }
  catch( itk::ExceptionObject & excp )
    {
    std::cerr << "Error while reading the transforms" << std::endl;
    std::cerr << excp << std::endl;
    std::cout << "[FAILED]" << std::endl;
    return EXIT_FAILURE;
    }

  if( isRealDisplacementField )
    {
    //New verify that the transform was read properly from disk without loss
    //Read first transform
    const typename itk::TransformFileReaderTemplate<TParametersValueType>::TransformListType *
    list = reader->GetTransformList();
    typename DisplacementTransformType::ConstPointer readDisplacementTransform =
    static_cast<DisplacementTransformType *>( (*(list->begin())).GetPointer() );
    if (readDisplacementTransform.IsNull() )
      {
      std::cerr << " ERROR: Read DisplacementTransform is null! " << std::endl;
      std::cerr << typeid(TParametersValueType).name() << std::endl;
      std::cerr << typeid(DisplacementTransformType).name() << std::endl;
      return EXIT_FAILURE;
      }
    typename DisplacementTransformType::DisplacementFieldType::ConstPointer
    readDisplacement = readDisplacementTransform->GetDisplacementField();
    if( readDisplacement.IsNull() )
      {
      std::cerr << " ERROR: GetDisplacementField failed! " << std::endl;
      std::cerr << typeid(TParametersValueType).name() << std::endl;
      std::cerr << typeid(DisplacementTransformType).name() << std::endl;
      std::cerr << "\n\n\n" << std::endl;

      (*list->begin())->Print ( std::cerr );
      return EXIT_FAILURE;
      }

    if ( ( readDisplacement->GetSpacing() != knownField->GetSpacing() ) )
      //  || ( readDisplacement->GetSpacing()[0] != requiredSpacing ) )
      {
      std::cerr << "Error invalid spacing restored from disk" << std::endl;
      std::cerr << std::setprecision(17) << "\n"
      << readDisplacement->GetSpacing() << " != "
      << knownField->GetSpacing() << "\n"
      << requiredSpacing
      << "It is likely going trough a float truncation " << (float)requiredSpacing << std::endl;
      return EXIT_FAILURE;
      }
    if ( ( readDisplacement->GetOrigin() != knownField->GetOrigin() )
        || ( itk::Math::NotExactlyEquals(readDisplacement->GetOrigin()[0], requiredOrigin) ) )
      {
      std::cerr << "Error invalid origin restored from disk" << std::endl;
      std::cerr << std::setprecision(17) << "\n"
      << readDisplacement->GetOrigin() << " != "
      << knownField->GetOrigin() << "\n"
      << requiredOrigin << std::endl;
      return EXIT_FAILURE;
      }
    }
  return EXIT_SUCCESS;
}

template<typename TParametersValueType>
static int oneTest(const char *const goodname,const char *const badname)
{
  typedef typename itk::AffineTransform<TParametersValueType,4>  AffineTransformType;
  typedef typename itk::AffineTransform<TParametersValueType,10> AffineTransformTypeNotRegistered;
  typename AffineTransformType::Pointer        affine = AffineTransformType::New();

  itk::ObjectFactoryBase::RegisterFactory(itk::HDF5TransformIOFactory::New() );

  // Set it's parameters
  {
  typename AffineTransformType::ParametersType p =
           affine->GetParameters();
  for ( unsigned int i = 0; i < p.GetSize(); i++ )
    {
    p[i] = i;
    }
  affine->SetParameters ( p );

  }
  {
  typename AffineTransformType::FixedParametersType p =
           affine->GetFixedParameters ();
  for ( unsigned int i = 0; i < p.GetSize(); i++ )
    {
    p[i] = i;
    }
  affine->SetFixedParameters ( p );
  }
  typename itk::TransformFileWriterTemplate<TParametersValueType>::Pointer
    writer = itk::TransformFileWriterTemplate<TParametersValueType>::New();
  typename itk::TransformFileReaderTemplate<TParametersValueType>::Pointer
    reader = itk::TransformFileReaderTemplate<TParametersValueType>::New();

  writer->AddTransform(affine);

  writer->SetFileName( goodname );
  reader->SetFileName( goodname );

  // Testing writing std::cout << "Testing write : ";
  affine->Print ( std::cout );
  try
    {
    writer->Update();
    std::cout << std::endl;
    std::cout << "Testing read : " << std::endl;
    reader->Update();
    }
  catch( itk::ExceptionObject & excp )
    {
    std::cerr << "Error while saving the transforms" << std::endl;
    std::cerr << excp << std::endl;
    std::cout << "[FAILED]" << std::endl;
    return EXIT_FAILURE;
    }

  try
    {
    const typename itk::TransformFileReaderTemplate<TParametersValueType>::TransformListType * list = reader->GetTransformList();
    typename itk::TransformFileReaderTemplate<TParametersValueType>::TransformListType::const_iterator lit = list->begin();
    while ( lit != list->end() )
      {
      (*lit)->Print ( std::cout );
      ++lit;
      }
    }
  catch( itk::ExceptionObject & excp )
    {
    std::cerr << "Error while saving the transforms" << std::endl;
    std::cerr << excp << std::endl;
    std::cout << "[FAILED]" << std::endl;
    return EXIT_FAILURE;
    }

  std::cout << "Creating bad writer" << std::endl;
  typename AffineTransformTypeNotRegistered::Pointer Bogus = AffineTransformTypeNotRegistered::New();

  // Set it's parameters
  {
  typename AffineTransformType::ParametersType p = Bogus->GetParameters();
  for ( unsigned int i = 0; i < p.GetSize(); i++ )
    {
    p[i] = i;
    }
  Bogus->SetParameters ( p );
   }
  {
  typename AffineTransformType::FixedParametersType p = Bogus->GetFixedParameters ();
  for ( unsigned int i = 0; i < p.GetSize(); i++ )
    {
    p[i] = i;
    }
  Bogus->SetFixedParameters ( p );
  }
  typename itk::TransformFileWriterTemplate<TParametersValueType>::Pointer
    badwriter = itk::TransformFileWriterTemplate<TParametersValueType>::New();
  typename itk::TransformFileReaderTemplate<TParametersValueType>::Pointer
    badreader = itk::TransformFileReaderTemplate<TParametersValueType>::New();
  badwriter->AddTransform(Bogus);
  badwriter->SetFileName(badname);
  badreader->SetFileName(badname);

  // Testing writing
  std::cout << "Testing write of non register transform : " << std::endl;
  std::cout << std::flush;
  try
    {
    badwriter->Update();
    }
  catch( itk::ExceptionObject & excp )
    {
    std::cerr << "Error while saving the transforms" << std::endl;
    std::cerr << excp << std::endl;
    std::cout << "[FAILED]" << std::endl;
    return EXIT_FAILURE;
    }

  // Testing writing
  std::cout << "Testing read of non register transform : " << std::endl;
  std::cout << std::flush;
  bool caught = false;
  try
    {
    badreader->Update();
    }
  catch( itk::ExceptionObject & excp )
    {
    caught = true;
    std::cout << "Caught exception as expected" << std::endl;
    std::cout << excp << std::endl;
    }
  if ( !caught )
    {
    std::cerr << "Did not catch non registered transform" << std::endl;
    std::cout << "[FAILED]" << std::endl;
    return EXIT_FAILURE;
    }

  int error_sum = 0;
  error_sum += ReadWriteTest< float, itk::BSplineSmoothingOnUpdateDisplacementFieldTransform<float,2> >(goodname, true);
  error_sum += ReadWriteTest< float, itk::BSplineSmoothingOnUpdateDisplacementFieldTransform<float,3> >(goodname, true);
  error_sum += ReadWriteTest< double, itk::BSplineSmoothingOnUpdateDisplacementFieldTransform<double,2> >(goodname, true);
  error_sum += ReadWriteTest< double, itk::BSplineSmoothingOnUpdateDisplacementFieldTransform<double,3> >(goodname, true);

  error_sum += ReadWriteTest< float, itk::DisplacementFieldTransform<float, 2> >(goodname, true);
  error_sum += ReadWriteTest< float, itk::DisplacementFieldTransform<float, 3> >(goodname, true);
  error_sum += ReadWriteTest< double, itk::DisplacementFieldTransform<double, 2> >(goodname, true);
  error_sum += ReadWriteTest< double, itk::DisplacementFieldTransform<double, 3> >(goodname, true);

  error_sum += ReadWriteTest< float, itk::BSplineSmoothingOnUpdateDisplacementFieldTransform<float,2> >(goodname, true);
  error_sum += ReadWriteTest< float, itk::BSplineSmoothingOnUpdateDisplacementFieldTransform<float,3> >(goodname, true);
  error_sum += ReadWriteTest< double, itk::BSplineSmoothingOnUpdateDisplacementFieldTransform<double,2> >(goodname, true);
  error_sum += ReadWriteTest< double, itk::BSplineSmoothingOnUpdateDisplacementFieldTransform<double,3> >(goodname, true);

  error_sum += ReadWriteTest< float, itk::GaussianSmoothingOnUpdateDisplacementFieldTransform<float,2> >(goodname, true);
  error_sum += ReadWriteTest< float, itk::GaussianSmoothingOnUpdateDisplacementFieldTransform<float,3> >(goodname, true);
  error_sum += ReadWriteTest< double, itk::GaussianSmoothingOnUpdateDisplacementFieldTransform<double,2> >(goodname, true);
  error_sum += ReadWriteTest< double, itk::GaussianSmoothingOnUpdateDisplacementFieldTransform<double,3> >(goodname, true);

   error_sum += ReadWriteTest< float, itk::ConstantVelocityFieldTransform<float,2> >(goodname, false);
   error_sum += ReadWriteTest< float, itk::ConstantVelocityFieldTransform<float,3> >(goodname, false);
   error_sum += ReadWriteTest< double, itk::ConstantVelocityFieldTransform<double,2> >(goodname, false);
   error_sum += ReadWriteTest< double, itk::ConstantVelocityFieldTransform<double,3> >(goodname, false);

   error_sum += ReadWriteTest< float, itk::GaussianExponentialDiffeomorphicTransform<float,2> >(goodname, false);
   error_sum += ReadWriteTest< float, itk::GaussianExponentialDiffeomorphicTransform<float,3> >(goodname, false);
   error_sum += ReadWriteTest< double, itk::GaussianExponentialDiffeomorphicTransform<double,2> >(goodname, false);
   error_sum += ReadWriteTest< double, itk::GaussianExponentialDiffeomorphicTransform<double,3> >(goodname, false);

  if( error_sum > 0 )
    {
    std::cerr << "Atleast 1 transform type could not be read/written " << error_sum << std::endl;
    std::cout << "[FAILED]" << std::endl;
    return EXIT_FAILURE;
    }

  std::cout << "[PASSED]" << std::endl;
  return EXIT_SUCCESS;
}

int itkIOTransformHDF5Test(int argc, char* argv[])
{
  if (argc > 1)
    {
    itksys::SystemTools::ChangeDirectory(argv[1]);
    }
  const int result1 =  oneTest<float>("Transforms_float.h5", "TransformsBad_float.h5" );
  const int result2 =  oneTest<double>("Transforms_double.hdf5", "TransformsBad_double.hdf5" );

  return ( !( result1 == EXIT_SUCCESS && result2 == EXIT_SUCCESS) );
}
<|endoftext|>


<commit_before>#include "ros/ros.h"
#include "geometry_msgs/Accel.h"
#include "geometry_msgs/Vector3.h"
#include "sensor_msgs/JointState.h"
#include "riptide_msgs/ThrustStamped.h"
#include <math.h>
#include <vector>
#include "ceres/ceres.h"
#include "glog/logging.h"

#undef debug

double mass = 24.9476;	// Mass of the vehicle (KG)

double desiredSurge = 0.0; // Desired linear acceleration in X (m/s^2)
double desiredSway = 0.0; // Desired linear acceleration in Y (m/s^2)
double desiredHeave = 0.0; // Desired linear acceleration in Z (m/s^2)

double desiredRoll = 0.0; // Desired angular acceleration about X (rad/s^2)
double desiredPitch = 0.0; // Desired angular acceleration about Y (rad/s^2)
double desiredYaw = 0.0; // Desired angular acceleration about Z (rad/s^2)

double IX = 1.0; // Moment of inertia about X
double IY = 1.0; // Moment of inertia about Y
double IZ = 1.0; // Moment of inertia about Z

struct vector
{
		double x;
		double y;
		double z;
};

vector makeVector(double X, double Y, double Z)
{
	vector v;
	v.x = X;
	v.y = Y;
	v.z = Z;
	return v;
}

/*** Thruster Positions ***/
// Positions are in meters relative to the center of mass.
// Forward Thrusters
vector pos_u1 = makeVector(-.25, .127, -.05);
vector pos_u2 = makeVector(-.25, -.127, -.05);
vector pos_u3 = makeVector(.25, -.127, -.05);
vector pos_u4 = makeVector(.25, .127, -.05);
// Upwards Thrusters
vector pos_f1 = makeVector(-.3, -.127, 1.0);
vector pos_f2 = makeVector(-.3, .127, 1.0);
vector pos_f3 = makeVector(-.3, .127, -1.0);
vector pos_f4 = makeVector(-.3, -.127, -1.0);
// Sideways Thrusters
vector pos_s1 = makeVector(.5, 0.0, -1.0);
vector pos_s2 = makeVector(-.5, 0.0, -1.0);

/*** Thruster Definitions ***/
// f1 = forward top right		u1 = vertical back left		s1 = sideways front
// f2 = forward top left		u2 = vertical back right	s2 = sideways back
// f3 = forward bottom left		u3 = vertical front right
// f4 = forward bottom right	u4 = vertical front left

/*** EQUATIONS ***/
// These equations solve for linear/angular acceleration in all axes
// Linear Equations
struct a_surge {
	template <typename T> bool operator()(const T* const f1,
		const T* const f2,
		const T* const f3,
		const T* const f4,
		T* residual) const {
		residual[0] = (f1[0] + f2[0] + f3[0] + f4[0]) / T(mass) - T(desiredSurge); // Sum forces and subtract desired
		return true;
	}
};

struct a_sway {
	template <typename T> bool operator()(const T* const s1,
		const T* const s2,
		T* residual) const {
		residual[0] = (s1[0] + s2[0]) / T(mass) - T(desiredSway);
		return true;
	}
};

struct a_heave {
	template <typename T> bool operator()(const T* const u1,
		const T* const u2,
		const T* const u3,
		const T* const u4,
		T* residual) const {
		residual[0] = (u1[0] + u2[0] + u3[0] + u4[0]) / T(mass) - T(desiredHeave);
		return true;
	}
};

// Angular equations
struct a_roll {
	template <typename T> bool operator()(const T* const u1,
		const T* const u2,
		const T* const u3,
		const T* const u4,
		const T* const s1,
		const T* const s2,
		T* residual) const {
		residual[0] = (u1[0]*T(pos_u1.y)+u2[0]*T(pos_u2.y)+u3[0]*T(pos_u3.y)+u4[0]*T(pos_u4.y)+s1[0]*T(pos_s1.z)+s1[0]*T(pos_s2.z)) / T(IX) - T(desiredRoll);
		return true;
	}
};

struct a_pitch {
	template <typename T> bool operator()(const T* const f1,
		const T* const f2,
		const T* const f3,
		const T* const f4,
		const T* const u1,
		const T* const u2,
		const T* const u3,
		const T* const u4,
		T* residual) const {
		residual[0] = (f1[0] * T(pos_f1.z) + f2[0]*T(pos_f2.z) + f3[0]*T(pos_f3.z) + f4[0]*T(pos_f4.z) + u1[0]*T(pos_u1.x) + u2[0]*T(pos_u2.x) + u3[0]*T(pos_u3.x) + u4[0]*T(pos_u4.x)) / T(IY) - T(desiredPitch);
		return true;
	}
};

struct a_yaw{
	template <typename T> bool operator()(const T* const f1,
		const T* const f2,
		const T* const f3,
		const T* const f4,
		const T* const s1,
		const T* const s2,
		T* residual) const {
	residual[0] = (f1[0]*T(pos_f1.y) + f2[0]*T(pos_f2.y) + f3[0]*T(pos_f3.y) + f4[0]*T(pos_f4.y) + s1[0]*T(pos_s1.x) + s1[0]*T(pos_s2.x)) / T(IZ) - T(desiredYaw);
	return true;
}
};

// Solver class
class Solver
{
private:
	ros::NodeHandle nh;
	ros::Subscriber accels;
	ros::Publisher forces;
	riptide_msgs::ThrustStamped thrust_msg;
	double f1, f2, f3, f4;
	double u1, u2, u3, u4;
	double s1, s2;
	ceres::Problem problem;
	ceres::Solver::Options options;
	ceres::Solver::Summary summary;

public:
	Solver(char** argv);
	void callback(const geometry_msgs::Accel::ConstPtr& a);
	void loop();
};

int main(int argc, char **argv)
{
	ros::init(argc, argv, "thrust_solver");
	Solver solver(argv);
	solver.loop();
}

Solver::Solver(char** argv)
{
	accels = nh.subscribe<geometry_msgs::Accel>("accel_error", 1, &Solver::callback, this); // Message to give desired accelerations
	forces = nh.advertise<riptide_msgs::ThrustStamped>("solver/thrust", 1); // Message containing force required from each thruster to achieve given acceleration

	google::InitGoogleLogging(argv[0]);

	// PROBLEM SETUP
	// Add residual blocks (equations)
	// Linear
	problem.AddResidualBlock(new ceres::AutoDiffCostFunction<a_surge, 1, 1, 1, 1, 1>(new a_surge),
		NULL,
		&f1, &f2, &f3, &f4);
	problem.AddResidualBlock(new ceres::AutoDiffCostFunction<a_sway, 1, 1, 1>(new a_sway),
		NULL,
		&s1, &s2);
	problem.AddResidualBlock(new ceres::AutoDiffCostFunction<a_heave, 1, 1, 1, 1, 1>(new a_heave),
		NULL,
		&u1, &u2, &u3, &u4);
	// Angular
	problem.AddResidualBlock(new ceres::AutoDiffCostFunction<a_roll, 1, 1, 1, 1, 1, 1, 1>(new a_roll),
		NULL,
		&u1, &u2, &u3, &u4, &s1, &s2);
	problem.AddResidualBlock(new ceres::AutoDiffCostFunction<a_pitch, 1, 1, 1, 1, 1, 1, 1, 1, 1>(new a_pitch),
		NULL,
		&f1, &f2, &f3, &f4, &u1, &u2, &u3, &u4);
	problem.AddResidualBlock(new ceres::AutoDiffCostFunction<a_yaw, 1, 1, 1, 1, 1, 1, 1>(new a_yaw),
		NULL,
		&f1, &f2, &f3, &f4, &s1, &s2);
	// Set constraints (min, max thruster force)
	// Forward thrusters
	problem.SetParameterLowerBound(&f1, 0, -25.0);
	problem.SetParameterUpperBound(&f1, 0, 25.0);

	problem.SetParameterLowerBound(&f2, 0, -25.0);
	problem.SetParameterUpperBound(&f2, 0, 25.0);

	problem.SetParameterLowerBound(&f3, 0, -25.0);
	problem.SetParameterUpperBound(&f3, 0, 25.0);

	problem.SetParameterLowerBound(&f4, 0, -25.0);
	problem.SetParameterUpperBound(&f4, 0, 25.0);
	// Upward thrusters
	problem.SetParameterLowerBound(&u1, 0, -25.0);
	problem.SetParameterUpperBound(&u1, 0, 25.0);

	problem.SetParameterLowerBound(&u2, 0, -25.0);
	problem.SetParameterUpperBound(&u2, 0, 25.0);

	problem.SetParameterLowerBound(&u3, 0, -25.0);
	problem.SetParameterUpperBound(&u3, 0, 25.0);

	problem.SetParameterLowerBound(&u4, 0, -25.0);
	problem.SetParameterUpperBound(&u4, 0, 25.0);
	// Sideways thrusters
	problem.SetParameterLowerBound(&s1, 0, -25.0);
	problem.SetParameterUpperBound(&s1, 0, 25.0);

	problem.SetParameterLowerBound(&s2, 0, -25.0);
	problem.SetParameterUpperBound(&s2, 0, 25.0);

	// Configure solver
	options.max_num_iterations = 100;
	options.linear_solver_type = ceres::DENSE_QR;

#ifdef debug
	options.minimizer_progress_to_stdout = true;
#endif
}

void Solver::callback(const geometry_msgs::Accel::ConstPtr& a)
{
	desiredSurge = a->linear.x;
	desiredSway = a->linear.y;
	desiredHeave = a->linear.z;

	desiredRoll = a->angular.x;
	desiredPitch = a->angular.y;
	desiredYaw = a->angular.z;

	// These forced initial guesses don't make much of a difference.
	// We currently experience a sort of gimbal lock w/ or w/o them.
	f1 = 0.0;
	f2 = 0.0;
	f3 = 0.0;
	f4 = 0.0;
	u1 = 0.0;
	u2 = 0.0;
	u3 = 0.0;
	u4 = 0.0;
	s1 = 0.0;
	s2 = 0.0;

	std::cout << "Initial f1 = " << f1
		<< ", f2 = " << f2
		<< ", f3 = " << f3
		<< ", f4 = " << f4
		<< ", u1 = " << u1
		<< ", u2 = " << u2
		<< ", u3 = " << u3
		<< ", u4 = " << u4
		<< ", s1 = " << s1
		<< ", s2 = " << s2
		<< std::endl;

	// Solve all my problems
	ceres::Solve(options, &problem, &summary);

	std::cout << summary.FullReport() << std::endl;
	std::cout << "Final f1 = " << f1
		<< ", f2 = " << f2
		<< ", f3 = " << f3
		<< ", f4 = " << f4
		<< ", u1 = " << u1
		<< ", u2 = " << u2
		<< ", u3 = " << u3
		<< ", u4 = " << u4
		<< ", s1 = " << s1
		<< ", s2 = " << s2
		<< std::endl;

	// Get time
	ros::Time time = ros::Time::now();

	// Create stamped thrust message
	thrust_msg.header.stamp = time;
	thrust_msg.thrust.f1 = f1;
	thrust_msg.thrust.f2 = f2;
	thrust_msg.thrust.f3 = f3;
	thrust_msg.thrust.f4 = f4;
	thrust_msg.thrust.u1 = u1;
	thrust_msg.thrust.u2 = u2;
	thrust_msg.thrust.u3 = u3;
	thrust_msg.thrust.u4 = u4;
	thrust_msg.thrust.s1 = s1;
	thrust_msg.thrust.s2 = s2;
	forces.publish(thrust_msg);
}

void Solver::loop()
{
	ros::spin();
}
<commit_msg>Fixed sway thruster error.<commit_after>#include "ros/ros.h"
#include "geometry_msgs/Accel.h"
#include "geometry_msgs/Vector3.h"
#include "sensor_msgs/JointState.h"
#include "riptide_msgs/ThrustStamped.h"
#include <math.h>
#include <vector>
#include "ceres/ceres.h"
#include "glog/logging.h"

#undef debug

double mass = 24.9476;	// Mass of the vehicle (KG)

double desiredSurge = 0.0; // Desired linear acceleration in X (m/s^2)
double desiredSway = 0.0; // Desired linear acceleration in Y (m/s^2)
double desiredHeave = 0.0; // Desired linear acceleration in Z (m/s^2)

double desiredRoll = 0.0; // Desired angular acceleration about X (rad/s^2)
double desiredPitch = 0.0; // Desired angular acceleration about Y (rad/s^2)
double desiredYaw = 0.0; // Desired angular acceleration about Z (rad/s^2)

double IX = 1.0; // Moment of inertia about X
double IY = 1.0; // Moment of inertia about Y
double IZ = 1.0; // Moment of inertia about Z

struct vector
{
		double x;
		double y;
		double z;
};

vector makeVector(double X, double Y, double Z)
{
	vector v;
	v.x = X;
	v.y = Y;
	v.z = Z;
	return v;
}

/*** Thruster Positions ***/
// Positions are in meters relative to the center of mass.
// Forward Thrusters
vector pos_u1 = makeVector(-.25, .127, -.05);
vector pos_u2 = makeVector(-.25, -.127, -.05);
vector pos_u3 = makeVector(.25, -.127, -.05);
vector pos_u4 = makeVector(.25, .127, -.05);
// Upwards Thrusters
vector pos_f1 = makeVector(-.3, -.127, 1.0);
vector pos_f2 = makeVector(-.3, .127, 1.0);
vector pos_f3 = makeVector(-.3, .127, -1.0);
vector pos_f4 = makeVector(-.3, -.127, -1.0);
// Sideways Thrusters
vector pos_s1 = makeVector(.5, 0.0, -1.0);
vector pos_s2 = makeVector(-.5, 0.0, -1.0);

/*** Thruster Definitions ***/
// f1 = forward top right		u1 = vertical back left		s1 = sideways front
// f2 = forward top left		u2 = vertical back right	s2 = sideways back
// f3 = forward bottom left		u3 = vertical front right
// f4 = forward bottom right	u4 = vertical front left

/*** EQUATIONS ***/
// These equations solve for linear/angular acceleration in all axes
// Linear Equations
struct a_surge {
	template <typename T> bool operator()(const T* const f1,
		const T* const f2,
		const T* const f3,
		const T* const f4,
		T* residual) const {
		residual[0] = (f1[0] + f2[0] + f3[0] + f4[0]) / T(mass) - T(desiredSurge); // Sum forces and subtract desired
		return true;
	}
};

struct a_sway {
	template <typename T> bool operator()(const T* const s1,
		const T* const s2,
		T* residual) const {
		residual[0] = (s1[0] + s2[0]) / T(mass) - T(desiredSway);
		return true;
	}
};

struct a_heave {
	template <typename T> bool operator()(const T* const u1,
		const T* const u2,
		const T* const u3,
		const T* const u4,
		T* residual) const {
		residual[0] = (u1[0] + u2[0] + u3[0] + u4[0]) / T(mass) - T(desiredHeave);
		return true;
	}
};

// Angular equations
struct a_roll {
	template <typename T> bool operator()(const T* const u1,
		const T* const u2,
		const T* const u3,
		const T* const u4,
		const T* const s1,
		const T* const s2,
		T* residual) const {
		residual[0] = (u1[0]*T(pos_u1.y)+u2[0]*T(pos_u2.y)+u3[0]*T(pos_u3.y)+u4[0]*T(pos_u4.y)+s1[0]*T(pos_s1.z)+s2[0]*T(pos_s2.z)) / T(IX) - T(desiredRoll);
		return true;
	}
};

struct a_pitch {
	template <typename T> bool operator()(const T* const f1,
		const T* const f2,
		const T* const f3,
		const T* const f4,
		const T* const u1,
		const T* const u2,
		const T* const u3,
		const T* const u4,
		T* residual) const {
		residual[0] = (f1[0] * T(pos_f1.z) + f2[0]*T(pos_f2.z) + f3[0]*T(pos_f3.z) + f4[0]*T(pos_f4.z) + u1[0]*T(pos_u1.x) + u2[0]*T(pos_u2.x) + u3[0]*T(pos_u3.x) + u4[0]*T(pos_u4.x)) / T(IY) - T(desiredPitch);
		return true;
	}
};

struct a_yaw{
	template <typename T> bool operator()(const T* const f1,
		const T* const f2,
		const T* const f3,
		const T* const f4,
		const T* const s1,
		const T* const s2,
		T* residual) const {
	residual[0] = (f1[0]*T(pos_f1.y) + f2[0]*T(pos_f2.y) + f3[0]*T(pos_f3.y) + f4[0]*T(pos_f4.y) + s1[0]*T(pos_s1.x) + s2[0]*T(pos_s2.x)) / T(IZ) - T(desiredYaw);
	return true;
}
};

// Solver class
class Solver
{
private:
	ros::NodeHandle nh;
	ros::Subscriber accels;
	ros::Publisher forces;
	riptide_msgs::ThrustStamped thrust_msg;
	double f1, f2, f3, f4;
	double u1, u2, u3, u4;
	double s1, s2;
	ceres::Problem problem;
	ceres::Solver::Options options;
	ceres::Solver::Summary summary;

public:
	Solver(char** argv);
	void callback(const geometry_msgs::Accel::ConstPtr& a);
	void loop();
};

int main(int argc, char **argv)
{
	ros::init(argc, argv, "thrust_solver");
	Solver solver(argv);
	solver.loop();
}

Solver::Solver(char** argv)
{
	accels = nh.subscribe<geometry_msgs::Accel>("accel_error", 1, &Solver::callback, this); // Message to give desired accelerations
	forces = nh.advertise<riptide_msgs::ThrustStamped>("solver/thrust", 1); // Message containing force required from each thruster to achieve given acceleration

	google::InitGoogleLogging(argv[0]);

	// PROBLEM SETUP
	// Add residual blocks (equations)
	// Linear
	problem.AddResidualBlock(new ceres::AutoDiffCostFunction<a_surge, 1, 1, 1, 1, 1>(new a_surge),
		NULL,
		&f1, &f2, &f3, &f4);
	problem.AddResidualBlock(new ceres::AutoDiffCostFunction<a_sway, 1, 1, 1>(new a_sway),
		NULL,
		&s1, &s2);
	problem.AddResidualBlock(new ceres::AutoDiffCostFunction<a_heave, 1, 1, 1, 1, 1>(new a_heave),
		NULL,
		&u1, &u2, &u3, &u4);
	// Angular
	problem.AddResidualBlock(new ceres::AutoDiffCostFunction<a_roll, 1, 1, 1, 1, 1, 1, 1>(new a_roll),
		NULL,
		&u1, &u2, &u3, &u4, &s1, &s2);
	problem.AddResidualBlock(new ceres::AutoDiffCostFunction<a_pitch, 1, 1, 1, 1, 1, 1, 1, 1, 1>(new a_pitch),
		NULL,
		&f1, &f2, &f3, &f4, &u1, &u2, &u3, &u4);
	problem.AddResidualBlock(new ceres::AutoDiffCostFunction<a_yaw, 1, 1, 1, 1, 1, 1, 1>(new a_yaw),
		NULL,
		&f1, &f2, &f3, &f4, &s1, &s2);
	// Set constraints (min, max thruster force)
	// Forward thrusters
	problem.SetParameterLowerBound(&f1, 0, -25.0);
	problem.SetParameterUpperBound(&f1, 0, 25.0);

	problem.SetParameterLowerBound(&f2, 0, -25.0);
	problem.SetParameterUpperBound(&f2, 0, 25.0);

	problem.SetParameterLowerBound(&f3, 0, -25.0);
	problem.SetParameterUpperBound(&f3, 0, 25.0);

	problem.SetParameterLowerBound(&f4, 0, -25.0);
	problem.SetParameterUpperBound(&f4, 0, 25.0);
	// Upward thrusters
	problem.SetParameterLowerBound(&u1, 0, -25.0);
	problem.SetParameterUpperBound(&u1, 0, 25.0);

	problem.SetParameterLowerBound(&u2, 0, -25.0);
	problem.SetParameterUpperBound(&u2, 0, 25.0);

	problem.SetParameterLowerBound(&u3, 0, -25.0);
	problem.SetParameterUpperBound(&u3, 0, 25.0);

	problem.SetParameterLowerBound(&u4, 0, -25.0);
	problem.SetParameterUpperBound(&u4, 0, 25.0);
	// Sideways thrusters
	problem.SetParameterLowerBound(&s1, 0, -25.0);
	problem.SetParameterUpperBound(&s1, 0, 25.0);

	problem.SetParameterLowerBound(&s2, 0, -25.0);
	problem.SetParameterUpperBound(&s2, 0, 25.0);

	// Configure solver
	options.max_num_iterations = 100;
	options.linear_solver_type = ceres::DENSE_QR;

#ifdef debug
	options.minimizer_progress_to_stdout = true;
#endif
}

void Solver::callback(const geometry_msgs::Accel::ConstPtr& a)
{
	desiredSurge = a->linear.x;
	desiredSway = a->linear.y;
	desiredHeave = a->linear.z;

	desiredRoll = a->angular.x;
	desiredPitch = a->angular.y;
	desiredYaw = a->angular.z;

	// These forced initial guesses don't make much of a difference.
	// We currently experience a sort of gimbal lock w/ or w/o them.
	f1 = 0.0;
	f2 = 0.0;
	f3 = 0.0;
	f4 = 0.0;
	u1 = 0.0;
	u2 = 0.0;
	u3 = 0.0;
	u4 = 0.0;
	s1 = 0.0;
	s2 = 0.0;

	std::cout << "Initial f1 = " << f1
		<< ", f2 = " << f2
		<< ", f3 = " << f3
		<< ", f4 = " << f4
		<< ", u1 = " << u1
		<< ", u2 = " << u2
		<< ", u3 = " << u3
		<< ", u4 = " << u4
		<< ", s1 = " << s1
		<< ", s2 = " << s2
		<< std::endl;

	// Solve all my problems
	ceres::Solve(options, &problem, &summary);

	std::cout << summary.FullReport() << std::endl;
	std::cout << "Final f1 = " << f1
		<< ", f2 = " << f2
		<< ", f3 = " << f3
		<< ", f4 = " << f4
		<< ", u1 = " << u1
		<< ", u2 = " << u2
		<< ", u3 = " << u3
		<< ", u4 = " << u4
		<< ", s1 = " << s1
		<< ", s2 = " << s2
		<< std::endl;

	// Get time
	ros::Time time = ros::Time::now();

	// Create stamped thrust message
	thrust_msg.header.stamp = time;
	thrust_msg.thrust.f1 = f1;
	thrust_msg.thrust.f2 = f2;
	thrust_msg.thrust.f3 = f3;
	thrust_msg.thrust.f4 = f4;
	thrust_msg.thrust.u1 = u1;
	thrust_msg.thrust.u2 = u2;
	thrust_msg.thrust.u3 = u3;
	thrust_msg.thrust.u4 = u4;
	thrust_msg.thrust.s1 = s1;
	thrust_msg.thrust.s2 = s2;
	forces.publish(thrust_msg);
}

void Solver::loop()
{
	ros::spin();
}
<|endoftext|>


<commit_before>#include <QtCore/QEventLoop>
#include <QtCore/QVariant>
#include <QtTest/QtTest>

#include "json/qjsondocument.h"
#include "qjsonrpcservice.h"
#include "qjsonrpcmessage.h"

class TestQJsonRpcService: public QObject
{
    Q_OBJECT
private Q_SLOTS:
    void initTestCase();
    void cleanupTestCase();
    void init();
    void cleanup();
    void testSocket();
    void testServiceProviderValidRequests();
    void testServiceProviderInvalidRequests();
};

class TestService : public QJsonRpcService
{
    Q_OBJECT
public:
    TestService(QObject *parent = 0)
        : QJsonRpcService(parent)
    {
    }

    ~TestService()
    {
    }

    QString serviceName() const
    {
        return QString("service");
    }

public Q_SLOTS:
    void noParam() const
    {
    }

    QString singleParam(const QString &string) const
    {
        return string;
    }

    QString multipleParam(const QString &first,
                          const QString &second,
                          const QString &third) const
    {
        return first + second + third;
    }
};

void TestQJsonRpcService::initTestCase()
{
    qRegisterMetaType<QJsonRpcMessage>("QJsonRpcMessage");
}

void TestQJsonRpcService::cleanupTestCase()
{
}

void TestQJsonRpcService::init()
{
}

void TestQJsonRpcService::cleanup()
{
}

void TestQJsonRpcService::testSocket()
{
    // No parameters
    {
        QBuffer buffer;
        buffer.open(QIODevice::ReadWrite);
        QJsonRpcServiceSocket serviceSocket(&buffer, this);
        QSignalSpy spyMessageReceived(&serviceSocket,
                                      SIGNAL(messageReceived(QJsonRpcMessage)));
        QVERIFY(serviceSocket.isValid());

        QJsonRpcMessage request = QJsonRpcMessage::createRequest(QString("test.noParam"));

        QJsonRpcServiceReply *reply;
        reply = serviceSocket.sendMessage(request);

        QJsonDocument document = QJsonDocument::fromJson(buffer.data());
        QVERIFY(!document.isEmpty());
        QJsonRpcMessage bufferMessage(document.object());

        QCOMPARE(request.id(), bufferMessage.id());
        QCOMPARE(request.type(), bufferMessage.type());
        QCOMPARE(request.method(), bufferMessage.method());
        QCOMPARE(request.params(), bufferMessage.params());
        QCOMPARE(spyMessageReceived.count(), 0);
    }

    // Multiparameter
    {
        QBuffer buffer;
        buffer.open(QIODevice::ReadWrite);
        QJsonRpcServiceSocket serviceSocket(&buffer, this);
        QSignalSpy spyMessageReceived(&serviceSocket,
                                      SIGNAL(messageReceived(QJsonRpcMessage)));
        QVERIFY(serviceSocket.isValid());

        QJsonRpcMessage request = QJsonRpcMessage::createRequest(QString("test.multiParam"),
                                                                 QVariantList() << false << true);

        QJsonRpcServiceReply *reply;
        reply = serviceSocket.sendMessage(request);

        QJsonDocument document = QJsonDocument::fromJson(buffer.data());
        QVERIFY(!document.isEmpty());
        QJsonRpcMessage bufferMessage(document.object());

        QCOMPARE(request.id(), bufferMessage.id());
        QCOMPARE(request.type(), bufferMessage.type());
        QCOMPARE(request.method(), bufferMessage.method());
        QCOMPARE(request.params(), bufferMessage.params());
        QCOMPARE(spyMessageReceived.count(), 0);
    }

    // Notification
    {
        QBuffer buffer;
        buffer.open(QIODevice::ReadWrite);
        QJsonRpcServiceSocket serviceSocket(&buffer, this);
        QSignalSpy spyMessageReceived(&serviceSocket,
                                      SIGNAL(messageReceived(QJsonRpcMessage)));
        QVERIFY(serviceSocket.isValid());

        QJsonRpcMessage notification = QJsonRpcMessage::createNotification("test.notify");

        QJsonRpcServiceReply *reply;
        reply = serviceSocket.sendMessage(notification);

        QJsonDocument document = QJsonDocument::fromJson(buffer.data());
        QVERIFY(!document.isEmpty());
        QJsonRpcMessage bufferMessage(document.object());

        QCOMPARE(notification.id(), bufferMessage.id());
        QCOMPARE(notification.type(), bufferMessage.type());
        QCOMPARE(notification.method(), bufferMessage.method());
        QCOMPARE(notification.params(), bufferMessage.params());
        QCOMPARE(spyMessageReceived.count(), 0);
    }

    // Response
    {
        QBuffer buffer;
        buffer.open(QIODevice::ReadWrite);
        QJsonRpcServiceSocket serviceSocket(&buffer, this);
        QSignalSpy spyMessageReceived(&serviceSocket,
                                      SIGNAL(messageReceived(QJsonRpcMessage)));
        QVERIFY(serviceSocket.isValid());

        QJsonRpcMessage response = QJsonRpcMessage::createRequest(QString("test.response"));
        response = response.createResponse(QVariant());

        QJsonRpcServiceReply *reply;
        reply = serviceSocket.sendMessage(response);

        QJsonDocument document = QJsonDocument::fromJson(buffer.data());
        QVERIFY(!document.isEmpty());
        QJsonRpcMessage bufferMessage(document.object());

        QCOMPARE(response.id(), bufferMessage.id());
        QCOMPARE(response.type(), bufferMessage.type());
        QCOMPARE(response.method(), bufferMessage.method());
        QCOMPARE(response.params(), bufferMessage.params());
        QCOMPARE(spyMessageReceived.count(), 0);
    }
}

void TestQJsonRpcService::testServiceProviderValidRequests()
{
    // Initialize the service provider.
    QEventLoop loop;
    TestService service;
    QJsonRpcServiceProvider serviceProvider;
    serviceProvider.addService(&service);
    QVERIFY(serviceProvider.listen("test"));

    // Connect to the socket.
    QLocalSocket socket;
    socket.connectToServer("test");
    QJsonRpcServiceSocket serviceSocket(&socket, this);
    QSignalSpy spyMessageReceived(&serviceSocket,
                                  SIGNAL(messageReceived(QJsonRpcMessage)));

    // No parameter
    QJsonRpcMessage request = QJsonRpcMessage::createRequest("service.noParam");
    QJsonRpcServiceReply *reply = serviceSocket.sendMessage(request);
    connect(&serviceSocket, SIGNAL(messageReceived(QJsonRpcMessage)), &loop, SLOT(quit()));
    connect(reply, SIGNAL(finished()), &loop, SLOT(quit()));
    loop.exec();

    QJsonRpcMessage response = reply->response();
    QCOMPARE(spyMessageReceived.count(), 0);
    QVERIFY(response.errorCode() == 0);
    QCOMPARE(request.id(), response.id());

    // Single parameter
    request = QJsonRpcMessage::createRequest("service.singleParam", QString("single"));
    reply = serviceSocket.sendMessage(request);
    connect(&serviceSocket, SIGNAL(messageReceived(QJsonRpcMessage)), &loop, SLOT(quit()));
    connect(reply, SIGNAL(finished()), &loop, SLOT(quit()));
    loop.exec();

    response = reply->response();
    QCOMPARE(spyMessageReceived.count(), 0);
    QVERIFY(response.errorCode() == 0);
    QCOMPARE(request.id(), response.id());
    QVERIFY(response.result() == QString("single"));

    // Multiparameter
    request = QJsonRpcMessage::createRequest("service.multipleParam",
                                             QVariantList() << QVariant(QString("a"))
                                                            << QVariant(QString("b"))
                                                            << QVariant(QString("c")));
    reply = serviceSocket.sendMessage(request);
    connect(&serviceSocket, SIGNAL(messageReceived(QJsonRpcMessage)), &loop, SLOT(quit()));
    connect(reply, SIGNAL(finished()), &loop, SLOT(quit()));
    loop.exec();

    response = reply->response();
    QCOMPARE(spyMessageReceived.count(), 0);
    QVERIFY(response.errorCode() == 0);
    QCOMPARE(request.id(), response.id());
    QVERIFY(response.result() == QString("abc"));
}

void TestQJsonRpcService::testServiceProviderInvalidRequests()
{
    // Initialize the service provider.
    QEventLoop loop;
    TestService service;
    QJsonRpcServiceProvider serviceProvider;
    serviceProvider.addService(&service);
    QVERIFY(serviceProvider.listen("test"));

    // Connect to the socket.
    QLocalSocket socket;
    socket.connectToServer("test");
    QJsonRpcServiceSocket serviceSocket(&socket, this);
    QSignalSpy spyMessageReceived(&serviceSocket,
                                  SIGNAL(messageReceived(QJsonRpcMessage)));

    // Failing request (Invalid args)
    QJsonRpcMessage request = QJsonRpcMessage::createRequest("service.noParam",
                                                             QVariantList() << false);
    QJsonRpcServiceReply *reply = serviceSocket.sendMessage(request);
    connect(&serviceSocket, SIGNAL(messageReceived(QJsonRpcMessage)), &loop, SLOT(quit()));
    connect(reply, SIGNAL(finished()), &loop, SLOT(quit()));
    loop.exec();

    QCOMPARE(spyMessageReceived.count(), 1);
    if (spyMessageReceived.count() == 1) {
        QVariant message = spyMessageReceived.takeFirst().at(0);
        QJsonRpcMessage error = message.value<QJsonRpcMessage>();
        QCOMPARE(request.id(), error.id());
        QVERIFY(error.errorCode() == QJsonRpc::InvalidParams);
    }

    // Failing request (Method not found)
    request = QJsonRpcMessage::createRequest("service.doesNotExist");
    reply = serviceSocket.sendMessage(request);
    connect(&serviceSocket, SIGNAL(messageReceived(QJsonRpcMessage)), &loop, SLOT(quit()));
    connect(reply, SIGNAL(finished()), &loop, SLOT(quit()));
    loop.exec();

    QCOMPARE(spyMessageReceived.count(), 1);
    if (spyMessageReceived.count() == 1) {
        QVariant message = spyMessageReceived.takeFirst().at(0);
        QJsonRpcMessage error = message.value<QJsonRpcMessage>();
        QCOMPARE(request.id(), error.id());
        QVERIFY(error.errorCode() == QJsonRpc::MethodNotFound);
    }
}

QTEST_MAIN(TestQJsonRpcService)
#include "tst_qjsonrpcservice.moc"
<commit_msg>Split up test cases in QJsonRpcService test.<commit_after>#include <QtCore/QEventLoop>
#include <QtCore/QVariant>
#include <QtTest/QtTest>

#include "json/qjsondocument.h"
#include "qjsonrpcservice.h"
#include "qjsonrpcmessage.h"

class TestQJsonRpcService: public QObject
{
    Q_OBJECT
private Q_SLOTS:
    void initTestCase();
    void cleanupTestCase();
    void init();
    void cleanup();
    void testSocketNoParameters();
    void testSocketMultiparamter();
    void testSocketNotification();
    void testSocketResponse();
    void testServiceProviderNoParameter();
    void testServiceProviderSingleParameter();
    void testServiceProviderMultiparameter();
    void testServiceProviderInvalidArgs();
    void testServiceProviderMethodNotFound();
};

class TestService : public QJsonRpcService
{
    Q_OBJECT
public:
    TestService(QObject *parent = 0)
        : QJsonRpcService(parent)
    {
    }

    ~TestService()
    {
    }

    QString serviceName() const
    {
        return QString("service");
    }

public Q_SLOTS:
    void noParam() const
    {
    }

    QString singleParam(const QString &string) const
    {
        return string;
    }

    QString multipleParam(const QString &first,
                          const QString &second,
                          const QString &third) const
    {
        return first + second + third;
    }
};

void TestQJsonRpcService::initTestCase()
{
    qRegisterMetaType<QJsonRpcMessage>("QJsonRpcMessage");
}

void TestQJsonRpcService::cleanupTestCase()
{
}

void TestQJsonRpcService::init()
{
}

void TestQJsonRpcService::cleanup()
{
}

void TestQJsonRpcService::testSocketNoParameters()
{
    QBuffer buffer;
    buffer.open(QIODevice::ReadWrite);
    QJsonRpcServiceSocket serviceSocket(&buffer, this);
    QSignalSpy spyMessageReceived(&serviceSocket,
                                  SIGNAL(messageReceived(QJsonRpcMessage)));
    QVERIFY(serviceSocket.isValid());

    QJsonRpcMessage request = QJsonRpcMessage::createRequest(QString("test.noParam"));

    QJsonRpcServiceReply *reply;
    reply = serviceSocket.sendMessage(request);

    QJsonDocument document = QJsonDocument::fromJson(buffer.data());
    QVERIFY(!document.isEmpty());
    QJsonRpcMessage bufferMessage(document.object());

    QCOMPARE(request.id(), bufferMessage.id());
    QCOMPARE(request.type(), bufferMessage.type());
    QCOMPARE(request.method(), bufferMessage.method());
    QCOMPARE(request.params(), bufferMessage.params());
    QCOMPARE(spyMessageReceived.count(), 0);
}

void TestQJsonRpcService::testSocketMultiparamter()
{
    QBuffer buffer;
    buffer.open(QIODevice::ReadWrite);
    QJsonRpcServiceSocket serviceSocket(&buffer, this);
    QSignalSpy spyMessageReceived(&serviceSocket,
                                  SIGNAL(messageReceived(QJsonRpcMessage)));
    QVERIFY(serviceSocket.isValid());

    QJsonRpcMessage request = QJsonRpcMessage::createRequest(QString("test.multiParam"),
                                                             QVariantList() << false << true);

    QJsonRpcServiceReply *reply;
    reply = serviceSocket.sendMessage(request);

    QJsonDocument document = QJsonDocument::fromJson(buffer.data());
    QVERIFY(!document.isEmpty());
    QJsonRpcMessage bufferMessage(document.object());

    QCOMPARE(request.id(), bufferMessage.id());
    QCOMPARE(request.type(), bufferMessage.type());
    QCOMPARE(request.method(), bufferMessage.method());
    QCOMPARE(request.params(), bufferMessage.params());
    QCOMPARE(spyMessageReceived.count(), 0);
}

void TestQJsonRpcService::testSocketNotification()
{
    QBuffer buffer;
    buffer.open(QIODevice::ReadWrite);
    QJsonRpcServiceSocket serviceSocket(&buffer, this);
    QSignalSpy spyMessageReceived(&serviceSocket,
                                  SIGNAL(messageReceived(QJsonRpcMessage)));
    QVERIFY(serviceSocket.isValid());

    QJsonRpcMessage notification = QJsonRpcMessage::createNotification("test.notify");

    QJsonRpcServiceReply *reply;
    reply = serviceSocket.sendMessage(notification);

    QJsonDocument document = QJsonDocument::fromJson(buffer.data());
    QVERIFY(!document.isEmpty());
    QJsonRpcMessage bufferMessage(document.object());

    QCOMPARE(notification.id(), bufferMessage.id());
    QCOMPARE(notification.type(), bufferMessage.type());
    QCOMPARE(notification.method(), bufferMessage.method());
    QCOMPARE(notification.params(), bufferMessage.params());
    QCOMPARE(spyMessageReceived.count(), 0);
}

void TestQJsonRpcService::testSocketResponse()
{
    QBuffer buffer;
    buffer.open(QIODevice::ReadWrite);
    QJsonRpcServiceSocket serviceSocket(&buffer, this);
    QSignalSpy spyMessageReceived(&serviceSocket,
                                  SIGNAL(messageReceived(QJsonRpcMessage)));
    QVERIFY(serviceSocket.isValid());

    QJsonRpcMessage response = QJsonRpcMessage::createRequest(QString("test.response"));
    response = response.createResponse(QVariant());

    QJsonRpcServiceReply *reply;
    reply = serviceSocket.sendMessage(response);

    QJsonDocument document = QJsonDocument::fromJson(buffer.data());
    QVERIFY(!document.isEmpty());
    QJsonRpcMessage bufferMessage(document.object());

    QCOMPARE(response.id(), bufferMessage.id());
    QCOMPARE(response.type(), bufferMessage.type());
    QCOMPARE(response.method(), bufferMessage.method());
    QCOMPARE(response.params(), bufferMessage.params());
    QCOMPARE(spyMessageReceived.count(), 0);
}

void TestQJsonRpcService::testServiceProviderNoParameter()
{
    // Initialize the service provider.
    QEventLoop loop;
    TestService service;
    QJsonRpcServiceProvider serviceProvider;
    serviceProvider.addService(&service);
    QVERIFY(serviceProvider.listen("test"));

    // Connect to the socket.
    QLocalSocket socket;
    socket.connectToServer("test");
    QJsonRpcServiceSocket serviceSocket(&socket, this);
    QSignalSpy spyMessageReceived(&serviceSocket,
                                  SIGNAL(messageReceived(QJsonRpcMessage)));

    QJsonRpcMessage request = QJsonRpcMessage::createRequest("service.noParam");
    QJsonRpcServiceReply *reply = serviceSocket.sendMessage(request);
    connect(&serviceSocket, SIGNAL(messageReceived(QJsonRpcMessage)), &loop, SLOT(quit()));
    connect(reply, SIGNAL(finished()), &loop, SLOT(quit()));
    loop.exec();

    QJsonRpcMessage response = reply->response();
    QCOMPARE(spyMessageReceived.count(), 0);
    QVERIFY(response.errorCode() == 0);
    QCOMPARE(request.id(), response.id());
}

void TestQJsonRpcService::testServiceProviderSingleParameter()
{
    // Initialize the service provider.
    QEventLoop loop;
    TestService service;
    QJsonRpcServiceProvider serviceProvider;
    serviceProvider.addService(&service);
    QVERIFY(serviceProvider.listen("test"));

    // Connect to the socket.
    QLocalSocket socket;
    socket.connectToServer("test");
    QJsonRpcServiceSocket serviceSocket(&socket, this);
    QSignalSpy spyMessageReceived(&serviceSocket,
                                  SIGNAL(messageReceived(QJsonRpcMessage)));

    QJsonRpcMessage request = QJsonRpcMessage::createRequest("service.singleParam", QString("single"));
    QJsonRpcServiceReply *reply = serviceSocket.sendMessage(request);
    connect(&serviceSocket, SIGNAL(messageReceived(QJsonRpcMessage)), &loop, SLOT(quit()));
    connect(reply, SIGNAL(finished()), &loop, SLOT(quit()));
    loop.exec();

    QJsonRpcMessage response = reply->response();
    QCOMPARE(spyMessageReceived.count(), 0);
    QVERIFY(response.errorCode() == 0);
    QCOMPARE(request.id(), response.id());
    QVERIFY(response.result() == QString("single"));
}

void TestQJsonRpcService::testServiceProviderMultiparameter()
{
    // Initialize the service provider.
    QEventLoop loop;
    TestService service;
    QJsonRpcServiceProvider serviceProvider;
    serviceProvider.addService(&service);
    QVERIFY(serviceProvider.listen("test"));

    // Connect to the socket.
    QLocalSocket socket;
    socket.connectToServer("test");
    QJsonRpcServiceSocket serviceSocket(&socket, this);
    QSignalSpy spyMessageReceived(&serviceSocket,
                                  SIGNAL(messageReceived(QJsonRpcMessage)));

    QJsonRpcMessage request = QJsonRpcMessage::createRequest("service.multipleParam",
                                                             QVariantList() << QVariant(QString("a"))
                                                                            << QVariant(QString("b"))
                                                                            << QVariant(QString("c")));
    QJsonRpcServiceReply *reply = serviceSocket.sendMessage(request);
    connect(&serviceSocket, SIGNAL(messageReceived(QJsonRpcMessage)), &loop, SLOT(quit()));
    connect(reply, SIGNAL(finished()), &loop, SLOT(quit()));
    loop.exec();

    QJsonRpcMessage response = reply->response();
    QCOMPARE(spyMessageReceived.count(), 0);
    QVERIFY(response.errorCode() == 0);
    QCOMPARE(request.id(), response.id());
    QVERIFY(response.result() == QString("abc"));
}

void TestQJsonRpcService::testServiceProviderInvalidArgs()
{
    // Initialize the service provider.
    QEventLoop loop;
    TestService service;
    QJsonRpcServiceProvider serviceProvider;
    serviceProvider.addService(&service);
    QVERIFY(serviceProvider.listen("test"));

    // Connect to the socket.
    QLocalSocket socket;
    socket.connectToServer("test");
    QJsonRpcServiceSocket serviceSocket(&socket, this);
    QSignalSpy spyMessageReceived(&serviceSocket,
                                  SIGNAL(messageReceived(QJsonRpcMessage)));

    QJsonRpcMessage request = QJsonRpcMessage::createRequest("service.noParam",
                                                             QVariantList() << false);
    QJsonRpcServiceReply *reply = serviceSocket.sendMessage(request);
    connect(&serviceSocket, SIGNAL(messageReceived(QJsonRpcMessage)), &loop, SLOT(quit()));
    connect(reply, SIGNAL(finished()), &loop, SLOT(quit()));
    loop.exec();

    QCOMPARE(spyMessageReceived.count(), 1);
    if (spyMessageReceived.count() == 1) {
        QVariant message = spyMessageReceived.takeFirst().at(0);
        QJsonRpcMessage error = message.value<QJsonRpcMessage>();
        QCOMPARE(request.id(), error.id());
        QVERIFY(error.errorCode() == QJsonRpc::InvalidParams);
    }
}

void TestQJsonRpcService::testServiceProviderMethodNotFound()
{
    // Initialize the service provider.
    QEventLoop loop;
    TestService service;
    QJsonRpcServiceProvider serviceProvider;
    serviceProvider.addService(&service);
    QVERIFY(serviceProvider.listen("test"));

    // Connect to the socket.
    QLocalSocket socket;
    socket.connectToServer("test");
    QJsonRpcServiceSocket serviceSocket(&socket, this);
    QSignalSpy spyMessageReceived(&serviceSocket,
                                  SIGNAL(messageReceived(QJsonRpcMessage)));

    QJsonRpcMessage request = QJsonRpcMessage::createRequest("service.doesNotExist");
    QJsonRpcServiceReply *reply = serviceSocket.sendMessage(request);
    connect(&serviceSocket, SIGNAL(messageReceived(QJsonRpcMessage)), &loop, SLOT(quit()));
    connect(reply, SIGNAL(finished()), &loop, SLOT(quit()));
    loop.exec();

    QCOMPARE(spyMessageReceived.count(), 1);
    if (spyMessageReceived.count() == 1) {
        QVariant message = spyMessageReceived.takeFirst().at(0);
        QJsonRpcMessage error = message.value<QJsonRpcMessage>();
        QCOMPARE(request.id(), error.id());
        QVERIFY(error.errorCode() == QJsonRpc::MethodNotFound);
    }
}

QTEST_MAIN(TestQJsonRpcService)
#include "tst_qjsonrpcservice.moc"
<|endoftext|>


<commit_before>/****************************************************************************
**
** Copyright (c) 2008-2009 Nokia Corporation and/or its subsidiary(-ies).
** Contact: Nokia Corporation (qt-info@nokia.com)
**
** This file is part of the Qt Mobility Components.
**
** $QT_BEGIN_LICENSE:LGPL$
** No Commercial Usage
** This file contains pre-release code and may not be distributed.
** You may use this file in accordance with the terms and conditions
** contained in Technology Preview License Agreement accompanying
** this package.
**
** GNU Lesser General Public License Usage
** Alternatively, this file may be used under the terms of the GNU Lesser
** General Public License version 2.1 as published by the Free Software
** Foundation and appearing in the file LICENSE.LGPL included in the
** packaging of this file.  Please review the following information to
** ensure the GNU Lesser General Public License version 2.1 requirements
** will be met: http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
**
** In addition, as a special exception, Nokia gives you certain
** additional rights. These rights are described in the Nokia Qt LGPL
** Exception version 1.0, included in the file LGPL_EXCEPTION.txt in this
** package.
**
** If you have questions regarding the use of this file, please
** contact Nokia at http://qt.nokia.com/contact.
** $QT_END_LICENSE$
**
****************************************************************************/

#include <qpacketprotocol.h>

#include <QByteArray>
#include <QBuffer>
#include <QLocalServer>
#include <QLocalSocket>

#include <QtTest/QTest>
#include <QSignalSpy>

#include <QDebug>

#define QTRY_COMPARE(a,e)                       \
    for (int _i = 0; _i < 5000; _i += 100) {    \
        if ((a) == (e)) break;                  \
        QTest::qWait(100);                      \
    }                                           \
    QCOMPARE(a, e)

#define QTRY_VERIFY(a)                       \
    for (int _i = 0; _i < 5000; _i += 100) {    \
        if (a) break;                  \
        QTest::qWait(100);                      \
    }                                           \
    QVERIFY(a)

Q_DECLARE_METATYPE(QList<int>)

class tst_QPacketProtocol : public QObject
{
    Q_OBJECT

private slots:
    void constructor();

    void maximumPacketSize_data();
    void maximumPacketSize();

    void sendReceive();
};

void tst_QPacketProtocol::constructor()
{
    QByteArray data;
    QBuffer buffer(&data);

    {
        QPacketProtocol protocol(&buffer);

        QCOMPARE(protocol.device(), &buffer);
    }
}

#define ROW_FOR_SIZE(x) x << x << (QList<int>() << 1 << 1 << (x - 4) << 1 << (x - 3) << 0)

void tst_QPacketProtocol::maximumPacketSize_data()
{
    QTest::addColumn<qint32>("maxPacketSize");
    QTest::addColumn<qint32>("actualMaxPacketSize");
    QTest::addColumn<QList<int> >("packetSizes");    // pairs of packetSize, readCount

    QTest::newRow("10MB") << ROW_FOR_SIZE(10 * 1024 * 1024);
    QTest::newRow("1MB") << ROW_FOR_SIZE(1024 * 1024);
    QTest::newRow("100kB") << ROW_FOR_SIZE(100 * 1024);
    QTest::newRow("10kB") << ROW_FOR_SIZE(10 * 1024);
    QTest::newRow("1kB") << ROW_FOR_SIZE(1024);
    QTest::newRow("100B") << ROW_FOR_SIZE(100);
    QTest::newRow("10B") << ROW_FOR_SIZE(10);
    QTest::newRow("5B") << ROW_FOR_SIZE(5);
    QTest::newRow("4B") << 4 << 0x7FFFFFFF << QList<int>();
    QTest::newRow("0B") << 0 << 0x7FFFFFFF << QList<int>();
    QTest::newRow("-1") << -1 << 0x7FFFFFFF << QList<int>();
}

void tst_QPacketProtocol::maximumPacketSize()
{
    QFETCH(qint32, maxPacketSize);
    QFETCH(qint32, actualMaxPacketSize);
    QFETCH(QList<int>, packetSizes);

    QLocalServer server;
    server.listen("tst_QPacketProtocol");

    QLocalSocket socket;
    socket.connectToServer("tst_QPacketProtocol", QIODevice::WriteOnly);

    bool timedOut;
    server.waitForNewConnection(5000, &timedOut);
    QVERIFY(!timedOut);

    QLocalSocket *serverSocket = server.nextPendingConnection();

    QVERIFY(serverSocket);

    QPacketProtocol writeProtocol(&socket);
    QPacketProtocol readProtocol(serverSocket);

    readProtocol.setMaximumPacketSize(maxPacketSize);

    QCOMPARE(readProtocol.maximumPacketSize(), actualMaxPacketSize);

    if (maxPacketSize == actualMaxPacketSize) {
        QVERIFY(!packetSizes.isEmpty());
        QVERIFY(packetSizes.count() % 2 == 0);

        while (!packetSizes.isEmpty()) {
            int packetSize = packetSizes.takeFirst();
            int readCount = packetSizes.takeFirst();

            QByteArray b(packetSize, 'T');
            QPacket sendPacket;
            sendPacket.writeRawData(b.constData(), packetSize);
            writeProtocol.send(sendPacket);

            QSignalSpy spy(&readProtocol, SIGNAL(readyRead()));
            QTRY_COMPARE(spy.count(), readCount);

            QCOMPARE(readProtocol.packetsAvailable(), qint64(readCount));

            QPacket packet = readProtocol.read();

            QCOMPARE(packet.device()->size(), qint64(readCount * packetSize));
        }
    }
}

void tst_QPacketProtocol::sendReceive()
{
    QLocalServer server;
    server.listen("tst_QPacketProtocol");

    QLocalSocket socket;
    socket.connectToServer("tst_QPacketProtocol", QIODevice::WriteOnly);

    bool timedOut;
    server.waitForNewConnection(5000, &timedOut);
    QVERIFY(!timedOut);

    QLocalSocket *serverSocket = server.nextPendingConnection();
    QVERIFY(serverSocket);

    QPacketProtocol writeProtocol(&socket);
    QPacketProtocol readProtocol(serverSocket);

    QByteArray dataString("Data string");

    // Test sending with ::send(QPacket).
    for (int i = 1; i <= 10; ++i) {
        QSignalSpy writeSpy(&writeProtocol, SIGNAL(packetWritten()));
        QSignalSpy readSpy(&readProtocol, SIGNAL(readyRead()));

        QPacket sendPacket;
        sendPacket << dataString;
        for (int j = 0; j < i; ++j)
            writeProtocol.send(sendPacket);

        QTRY_COMPARE(writeSpy.count(), i);
        QTRY_COMPARE(readSpy.count(), i);

        QCOMPARE(readProtocol.packetsAvailable(), qint64(i));
        for (int j = 0; j < i; ++j) {
            QPacket packet = readProtocol.read();
            QByteArray r;
            packet >> r;
            QCOMPARE(r, dataString);
        }

        QCOMPARE(readProtocol.packetsAvailable(), qint64(0));
    }

    // Test sending with ::send() << var.
    for (int i = 1; i <= 10; ++i) {
        QSignalSpy writeSpy(&writeProtocol, SIGNAL(packetWritten()));
        QSignalSpy readSpy(&readProtocol, SIGNAL(readyRead()));

        for (int j = 0; j < i; ++j)
            writeProtocol.send() << dataString.constData();

        QTRY_COMPARE(writeSpy.count(), i);
        QTRY_COMPARE(readSpy.count(), i);

        QCOMPARE(readProtocol.packetsAvailable(), qint64(i));
        for (int j = 0; j < i; ++j) {
            QPacket packet = readProtocol.read();
            char *string;
            packet >> string;
            QByteArray r(string);
            QCOMPARE(r, dataString);
        }

        QCOMPARE(readProtocol.packetsAvailable(), qint64(0));
    }

    // Test ::clear().
    for (int i = 1; i <= 10; ++i) {
        QByteArray b("Data string");

        QSignalSpy writeSpy(&writeProtocol, SIGNAL(packetWritten()));
        QSignalSpy readSpy(&readProtocol, SIGNAL(readyRead()));

        QPacket sendPacket;
        sendPacket << b;
        for (int j = 0; j < i; ++j)
            writeProtocol.send(sendPacket);

        QTRY_COMPARE(writeSpy.count(), i);
        QTRY_COMPARE(readSpy.count(), i);

        QCOMPARE(readProtocol.packetsAvailable(), qint64(i));

        readProtocol.clear();

        QCOMPARE(readProtocol.packetsAvailable(), qint64(0));
    }

    {
        QSignalSpy writeSpy(&writeProtocol, SIGNAL(packetWritten()));
        QSignalSpy readSpy(&readProtocol, SIGNAL(readyRead()));
        QSignalSpy closeSpy(serverSocket, SIGNAL(aboutToClose()));

        writeProtocol.send() << dataString.constData();

        serverSocket->close();
        socket.close();

        QTRY_VERIFY(!closeSpy.isEmpty());

        QTRY_VERIFY(writeSpy.isEmpty());
        QTRY_VERIFY(readSpy.isEmpty());
    }
}

QTEST_MAIN(tst_QPacketProtocol)
#include "tst_qpacketprotocol.moc"
<commit_msg>Verify that invalidPacket() signal is emitted.<commit_after>/****************************************************************************
**
** Copyright (c) 2008-2009 Nokia Corporation and/or its subsidiary(-ies).
** Contact: Nokia Corporation (qt-info@nokia.com)
**
** This file is part of the Qt Mobility Components.
**
** $QT_BEGIN_LICENSE:LGPL$
** No Commercial Usage
** This file contains pre-release code and may not be distributed.
** You may use this file in accordance with the terms and conditions
** contained in Technology Preview License Agreement accompanying
** this package.
**
** GNU Lesser General Public License Usage
** Alternatively, this file may be used under the terms of the GNU Lesser
** General Public License version 2.1 as published by the Free Software
** Foundation and appearing in the file LICENSE.LGPL included in the
** packaging of this file.  Please review the following information to
** ensure the GNU Lesser General Public License version 2.1 requirements
** will be met: http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
**
** In addition, as a special exception, Nokia gives you certain
** additional rights. These rights are described in the Nokia Qt LGPL
** Exception version 1.0, included in the file LGPL_EXCEPTION.txt in this
** package.
**
** If you have questions regarding the use of this file, please
** contact Nokia at http://qt.nokia.com/contact.
** $QT_END_LICENSE$
**
****************************************************************************/

#include <qpacketprotocol.h>

#include <QByteArray>
#include <QBuffer>
#include <QLocalServer>
#include <QLocalSocket>

#include <QtTest/QTest>
#include <QSignalSpy>

#include <QDebug>

#define QTRY_COMPARE(a,e)                       \
    for (int _i = 0; _i < 5000; _i += 100) {    \
        if ((a) == (e)) break;                  \
        QTest::qWait(100);                      \
    }                                           \
    QCOMPARE(a, e)

#define QTRY_VERIFY(a)                       \
    for (int _i = 0; _i < 5000; _i += 100) {    \
        if (a) break;                  \
        QTest::qWait(100);                      \
    }                                           \
    QVERIFY(a)

Q_DECLARE_METATYPE(QList<int>)

class tst_QPacketProtocol : public QObject
{
    Q_OBJECT

private slots:
    void constructor();

    void maximumPacketSize_data();
    void maximumPacketSize();

    void sendReceive();
};

void tst_QPacketProtocol::constructor()
{
    QByteArray data;
    QBuffer buffer(&data);

    {
        QPacketProtocol protocol(&buffer);

        QCOMPARE(protocol.device(), &buffer);
    }
}

#define ROW_FOR_SIZE(x) x << x << (QList<int>() << 1 << 1 << (x - 4) << 1 << (x - 3) << 0)

void tst_QPacketProtocol::maximumPacketSize_data()
{
    QTest::addColumn<qint32>("maxPacketSize");
    QTest::addColumn<qint32>("actualMaxPacketSize");
    QTest::addColumn<QList<int> >("packetSizes");    // pairs of packetSize, readCount

    QTest::newRow("10MB") << ROW_FOR_SIZE(10 * 1024 * 1024);
    QTest::newRow("1MB") << ROW_FOR_SIZE(1024 * 1024);
    QTest::newRow("100kB") << ROW_FOR_SIZE(100 * 1024);
    QTest::newRow("10kB") << ROW_FOR_SIZE(10 * 1024);
    QTest::newRow("1kB") << ROW_FOR_SIZE(1024);
    QTest::newRow("100B") << ROW_FOR_SIZE(100);
    QTest::newRow("10B") << ROW_FOR_SIZE(10);
    QTest::newRow("5B") << ROW_FOR_SIZE(5);
    QTest::newRow("4B") << 4 << 0x7FFFFFFF << QList<int>();
    QTest::newRow("0B") << 0 << 0x7FFFFFFF << QList<int>();
    QTest::newRow("-1") << -1 << 0x7FFFFFFF << QList<int>();
}

void tst_QPacketProtocol::maximumPacketSize()
{
    QFETCH(qint32, maxPacketSize);
    QFETCH(qint32, actualMaxPacketSize);
    QFETCH(QList<int>, packetSizes);

    QLocalServer server;
    server.listen("tst_QPacketProtocol");

    QLocalSocket socket;
    socket.connectToServer("tst_QPacketProtocol", QIODevice::WriteOnly);

    bool timedOut;
    server.waitForNewConnection(5000, &timedOut);
    QVERIFY(!timedOut);

    QLocalSocket *serverSocket = server.nextPendingConnection();

    QVERIFY(serverSocket);

    QPacketProtocol writeProtocol(&socket);
    QPacketProtocol readProtocol(serverSocket);

    readProtocol.setMaximumPacketSize(maxPacketSize);

    QCOMPARE(readProtocol.maximumPacketSize(), actualMaxPacketSize);

    if (maxPacketSize == actualMaxPacketSize) {
        QVERIFY(!packetSizes.isEmpty());
        QVERIFY(packetSizes.count() % 2 == 0);

        while (!packetSizes.isEmpty()) {
            int packetSize = packetSizes.takeFirst();
            int readCount = packetSizes.takeFirst();

            QByteArray b(packetSize, 'T');
            QPacket sendPacket;
            sendPacket.writeRawData(b.constData(), packetSize);
            writeProtocol.send(sendPacket);

            QSignalSpy spy(&readProtocol, SIGNAL(readyRead()));
            QSignalSpy invalidSpy(&readProtocol, SIGNAL(invalidPacket()));

            QTRY_COMPARE(spy.count(), readCount);
            QTRY_COMPARE(invalidSpy.count(), 1 - readCount);

            QCOMPARE(readProtocol.packetsAvailable(), qint64(readCount));

            QPacket packet = readProtocol.read();

            QCOMPARE(packet.device()->size(), qint64(readCount * packetSize));
        }
    }
}

void tst_QPacketProtocol::sendReceive()
{
    QLocalServer server;
    server.listen("tst_QPacketProtocol");

    QLocalSocket socket;
    socket.connectToServer("tst_QPacketProtocol", QIODevice::WriteOnly);

    bool timedOut;
    server.waitForNewConnection(5000, &timedOut);
    QVERIFY(!timedOut);

    QLocalSocket *serverSocket = server.nextPendingConnection();
    QVERIFY(serverSocket);

    QPacketProtocol writeProtocol(&socket);
    QPacketProtocol readProtocol(serverSocket);

    QByteArray dataString("Data string");

    // Test sending with ::send(QPacket).
    for (int i = 1; i <= 10; ++i) {
        QSignalSpy writeSpy(&writeProtocol, SIGNAL(packetWritten()));
        QSignalSpy readSpy(&readProtocol, SIGNAL(readyRead()));

        QPacket sendPacket;
        sendPacket << dataString;
        for (int j = 0; j < i; ++j)
            writeProtocol.send(sendPacket);

        QTRY_COMPARE(writeSpy.count(), i);
        QTRY_COMPARE(readSpy.count(), i);

        QCOMPARE(readProtocol.packetsAvailable(), qint64(i));
        for (int j = 0; j < i; ++j) {
            QPacket packet = readProtocol.read();
            QByteArray r;
            packet >> r;
            QCOMPARE(r, dataString);
        }

        QCOMPARE(readProtocol.packetsAvailable(), qint64(0));
    }

    // Test sending with ::send() << var.
    for (int i = 1; i <= 10; ++i) {
        QSignalSpy writeSpy(&writeProtocol, SIGNAL(packetWritten()));
        QSignalSpy readSpy(&readProtocol, SIGNAL(readyRead()));

        for (int j = 0; j < i; ++j)
            writeProtocol.send() << dataString.constData();

        QTRY_COMPARE(writeSpy.count(), i);
        QTRY_COMPARE(readSpy.count(), i);

        QCOMPARE(readProtocol.packetsAvailable(), qint64(i));
        for (int j = 0; j < i; ++j) {
            QPacket packet = readProtocol.read();
            char *string;
            packet >> string;
            QByteArray r(string);
            QCOMPARE(r, dataString);
        }

        QCOMPARE(readProtocol.packetsAvailable(), qint64(0));
    }

    // Test ::clear().
    for (int i = 1; i <= 10; ++i) {
        QByteArray b("Data string");

        QSignalSpy writeSpy(&writeProtocol, SIGNAL(packetWritten()));
        QSignalSpy readSpy(&readProtocol, SIGNAL(readyRead()));

        QPacket sendPacket;
        sendPacket << b;
        for (int j = 0; j < i; ++j)
            writeProtocol.send(sendPacket);

        QTRY_COMPARE(writeSpy.count(), i);
        QTRY_COMPARE(readSpy.count(), i);

        QCOMPARE(readProtocol.packetsAvailable(), qint64(i));

        readProtocol.clear();

        QCOMPARE(readProtocol.packetsAvailable(), qint64(0));
    }

    {
        QSignalSpy writeSpy(&writeProtocol, SIGNAL(packetWritten()));
        QSignalSpy readSpy(&readProtocol, SIGNAL(readyRead()));
        QSignalSpy closeSpy(serverSocket, SIGNAL(aboutToClose()));

        writeProtocol.send() << dataString.constData();

        serverSocket->close();
        socket.close();

        QTRY_VERIFY(!closeSpy.isEmpty());

        QTRY_VERIFY(writeSpy.isEmpty());
        QTRY_VERIFY(readSpy.isEmpty());
    }
}

QTEST_MAIN(tst_QPacketProtocol)
#include "tst_qpacketprotocol.moc"
<|endoftext|>


<commit_before>#include <qif>
#include <vector>
#include <iostream>
using namespace qif;
using namespace std;

// configuration
std::vector<double> levels = { 2, 4, 8 };	// eps = log(level)/100
uint width = 20;
uint split_factor = 10;
uint border = 7;
double cell_size = 150;
uint samples = 1e7;

// computed
uint width_out = (width + 2*border) * split_factor;
double cell_size_out = cell_size / split_factor;

// cell-point mapping. The in grid is centered in the out grid. The corner of the out grid is (0,0), the corder of the in grid is corner_in
point corner_in(border * cell_size, border * cell_size);

auto c2p_in = geo::cell_to_point<double>(width, cell_size, corner_in);
auto c2p_out = geo::cell_to_point<double>(width_out, cell_size_out);
auto p2c_out = geo::point_to_cell<double>(width_out, cell_size_out);

auto euclid = metric::euclidean<double,point>();

void generate_arimoto(prob& pi, double eps, ofstream &file) {

	// distance between inputs and outputs
	auto d_inout = metric::compose(euclid, c2p_in, c2p_out);

	// generate mechanism
	prob out = probab::uniform(width_out * width_out);
	chan C = shannon::min_distortion(pi, out, eps/2 * d_inout, 1e-5, 1e-5);

	// draw
	arma::Mat<uint> drawn = channel::draw(pi, C, samples);
	LargeAvg<double> mean;

	for(uint i = 0; i < samples; i++) {
		uint x_i = drawn(i,0);
		uint z_i = drawn(i,1);
		point x = c2p_in(x_i);
		point z = c2p_out(z_i);

		mean.add(euclid(x, z));

		file << x_i << "," << z_i << "," << z.x << "," << z.y << "\n";
	}

	std::cout << "arimoto utility: " << mean.value() << "\n";
}

void generate_laplace(prob& pi, double eps, ofstream& file) {

	auto drawn = probab::draw(pi, samples);
	LargeAvg<double> mean;

	for(uint x_i : drawn) {
		point x = c2p_in(x_i);
		point z = x + mechanism::planar_laplace_draw(eps);

		mean.add(euclid(x, z));

		file << x_i << ",," <<  z.x << "," << z.y << "\n";
	}

	std::cout << "laplace utility: " << mean.value() << "\n";
}

void generate_geometric(prob& pi, double eps, ofstream& file) {

	uint out_of_grid = width_out * width_out;		// extra observation meaning "we got outside"

	// for efficiency, we batch sample n secrets, and n observations drawn from (0,0). Each z is added to the corresponding origin secret
	auto drawn_x = probab::draw(pi, samples);
	auto drawn_z = mechanism::planar_geometric_draw(cell_size_out, eps, samples);

	LargeAvg<double> mean;

	for(uint i = 0; i < samples; i++) {
		uint x_i = drawn_x(i);
		point x = c2p_in(x_i);
		point z = x + drawn_z[i];		// drawn_z's are drawn from (0,0), so add x

		// map z back to output grid
		uint z_i;
		try {
			z_i = min(p2c_out(z), out_of_grid);
		} catch(std::runtime_error e) {
			z_i = out_of_grid;
		}

		mean.add(euclid(x, z));

		file << x_i << "," << z_i << "," <<  z.x << "," << z.y << "\n";
	}

	std::cout << "geometric utility: " << mean.value() << "\n";
}

int main() {
	qif::rng::set_seed_random();

	string city = "san_francisco";
	string filename = "/home/vagabond/Downloads/datasets/gowalla/Gowalla_totalCheckins.txt";

	auto db = gowalla::read_dataset(filename);
	prob pi = gowalla::to_grid_prior(db, qif::locations[city], width, width, cell_size);
	// prob pi = probab::uniform(width * width);

	// file to write
	ofstream file;

	for(double l : levels) {
		double eps = log(l)/100;

		cout << "eps = ln(" << l << ")/100\n";

		file.open("arimoto-" + to_string(l) + ".csv");
		generate_arimoto(pi, eps, file);
		file.close();

		file.open("laplace-" + to_string(l) + ".csv");
		generate_laplace(pi, eps, file);
		file.close();

		file.open("geometric-" + to_string(l) + ".csv");
		generate_geometric(pi, eps, file);
		file.close();
	}
}
<commit_msg>qif-ml: print arimoto channel & bayes risk<commit_after>#include <qif>
#include <vector>
#include <iostream>
#include "getopt.h"
using namespace qif;
using namespace std;

// configuration
std::vector<double> levels = { 2, 4, 8 };	// eps = log(level)/100
uint width = 20;
uint split_factor = 10;
uint border = 7;
double cell_size = 150;
uint samples = 1e7;

// computed
uint width_out = (width + 2*border) * split_factor;
double cell_size_out = cell_size / split_factor;

// cell-point mapping. The in grid is centered in the out grid. The corner of the out grid is (0,0), the corder of the in grid is corner_in
point corner_in(border * cell_size, border * cell_size);

auto c2p_in = geo::cell_to_point<double>(width, cell_size, corner_in);
auto c2p_out = geo::cell_to_point<double>(width_out, cell_size_out);
auto p2c_out = geo::point_to_cell<double>(width_out, cell_size_out);

auto euclid = metric::euclidean<double,point>();

void generate_arimoto(prob& pi, double eps, ofstream &file, ofstream &file2) {

	// distance between inputs and outputs
	auto d_inout = metric::compose(euclid, c2p_in, c2p_out);

	// generate mechanism
	prob out = probab::uniform(width_out * width_out);
	chan C = shannon::min_distortion(pi, out, eps/2 * d_inout, 1e-5, 1e-5);

	file2 << C;

	// draw
	arma::Mat<uint> drawn = channel::draw(pi, C, samples);
	LargeAvg<double> mean;

	for(uint i = 0; i < samples; i++) {
		uint x_i = drawn(i,0);
		uint z_i = drawn(i,1);
		point x = c2p_in(x_i);
		point z = c2p_out(z_i);

		mean.add(euclid(x, z));

		file << x_i << "," << z_i << "," << z.x << "," << z.y << "\n";
	}

	std::cout << "arimoto sampled utility: " << mean.value() << "\n";
	std::cout << "arimoto exact utility: " << utility::expected_distance(d_inout, pi, C) << "\n";
	std::cout << "arimoto bayes risk: " << (1-bayes::post_vulnerability(pi, C)) << "\n";
}

void generate_laplace(prob& pi, double eps, ofstream& file) {

	auto drawn = probab::draw(pi, samples);
	LargeAvg<double> mean;

	for(uint x_i : drawn) {
		point x = c2p_in(x_i);
		point z = x + mechanism::planar_laplace_draw(eps);

		mean.add(euclid(x, z));

		file << x_i << ",," <<  z.x << "," << z.y << "\n";
	}

	std::cout << "laplace utility: " << mean.value() << "\n";
}

void generate_geometric(prob& pi, double eps, ofstream& file) {

	uint out_of_grid = width_out * width_out;		// extra observation meaning "we got outside"

	// for efficiency, we batch sample n secrets, and n observations drawn from (0,0). Each z is added to the corresponding origin secret
	auto drawn_x = probab::draw(pi, samples);
	auto drawn_z = mechanism::planar_geometric_draw(cell_size_out, eps, samples);

	LargeAvg<double> mean;

	for(uint i = 0; i < samples; i++) {
		uint x_i = drawn_x(i);
		point x = c2p_in(x_i);
		point z = x + drawn_z[i];		// drawn_z's are drawn from (0,0), so add x

		// map z back to output grid
		uint z_i;
		try {
			z_i = min(p2c_out(z), out_of_grid);
		} catch(std::runtime_error e) {
			z_i = out_of_grid;
		}

		mean.add(euclid(x, z));

		file << x_i << "," << z_i << "," <<  z.x << "," << z.y << "\n";
	}

	std::cout << "geometric utility: " << mean.value() << "\n";
}

int main(int argc, char** argv) {
	qif::rng::set_seed_random();

	// parse command line opts
	string gowalla_path = "/home/vagabond/Downloads/datasets/gowalla/Gowalla_totalCheckins.txt";

	struct option longopts[] = {
		{ "gowalla",       required_argument, NULL,         'g' },
		{ 0, 0, 0, 0 } // mark end
	};
	int c;
	while ((c = getopt_long(argc, argv, ":f:", longopts, NULL)) != -1) {
		switch (c) {
		 case 'g':
			gowalla_path = optarg;
			break;
		}
	}

	string city = "san_francisco";

	auto db = gowalla::read_dataset(gowalla_path);
	prob pi = gowalla::to_grid_prior(db, qif::locations[city], width, width, cell_size);
	// prob pi = probab::uniform(width * width);

	// file to write
	ofstream file;
	ofstream file2;

	for(double l : levels) {
		double eps = log(l)/100;

		cout << "eps = ln(" << l << ")/100\n";

		file.open("arimoto-" + to_string(l) + ".csv");
		file2.open("arimoto-" + to_string(l) + ".channel");
		generate_arimoto(pi, eps, file, file2);
		file.close();
		file2.close();

		file.open("laplace-" + to_string(l) + ".csv");
		generate_laplace(pi, eps, file);
		file.close();

		file.open("geometric-" + to_string(l) + ".csv");
		generate_geometric(pi, eps, file);
		file.close();
	}
}
<|endoftext|>