text:
/**
 * trajectory generator for testing the visual ekf
 */
#include <iostream>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/TwistStamped.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "qp_generator.h"

using namespace std;
using namespace Eigen;
ros::Publisher imu_pub, visual_pub, debug_pub;

const int update_freq = 400; // 1 / dt
const double dt = 0.0025;

void pub_imu(const Vector3d& acc, const Quaterniond& orientation){
    sensor_msgs::Imu imu;
    imu.header.stamp = ros::Time::now();
    imu.orientation.w = orientation.w();
    imu.orientation.x = orientation.x();
    imu.orientation.y = orientation.y();
    imu.orientation.z = orientation.z();
    imu.linear_acceleration.x = acc[0];
    imu.linear_acceleration.y = acc[1];
    imu.linear_acceleration.z = acc[2] + 9.8;

    imu_pub.publish(imu);
}

/**
 * Change the world frame position to the camera frame, and publish in millimeter
 * @param pos
 */
void pub_visual(const Vector3d& pos) {
    geometry_msgs::TwistStamped pose;
    pose.header.stamp = ros::Time::now();

    MatrixXd imu_R_camera = MatrixXd::Identity(3, 3);
    imu_R_camera <<  0, 0, 1,
                    -1, 0, 0,
                     0,-1, 0;

    Vector3d pos_camera = imu_R_camera.transpose() * (1000 * pos);
    pose.twist.linear.x = pos_camera[0];
    pose.twist.linear.y = pos_camera[1];
    pose.twist.linear.z = pos_camera[2];

    visual_pub.publish(pose);
}

void pub_debug(const Vector3d& pos, const Vector3d& vel, const Vector3d& acc)
{
    nav_msgs::Odometry odom;
    odom.header.stamp = ros::Time::now();

    odom.child_frame_id = "world";
    odom.pose.pose.position.x = pos[0];
    odom.pose.pose.position.y = pos[1];
    odom.pose.pose.position.z = pos[2];
    odom.twist.twist.linear.x = vel[0];
    odom.twist.twist.linear.y = vel[1];
    odom.twist.twist.linear.z = vel[2];
    odom.twist.twist.angular.x= acc[0];
    odom.twist.twist.angular.y= acc[1];
    odom.twist.twist.angular.z= acc[2];

    debug_pub.publish(odom);
}

// get position from coefficient
void getPositionFromCoeff(Eigen::Vector3d &pos, const Eigen::MatrixXd &coeff,
                                             const int &index, const double &time)
{
    int s = index;
    double t = time;
    double x = coeff(s, 0) + coeff(s, 1) * t + coeff(s, 2) * pow(t, 2) + coeff(s, 3) * pow(t, 3) +
               coeff(s, 4) * pow(t, 4) + coeff(s, 5) * pow(t, 5);
    double y = coeff(s, 6) + coeff(s, 7) * t + coeff(s, 8) * pow(t, 2) + coeff(s, 9) * pow(t, 3) +
               coeff(s, 10) * pow(t, 4) + coeff(s, 11) * pow(t, 5);
    double z = coeff(s, 12) + coeff(s, 13) * t + coeff(s, 14) * pow(t, 2) + coeff(s, 15) * pow(t, 3) +
               coeff(s, 16) * pow(t, 4) + coeff(s, 17) * pow(t, 5);

    pos(0) = x;
    pos(1) = y;
    pos(2) = z;
}

// get velocity from cofficient
void getVelocityFromCoeff(Eigen::Vector3d &vel, const Eigen::MatrixXd &coeff,
                                             const int &index, const double &time)
{
    int s = index;
    double t = time;
    double vx = coeff(s, 1) + 2 * coeff(s, 2) * pow(t, 1) + 3 * coeff(s, 3) * pow(t, 2) +
                4 * coeff(s, 4) * pow(t, 3) + 5 * coeff(s, 5) * pow(t, 4);
    double vy = coeff(s, 7) + 2 * coeff(s, 8) * pow(t, 1) + 3 * coeff(s, 9) * pow(t, 2) +
                4 * coeff(s, 10) * pow(t, 3) + 5 * coeff(s, 11) * pow(t, 4);
    double vz = coeff(s, 13) + 2 * coeff(s, 14) * pow(t, 1) + 3 * coeff(s, 15) * pow(t, 2) +
                4 * coeff(s, 16) * pow(t, 3) + 5 * coeff(s, 17) * pow(t, 4);

    vel(0) = vx;
    vel(1) = vy;
    vel(2) = vz;
}

// get acceleration from coefficient
void getAccelerationFromCoeff(Eigen::Vector3d &acc, const Eigen::MatrixXd &coeff,
                                                 const int &index, const double &time)
{
    int s = index;
    double t = time;
    double ax = 2 * coeff(s, 2) + 6 * coeff(s, 3) * pow(t, 1) + 12 * coeff(s, 4) * pow(t, 2) +
               20 * coeff(s, 5) * pow(t, 3);
    double ay = 2 * coeff(s, 8) + 6 * coeff(s, 9) * pow(t, 1) + 12 * coeff(s, 10) * pow(t, 2) +
               20 * coeff(s, 11) * pow(t, 3);
    double az = 2 * coeff(s, 14) + 6 * coeff(s, 15) * pow(t, 1) + 12 * coeff(s, 16) * pow(t, 2) +
               20 * coeff(s, 17) * pow(t, 3);

    acc(0) = ax;
    acc(1) = ay;
    acc(2) = az;
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "visual_ekf_tester");
    ros::NodeHandle n("~");

    imu_pub    = n.advertise<sensor_msgs::Imu>("/visual_ekf/test_imu", 400);
    visual_pub = n.advertise<geometry_msgs::TwistStamped>("/visual_ekf/test_visual", 50);
    debug_pub  = n.advertise<nav_msgs::Odometry>("/visual_ekf_tester/odom", 50);

    ros::Rate r(update_freq);

    MatrixXd Path = MatrixXd::Zero(3, 3);
    Path.block<1, 3>(0, 0) = Vector3d(0.6, 0.2, 0.1);
    Path.block<1, 3>(1, 0) = Vector3d(0.8, 0.2, 0.1);
    Path.block<1, 3>(2, 0) = Vector3d(1, 0, 0);

    Vector3d Vel_init(0, 0, 0);
    Vector3d Acc_init(0, 0, 0);
    VectorXd time_period = MatrixXd::Zero(2, 1);
    time_period << 5.0, 5.0;
    VectorXd time_int = time_period;

    // generate a smooth curve
    TrajectoryGenerator generator;
    MatrixXd coeff = generator.PolyQPGeneration(
            Path, Vel_init, Acc_init, time_period, 1);
    cout << "polynomial " << endl << coeff << endl;

    auto t_start = ros::Time::now().toSec();
    int iterator = 0;
    auto time_period_size = (int)time_period.size();

    // provide the orientation, rotate in Z axis first
    Vector3d axis_z = Vector3d::UnitZ();
    VectorXd theta = MatrixXd::Zero(3, 1);
    theta[0] = 0.0;
    theta[1] = 0.1 * M_PI;
    theta[2] = 0.25 * M_PI;

    while (ros::ok()) {

        double dt = ros::Time::now().toSec() - t_start;

        if (iterator < time_period_size) {
            if (dt > time_period[iterator]) {
                dt -= time_period[iterator];
                t_start += time_period[iterator];
                iterator++;
                ROS_INFO("In polynomial section %d", iterator);
            }
        }

        if (iterator < time_period_size) {
            Vector3d pos, vel, acc;
            getPositionFromCoeff(pos, coeff, iterator, dt);
            getVelocityFromCoeff(vel, coeff, iterator, dt);
            getAccelerationFromCoeff(acc, coeff, iterator, dt);

            double omg = (theta[iterator+1] - theta[iterator]) / time_period[iterator];
            double angle = omg * dt + theta[iterator];
            AngleAxisd orientation(angle, axis_z);
            Quaterniond q(orientation);

            Vector3d acc_imu = q.conjugate() * acc;
            pub_imu(acc_imu, q);

            Vector3d pos_imu = q.conjugate() * pos;
            pub_visual(pos_imu);

            Vector3d vel_imu = q.conjugate() * vel;
            pub_debug(pos_imu, vel_imu, acc_imu);
        }

        r.sleep();
        ros::spinOnce();
    }
}


text:
#include <boost/python.hpp>

#include "../../src/chemist/molecule.h"
#include "../../src/fileio/moleculefile.h"

using Helium::Chemist::Molecule;
using namespace boost::python;

bool readMolecule_1(Helium::MoleculeFile &file, Molecule &mol)
{
  return file.readMolecule(mol);
}

bool readMolecule_2(Helium::MoleculeFile &file, unsigned int index, Molecule &mol)
{
  if (index >= file.numMolecules())
    throw std::runtime_error("Invalid molecule index");
  return file.readMolecule(index, mol);
}

bool readMolecule_3(Helium::MemoryMappedMoleculeFile &file, unsigned int index, Molecule &mol)
{
  if (index >= file.numMolecules())
    throw std::runtime_error("Invalid molecule index");
  return file.readMolecule(index, mol);
}

void export_moleculefile()
{

  class_<Helium::MoleculeOutputFile, boost::noncopyable>("MoleculeOutputFile", init<const std::string&>())
    .def("writeMolecule", &Helium::MoleculeOutputFile::writeMolecule<Molecule>)
    .def("error", &Helium::MoleculeOutputFile::error, return_value_policy<copy_const_reference>())
    ;

  class_<Helium::MoleculeFile, boost::noncopyable>("MoleculeFile")
    .def(init<const std::string&>())
    .def("load", &Helium::MoleculeFile::load)
    .def("numMolecules", &Helium::MoleculeFile::numMolecules)
    .def("readMolecule", &readMolecule_1)
    .def("readMolecule", &readMolecule_2)
    .def("close", &Helium::MoleculeFile::close)
    .def("error", &Helium::MoleculeFile::error, return_internal_reference<>())
    ;

  class_<Helium::MemoryMappedMoleculeFile, boost::noncopyable>("MemoryMappedMoleculeFile")
    .def(init<const std::string&>())
    .def("load", &Helium::MemoryMappedMoleculeFile::load)
    .def("numMolecules", &Helium::MemoryMappedMoleculeFile::numMolecules)
    .def("readMolecule", &readMolecule_3)
    .def("error", &Helium::MemoryMappedMoleculeFile::error, return_internal_reference<>())
    ;
}

ext: cpp

max_stars_repo_name: timvdm/Helium

max_issues_repo_name: timvdm/Helium

max_forks_repo_name: timvdm/Helium


text:
// =======================================================================
// Copyright 2015 by Ireneusz Szcześniak
// Authors: Ireneusz Szcześniak
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
// =======================================================================

// =======================================================================
// The custom Dijkstra call, which returns the optional path as a list
// of edges along with the cost of the path.  The search is stopped
// when the destination node is reached.
// =======================================================================

#ifndef BOOST_GRAPH_CUSTOM_DIJKSTRA_CALL
#define BOOST_GRAPH_CUSTOM_DIJKSTRA_CALL

#include <list>

#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/optional.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/utility/value_init.hpp>

namespace boost {

  // ========================================================================
  // Finish the search when a given node is examined, i.e. when the
  // shortest path to that node is found.
  // ========================================================================

  // The type of the exception thrown by the cdc_visitor.
  struct cdc_exception {};

  template <class Graph>
  struct cdc_visitor
  {
    typedef typename Graph::vertex_descriptor vertex_descriptor;
    typedef on_examine_vertex event_filter;
    cdc_visitor(vertex_descriptor t): m_t(t) {}
    void operator()(vertex_descriptor v, const Graph& g) {
      if (v == m_t)
        throw cdc_exception();
    }
    vertex_descriptor m_t;
  };

  // =======================================================================
  // The function.
  // =======================================================================

  template <typename Graph, typename WeightMap, typename IndexMap>
  optional<std::pair<typename WeightMap::value_type,
                     std::list<typename Graph::edge_descriptor>>>
  custom_dijkstra_call(const Graph &g,
                       typename Graph::vertex_descriptor s,
                       typename Graph::vertex_descriptor t,
                       WeightMap wm, IndexMap im)
  {
    typedef typename Graph::vertex_descriptor vertex_descriptor;
    typedef typename Graph::edge_descriptor edge_descriptor;
    typedef typename std::list<typename Graph::edge_descriptor> path_type;
    typedef typename WeightMap::value_type weight_type;
    typedef typename std::pair<weight_type, path_type> kr_type;

    optional<kr_type> result;

    if (s == t)
      result = std::make_pair(0, path_type());
    else
      {
        std::vector<edge_descriptor> pred_vec(num_vertices(g));
        auto pred = make_iterator_property_map(pred_vec.begin(), im);
        auto rep = record_edge_predecessors(pred, on_edge_relaxed());
        auto qat = cdc_visitor<Graph>(t);
        auto dv = make_dijkstra_visitor(std::make_pair(rep, qat));

        try
          {
            dijkstra_shortest_paths(g, s,
                                    weight_map(wm).vertex_index_map(im).
                                    visitor(dv));
          }
        catch (cdc_exception) {}

        // Was the solution found?
        if (pred[t] != edge_descriptor())
          {
            // The cost of the shortest path.
            value_initialized<weight_type> cost;
            // The path found.
            path_type p;

            // Trace the solution to the source.
            vertex_descriptor c = t;
            while (c != s)
              {
                const edge_descriptor &e = pred[c];
                // Build the path.
                p.push_front(e);
                // Calculate the cost of the path.
                cost += get(wm, e);
                // Find the predecessing vertex.
                c = source(e, g);
              }

            result = std::make_pair(cost, p);
          }
      }

    return result;
  }

} // boost

#endif /* BOOST_GRAPH_CUSTOM_DIJKSTRA_CALL */

ext: hpp

max_stars_repo_name: gunjanbaid/octopus

max_issues_repo_name: gunjanbaid/octopus

max_forks_repo_name: gunjanbaid/octopus


text:
/*
 * Copyright 2016 Adam Chyła, adam@chyla.org
 * All rights reserved. Distributed under the terms of the MIT License.
 */

#include "system.h"

#include <boost/log/trivial.hpp>
#include <bits/shared_ptr.h>

namespace bash
{

namespace analyzer
{

namespace detail
{

SystemPtr System::Create() {
  return std::make_shared<System>();
}

time_t System::Time(time_t *t) {
  BOOST_LOG_TRIVIAL(debug) << "bash::analyzer::detail::System::Time: Function call";
  return time(t);
}

struct tm* System::LocalTime(const time_t *timep) {
  BOOST_LOG_TRIVIAL(debug) << "bash::analyzer::detail::System::LocalTime: Function call";
  return localtime(timep);
}

}

}

}

max_stars_repo_name: chyla/slas

max_issues_repo_name: chyla/slas

max_forks_repo_name: chyla/slas


text:
//---------------------------------------------------------------------------//
//!
//! \file   tstFromStringTraits.cpp
//! \author Alex Robinson
//! \brief  FromStringTraits unit tests
//!
//---------------------------------------------------------------------------//

// Std Lib Includes
#include <iostream>
#include <sstream>
#include <type_traits>

// Boost Includes
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <boost/mpl/list.hpp>
#include <boost/mpl/insert_range.hpp>
#include <boost/units/systems/cgs/energy.hpp>
#include <boost/units/systems/si/energy.hpp>

// FRENSIE Includes
#include "Utility_FromStringTraits.hpp"
#include "Utility_ElectronVoltUnit.hpp"
#include "Utility_QuantityTraits.hpp"
#include "Utility_UnitTraits.hpp"

using namespace Utility::Units;
namespace si = boost::units::si;
namespace cgs = boost::units::cgs;

//---------------------------------------------------------------------------//
// Template Test Types
//---------------------------------------------------------------------------//
typedef boost::mpl::list<char, signed char, unsigned char> SingleByteTypes;

typedef boost::mpl::list<short, int, long, long long> MultipleByteTypes;

typedef boost::mpl::list<int, unsigned int, long, unsigned long, long long, unsigned long long, float, double> ComplexTestTypes;

template<typename Unit, typename RawTypeWrapper = void>
struct QuantityTypeList
{
  typedef boost::mpl::list<boost::units::quantity<Unit,float>, boost::units::quantity<Unit,double> > BasicFloatingPointQuantityTypes;

  typedef boost::mpl::list<boost::units::quantity<Unit,int>, boost::units::quantity<Unit,float>, boost::units::quantity<Unit,double> > BasicQuantityTypes;

  typedef boost::mpl::list<boost::units::quantity<Unit,std::complex<int> >, boost::units::quantity<Unit,std::complex<float> >, boost::units::quantity<Unit,std::complex<double> > > ComplexQuantityTypes;

  typedef typename boost::mpl::insert_range<BasicQuantityTypes, typename boost::mpl::end<BasicQuantityTypes>::type,ComplexQuantityTypes>::type type;
};

template<typename... TypeLists>
struct MergeTypeLists
{ /* ... */ };

template<typename FrontList, typename... TypeLists>
struct MergeTypeLists<FrontList,TypeLists...>
{
private:
  typedef typename MergeTypeLists<TypeLists...>::type BackMergedListType;

public:
  typedef typename boost::mpl::insert_range<FrontList,typename boost::mpl::end<FrontList>::type,BackMergedListType>::type type;
};

template<typename FrontList>
struct MergeTypeLists<FrontList>
{
  typedef FrontList type;
};

typedef typename MergeTypeLists<typename QuantityTypeList<cgs::energy>::BasicFloatingPointQuantityTypes, typename QuantityTypeList<si::energy>::BasicFloatingPointQuantityTypes, typename QuantityTypeList<ElectronVolt>::BasicFloatingPointQuantityTypes, typename QuantityTypeList<KiloElectronVolt>::BasicFloatingPointQuantityTypes>::type TestBasicFloatingPointQuantityTypes;

typedef typename MergeTypeLists<typename QuantityTypeList<cgs::energy>::BasicQuantityTypes, typename QuantityTypeList<si::energy>::BasicQuantityTypes, typename QuantityTypeList<ElectronVolt>::BasicQuantityTypes, typename QuantityTypeList<KiloElectronVolt>::BasicQuantityTypes>::type TestBasicQuantityTypes;

typedef typename MergeTypeLists<typename QuantityTypeList<cgs::energy>::ComplexQuantityTypes, typename QuantityTypeList<si::energy>::ComplexQuantityTypes, typename QuantityTypeList<ElectronVolt>::ComplexQuantityTypes, typename QuantityTypeList<KiloElectronVolt>::ComplexQuantityTypes>::type TestComplexQuantityTypes;

typedef typename MergeTypeLists<typename QuantityTypeList<cgs::energy>::type, typename QuantityTypeList<si::energy>::type, typename QuantityTypeList<ElectronVolt>::type, typename QuantityTypeList<KiloElectronVolt>::type>::type TestQuantityTypes;

//---------------------------------------------------------------------------//
// Tests
//---------------------------------------------------------------------------//
// Check that a string can be created from a string
BOOST_AUTO_TEST_CASE( string_fromString )
{
  BOOST_CHECK_EQUAL( Utility::fromString<std::string>( " " ), " " );
  BOOST_CHECK_EQUAL( Utility::fromString<std::string>( "testing" ), "testing" );
  BOOST_CHECK_EQUAL( Utility::fromString<std::string>( "{{1,0},{-1,2}}" ), "{{1,0},{-1,2}}" );
  BOOST_CHECK_EQUAL( Utility::fromString<std::string>( "{ {1,0}, {-1,2} }" ), "{ {1,0}, {-1,2} }" );
}

//---------------------------------------------------------------------------//
// Check that a string can be extracted from a stream
BOOST_AUTO_TEST_CASE( string_fromStream )
{
  std::istringstream iss;
  iss.str( " " );

  std::string string;

  Utility::fromStream( iss, string );

  BOOST_CHECK_EQUAL( string, " " );

  iss.str( "testing" );
  iss.clear();

  Utility::fromStream( iss, string );

  BOOST_CHECK_EQUAL( string, "testing" );

  string.clear();

  // Multiple booleans in the same stream with default deliminators
  iss.str( "testing-1\ntesting-2\ntesting-3\ntesting-4" );
  iss.clear();

  Utility::fromStream( iss, string );

  BOOST_CHECK_EQUAL( string, "testing-1" );

  Utility::fromStream( iss, string );

  BOOST_CHECK_EQUAL( string, "testing-2" );

  Utility::fromStream( iss, string );

  BOOST_CHECK_EQUAL( string, "testing-3" );

  Utility::fromStream( iss, string );

  BOOST_CHECK_EQUAL( string, "testing-4" );

  string.clear();

  // Multiple booleans in the same stream with white space deliminators
  iss.str( "testing-1 testing-2\ttesting-3\ntesting-4" );
  iss.clear();

  Utility::fromStream( iss, string, Utility::Details::white_space_delims );

  BOOST_CHECK_EQUAL( string, "testing-1" );

  Utility::fromStream( iss, string, Utility::Details::white_space_delims );

  BOOST_CHECK_EQUAL( string, "testing-2" );

  Utility::fromStream( iss, string, Utility::Details::white_space_delims );

  BOOST_CHECK_EQUAL( string, "testing-3" );

  Utility::fromStream( iss, string, Utility::Details::white_space_delims );

  BOOST_CHECK_EQUAL( string, "testing-4" );

  string.clear();

  // Multiple booleans in the same stream with custom deliminators
  iss.str( "testing-1,testing-2" );
  iss.clear();

  Utility::fromStream( iss, string, "," );

  BOOST_CHECK_EQUAL( string, "testing-1" );

  string.clear();

  Utility::moveInputStreamToNextElement( iss, ',', '}' );

  Utility::fromStream( iss, string );

  BOOST_CHECK_EQUAL( string, "testing-2" );

  // Multiple string elements in the same stream with container deliminators
  iss.str( "{1,0},{-1,2}, {a,b} , {key, {a,b}} }" );
  iss.clear();

  Utility::fromStream( iss, string, ",}" );

  BOOST_CHECK_EQUAL( string, "{1,0}" );

  Utility::moveInputStreamToNextElement( iss, ',', '}' );

  Utility::fromStream( iss, string, ",}" );

  BOOST_CHECK_EQUAL( string, "{-1,2}" );

  Utility::moveInputStreamToNextElement( iss, ',', '}' );

  Utility::fromStream( iss, string, ",}" );

  BOOST_CHECK_EQUAL( string, " {a,b}" );

  Utility::moveInputStreamToNextElement( iss, ',', '}' );

  Utility::fromStream( iss, string, ",}" );

  BOOST_CHECK_EQUAL( string, " {key, {a,b}}" );

  Utility::moveInputStreamToNextElement( iss, ',', '}' );
}

//---------------------------------------------------------------------------//
// Check that a LogRecordType can be created from a string
BOOST_AUTO_TEST_CASE( LogRecordType_fromString )
{
  Utility::LogRecordType record_type;

  BOOST_CHECK_EQUAL( Utility::fromString<Utility::LogRecordType>( "Error" ),
                     Utility::ERROR_RECORD );
  BOOST_CHECK_EQUAL( Utility::fromString<Utility::LogRecordType>( " Error " ),
                     Utility::ERROR_RECORD );
  BOOST_CHECK_THROW( Utility::fromString<Utility::LogRecordType>( "error" ),
                     Utility::StringConversionException );

  BOOST_CHECK_EQUAL( Utility::fromString<Utility::LogRecordType>( "Warning" ),
                     Utility::WARNING_RECORD );
  BOOST_CHECK_EQUAL( Utility::fromString<Utility::LogRecordType>( " Warning " ),
                     Utility::WARNING_RECORD );
  BOOST_CHECK_THROW( Utility::fromString<Utility::LogRecordType>( "warning" ),
                     Utility::StringConversionException );

  BOOST_CHECK_EQUAL( Utility::fromString<Utility::LogRecordType>( "Notification" ),
                     Utility::NOTIFICATION_RECORD );
  BOOST_CHECK_EQUAL( Utility::fromString<Utility::LogRecordType>( " Notification " ),
                     Utility::NOTIFICATION_RECORD );
  BOOST_CHECK_THROW( Utility::fromString<Utility::LogRecordType>( "notification" ),
                     Utility::StringConversionException );

  BOOST_CHECK_EQUAL( Utility::fromString<Utility::LogRecordType>( "Details" ),
                     Utility::DETAILS_RECORD );
  BOOST_CHECK_EQUAL( Utility::fromString<Utility::LogRecordType>( " Details " ),
                     Utility::DETAILS_RECORD );
  BOOST_CHECK_THROW( Utility::fromString<Utility::LogRecordType>( "details" ),
                     Utility::StringConversionException );

  BOOST_CHECK_EQUAL( Utility::fromString<Utility::LogRecordType>( "Pedantic Details" ),
                     Utility::PEDANTIC_DETAILS_RECORD );

  BOOST_CHECK_EQUAL( Utility::fromString<Utility::LogRecordType>( " Pedantic Details " ),
                     Utility::PEDANTIC_DETAILS_RECORD );

  BOOST_CHECK_THROW( Utility::fromString<Utility::LogRecordType>( "pedantic Details" ),
                     Utility::StringConversionException );
  BOOST_CHECK_THROW( Utility::fromString<Utility::LogRecordType>( "Pedantic details" ),
                     Utility::StringConversionException );
  BOOST_CHECK_THROW( Utility::fromString<Utility::LogRecordType>( "pedantic details" ),
                     Utility::StringConversionException );
}

//---------------------------------------------------------------------------//
// Check that a LogRecordType can be extracted from a stream
BOOST_AUTO_TEST_CASE( LogRecordType_fromStream )
{
  Utility::LogRecordType record_type;
  std::istringstream iss( "Error" );

  Utility::fromStream( iss, record_type );

  BOOST_CHECK_EQUAL( record_type, Utility::ERROR_RECORD );

  iss.str( " Error " );
  iss.clear();

  Utility::fromStream( iss, record_type );

  BOOST_CHECK_EQUAL( record_type, Utility::ERROR_RECORD );

  iss.str( "error" );
  iss.clear();

  BOOST_CHECK_THROW( Utility::fromStream( iss, record_type ), std::runtime_error );

  iss.str( "Warning" );
  iss.clear();

  Utility::fromStream( iss, record_type );

  BOOST_CHECK_EQUAL( record_type, Utility::WARNING_RECORD );

  iss.str( " Warning " );
  iss.clear();

  Utility::fromStream( iss, record_type );

  BOOST_CHECK_EQUAL( record_type, Utility::WARNING_RECORD );

  iss.str( "warning" );
  iss.clear();

  BOOST_CHECK_THROW( Utility::fromStream( iss, record_type ), std::runtime_error );

  iss.str( "Notification" );
  iss.clear();

  Utility::fromStream( iss, record_type );

  BOOST_CHECK_EQUAL( record_type, Utility::NOTIFICATION_RECORD );

  iss.str( " Notification " );
  iss.clear();

  Utility::fromStream( iss, record_type );

  BOOST_CHECK_EQUAL( record_type, Utility::NOTIFICATION_RECORD );

  iss.str( "notification" );
  iss.clear();

  BOOST_CHECK_THROW( Utility::fromStream( iss, record_type ), std::runtime_error );

  iss.str( "Details" );
  iss.clear();

  Utility::fromStream( iss, record_type );

  BOOST_CHECK_EQUAL( record_type, Utility::DETAILS_RECORD );

  iss.str( " Details " );
  iss.clear();

  Utility::fromStream( iss, record_type );

  BOOST_CHECK_EQUAL( record_type, Utility::DETAILS_RECORD );

  iss.str( "details" );
  iss.clear();

  BOOST_CHECK_THROW( Utility::fromStream( iss, record_type ), std::runtime_error );

  iss.str( "Pedantic Details" );
  iss.clear();

  Utility::fromStream( iss, record_type );

  BOOST_CHECK_EQUAL( record_type, Utility::PEDANTIC_DETAILS_RECORD );

  iss.str( " Pedantic  Details " );
  iss.clear();

  Utility::fromStream( iss, record_type );

  BOOST_CHECK_EQUAL( record_type, Utility::PEDANTIC_DETAILS_RECORD );

  iss.str( "pedantic Details" );
  iss.clear();

  BOOST_CHECK_THROW( Utility::fromStream( iss, record_type ), std::runtime_error );

  iss.str( "Pedantic details" );
  iss.clear();

  BOOST_CHECK_THROW( Utility::fromStream( iss, record_type ), std::runtime_error );

  iss.str( "pedantic details" );
  iss.clear();

  BOOST_CHECK_THROW( Utility::fromStream( iss, record_type ), std::runtime_error );
}

//---------------------------------------------------------------------------//
// Check that a boolean can be created from a string
BOOST_AUTO_TEST_CASE( bool_fromString )
{
  BOOST_CHECK_EQUAL( Utility::fromString<bool>( "0" ), false );
  BOOST_CHECK_EQUAL( Utility::fromString<bool>( "false" ), false );
  BOOST_CHECK_EQUAL( Utility::fromString<bool>( "False" ), false );
  BOOST_CHECK_EQUAL( Utility::fromString<bool>( "FaLsE" ), false );
  BOOST_CHECK_EQUAL( Utility::fromString<bool>( "FALSE" ), false );

  BOOST_CHECK_EQUAL( Utility::fromString<bool>( "1" ), true );
  BOOST_CHECK_EQUAL( Utility::fromString<bool>( "true" ), true );
  BOOST_CHECK_EQUAL( Utility::fromString<bool>( "True" ), true );
  BOOST_CHECK_EQUAL( Utility::fromString<bool>( "TrUe" ), true );
  BOOST_CHECK_EQUAL( Utility::fromString<bool>( "TRUE" ), true );

  BOOST_CHECK_THROW( Utility::fromString<bool>( "0 1" ),
                     Utility::StringConversionException );
  BOOST_CHECK_THROW( Utility::fromString<bool>( "1 false" ),
                     Utility::StringConversionException );
  BOOST_CHECK_THROW( Utility::fromString<bool>( "2" ),
                     Utility::StringConversionException );
  BOOST_CHECK_THROW( Utility::fromString<bool>( "abc" ),
                     Utility::StringConversionException );
}

//---------------------------------------------------------------------------//
// Check that a boolean can be extracted from a stream
BOOST_AUTO_TEST_CASE( bool_fromStream )
{
  std::istringstream iss( "0" );

  bool boolean;

  Utility::fromStream( iss, boolean );

  BOOST_CHECK_EQUAL( boolean, false );

  iss.str( "false" );
  iss.clear();

  Utility::fromStream( iss, boolean );

  BOOST_CHECK_EQUAL( boolean, false );

  iss.str( "1" );
  iss.clear();

  Utility::fromStream( iss, boolean );

  BOOST_CHECK_EQUAL( boolean, true );

  iss.str( "true" );
  iss.clear();

  Utility::fromStream( iss, boolean );

  BOOST_CHECK_EQUAL( boolean, true );

  // Multiple booleans in the same stream with default deliminators
  iss.str( "0 1  true   false  " );
  iss.clear();

  Utility::fromStream( iss, boolean );

  BOOST_CHECK_EQUAL( boolean, false );

  Utility::fromStream( iss, boolean );

  BOOST_CHECK_EQUAL( boolean, true );

  Utility::fromStream( iss, boolean );

  BOOST_CHECK_EQUAL( boolean, true );

  Utility::fromStream( iss, boolean );

  BOOST_CHECK_EQUAL( boolean, false );

  // Multiple booleans in the same stream with custom deliminators
  iss.str( "0, true" );
  iss.clear();

  Utility::fromStream( iss, boolean, "," );

  BOOST_CHECK_EQUAL( boolean, false );

  Utility::moveInputStreamToNextElement( iss, ',', '}' );

  Utility::fromStream( iss, boolean );

  BOOST_CHECK_EQUAL( boolean, true );
}

//---------------------------------------------------------------------------//
// Check that single byte integer types can be created from a string
BOOST_AUTO_TEST_CASE_TEMPLATE( byte_fromString, T, SingleByteTypes )
{
  BOOST_CHECK_EQUAL( Utility::fromString<T>( " " ), (T)32 );

  std::string test_string;
  test_string.push_back( '\t' );

  BOOST_CHECK_EQUAL( Utility::fromString<T>( test_string ), (T)9 );

  test_string.clear();
  test_string.push_back( '\n' );

  BOOST_CHECK_EQUAL( Utility::fromString<T>( test_string ), (T)10 );

  test_string = "-128";

  if( std::is_signed<T>::value )
  {
    BOOST_CHECK_EQUAL( Utility::fromString<T>( test_string ), (T)-128 );
  }
  else
  {
    BOOST_CHECK_THROW( Utility::fromString<T>( test_string ),
                       Utility::StringConversionException );
  }

  test_string = "127";

  BOOST_CHECK_EQUAL( Utility::fromString<T>( test_string ), (T)127 );

  test_string = "255";

  if( std::is_signed<T>::value )
  {
    BOOST_CHECK_THROW( Utility::fromString<T>( test_string ),
                       Utility::StringConversionException );
  }
  else
  {
    BOOST_CHECK_EQUAL( Utility::fromString<T>( test_string ), (T)255 );
  }

  test_string.clear();

  BOOST_CHECK_THROW( Utility::fromString<T>( "256" ),
                     Utility::StringConversionException );
  BOOST_CHECK_THROW( Utility::fromString<T>( "0.0" ),
                     Utility::StringConversionException );
  BOOST_CHECK_THROW( Utility::fromString<T>( "0.000000000e+00" ),
                     Utility::StringConversionException );
  BOOST_CHECK_THROW( Utility::fromString<T>( "0 0" ),
                     Utility::StringConversionException );
  BOOST_CHECK_THROW( Utility::fromString<T>( "ab" ),
                     Utility::StringConversionException );
  BOOST_CHECK_THROW( Utility::fromString<T>( "a b" ),
                     Utility::StringConversionException );
}

//---------------------------------------------------------------------------//
// Check that single byte integer types can be created from a stream
BOOST_AUTO_TEST_CASE_TEMPLATE( byte_fromStream, T, SingleByteTypes )
{
  std::istringstream iss( " " );

  T test_byte;

  Utility::fromStream( iss, test_byte );

  BOOST_CHECK_EQUAL( test_byte, (T)32 );

  {
    std::string test_string;
    test_string.push_back( '\t' );

    iss.str( test_string );
    iss.clear();
  }

  Utility::fromStream( iss, test_byte );

  BOOST_CHECK_EQUAL( test_byte, (T)9 );

  {
    std::string test_string;
    test_string.push_back( '\n' );

    iss.str( test_string );
    iss.clear();
  }

  Utility::fromStream( iss, test_byte );

  BOOST_CHECK_EQUAL( test_byte, (T)10 );

  // Multiple bytes in the same stream with no deliminators
  iss.str( "-128" );
  iss.clear();

  Utility::fromStream( iss, test_byte );

  BOOST_CHECK_EQUAL( test_byte, (T)'-' );

  Utility::fromStream( iss, test_byte );

  BOOST_CHECK_EQUAL( test_byte, (T)'1' );

  Utility::fromStream( iss, test_byte );

  BOOST_CHECK_EQUAL( test_byte, (T)'2' );

  Utility::fromStream( iss, test_byte );

  BOOST_CHECK_EQUAL( test_byte, (T)'8' );

  // Multiple bytes in the same stream with white space deliminators
  iss.str( "127" );
  iss.clear();

  Utility::fromStream( iss, test_byte, Utility::Details::white_space_delims );

  BOOST_CHECK_EQUAL( test_byte, (T)127 );

  iss.str( "a 0\n127\t-" );
  iss.clear();

  Utility::fromStream( iss, test_byte, Utility::Details::white_space_delims );

  BOOST_CHECK_EQUAL( test_byte, (T)'a' );

  Utility::fromStream( iss, test_byte, Utility::Details::white_space_delims );

  BOOST_CHECK_EQUAL( test_byte, (T)'0' );

  Utility::fromStream( iss, test_byte, Utility::Details::white_space_delims );

  BOOST_CHECK_EQUAL( test_byte, (T)127 );

  Utility::fromStream( iss, test_byte, Utility::Details::white_space_delims );

  BOOST_CHECK_EQUAL( test_byte, (T)'-' );

  // Multiple bytes in the same stream with custom deliminators
  iss.str( "a, b, c, d, 127, , -}" );
  iss.clear();

  Utility::fromStream( iss, test_byte, "," );

  BOOST_CHECK_EQUAL( test_byte, (T)'a' );

  Utility::moveInputStreamToNextElement( iss, ',', '}' );

  Utility::fromStream( iss, test_byte, "," );

  BOOST_CHECK_EQUAL( test_byte, (T)'b' );

  Utility::moveInputStreamToNextElement( iss, ',', '}' );

  Utility::fromStream( iss, test_byte, "," );

  BOOST_CHECK_EQUAL( test_byte, (T)'c' );

  Utility::moveInputStreamToNextElement( iss, ',', '}' );

  Utility::fromStream( iss, test_byte, "," );

  BOOST_CHECK_EQUAL( test_byte, (T)'d' );

  Utility::moveInputStreamToNextElement( iss, ',', '}' );

  Utility::fromStream( iss, test_byte, "," );

  BOOST_CHECK_EQUAL( test_byte, (T)127 );

  Utility::moveInputStreamToNextElement( iss, ',', '}' );

  Utility::fromStream( iss, test_byte, "," );

  BOOST_CHECK_EQUAL( test_byte, (T)' ' );

  Utility::moveInputStreamToNextElement( iss, ',', '}' );

  Utility::fromStream( iss, test_byte, "}" );

  BOOST_CHECK_EQUAL( test_byte, (T)'-' );
}

//---------------------------------------------------------------------------//
// Check that signed integer types can be created from a string
BOOST_AUTO_TEST_CASE_TEMPLATE( fromString, T, MultipleByteTypes )
{
  BOOST_CHECK_EQUAL( Utility::fromString<T>( "-10" ), -10 );
  BOOST_CHECK_EQUAL( Utility::fromString<T>( "0" ), 0 );
  BOOST_CHECK_EQUAL( Utility::fromString<T>( " 10 " ), 10 );

  BOOST_CHECK_THROW( Utility::fromString<T>( "0.0" ),
                     Utility::StringConversionException );
  BOOST_CHECK_THROW( Utility::fromString<T>( "0.000000000e+00" ),
                     Utility::StringConversionException );
  BOOST_CHECK_THROW( Utility::fromString<T>( "0 0" ),
                     Utility::StringConversionException );
  BOOST_CHECK_THROW( Utility::fromString<T>( "abc" ),
                     Utility::StringConversionException );
  // Overflow should result in an error: #(2^63) > 2^63 - 1
  BOOST_CHECK_THROW( Utility::fromString<T>( "9223372036854775808" ),
                     Utility::StringConversionException );
}

//---------------------------------------------------------------------------//
// Check that signed integer types can be extracted from a stream
BOOST_AUTO_TEST_CASE_TEMPLATE( fromStream, T, MultipleByteTypes )
{
  std::istringstream iss( "-10" );

  T test_int;

  Utility::fromStream( iss, test_int );

  BOOST_CHECK_EQUAL( test_int, -10 );

  iss.str( "0" );
  iss.clear();

  Utility::fromStream( iss, test_int );

  BOOST_CHECK_EQUAL( test_int, 0 );

  iss.str( " 10 " );
  iss.clear();

  Utility::fromStream( iss, test_int );

  BOOST_CHECK_EQUAL( test_int, 10 );

  // Multiple integers in the same stream with default deliminators
  iss.str( "-10 0   10 " );
  iss.clear();

  Utility::fromStream( iss, test_int );

  BOOST_CHECK_EQUAL( test_int, -10 );

  Utility::fromStream( iss, test_int );

  BOOST_CHECK_EQUAL( test_int, 0 );

  Utility::fromStream( iss, test_int );

  BOOST_CHECK_EQUAL( test_int, 10 );

  // Multiple integers in the same stream with custom deliminators
  iss.str( "-10, 1" );
  iss.clear();

  Utility::fromStream( iss, test_int, "," );

  BOOST_CHECK_EQUAL( test_int, -10 );

  Utility::moveInputStreamToNextElement( iss, ',', '}' );

  Utility::fromStream( iss, test_int );

  BOOST_CHECK_EQUAL( test_int, 1 );
}

//---------------------------------------------------------------------------//
// Check that unsigned integer types can be created from a string
BOOST_AUTO_TEST_CASE_TEMPLATE( unsigned_fromString, T, MultipleByteTypes )
{
  typedef typename std::make_unsigned<T>::type UnsignedT;

  BOOST_CHECK_EQUAL( Utility::fromString<UnsignedT>( "0" ), 0 );
  BOOST_CHECK_EQUAL( Utility::fromString<UnsignedT>( "10" ), 10 );
  BOOST_CHECK_EQUAL( Utility::fromString<UnsignedT>( "255" ), 255 );

  BOOST_CHECK_THROW( Utility::fromString<UnsignedT>( "-10" ),
                     Utility::StringConversionException );
  BOOST_CHECK_THROW( Utility::fromString<UnsignedT>( "0.0" ),
                     Utility::StringConversionException );
  BOOST_CHECK_THROW( Utility::fromString<UnsignedT>( "0.000000000e+00" ),
                     Utility::StringConversionException );
  BOOST_CHECK_THROW( Utility::fromString<UnsignedT>( "-1." ),
                     Utility::StringConversionException );
  BOOST_CHECK_THROW( Utility::fromString<UnsignedT>( "1 1" ),
                     Utility::StringConversionException );
  // Overflow should result in an error: #(2^64) > 2^64 - 1
  BOOST_CHECK_THROW( Utility::fromString<UnsignedT>( "18446744073709551616" ),
                     Utility::StringConversionException );
}

//---------------------------------------------------------------------------//
// Check that unsigned integer types can be extracted from a stream
BOOST_AUTO_TEST_CASE_TEMPLATE( unsigned_fromStream, T, MultipleByteTypes )
{
  std::istringstream iss( "0" );

  T test_int;

  Utility::fromStream( iss, test_int );

  BOOST_CHECK_EQUAL( test_int, 0 );

  iss.str( "10" );
  iss.clear();

  Utility::fromStream( iss, test_int );

  BOOST_CHECK_EQUAL( test_int, 10 );

  iss.str( "255" );
  iss.clear();

  Utility::fromStream( iss, test_int );

  BOOST_CHECK_EQUAL( test_int, 255 );

  // Multiple integers in the same stream with default deliminators
  iss.str( "0 10   255 " );
  iss.clear();

  Utility::fromStream( iss, test_int );

  BOOST_CHECK_EQUAL( test_int, 0 );

  Utility::fromStream( iss, test_int );

  BOOST_CHECK_EQUAL( test_int, 10 );

  Utility::fromStream( iss, test_int );

  BOOST_CHECK_EQUAL( test_int, 255 );

  // Multiple integers in the same stream with custom deliminators
  iss.str( "255, 1" );
  iss.clear();

  Utility::fromStream( iss, test_int, "," );

  BOOST_CHECK_EQUAL( test_int, 255 );

  Utility::moveInputStreamToNextElement( iss, ',', '}' );

  Utility::fromStream( iss, test_int );

  BOOST_CHECK_EQUAL( test_int, 1 );
}

//---------------------------------------------------------------------------//
// Check that a float can be created from a string
BOOST_AUTO_TEST_CASE( float_fromString )
{
  BOOST_CHECK_EQUAL( Utility::fromString<float>( "1" ), 1.0f );
  BOOST_CHECK_EQUAL( Utility::fromString<float>( "-1.0" ), -1.0f );
  BOOST_CHECK_EQUAL( Utility::fromString<float>( "0.000000000e+00" ), 0.0f );
  BOOST_CHECK_EQUAL( Utility::fromString<float>( "inf" ),
                       std::numeric_limits<float>::infinity() );
  BOOST_CHECK_EQUAL( Utility::fromString<float>( "infinity" ),
                       std::numeric_limits<float>::infinity() );
  BOOST_CHECK_EQUAL( Utility::fromString<float>( "Inf" ),
                       std::numeric_limits<float>::infinity() );
  BOOST_CHECK_EQUAL( Utility::fromString<float>( "Infinity" ),
                       std::numeric_limits<float>::infinity() );
  BOOST_CHECK_EQUAL( Utility::fromString<float>( "InFiNiTy" ),
                       std::numeric_limits<float>::infinity() );
  BOOST_CHECK_EQUAL( Utility::fromString<float>( "INF" ),
                       std::numeric_limits<float>::infinity() );
  BOOST_CHECK_EQUAL( Utility::fromString<float>( "INFINITY" ),
                       std::numeric_limits<float>::infinity() );
  BOOST_CHECK_EQUAL( Utility::fromString<float>( "-inf" ),
                       -std::numeric_limits<float>::infinity() );
  BOOST_CHECK_EQUAL( Utility::fromString<float>( "-infinity" ),
                       -std::numeric_limits<float>::infinity() );
  BOOST_CHECK_EQUAL( Utility::fromString<float>( "-Inf" ),
                       -std::numeric_limits<float>::infinity() );
  BOOST_CHECK_EQUAL( Utility::fromString<float>( "-Infinity" ),
                       -std::numeric_limits<float>::infinity() );
  BOOST_CHECK_EQUAL( Utility::fromString<float>( "-InFiNiTy" ),
                       -std::numeric_limits<float>::infinity() );
  BOOST_CHECK_EQUAL( Utility::fromString<float>( "-INF" ),
                       -std::numeric_limits<float>::infinity() );
  BOOST_CHECK_EQUAL( Utility::fromString<float>( "-INFINITY" ),
                       -std::numeric_limits<float>::infinity() );
  BOOST_CHECK_EQUAL( Utility::fromString<float>( "pi" ),
                       (float)Utility::PhysicalConstants::pi );
  BOOST_CHECK_EQUAL( Utility::fromString<float>( " Pi" ),
                       (float)Utility::PhysicalConstants::pi );
  BOOST_CHECK_EQUAL( Utility::fromString<float>( " PI " ),
                       (float)Utility::PhysicalConstants::pi );
  BOOST_CHECK_EQUAL( Utility::fromString<float>( "1*pi" ),
                       (float)Utility::PhysicalConstants::pi );
  BOOST_CHECK_EQUAL( Utility::fromString<float>( "2Pi" ),
                       2*(float)Utility::PhysicalConstants::pi );
  BOOST_CHECK_EQUAL( Utility::fromString<float>( "-pI" ),
                       -(float)Utility::PhysicalConstants::pi );
  BOOST_CHECK_CLOSE_FRACTION( Utility::fromString<float>( "-5*pi" ),
                              -5*(float)Utility::PhysicalConstants::pi,
                              1e-7 );
  BOOST_CHECK_EQUAL( Utility::fromString<float>( "pi/2" ),
                       (float)Utility::PhysicalConstants::pi/2 );
  BOOST_CHECK_EQUAL( Utility::fromString<float>( "-pi/2" ),
                       -(float)Utility::PhysicalConstants::pi/2 );
  BOOST_CHECK_EQUAL( Utility::fromString<float>( "pi/3" ),
                       (float)Utility::PhysicalConstants::pi/3 );
  BOOST_CHECK_EQUAL( Utility::fromString<float>( "2pi/3" ),
                       2*(float)Utility::PhysicalConstants::pi/3 );
  BOOST_CHECK_EQUAL( Utility::fromString<float>( "-3pi/7" ),
                       -3*(float)Utility::PhysicalConstants::pi/7 );
  BOOST_CHECK_CLOSE_FRACTION( Utility::fromString<float>( "5*pi/3" ),
                              5*(float)Utility::PhysicalConstants::pi/3,
                              1e-7 );
  BOOST_CHECK_CLOSE_FRACTION( Utility::fromString<float>( "-5*PI/3" ),
                              -5*(float)Utility::PhysicalConstants::pi/3,
                              1e-7 );

  BOOST_CHECK_THROW( Utility::fromString<float>( "1.0 1.0" ),
                     Utility::StringConversionException );
  BOOST_CHECK_THROW( Utility::fromString<float>( "1 1" ),
                     Utility::StringConversionException );
  BOOST_CHECK_THROW( Utility::fromString<float>( "-pi / 2 5*pi/3" ),
                     Utility::StringConversionException );
  BOOST_CHECK_THROW( Utility::fromString<float>( "1.01.0" ),
                     Utility::StringConversionException );
}

//---------------------------------------------------------------------------//
// Check that a float can be extracted from a stream
BOOST_AUTO_TEST_CASE( float_fromStream )
{
  std::istringstream iss( "1" );

  float test_float;

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float, 1.0f );

  iss.str( "-1.0" );
  iss.clear();

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float, -1.0f );

  iss.str( "0.000000000e+00" );
  iss.clear();

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float, 0.0f );

  iss.str( "inf" );
  iss.clear();

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float, std::numeric_limits<float>::infinity() );

  iss.str( "infinity" );
  iss.clear();

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float, std::numeric_limits<float>::infinity() );

  iss.str( "Inf" );
  iss.clear();

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float, std::numeric_limits<float>::infinity() );

  iss.str( "Infinity" );
  iss.clear();

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float, std::numeric_limits<float>::infinity() );

  iss.str( "InFiNiTy" );
  iss.clear();

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float, std::numeric_limits<float>::infinity() );

  iss.str( "INF" );
  iss.clear();

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float, std::numeric_limits<float>::infinity() );

  iss.str( "INFINITY" );
  iss.clear();

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float, std::numeric_limits<float>::infinity() );

  iss.str( "-inf" );
  iss.clear();

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float, -std::numeric_limits<float>::infinity() );

  iss.str( "-infinity" );
  iss.clear();

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float, -std::numeric_limits<float>::infinity() );

  iss.str( "-Inf" );
  iss.clear();

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float, -std::numeric_limits<float>::infinity() );

  iss.str( "-Infinity" );
  iss.clear();

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float, -std::numeric_limits<float>::infinity() );

  iss.str( "-InFiNiTy" );
  iss.clear();

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float, -std::numeric_limits<float>::infinity() );

  iss.str( "-INF" );
  iss.clear();

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float, -std::numeric_limits<float>::infinity() );

  iss.str( "-INFINITY" );
  iss.clear();

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float, -std::numeric_limits<float>::infinity() );

  iss.str( "pi" );
  iss.clear();

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float, (float)Utility::PhysicalConstants::pi );

  iss.str( " Pi" );
  iss.clear();

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float, (float)Utility::PhysicalConstants::pi );

  iss.str( " PI " );
  iss.clear();

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float, (float)Utility::PhysicalConstants::pi );

  iss.str( "1*pi" );
  iss.clear();

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float, (float)Utility::PhysicalConstants::pi );

  iss.str( "2Pi" );
  iss.clear();

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float, 2*(float)Utility::PhysicalConstants::pi );

  iss.str( "-pI" );
  iss.clear();

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float, -(float)Utility::PhysicalConstants::pi );

  iss.str( "-5*pi" );
  iss.clear();

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_CLOSE_FRACTION( test_float,
                              -5*(float)Utility::PhysicalConstants::pi,
                              1e-7 );

  iss.str( "pi/2" );
  iss.clear();

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float, (float)Utility::PhysicalConstants::pi/2 );

  iss.str( "-pi/2" );
  iss.clear();

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float, -(float)Utility::PhysicalConstants::pi/2 );

  iss.str( "pi/3" );
  iss.clear();

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float, (float)Utility::PhysicalConstants::pi/3 );

  iss.str( "2pi/3" );
  iss.clear();

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float, 2*(float)Utility::PhysicalConstants::pi/3 );

  iss.str( "-3pi/7" );
  iss.clear();

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float,
                       -3*(float)Utility::PhysicalConstants::pi/7 );

  iss.str( "5*pi/3" );
  iss.clear();

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_CLOSE_FRACTION( test_float,
                              5*(float)Utility::PhysicalConstants::pi/3,
                              1e-7 );

  iss.str( "-5*PI/3" );
  iss.clear();

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_CLOSE_FRACTION( test_float,
                              -5*(float)Utility::PhysicalConstants::pi/3,
                              1e-7 );

  // Multiple floats in the same stream with default deliminators
  iss.str( "-Pi/2 5*pi/3  -inf INFTY  1.0e+00 -1.00000e+00 0" );
  iss.clear();

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_CLOSE_FRACTION( test_float,
                              -(float)Utility::PhysicalConstants::pi/2,
                              1e-7 );

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_CLOSE_FRACTION( test_float,
                              5*(float)Utility::PhysicalConstants::pi/3,
                              1e-7 );

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float, -std::numeric_limits<float>::infinity() );

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float, std::numeric_limits<float>::infinity() );

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_CLOSE_FRACTION( test_float, 1.0f, 1e-7 );

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_CLOSE_FRACTION( test_float, -1.0f, 1e-7 );

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_SMALL( test_float, 1e-7f );

  // Multiple floats in the same stream with custom deliminators
  iss.str( "-1, 1.000000000000000000e+00" );
  iss.clear();

  Utility::fromStream( iss, test_float, "," );

  BOOST_CHECK_EQUAL( test_float, -1.0f );

  Utility::moveInputStreamToNextElement( iss, ',', '}' );

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float, 1.0f );

  iss.str( "-1, 2*pi/3" );
  iss.clear();

  Utility::fromStream( iss, test_float, "," );

  BOOST_CHECK_EQUAL( test_float, -1.0f );

  Utility::moveInputStreamToNextElement( iss, ',', '}' );

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float, 2*(float)Utility::PhysicalConstants::pi/3 );

  iss.str( "-PI, Pi/2" );
  iss.clear();

  Utility::fromStream( iss, test_float, "," );

  BOOST_CHECK_EQUAL( test_float, -(float)Utility::PhysicalConstants::pi );

  Utility::moveInputStreamToNextElement( iss, ',', '}' );

  Utility::fromStream( iss, test_float );

  BOOST_CHECK_EQUAL( test_float, (float)Utility::PhysicalConstants::pi/2 );
}

//---------------------------------------------------------------------------//
// Check that a double can be created from a string
BOOST_AUTO_TEST_CASE( double_fromString )
{
  BOOST_CHECK_EQUAL( Utility::fromString<double>( "1" ), 1.0 );
  BOOST_CHECK_EQUAL( Utility::fromString<double>( "-1.0" ), -1.0 );
  BOOST_CHECK_EQUAL( Utility::fromString<double>( "0.000000000000000000e+00" ), 0.0 );
  BOOST_CHECK_EQUAL( Utility::fromString<double>( "inf" ),
                       std::numeric_limits<double>::infinity() );
  BOOST_CHECK_EQUAL( Utility::fromString<double>( "infinity" ),
                       std::numeric_limits<double>::infinity() );
  BOOST_CHECK_EQUAL( Utility::fromString<double>( "Inf" ),
                       std::numeric_limits<double>::infinity() );
  BOOST_CHECK_EQUAL( Utility::fromString<double>( "Infinity" ),
                       std::numeric_limits<double>::infinity() );
  BOOST_CHECK_EQUAL( Utility::fromString<double>( "InFiNiTy" ),
                       std::numeric_limits<double>::infinity() );
  BOOST_CHECK_EQUAL( Utility::fromString<double>( "INF" ),
                       std::numeric_limits<double>::infinity() );
  BOOST_CHECK_EQUAL( Utility::fromString<double>( "INFINITY" ),
                       std::numeric_limits<double>::infinity() );
  BOOST_CHECK_EQUAL( Utility::fromString<double>( "-inf" ),
                       -std::numeric_limits<double>::infinity() );
  BOOST_CHECK_EQUAL( Utility::fromString<double>( "-infinity" ),
                       -std::numeric_limits<double>::infinity() );
  BOOST_CHECK_EQUAL( Utility::fromString<double>( "-Inf" ),
                       -std::numeric_limits<double>::infinity() );
  BOOST_CHECK_EQUAL( Utility::fromString<double>( "-Infinity" ),
                       -std::numeric_limits<double>::infinity() );
  BOOST_CHECK_EQUAL( Utility::fromString<double>( "-InFiNiTy" ),
                       -std::numeric_limits<double>::infinity() );
  BOOST_CHECK_EQUAL( Utility::fromString<double>( "-INF" ),
                       -std::numeric_limits<double>::infinity() );
  BOOST_CHECK_EQUAL( Utility::fromString<double>( "-INFINITY" ),
                       -std::numeric_limits<double>::infinity() );
  BOOST_CHECK_EQUAL( Utility::fromString<double>( "pi" ),
                       Utility::PhysicalConstants::pi );
  BOOST_CHECK_EQUAL( Utility::fromString<double>( " Pi" ),
                       Utility::PhysicalConstants::pi );
  BOOST_CHECK_EQUAL( Utility::fromString<double>( " PI " ),
                       Utility::PhysicalConstants::pi );
  BOOST_CHECK_EQUAL( Utility::fromString<double>( "1*pi" ),
                       Utility::PhysicalConstants::pi );
  BOOST_CHECK_EQUAL( Utility::fromString<double>( "2Pi" ),
                       2*Utility::PhysicalConstants::pi );
  BOOST_CHECK_EQUAL( Utility::fromString<double>( "-pI" ),
                       -Utility::PhysicalConstants::pi );
  BOOST_CHECK_CLOSE_FRACTION( Utility::fromString<double>( "-5*pi" ),
                              -5*Utility::PhysicalConstants::pi,
                              1e-7 );
  BOOST_CHECK_EQUAL( Utility::fromString<double>( "pi/2" ),
                       Utility::PhysicalConstants::pi/2 );
  BOOST_CHECK_EQUAL( Utility::fromString<double>( "-pi/2" ),
                       -Utility::PhysicalConstants::pi/2 );
  BOOST_CHECK_EQUAL( Utility::fromString<double>( "pi/3" ),
                       Utility::PhysicalConstants::pi/3 );
  BOOST_CHECK_EQUAL( Utility::fromString<double>( "2pi/3" ),
                       2*Utility::PhysicalConstants::pi/3 );
  BOOST_CHECK_EQUAL( Utility::fromString<double>( "-3pi/7" ),
                       -3*Utility::PhysicalConstants::pi/7 );
  BOOST_CHECK_CLOSE_FRACTION( Utility::fromString<double>( "5*pi/3" ),
                              5*Utility::PhysicalConstants::pi/3,
                              1e-7 );
  BOOST_CHECK_CLOSE_FRACTION( Utility::fromString<double>( "-5*PI/3" ),
                              -5*Utility::PhysicalConstants::pi/3,
                              1e-7 );

  BOOST_CHECK_THROW( Utility::fromString<double>( "1.0 1.0" ),
                     Utility::StringConversionException );
  BOOST_CHECK_THROW( Utility::fromString<double>( "1 1" ),
                     Utility::StringConversionException );
  BOOST_CHECK_THROW( Utility::fromString<double>( "-pi / 2 5*pi/3" ),
                     Utility::StringConversionException );
  BOOST_CHECK_THROW( Utility::fromString<double>( "1.01.0" ),
                     Utility::StringConversionException );
}

//---------------------------------------------------------------------------//
// Check that a double can be extracted from a stream
BOOST_AUTO_TEST_CASE( double_fromStream )
{
  std::istringstream iss( "1" );

  double test_double;

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double, 1.0 );

  iss.str( "-1.0" );
  iss.clear();

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double, -1.0 );

  iss.str( "0.000000000000000000e+00" );
  iss.clear();

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double, 0.0 );

  iss.str( "inf" );
  iss.clear();

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double, std::numeric_limits<double>::infinity() );

  iss.str( "infinity" );
  iss.clear();

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double, std::numeric_limits<double>::infinity() );

  iss.str( "Inf" );
  iss.clear();

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double, std::numeric_limits<double>::infinity() );

  iss.str( "Infinity" );
  iss.clear();

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double, std::numeric_limits<double>::infinity() );

  iss.str( "InFiNiTy" );
  iss.clear();

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double, std::numeric_limits<double>::infinity() );

  iss.str( "INF" );
  iss.clear();

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double, std::numeric_limits<double>::infinity() );

  iss.str( "INFINITY" );
  iss.clear();

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double, std::numeric_limits<double>::infinity() );

  iss.str( "-inf" );
  iss.clear();

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double, -std::numeric_limits<double>::infinity() );

  iss.str( "-infinity" );
  iss.clear();

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double, -std::numeric_limits<double>::infinity() );

  iss.str( "-Inf" );
  iss.clear();

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double, -std::numeric_limits<double>::infinity() );

  iss.str( "-Infinity" );
  iss.clear();

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double, -std::numeric_limits<double>::infinity() );

  iss.str( "-InFiNiTy" );
  iss.clear();

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double, -std::numeric_limits<double>::infinity() );

  iss.str( "-INF" );
  iss.clear();

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double, -std::numeric_limits<double>::infinity() );

  iss.str( "-INFINITY" );
  iss.clear();

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double, -std::numeric_limits<double>::infinity() );

  iss.str( "pi" );
  iss.clear();

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double, Utility::PhysicalConstants::pi );

  iss.str( " Pi" );
  iss.clear();

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double, Utility::PhysicalConstants::pi );

  iss.str( " PI " );
  iss.clear();

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double, Utility::PhysicalConstants::pi );

  iss.str( "1*pi" );
  iss.clear();

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double, Utility::PhysicalConstants::pi );

  iss.str( "2Pi" );
  iss.clear();

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double, 2*Utility::PhysicalConstants::pi );

  iss.str( "-pI" );
  iss.clear();

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double, -Utility::PhysicalConstants::pi );

  iss.str( "-5*pi" );
  iss.clear();

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_CLOSE_FRACTION( test_double,
                              -5*Utility::PhysicalConstants::pi,
                              1e-14 );

  iss.str( "pi/2" );
  iss.clear();

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double, Utility::PhysicalConstants::pi/2 );

  iss.str( "-pi/2" );
  iss.clear();

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double, -Utility::PhysicalConstants::pi/2 );

  iss.str( "pi/3" );
  iss.clear();

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double, Utility::PhysicalConstants::pi/3 );

  iss.str( "2pi/3" );
  iss.clear();

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double, 2*Utility::PhysicalConstants::pi/3 );

  iss.str( "-3pi/7" );
  iss.clear();

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double,
                       -3*Utility::PhysicalConstants::pi/7 );

  iss.str( "5*pi/3" );
  iss.clear();

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_CLOSE_FRACTION( test_double,
                              5*Utility::PhysicalConstants::pi/3,
                              1e-14 );

  iss.str( "-5*PI/3" );
  iss.clear();

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_CLOSE_FRACTION( test_double,
                              -5*Utility::PhysicalConstants::pi/3,
                              1e-14 );

  // Multiple doubles in the same stream with default deliminators
  iss.str( "-Pi/2 5*pi/3  -inf INFTY  1.0e+00 -1.00000e+00 0" );
  iss.clear();

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_CLOSE_FRACTION( test_double,
                              -Utility::PhysicalConstants::pi/2,
                              1e-14 );

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_CLOSE_FRACTION( test_double,
                              5*Utility::PhysicalConstants::pi/3,
                              1e-14 );

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double, -std::numeric_limits<double>::infinity() );

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double, std::numeric_limits<double>::infinity() );

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_CLOSE_FRACTION( test_double, 1.0, 1e-14 );

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_CLOSE_FRACTION( test_double, -1.0, 1e-14 );

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_SMALL( test_double, 1e-14 );

  // Multiple floats in the same stream with custom deliminators
  iss.str( "-1, 1.000000000e+00" );
  iss.clear();

  Utility::fromStream( iss, test_double, "," );

  BOOST_CHECK_EQUAL( test_double, -1.0 );

  Utility::moveInputStreamToNextElement( iss, ',', '}' );

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double, 1.0 );

  iss.str( "-1, 2*pi/3" );
  iss.clear();

  Utility::fromStream( iss, test_double, "," );

  BOOST_CHECK_EQUAL( test_double, -1.0 );

  Utility::moveInputStreamToNextElement( iss, ',', '}' );

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double, 2*Utility::PhysicalConstants::pi/3 );

  iss.str( "-PI, Pi/2" );
  iss.clear();

  Utility::fromStream( iss, test_double, "," );

  BOOST_CHECK_EQUAL( test_double, -Utility::PhysicalConstants::pi );

  Utility::moveInputStreamToNextElement( iss, ',', '}' );

  Utility::fromStream( iss, test_double );

  BOOST_CHECK_EQUAL( test_double, Utility::PhysicalConstants::pi/2 );
}

//---------------------------------------------------------------------------//
// Check that a std::complex can be created from a string
BOOST_AUTO_TEST_CASE_TEMPLATE( complex_fromString, T, ComplexTestTypes )
{
  std::string complex_string = Utility::toString( std::complex<T>(0, 0) );

  std::complex<T> complex_value =
    Utility::fromString<std::complex<T> >( complex_string );

  BOOST_CHECK_EQUAL( complex_value, std::complex<T>(0, 0) );

  complex_string = Utility::toString( std::complex<T>(1, 0) );
  complex_value = Utility::fromString<std::complex<T> >( complex_string );

  BOOST_CHECK_EQUAL( complex_value, std::complex<T>(1, 0) );

  complex_string = Utility::toString( std::complex<T>(0, 1) );
  complex_value = Utility::fromString<std::complex<T> >( complex_string );

  BOOST_CHECK_EQUAL( complex_value, std::complex<T>(0, 1) );

  complex_string = Utility::toString( std::complex<T>(1, 1) );
  complex_value = Utility::fromString<std::complex<T> >( complex_string );

  BOOST_CHECK_EQUAL( complex_value, std::complex<T>(1, 1) );

  complex_string = Utility::toString( std::complex<T>(2, 2) );
  complex_value = Utility::fromString<std::complex<T> >( complex_string );

  BOOST_CHECK_EQUAL( complex_value, std::complex<T>(2, 2) );

  if( std::is_signed<T>::value )
  {
    complex_string = Utility::toString( std::complex<T>(-1, 0) );
    complex_value = Utility::fromString<std::complex<T> >( complex_string );

    BOOST_CHECK_EQUAL( complex_value, std::complex<T>(-1, 0) );

    complex_string = Utility::toString( std::complex<T>(0, -1) );
    complex_value = Utility::fromString<std::complex<T> >( complex_string );

    BOOST_CHECK_EQUAL( complex_value, std::complex<T>(0, -1) );

    complex_string = Utility::toString( std::complex<T>(-1, -1) );
    complex_value = Utility::fromString<std::complex<T> >( complex_string );

    BOOST_CHECK_EQUAL( complex_value, std::complex<T>(-1, -1) );

    complex_string = Utility::toString( std::complex<T>(-2, -2) );
    complex_value = Utility::fromString<std::complex<T> >( complex_string );

    BOOST_CHECK_EQUAL( complex_value, std::complex<T>(-2, -2) );
  }

  complex_string = std::string("{") + Utility::toString(0) + "," +
    Utility::toString(0) + "," + Utility::toString(0) + "}";

  BOOST_CHECK_THROW( Utility::fromString<std::complex<T> >( complex_string ),
                     Utility::StringConversionException );

  complex_string = std::string("{") + Utility::toString(0) + "}";

  BOOST_CHECK_THROW( Utility::fromString<std::complex<T> >( complex_string ),
                     Utility::StringConversionException );
}

//---------------------------------------------------------------------------//
// Check that a std::complex can be extracted from a stream
BOOST_AUTO_TEST_CASE_TEMPLATE( complex_fromStream, T, ComplexTestTypes )
{
  std::istringstream iss;

  iss.str( Utility::toString( std::complex<T>(0, 0) ) );

  std::complex<T> complex_value;

  Utility::fromStream( iss, complex_value );

  BOOST_CHECK_EQUAL( complex_value, std::complex<T>(0, 0) );

  iss.str( Utility::toString( std::complex<T>(1, 0) ) );
  iss.clear();

  Utility::fromStream( iss, complex_value );

  BOOST_CHECK_EQUAL( complex_value, std::complex<T>(1, 0) );

  iss.str( Utility::toString( std::complex<T>(0, 1) ) );
  iss.clear();

  Utility::fromStream( iss, complex_value );

  BOOST_CHECK_EQUAL( complex_value, std::complex<T>(0, 1) );

  iss.str( Utility::toString( std::complex<T>(1, 1) ) );
  iss.clear();

  Utility::fromStream( iss, complex_value );

  BOOST_CHECK_EQUAL( complex_value, std::complex<T>(1, 1) );

  iss.str( Utility::toString( std::complex<T>(2, 2) ) );
  iss.clear();

  Utility::fromStream( iss, complex_value );

  BOOST_CHECK_EQUAL( complex_value, std::complex<T>(2, 2) );

  // Multiple complex values in the same stream with default deliminators
  iss.str( Utility::toString( std::complex<T>(0, 0) ) + " " +
           Utility::toString( std::complex<T>(1, 0) ) + " " +
           Utility::toString( std::complex<T>(0, 1) ) + "  " +
           Utility::toString( std::complex<T>(1, 1) ) + "\t" +
           Utility::toString( std::complex<T>(2, 2) ) + "\n" +
           Utility::toString( std::complex<T>(3, 3) ) );
  iss.clear();

  Utility::fromStream( iss, complex_value );

  BOOST_CHECK_EQUAL( complex_value, std::complex<T>(0, 0) );

  Utility::fromStream( iss, complex_value );

  BOOST_CHECK_EQUAL( complex_value, std::complex<T>(1, 0) );

  Utility::fromStream( iss, complex_value );

  BOOST_CHECK_EQUAL( complex_value, std::complex<T>(0, 1) );

  Utility::fromStream( iss, complex_value );

  BOOST_CHECK_EQUAL( complex_value, std::complex<T>(1, 1) );

  Utility::fromStream( iss, complex_value );

  BOOST_CHECK_EQUAL( complex_value, std::complex<T>(2, 2) );

  Utility::fromStream( iss, complex_value );

  BOOST_CHECK_EQUAL( complex_value, std::complex<T>(3, 3) );

  // Multiple complex values in the same stream with custom deliminators
  iss.str( Utility::toString( std::complex<T>(0, 0) ) + ", " +
           Utility::toString( std::complex<T>(1, 0) ) + " , " +
           Utility::toString( std::complex<T>(0, 1) ) + ",  " +
           Utility::toString( std::complex<T>(1, 1) ) + ",\t" +
           Utility::toString( std::complex<T>(2, 2) ) + "," +
           Utility::toString( std::complex<T>(3, 3) ) );
  iss.clear();

  Utility::fromStream( iss, complex_value, "," );

  BOOST_CHECK_EQUAL( complex_value, std::complex<T>(0, 0) );

  Utility::moveInputStreamToNextElement( iss, ',', '}' );

  Utility::fromStream( iss, complex_value, "," );

  BOOST_CHECK_EQUAL( complex_value, std::complex<T>(1, 0) );

  Utility::moveInputStreamToNextElement( iss, ',', '}' );

  Utility::fromStream( iss, complex_value, "," );

  BOOST_CHECK_EQUAL( complex_value, std::complex<T>(0, 1) );

  Utility::moveInputStreamToNextElement( iss, ',', '}' );

  Utility::fromStream( iss, complex_value, "," );

  BOOST_CHECK_EQUAL( complex_value, std::complex<T>(1, 1) );

  Utility::moveInputStreamToNextElement( iss, ',', '}' );

  Utility::fromStream( iss, complex_value, "," );

  BOOST_CHECK_EQUAL( complex_value, std::complex<T>(2, 2) );

  Utility::moveInputStreamToNextElement( iss, ',', '}' );

  Utility::fromStream( iss, complex_value );

  BOOST_CHECK_EQUAL( complex_value, std::complex<T>(3, 3) );

  if( std::is_signed<T>::value )
  {
    iss.str( Utility::toString( std::complex<T>(-1, 0) ) );
    iss.clear();

    Utility::fromStream( iss, complex_value );

    BOOST_CHECK_EQUAL( complex_value, std::complex<T>(-1, 0) );

    iss.str( Utility::toString( std::complex<T>(0, -1) ) );
    iss.clear();

    Utility::fromStream( iss, complex_value );

    BOOST_CHECK_EQUAL( complex_value, std::complex<T>(0, -1) );

    iss.str( Utility::toString( std::complex<T>(-1, -1) ) );
    iss.clear();

    Utility::fromStream( iss, complex_value );

    BOOST_CHECK_EQUAL( complex_value, std::complex<T>(-1, -1) );

    iss.str( Utility::toString( std::complex<T>(-2, -2) ) );
    iss.clear();

    Utility::fromStream( iss, complex_value );

    BOOST_CHECK_EQUAL( complex_value, std::complex<T>(-2, -2) );
  }

  iss.str( std::string("{") + Utility::toString(0) + "," +
           Utility::toString(0) + "," + Utility::toString(0) + "}" );
  iss.clear();

  BOOST_CHECK_THROW( Utility::fromStream( iss, complex_value ),
                     Utility::StringConversionException );

  iss.str( std::string("{") + Utility::toString(0) + "}" );
  iss.clear();

  BOOST_CHECK_THROW( Utility::fromStream( iss, complex_value ),
                     Utility::StringConversionException );
}

//---------------------------------------------------------------------------//
// Check that a boost::units::quantity can be created from a string
BOOST_AUTO_TEST_CASE_TEMPLATE( quantity_fromString,
                               QuantityType,
                               TestBasicQuantityTypes )
{
  typedef typename Utility::QuantityTraits<QuantityType>::UnitType UnitType;
  typedef typename Utility::QuantityTraits<QuantityType>::RawType RawType;

  // Basic quantity types
  std::string quantity_string =
    Utility::toString( QuantityType::from_value(RawType(0)) );

  QuantityType quantity =
    Utility::fromString<QuantityType>( quantity_string );

  BOOST_CHECK_EQUAL( quantity, QuantityType::from_value(RawType(0)) );

  quantity_string = Utility::toString( QuantityType::from_value(RawType(1)) );

  quantity = Utility::fromString<QuantityType>( quantity_string );

  BOOST_CHECK_EQUAL( quantity, QuantityType::from_value(RawType(1)) );

  quantity_string = Utility::toString( QuantityType::from_value(RawType(2)) );

  quantity = Utility::fromString<QuantityType>( quantity_string );

  BOOST_CHECK_EQUAL( quantity, QuantityType::from_value(RawType(2)) );

  // Complex quantity types
  typedef std::complex<RawType> ComplexRawType;
  typedef boost::units::quantity<UnitType,ComplexRawType> ComplexQuantityType;

  quantity_string =
    Utility::toString( ComplexQuantityType::from_value(ComplexRawType(0, 0)) );

  ComplexQuantityType complex_quantity =
    Utility::fromString<ComplexQuantityType>( quantity_string );

  BOOST_CHECK_EQUAL( complex_quantity, ComplexQuantityType::from_value(ComplexRawType(0, 0)) );

  quantity_string =
    Utility::toString( ComplexQuantityType::from_value(ComplexRawType(1, 0)) );

  complex_quantity =
    Utility::fromString<ComplexQuantityType>( quantity_string );

  BOOST_CHECK_EQUAL( complex_quantity, ComplexQuantityType::from_value(ComplexRawType(1, 0)) );

  quantity_string =
    Utility::toString( ComplexQuantityType::from_value(ComplexRawType(0, 1)) );

  complex_quantity =
    Utility::fromString<ComplexQuantityType>( quantity_string );

  BOOST_CHECK_EQUAL( complex_quantity, ComplexQuantityType::from_value(ComplexRawType(0, 1)) );

  quantity_string =
    Utility::toString( ComplexQuantityType::from_value(ComplexRawType(1, 1)) );

  complex_quantity =
    Utility::fromString<ComplexQuantityType>( quantity_string );

  BOOST_CHECK_EQUAL( complex_quantity, ComplexQuantityType::from_value(ComplexRawType(1, 1)) );

  quantity_string =
    Utility::toString( ComplexQuantityType::from_value(ComplexRawType(2, 2)) );

  complex_quantity =
    Utility::fromString<ComplexQuantityType>( quantity_string );

  BOOST_CHECK_EQUAL( complex_quantity, ComplexQuantityType::from_value(ComplexRawType(2, 2)) );

  if( Utility::QuantityTraits<QuantityType>::is_signed::value )
  {
    // Basic quantity types
    quantity_string =
      Utility::toString( QuantityType::from_value(RawType(-1)) );

    quantity = Utility::fromString<QuantityType>( quantity_string );

    BOOST_CHECK_EQUAL( quantity, QuantityType::from_value(RawType(-1)) );

    quantity_string =
      Utility::toString( QuantityType::from_value(RawType(-2)) );

    quantity = Utility::fromString<QuantityType>( quantity_string );

    BOOST_CHECK_EQUAL( quantity, QuantityType::from_value(RawType(-2)) );

    // Complex quantity types
    quantity_string =
      Utility::toString( ComplexQuantityType::from_value(ComplexRawType(-1, 0)) );

    complex_quantity =
      Utility::fromString<ComplexQuantityType>( quantity_string );

    BOOST_CHECK_EQUAL( complex_quantity, ComplexQuantityType::from_value(ComplexRawType(-1, 0)) );

    quantity_string =
      Utility::toString( ComplexQuantityType::from_value(ComplexRawType(0, -1)) );

    complex_quantity =
      Utility::fromString<ComplexQuantityType>( quantity_string );

    BOOST_CHECK_EQUAL( complex_quantity, ComplexQuantityType::from_value(ComplexRawType(0, -1)) );

    quantity_string =
      Utility::toString( ComplexQuantityType::from_value(ComplexRawType(-1, -1)) );

    complex_quantity =
      Utility::fromString<ComplexQuantityType>( quantity_string );

    BOOST_CHECK_EQUAL( complex_quantity, ComplexQuantityType::from_value(ComplexRawType(-1, -1)) );

    quantity_string =
      Utility::toString( ComplexQuantityType::from_value(ComplexRawType(-2, -2)) );

    complex_quantity =
      Utility::fromString<ComplexQuantityType>( quantity_string );

    BOOST_CHECK_EQUAL( complex_quantity, ComplexQuantityType::from_value(ComplexRawType(-2, -2)) );
  }
}

//---------------------------------------------------------------------------//
// Check that a boost::units::quantity can be extracted from a stream
BOOST_AUTO_TEST_CASE_TEMPLATE( quantity_fromStream,
                               QuantityType,
                               TestBasicQuantityTypes )
{
  typedef typename Utility::QuantityTraits<QuantityType>::UnitType UnitType;
  typedef typename Utility::QuantityTraits<QuantityType>::RawType RawType;

  std::istringstream iss;

  // Basic quantity types
  iss.str( Utility::toString( QuantityType::from_value(RawType(0)) ) );

  QuantityType quantity;

  Utility::fromStream( iss, quantity );

  BOOST_CHECK_EQUAL( quantity, QuantityType::from_value(RawType(0)) );

  iss.str( Utility::toString( QuantityType::from_value(RawType(1)) ) );
  iss.clear();

  Utility::fromStream( iss, quantity );

  BOOST_CHECK_EQUAL( quantity, QuantityType::from_value(RawType(1)) );

  iss.str( Utility::toString( QuantityType::from_value(RawType(2)) ) );
  iss.clear();

  Utility::fromStream( iss, quantity );

  BOOST_CHECK_EQUAL( quantity, QuantityType::from_value(RawType(2)) );

  // Complex quantity types
  typedef std::complex<RawType> ComplexRawType;
  typedef boost::units::quantity<UnitType,ComplexRawType> ComplexQuantityType;

  iss.str( Utility::toString( ComplexQuantityType::from_value(ComplexRawType(0, 0)) ) );
  iss.clear();

  ComplexQuantityType complex_quantity;

  Utility::fromStream( iss, complex_quantity );

  BOOST_CHECK_EQUAL( complex_quantity, ComplexQuantityType::from_value(ComplexRawType(0, 0)) );

  iss.str( Utility::toString( ComplexQuantityType::from_value(ComplexRawType(1, 0)) ) );
  iss.clear();

  Utility::fromStream( iss, complex_quantity );

  BOOST_CHECK_EQUAL( complex_quantity, ComplexQuantityType::from_value(ComplexRawType(1, 0)) );

  iss.str( Utility::toString( ComplexQuantityType::from_value(ComplexRawType(0, 1)) ) );
  iss.clear();

  Utility::fromStream( iss, complex_quantity );

  BOOST_CHECK_EQUAL( complex_quantity, ComplexQuantityType::from_value(ComplexRawType(0, 1)) );

  iss.str( Utility::toString( ComplexQuantityType::from_value(ComplexRawType(1, 1)) ) );
  iss.clear();

  Utility::fromStream( iss, complex_quantity );

  BOOST_CHECK_EQUAL( complex_quantity, ComplexQuantityType::from_value(ComplexRawType(1, 1)) );

  iss.str( Utility::toString( ComplexQuantityType::from_value(ComplexRawType(2, 2)) ) );
  iss.clear();

  Utility::fromStream( iss, complex_quantity );

  BOOST_CHECK_EQUAL( complex_quantity, ComplexQuantityType::from_value(ComplexRawType(2, 2)) );

  // Multiple quantities in the same stream with default deliminators
  iss.str( Utility::toString( QuantityType::from_value(RawType(0)) ) + " " +
           Utility::toString( QuantityType::from_value(RawType(1)) ) + "  " +
           Utility::toString( ComplexQuantityType::from_value(ComplexRawType(0, 0)) ) + "\t" +
           Utility::toString( ComplexQuantityType::from_value(ComplexRawType(1, 1)) ) + "\n" +
           Utility::toString( ComplexQuantityType::from_value(ComplexRawType(2, 2)) ) );
  iss.clear();

  Utility::fromStream( iss, quantity );

  BOOST_CHECK_EQUAL( quantity, QuantityType::from_value(RawType(0)) );

  Utility::fromStream( iss, quantity );

  BOOST_CHECK_EQUAL( quantity, QuantityType::from_value(RawType(1)) );

  Utility::fromStream( iss, complex_quantity );

  BOOST_CHECK_EQUAL( complex_quantity, ComplexQuantityType::from_value(ComplexRawType(0, 0)) );

  Utility::fromStream( iss, complex_quantity );

  BOOST_CHECK_EQUAL( complex_quantity, ComplexQuantityType::from_value(ComplexRawType(1, 1)) );

  Utility::fromStream( iss, complex_quantity );

  BOOST_CHECK_EQUAL( complex_quantity, ComplexQuantityType::from_value(ComplexRawType(2, 2)) );

  // Multiple quantities in the same stream with custom deliminators
  iss.str( Utility::toString( QuantityType::from_value(RawType(0)) ) + ", " +
           Utility::toString( QuantityType::from_value(RawType(1)) ) + ",  " +
           Utility::toString( ComplexQuantityType::from_value(ComplexRawType(0, 0)) ) + ",\t" +
           Utility::toString( ComplexQuantityType::from_value(ComplexRawType(1, 1)) ) + " ,\n" +
           Utility::toString( ComplexQuantityType::from_value(ComplexRawType(2, 2)) ) );
  iss.clear();

  Utility::fromStream( iss, quantity, "," );

  BOOST_CHECK_EQUAL( quantity, QuantityType::from_value(RawType(0)) );

  Utility::moveInputStreamToNextElement( iss, ',', '}' );

  Utility::fromStream( iss, quantity, "," );

  BOOST_CHECK_EQUAL( quantity, QuantityType::from_value(RawType(1)) );

  Utility::moveInputStreamToNextElement( iss, ',', '}' );

  Utility::fromStream( iss, complex_quantity, "," );

  BOOST_CHECK_EQUAL( complex_quantity, ComplexQuantityType::from_value(ComplexRawType(0, 0)) );

  Utility::moveInputStreamToNextElement( iss, ',', '}' );

  Utility::fromStream( iss, complex_quantity, "," );

  BOOST_CHECK_EQUAL( complex_quantity, ComplexQuantityType::from_value(ComplexRawType(1, 1)) );

  Utility::moveInputStreamToNextElement( iss, ',', '}' );

  Utility::fromStream( iss, complex_quantity );

  BOOST_CHECK_EQUAL( complex_quantity, ComplexQuantityType::from_value(ComplexRawType(2, 2)) );

  if( Utility::QuantityTraits<QuantityType>::is_signed::value )
  {
    // Basic quantity types
    iss.str( Utility::toString( QuantityType::from_value(RawType(-1)) ) );
    iss.clear();

    Utility::fromStream( iss, quantity );

    BOOST_CHECK_EQUAL( quantity, QuantityType::from_value(RawType(-1)) );

    iss.str( Utility::toString( QuantityType::from_value(RawType(-2)) ) );
    iss.clear();

    Utility::fromStream( iss, quantity );

    BOOST_CHECK_EQUAL( quantity, QuantityType::from_value(RawType(-2)) );

    // Complex quantity types
    iss.str( Utility::toString( ComplexQuantityType::from_value(ComplexRawType(-1, 0)) ) );
    iss.clear();

    Utility::fromStream( iss, complex_quantity );

    BOOST_CHECK_EQUAL( complex_quantity, ComplexQuantityType::from_value(ComplexRawType(-1, 0)) );

    iss.str( Utility::toString( ComplexQuantityType::from_value(ComplexRawType(0, -1)) ) );
    iss.clear();

    Utility::fromStream( iss, complex_quantity );

    BOOST_CHECK_EQUAL( complex_quantity, ComplexQuantityType::from_value(ComplexRawType(0, -1)) );

    iss.str( Utility::toString( ComplexQuantityType::from_value(ComplexRawType(-1, -1)) ) );
    iss.clear();

    Utility::fromStream( iss, complex_quantity );

    BOOST_CHECK_EQUAL( complex_quantity, ComplexQuantityType::from_value(ComplexRawType(-1, -1)) );

    iss.str( Utility::toString( ComplexQuantityType::from_value(ComplexRawType(-2, -2)) ) );
    iss.clear();

    Utility::fromStream( iss, complex_quantity );

    BOOST_CHECK_EQUAL( complex_quantity, ComplexQuantityType::from_value(ComplexRawType(-2, -2)) );
  }
}

//---------------------------------------------------------------------------//
// end tstFromStringTraits.cpp
//---------------------------------------------------------------------------//


text:
#pragma once

#ifdef _MSC_VER
#pragma warning( push )
#pragma warning( disable : 4996)
#endif
#include <boost/numeric/ublas/blas.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#ifdef _MSC_VER
#pragma warning( pop )
#endif

namespace dungeng
{
    namespace blas = ::boost::numeric::ublas;

    /*template <size_t N>
    using vec = blas::bounded_vector<double, N>;*/

    template <size_t N>
    class vec : public blas::bounded_vector<double, N>
    {
    private:
        using base = blas::bounded_vector<double, N>;
        using ArrayOfDouble = const double[N];

    public:
        using base::base;

        constexpr vec(ArrayOfDouble& src)
            : base(Initer(src))
        {
        }

    private:
        constexpr static base Initer(ArrayOfDouble& src)
        {
            base result;
            std::copy(std::cbegin(src), std::cend(src), std::begin(result));
            return result;
        }
    };

    /*template <size_t M, size_t N>
    using mat = blas::bounded_matrix<double, M, N>;*/

    template <size_t M, size_t N>
    class mat : public blas::bounded_matrix<double, M, N>
    {
	private:
		using base = blas::bounded_matrix<double, M, N>;
        using ArrayOfDouble = const double[N];
        using ArrayWithArrayOfDouble = const ArrayOfDouble[M];

    public:
        using base::base;

        constexpr mat(ArrayWithArrayOfDouble& src)
            : base(Initer(src))
        {
        }

    private:
        constexpr static base Initer(ArrayWithArrayOfDouble& src)
        {
            base result;
            for (size_t i = 0; i < std::size(src); ++i)
                for (size_t j = 0; j < std::size(src[i]); ++j)
                    result(i, j) = src[i][j];
            return result;
        }
    };

    using vec2 = vec<2>;
    using vec3 = vec<3>;

    using mat33 = mat<3, 3>;

    mat33 rotate_matrix_ccw(double angle);
    mat33 translate_matrix(const vec3& xy);
    mat33 translate_matrix(const vec2& xy);
    mat33 scale_matrix(const vec3& xy);
    mat33 scale_matrix(const vec2& xy);
}

max_stars_repo_name: SergeyZhuravlev/DungeonEngine

max_issues_repo_name: SergeyZhuravlev/DungeonEngine

max_forks_repo_name: SergeyZhuravlev/DungeonEngine


text:
/*
 * TileLoader.cpp
 *
 *  Copyright (c) 2019 Markus Lamprecht BSD license.
 *
 *  This file is part of rviz_pics
 *
 *	Created on: 13/03/2019
 */

#include "tileloader.h"

#include <QDir>
#include <QFile>
#include <QImage>
#include <QImageReader>
#include <QNetworkProxy>
#include <QNetworkRequest>
#include <QStringList>
#include <QUrl>
#include <QVariant>
#include <boost/regex.hpp>
#include <functional> // for std::hash
#include <ros/package.h>
#include <ros/ros.h>
#include <stdexcept>

TileLoader::TileLoader(QObject *parent) : QObject(parent) {

}

void TileLoader::start() {
  //  discard previous set of tiles and all pending requests
  abort();

  const std::string package_path = ros::package::getPath("rviz_pics");
  QString my_path = QDir::cleanPath(QString::fromStdString(package_path) +
                                    QDir::separator() + QString("map"));

  QDir directory(my_path);
  QStringList image_names = directory.entryList(QStringList() << "*.jpg"
                                                              << "*.JPG",
                                                QDir::Files);

  std::cout << "Number of Images to add: " << image_names.length() << std::endl;

  // /home/markus/ros_space/alfons_ws/src/alfons/rviz_pics/storage_pics/5484377250584657929/x0_y0_z0_0_0_0_0.jpg
  // TODO:
  for (int i = 0; i < image_names.length(); i++) {
    const QString full_path = my_path + QDir::separator() + image_names[i];
    QStringList list = image_names[i].split('_');

    for(int j=0;j<list.length();j++)
    {
      std::cout<<list[j].toUtf8().constData()<<std::endl;
    }

    double x = list[0].toDouble();
    double y = list[1].toDouble();
    double z = list[2].toDouble();
    double xq = list[3].toDouble();
    double yq = list[4].toDouble();
    double zq = list[5].toDouble();
    double wq = list[6].toDouble();
    std::cout << x << " " << y << " " << z << "      " << xq << " " << yq << " "
              << zq << " " << wq << std::endl;
    QFile tile(full_path);
    if (tile.exists()) {
      QImage image(full_path);
      my_tiles_.push_back(
          MapTile(x, y, z, xq, yq, zq, wq,
                  image)); // The tiles that are pushed back in here are loaded!
      std::cout << "Img found at: " << full_path.toUtf8().constData()
                << " was added" << std::endl;
    } else {
      std::cout << "Img NOT found at: " << full_path.toUtf8().constData()
                << " not added" << std::endl;
    }
  }

  checkIfLoadingComplete();
}

double TileLoader::resolution() const { return 0.026458333; } // 0.00026458333 // um faktor 1000 groeßer

bool TileLoader::MapTile::hasImage() const { return !image_.isNull(); }

bool TileLoader::checkIfLoadingComplete() {
  const bool loaded =
      std::all_of(my_tiles_.begin(), my_tiles_.end(),
                  [](const MapTile &tile) { return tile.hasImage(); });
  if (loaded) {
    emit finishedLoading();
  }
  return loaded;
}

void TileLoader::abort() { my_tiles_.clear(); }


text:
#include "vector.h"
#include "MinIMU9.h"
#include "version.h"
#include <iostream>
#include <iomanip>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <sys/time.h>
#include <system_error>
#include <boost/program_options.hpp>

#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/MagneticField.h"

#define LOOP_RATE 10

namespace opts = boost::program_options;

// TODO: print warning if accelerometer magnitude is not close to 1 when starting up

// An Euler angle could take 8 chars: -234.678, but usually we only need 6.
float field_width = 6;
#define FLOAT_FORMAT std::fixed << std::setprecision(3) << std::setw(field_width)

std::ostream & operator << (std::ostream & os, const vector & vector)
{
    return os << FLOAT_FORMAT << vector(0) << ' '
              << FLOAT_FORMAT << vector(1) << ' '
              << FLOAT_FORMAT << vector(2);
}

std::ostream & operator << (std::ostream & os, const matrix & matrix)
{
    return os << (vector)matrix.row(0) << ' '
              << (vector)matrix.row(1) << ' '
              << (vector)matrix.row(2);
}

std::ostream & operator << (std::ostream & os, const quaternion & quat)
{
    return os << FLOAT_FORMAT << quat.w() << ' '
              << FLOAT_FORMAT << quat.x() << ' '
              << FLOAT_FORMAT << quat.y() << ' '
              << FLOAT_FORMAT << quat.z();
}

typedef void rotation_output_function(quaternion& rotation);

void output_quaternion(quaternion & rotation)
{
    std::cout << rotation;
}

void output_matrix(quaternion & rotation)
{
    std::cout << rotation.toRotationMatrix();
}

void output_euler(quaternion & rotation)
{
    std::cout << (vector)(rotation.toRotationMatrix().eulerAngles(2, 1, 0) * (180 / M_PI));
}

int millis()
{
/*    struct timeval tv;
    gettimeofday(&tv, NULL);
    return (tv.tv_sec) * 1000 + (tv.tv_usec)/1000;*/
    return ros::Time::now().toSec();
}

void streamRawValues(IMU& imu)
{
    imu.enable();
    while(1)
    {
        imu.read();
        printf("%7d %7d %7d  %7d %7d %7d  %7d %7d %7d\n",
               imu.raw_m[0], imu.raw_m[1], imu.raw_m[2],
               imu.raw_a[0], imu.raw_a[1], imu.raw_a[2],
               imu.raw_g[0], imu.raw_g[1], imu.raw_g[2]
        );
        usleep(20*1000);
    }
}

//! Uses the acceleration and magnetic field readings from the compass
// to get a noisy estimate of the current rotation matrix.
// This function is where we define the coordinate system we are using
// for the ground coords:  North, East, Down.
matrix rotationFromCompass(const vector& acceleration, const vector& magnetic_field)
{
    vector down = -acceleration;     // usually true
    vector east = down.cross(magnetic_field); // actually it's magnetic east
    vector north = east.cross(down);

    east.normalize();
    north.normalize();
    down.normalize();

    matrix r;
    r.row(0) = north;
    r.row(1) = east;
    r.row(2) = down;
    return r;
}

typedef void fuse_function(quaternion& rotation, float dt, const vector& angular_velocity,
                  const vector& acceleration, const vector& magnetic_field);

void fuse_compass_only(quaternion& rotation, float dt, const vector& angular_velocity,
  const vector& acceleration, const vector& magnetic_field)
{
    // Implicit conversion of rotation matrix to quaternion.
    rotation = rotationFromCompass(acceleration, magnetic_field);
}

// Uses the given angular velocity and time interval to calculate
// a rotation and applies that rotation to the given quaternion.
// w is angular velocity in radians per second.
// dt is the time.
void rotate(quaternion& rotation, const vector& w, float dt)
{
    // Multiply by first order approximation of the
    // quaternion representing this rotation.
    rotation *= quaternion(1, w(0)*dt/2, w(1)*dt/2, w(2)*dt/2);
    rotation.normalize();
}

void fuse_gyro_only(quaternion& rotation, float dt, const vector& angular_velocity,
  const vector& acceleration, const vector& magnetic_field)
{
    rotate(rotation, angular_velocity, dt);
}

void fuse_default(quaternion& rotation, float dt, const vector& angular_velocity,
  const vector& acceleration, const vector& magnetic_field)
{
    vector correction = vector(0, 0, 0);

    if (abs(acceleration.norm() - 1) <= 0.3)
    {
        // The magnetidude of acceleration is close to 1 g, so
        // it might be pointing up and we can do drift correction.

        const float correction_strength = 1;

        matrix rotationCompass = rotationFromCompass(acceleration, magnetic_field);
        matrix rotationMatrix = rotation.toRotationMatrix();

        correction = (
            rotationCompass.row(0).cross(rotationMatrix.row(0)) +
            rotationCompass.row(1).cross(rotationMatrix.row(1)) +
            rotationCompass.row(2).cross(rotationMatrix.row(2))
          ) * correction_strength;

    }

    rotate(rotation, angular_velocity + correction, dt);
}

void ahrs(IMU & imu, fuse_function * fuse, rotation_output_function * output)
{
    imu.loadCalibration();
    imu.enable();
    imu.measureOffsets();

    // The quaternion that can convert a vector in body coordinates
    // to ground coordinates when it its changed to a matrix.
    quaternion rotation = quaternion::Identity();

    int start = millis(); // truncate 64-bit return value
    ros::NodeHandle nh;
    ros::Publisher imu_publisher = nh.advertise<sensor_msgs::Imu>("imu", 1);
    ros::Publisher imu_raw_publisher = nh.advertise<sensor_msgs::Imu>("imu/raw", 1);
    ros::Publisher magnetic_field_publisher = nh.advertise<sensor_msgs::MagneticField>("magnetic_field", 1);
    ros::Rate rate(LOOP_RATE);
    while(ros::ok())
    {
        int last_start = start;
        start = millis();
        float dt = (start-last_start)/1000.0;
        if (dt < 0){ throw std::runtime_error("Time went backwards."); }

        vector angular_velocity = imu.readGyro();
        vector acceleration = imu.readAcc();
        vector magnetic_field = imu.readMag();

        ros::Time generated_message_time = ros::Time::now();

        sensor_msgs::MagneticField magnetic_field_raw;
        magnetic_field_raw.header.stamp = generated_message_time;
        magnetic_field_raw.header.frame_id = "imu_link"
        magnetic_field_raw.magnetic_field.x = magnetic_field[0];
        magnetic_field_raw.magnetic_field.y = magnetic_field[1];
        magnetic_field_raw.magnetic_field.z = magnetic_field[2];

        sensor_msgs::IMU imu_msg_raw;
        imu_msg_raw.header.stamp = generated_message_time;
        imu_msg_raw.header.frame_id = "imu_link"
        imu_msg_raw.linear_acceleration.x = acceleration[0];
        imu_msg_raw.linear_acceleration.y = acceleration[1];
        imu_msg_raw.linear_acceleration.z = acceleration[2];

        imu_msg_raw.angular_velocity.x = angular_velocity[0];
        imu_msg_raw.angular_velocity.y = angular_velocity[1];
        imu_msg_raw.angular_velocity.z = angular_velocity[2];

        fuse(rotation, dt, angular_velocity, acceleration, magnetic_field);

        sensor_msgs::IMU imu_msg;
        imu_msg.header.stamp = generated_message_time;
        imu_msg.header.frame_id = "imu_link"
        imu_msg.orientation.x = rotation.x()
        imu_msg.orientation.y = rotation.y()
        imu_msg.orientation.z = rotation.z()
        imu_msg.orientation.w = rotation.w()

        imu_msg.angular_velocity.x = angular_velocity[0];
        imu_msg.angular_velocity.y = angular_velocity[1];
        imu_msg.angular_velocity.z = angular_velocity[2];

        imu_msg.linear_acceleration.x = acceleration[0];
        imu_msg.linear_acceleration.y = acceleration[1];
        imu_msg.linear_acceleration.z = acceleration[2];

        imu_raw_publisher.publish(imu_msg_raw);
        magnetic_field_publisher.publish(magnetic_field_raw);
        imu_publisher.publish(imu_msg);

        //output(rotation);
        //std::cout << "  " << acceleration << "  " << magnetic_field << std::endl << std::flush;

        // Ensure that each iteration of the loop takes at least 20 ms.
        /*while(millis() - start < 20)
        {
            usleep(1000);
        }*/

        ros::spinOnce(); // Need to call this function often to allow ROS to process incoming messages
        rate.sleep(); // Sleep for the rest of the cycle, to enforce the FSM loop rate
    }
}

int main(int argc, char *argv[])
{
    try
    {
        // Define what all the command-line parameters are.
        std::string mode, output_mode, i2cDevice;
        opts::options_description desc("Allowed options");
        desc.add_options()
            ("help,h", "produce help message")
            ("version,v", "print version number")
            ("i2c-bus,b", opts::value<std::string>(&i2cDevice)->default_value("/dev/i2c-0"),
             "i2c-bus the IMU is connected to")
            ("mode", opts::value<std::string>(&mode)->default_value("normal"),
             "specifies what algorithm to use.\n"
             "normal: Fuse compass and gyro.\n"
             "gyro-only:  Use only gyro (drifts).\n"
             "compass-only:  Use only compass (noisy).\n"
             "raw: Just print raw values from sensors.")
            ("output", opts::value<std::string>(&output_mode)->default_value("matrix"),
             "specifies how to output the orientation.\n"
             "matrix: Direction Cosine Matrix.\n"
             "quaternion: Quaternion.\n"
             "euler: Euler angles (yaw, pitch, roll).\n")
            ;
        opts::variables_map options;
        opts::store(opts::command_line_parser(argc, argv).options(desc).run(), options);
        opts::notify(options);

        if(options.count("help"))
        {
            std::cout << desc << std::endl;
            std::cout << "For more information, run: man minimu9-ahrs" << std::endl;
            return 0;
        }

        if (options.count("version"))
        {
            std::cout << VERSION << std::endl;
            return 0;
        }

        MinIMU9 imu(i2cDevice.c_str());

        rotation_output_function * output;

        // Figure out the output mode.
        if (output_mode == "matrix")
        {
            output = &output_matrix;
        }
        else if (output_mode == "quaternion")
        {
            output = &output_quaternion;
        }
        else if (output_mode == "euler")
        {
            field_width += 2;  // See comment above for field_width.
            output = &output_euler;
        }
        else
        {
            std::cerr << "Unknown output mode '" << output_mode << "'" << std::endl;
            return 1;
        }

        ros::init(argc, argv, "minimu9_ahrs");

        // Figure out the basic operating mode and start running.
        if (mode == "raw")
        {
            streamRawValues(imu);
        }
        else if (mode == "gyro-only")
        {
            ahrs(imu, &fuse_gyro_only, output);
        }
        else if (mode == "compass-only")
        {
            ahrs(imu, &fuse_compass_only, output);
        }
        else if (mode == "normal")
        {
            ahrs(imu, &fuse_default, output);
        }
        else
        {
            std::cerr << "Unknown mode '" << mode << "'" << std::endl;
            return 1;
        }
        return 0;
    }
    catch(const std::system_error & error)
    {
        std::string what = error.what();
        const std::error_code & code = error.code();
        std::cerr << "Error: " << what << "  " << code.message() << " (" << code << ")" << std::endl;
        return 2;
    }
    catch(const opts::multiple_occurrences & error)
    {
        std::cerr << "Error: " << error.what() << " of " << error.get_option_name() << " option." << std::endl;
        return 1;
    }
    catch(const std::exception & error)
    {
        std::cerr << "Error: " << error.what() << std::endl;
        return 9;
    }
}