// Copyright John Maddock 2006.
// Use, modification and distribution are subject to 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)

#include <boost/math/bindings/rr.hpp>
//#include <boost/math/tools/dual_precision.hpp>
#include <boost/math/tools/test_data.hpp>
#include <boost/test/included/test_exec_monitor.hpp>
#include <boost/math/special_functions/ellint_3.hpp>
#include <fstream>
#include <boost/math/tools/test_data.hpp>
#include <boost/tr1/random.hpp>

float extern_val;
// confuse the compilers optimiser, and force a truncation to float precision:
float truncate_to_float(float const * pf)
{
   extern_val = *pf;
   return *pf;
}

boost::math::tuple<boost::math::ntl::RR, boost::math::ntl::RR, boost::math::ntl::RR, boost::math::ntl::RR> generate_rf_data(boost::math::ntl::RR n)
{
   static std::tr1::mt19937 r;
   std::tr1::uniform_real<float> ur(0, 1);
   std::tr1::uniform_int<int> ui(-100, 100);
   float x = ur(r);
   x = ldexp(x, ui(r));
   boost::math::ntl::RR xr(truncate_to_float(&x));
   float y = ur(r);
   y = ldexp(y, ui(r));
   boost::math::ntl::RR yr(truncate_to_float(&y));
   float z = ur(r);
   z = ldexp(z, ui(r));
   boost::math::ntl::RR zr(truncate_to_float(&z));

   boost::math::ntl::RR result = boost::math::ellint_rf(xr, yr, zr);
   return boost::math::make_tuple(xr, yr, zr, result);
}

boost::math::tuple<boost::math::ntl::RR, boost::math::ntl::RR, boost::math::ntl::RR> generate_rc_data(boost::math::ntl::RR n)
{
   static std::tr1::mt19937 r;
   std::tr1::uniform_real<float> ur(0, 1);
   std::tr1::uniform_int<int> ui(-100, 100);
   float x = ur(r);
   x = ldexp(x, ui(r));
   boost::math::ntl::RR xr(truncate_to_float(&x));
   float y = ur(r);
   y = ldexp(y, ui(r));
   boost::math::ntl::RR yr(truncate_to_float(&y));

   boost::math::ntl::RR result = boost::math::ellint_rc(xr, yr);
   return boost::math::make_tuple(xr, yr, result);
}

boost::math::tuple<boost::math::ntl::RR, boost::math::ntl::RR, boost::math::ntl::RR, boost::math::ntl::RR, boost::math::ntl::RR> generate_rj_data(boost::math::ntl::RR n)
{
   static std::tr1::mt19937 r;
   std::tr1::uniform_real<float> ur(0, 1);
   std::tr1::uniform_real<float> nur(-1, 1);
   std::tr1::uniform_int<int> ui(-100, 100);
   float x = ur(r);
   x = ldexp(x, ui(r));
   boost::math::ntl::RR xr(truncate_to_float(&x));
   float y = ur(r);
   y = ldexp(y, ui(r));
   boost::math::ntl::RR yr(truncate_to_float(&y));
   float z = ur(r);
   z = ldexp(z, ui(r));
   boost::math::ntl::RR zr(truncate_to_float(&z));
   float p = nur(r);
   p = ldexp(p, ui(r));
   boost::math::ntl::RR pr(truncate_to_float(&p));

   boost::math::ellint_rj(x, y, z, p);

   boost::math::ntl::RR result = boost::math::ellint_rj(xr, yr, zr, pr);
   return boost::math::make_tuple(xr, yr, zr, pr, result);
}

boost::math::tuple<boost::math::ntl::RR, boost::math::ntl::RR, boost::math::ntl::RR, boost::math::ntl::RR> generate_rd_data(boost::math::ntl::RR n)
{
   static std::tr1::mt19937 r;
   std::tr1::uniform_real<float> ur(0, 1);
   std::tr1::uniform_int<int> ui(-100, 100);
   float x = ur(r);
   x = ldexp(x, ui(r));
   boost::math::ntl::RR xr(truncate_to_float(&x));
   float y = ur(r);
   y = ldexp(y, ui(r));
   boost::math::ntl::RR yr(truncate_to_float(&y));
   float z = ur(r);
   z = ldexp(z, ui(r));
   boost::math::ntl::RR zr(truncate_to_float(&z));

   boost::math::ntl::RR result = boost::math::ellint_rd(xr, yr, zr);
   return boost::math::make_tuple(xr, yr, zr, result);
}

int test_main(int argc, char*argv [])
{
   using namespace boost::math::tools;

   boost::math::ntl::RR::SetOutputPrecision(50);
   boost::math::ntl::RR::SetPrecision(1000);

   parameter_info<boost::math::ntl::RR> arg1, arg2;
   test_data<boost::math::ntl::RR> data;

   bool cont;
   std::string line;

   if(argc < 1)
      return 1;

   do{
      int count;
      std::cout << "Number of points: ";
      std::cin >> count;

      arg1 = make_periodic_param(boost::math::ntl::RR(0), boost::math::ntl::RR(1), count);
      arg1.type |= dummy_param;

      //
      // Change this next line to get the R variant you want:
      //
      data.insert(&generate_rd_data, arg1);

      std::cout << "Any more data [y/n]?";
      std::getline(std::cin, line);
      boost::algorithm::trim(line);
      cont = (line == "y");
   }while(cont);

   std::cout << "Enter name of test data file [default=ellint_rf_data.ipp]";
   std::getline(std::cin, line);
   boost::algorithm::trim(line);
   if(line == "")
      line = "ellint_rf_data.ipp";
   std::ofstream ofs(line.c_str());
   line.erase(line.find('.'));
   ofs << std::scientific;
   write_code(ofs, data, line.c_str());

   return 0;
}


#include <cmath>
#include <vector>
#include <string>
#include "stairs_recognition/common.h"
#include "stairs_recognition/tic_toc.h"
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/obj_io.h>
#include <pcl/PolygonMesh.h>
#include <pcl/point_cloud.h>
#include <pcl/io/vtk_lib_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/sample_consensus/sac_model_sphere.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/sample_consensus/sac_model_perpendicular_plane.h>
#include <pcl/filters/passthrough.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <ceres/ceres.h>
#include <pcl/features/vfh.h>
#include <pcl/features/normal_3d.h>
#include <boost/thread/thread.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/io/pcd_io.h>
#include <pcl/console/parse.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <sstream>
#include<vector>

#include<mutex>

#define PI 3.1415926

using std::atan2;
using std::cos;
using std::sin;

double lidar_z_height = -1;

ros::Publisher pubForwardCloud;

struct LineFitCost
{
    LineFitCost(double x,double y):_x(x),_y(y){}
    template<typename T>
    bool operator()(const T* const ab,T* residual)const
    {
        //y=ax+b
        residual[0]=T(_y)-(ab[0]*T(_x)+ab[1]);
        return true;

    }
    const double _x,_y;

};

void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{

    TicToc t_whole;
    TicToc t_prepare;

    pcl::PointCloud<pcl::PointXYZ>::Ptr laserCloudIn(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr laserCloudInROI(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn);

    for (int i=0; i<laserCloudIn->points.size(); i++)
    {
        double x = laserCloudIn->points[i].x;
        double y = laserCloudIn->points[i].y;
        double z = laserCloudIn->points[i].z;
        double r = std::sqrt(x*x+y*y+z*z);

        double theta = atan(y/x);

        if (theta < 0 && y > 0)
        {
            theta = theta + PI;
        }
        else if (theta > 0 && y < 0)
        {
            theta = theta - PI;
        }

        if (theta < 15*PI/180 && theta>-15*PI/180)
        // if (theta < 105*PI/180 && theta>75*PI/180)
        {
            laserCloudInROI->points.push_back(laserCloudIn->points[i]);
        }

    }

    sensor_msgs::PointCloud2 forwardCloudMsg;
    pcl::toROSMsg(*laserCloudInROI, forwardCloudMsg);
    forwardCloudMsg.header.stamp = laserCloudMsg->header.stamp;
    forwardCloudMsg.header.frame_id = "/rslidar";
    pubForwardCloud.publish(forwardCloudMsg);

    std::vector<double> x_data,y_data;
    for(int i=0;i<laserCloudInROI->points.size();++i)
    {
        x_data.push_back(laserCloudInROI->points[i].x);
        y_data.push_back(laserCloudInROI->points[i].z);
    }

    double ab[2]={};

    ceres::Problem problem;
    for(int i=0;i<laserCloudInROI->points.size();++i)
    {
        ceres::CostFunction *costfunction=new ceres::AutoDiffCostFunction<LineFitCost,1,2>(new LineFitCost(x_data[i],y_data[i]));

        problem.AddResidualBlock(costfunction,nullptr,ab);
    }

    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_SCHUR;
    options.minimizer_progress_to_stdout = false;
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    // std::cout << summary.BriefReport() << "\n";
    // cout<<"a,b:"<<endl;
    cout<<"final cost:"<<summary.final_cost<<endl;
    // for(auto i:ab)
    // {
    //     cout<<i<<endl;
    // }
    if (summary.final_cost < 10 && ab[0] <1 && ab[0] >0)
    {
        std::cout<< "Find the stairs"<<std::endl;
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "collect_datasets");
    ros::NodeHandle nh;

    ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/rslidar_points", 1, laserCloudHandler);
    pubForwardCloud = nh.advertise<sensor_msgs::PointCloud2>("/forward_points", 1);

    ros::MultiThreadedSpinner spinner(1); // Use x threads
    spinner.spin(); // spin() will not return until the node has been shutdown

    return 0;
}


#include <iostream>
#include <boost/program_options.hpp>

#include <El.hpp>
#include <boost/mpi.hpp>
#include <boost/format.hpp>

#define SKYLARK_NO_ANY
#include <skylark.hpp>

int seed, port;
std::string fname, outfile, hdfs;
bool as_sparse, high, use_single, directory;

namespace bpo = boost::program_options;

template<typename InputType, typename RhsType, typename SolType>
void execute(skylark::base::context_t &context) {

    boost::mpi::communicator world;
    int rank = world.rank();

    InputType A;
    RhsType b;

    boost::mpi::timer timer;

    // Load A and Y (Y is thrown away)
    if (rank == 0) {
        std::cout << "Reading the matrix... ";
        std::cout.flush();
        timer.restart();
    }

    if (!hdfs.empty()) {
#       if SKYLARK_HAVE_LIBHDFS

        hdfsFS fs;
        if (rank == 0)
            fs = hdfsConnect(hdfs.c_str(), port);

        skylark::utility::io::ReadLIBSVM(fs, fname, A, b, skylark::base::ROWS);

#       else

        SKYLARK_THROW_EXCEPTION(skylark::base::io_exception() <<
            skylark::base::error_msg("Install libhdfs for HDFS support!"));

#       endif
    } else {
        if (directory)
            skylark::utility::io::ReadDirLIBSVM(fname, A, b, skylark::base::ROWS);
        else
            skylark::utility::io::ReadLIBSVM(fname, A, b, skylark::base::ROWS);
    }

    if (rank == 0)
        std::cout <<"took " << boost::format("%.2e") % timer.elapsed()
                  << " sec\n";

    /* Compute approximate least squares */
    if (rank == 0) {
        std::cout << "Solving the least squares...";
        std::cout.flush();
        timer.restart();
    }

    // TODO shouldn't sizes be set inside?
    El::Int m = skylark::base::Height(A);
    El::Int n = skylark::base::Width(A);
    El::Int k = skylark::base::Width(b);

    SolType x(n, k);
    if (high)
        skylark::nla::FasterLeastSquares(El::NORMAL, A, b, x, context);
    else
        skylark::nla::ApproximateLeastSquares(El::NORMAL, A, b, x, context);

    if (rank == 0)
        std::cout <<"Took " << boost::format("%.2e") % timer.elapsed()
                  << " sec\n";

    /* Write results */
    if (rank == 0) {
        std::cout << "Writing results...";
        std::cout.flush();
        timer.restart();
    }

    El::Write(x, outfile, El::ASCII);

    if (rank == 0)
        std::cout <<"took " << boost::format("%.2e") % timer.elapsed()
                  << " sec\n";
}

int main(int argc, char* argv[]) {

    El::Initialize(argc, argv);

    boost::mpi::communicator world;
    int rank = world.rank();

    // Parse options
    bpo::options_description desc("Options");
    desc.add_options()
        ("help,h", "produce a help message")
        ("inputfile",
            bpo::value<std::string>(&fname),
            "Input file to run approximate linear regression on (libsvm format).")
        ("directory,d", "Whether inputfile is a directory of files whose"
            " concatination is the input.")
        ("seed,s",
            bpo::value<int>(&seed)->default_value(38734),
            "Seed for random number generation. OPTIONAL.")
        ("hdfs",
            bpo::value<std::string>(&hdfs)->default_value(""),
            "If not empty, will assume file is in an HDFS. "
            "Parameter is filesystem name.")
        ("port",
            bpo::value<int>(&port)->default_value(0),
            "For HDFS: port to use.")
        //("sparse", "Whether to load the matrix as a sparse one.")
        ("highprecision,p", "Solve to high precision.")
        ("single,f", "Whether to use single precision instead of double.")
        ("outputfile",
            bpo::value<std::string>(&outfile)->default_value("out"),
            "Prefix for output files (prefix.txt). OPTIONAL.");

    bpo::positional_options_description positional;
    positional.add("inputfile", 1);
    positional.add("outputfile", 2);

    bpo::variables_map vm;
    try {
        bpo::store(bpo::command_line_parser(argc, argv)
            .options(desc).positional(positional).run(), vm);

        if (vm.count("help")) {
            if (rank == 0) {
                std::cout << "Usage: " << argv[0] << " [options] input-file-name"
                          << std::endl;
                std::cout << desc;
            }
            world.barrier();
            return 0;
        }

        if (!vm.count("inputfile")) {
            if (rank == 0)
                std::cout << "Input file is required." << std::endl;
            world.barrier();
            return -1;
        }

        bpo::notify(vm);

        //as_sparse = vm.count("sparse");
        use_single = vm.count("single");
        directory = vm.count("directory");
        high = vm.count("highprecision");

    } catch(bpo::error& e) {
        if (rank == 0) {
            std::cerr << e.what() << std::endl;
            std::cerr << desc << std::endl;
        }
        world.barrier();
        return -1;
    }

    skylark::base::context_t context(seed);

    SKYLARK_BEGIN_TRY()

    if (use_single) {
        // if (as_sparse)
        //     execute<skylark::base::sparse_matrix_t<float>,
        //             El::Matrix<float>,
        //             El::Matrix<float> >(fname, outfile, high, directory, context);
        // else
            execute<El::DistMatrix<float>,
                    El::DistMatrix<float>,
                    El::DistMatrix<float> >(context);

    } else {
        // if (as_sparse)
        //     execute<skylark::base::sparse_matrix_t<double>,
        //             El::Matrix<double>,
        //             El::Matrix<double> >(fname, outfile, high, context);
        // else
            execute<El::DistMatrix<double>,
                    El::DistMatrix<double>,
                    El::DistMatrix<double> >(context);
    }

    SKYLARK_END_TRY() SKYLARK_CATCH_AND_PRINT((rank == 0))

    El::Finalize();
    return 0;
}


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#include <boost/simd/function/shift_left.hpp>
#include <boost/simd/logical.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/two.hpp>

namespace bs = boost::simd;
namespace bd = boost::dispatch;

template <typename T, int N, typename Env>
void test(Env& runtime)
{
  using p_t = bs::pack<T, N>;

  T a1[N];
  T b[N], c[N];
  std::size_t sh1 =  sizeof(T)*8-1;

  int sh2 =  1;

  for(int i = 0; i < N; ++i)
  {
    a1[i] = (i%2) ? T(i) : T(-i);
    b[i] = bs::shift_left(a1[i], sh1);
    c[i] = bs::shift_left(a1[i], sh2);
  }

  p_t aa1(&a1[0], &a1[0]+N);
  p_t bb(&b[0], &b[0]+N);
  p_t cc(&c[0], &c[0]+N);

  STF_EQUAL(bs::shift_left(aa1, sh1), bb);
  STF_EQUAL(bs::shift_left(aa1, sh2), cc);
  STF_EQUAL((aa1 << sh1), bb);
  STF_EQUAL((aa1 << sh2), cc);
}

STF_CASE_TPL("Check shift_left on pack with integral shift" , STF_INTEGRAL_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;

  test<T, N>(runtime);
  test<T, N/2>(runtime);
  test<T, N*2>(runtime);
}

template <typename T, int N, typename Env>
void tests(Env& runtime)
{
  using iT =  bd::as_integer_t<T>;
  using p_t = bs::pack<T, N>;
  using i_t = bs::pack<iT, N>;

  T a1[N], b[N];
  iT sh[N];

  for(int i = 0; i < N; ++i)
  {
    sh[i] = i%(8*sizeof(T));
    a1[i] = (i%2) ? T(i) : T(-i);
    b[i] = bs::shift_left(a1[i],sh[i]);
  }

  p_t aa1(&a1[0], &a1[0]+N);
  p_t bb(&b[0], &b[0]+N);
  i_t sh1(&sh[0], &sh[0]+N);
  STF_EQUAL(bs::shift_left(aa1, sh1), bb);
  STF_EQUAL((aa1 << sh1), bb);
}

STF_CASE_TPL("Check shift_left on pack with pack shift" , STF_INTEGRAL_TYPES)
{
  static const std::size_t N = bs::pack<T>::static_size;

  tests<T, N>(runtime);
  tests<T, N/2>(runtime);
  tests<T, N*2>(runtime);
}

STF_CASE_TPL (" shift_left_integer",  STF_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  using bs::shift_left;
  using p_t = bs::pack<T>;
  using ip_t =  boost::dispatch::as_integer_t<p_t>;
  using r_t = decltype(shift_left(p_t(),ip_t()));
  typedef p_t wished_r_t;

  // return type conformity test
  STF_TYPE_IS(r_t, wished_r_t);

  // specific values tests
  STF_EQUAL(shift_left(bs::One<p_t>(),bs::One<ip_t>()), bs::Two<r_t>());
  STF_EQUAL(shift_left(bs::One<p_t>(),bs::Zero<ip_t>()), bs::One<r_t>());
  STF_EQUAL(shift_left(bs::Zero<p_t>(),bs::One<ip_t>()), bs::Zero<r_t>());
} // end of test for integer_

STF_CASE_TPL (" shift_left_real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  using bs::shift_left;
  using p_t = bs::pack<T>;
  using ip_t =  boost::dispatch::as_integer_t<p_t>;
  using r_t = decltype(shift_left(p_t(),ip_t()));
  typedef p_t wished_r_t;

  // return type conformity test
  STF_TYPE_IS(r_t, wished_r_t);

  // specific values tests
  STF_EQUAL(shift_left(bs::One<p_t>(),bs::Zero<ip_t>()), bs::One<r_t>());
  STF_EQUAL(shift_left(bs::Zero<p_t>(),bs::One<ip_t>()), bs::Zero<r_t>());
} // end of test for floating_


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/boolean/include/functions/seldec.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/mone.hpp>

NT2_TEST_CASE_TPL ( seldec_signed_int__2_0,  BOOST_SIMD_INTEGRAL_SIGNED_TYPES)
{
  using boost::simd::seldec;
  using boost::simd::tag::seldec_;
  typedef typename boost::dispatch::meta::call<seldec_(T, T)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS( r_t, wished_r_t );

  // specific values tests
  NT2_TEST_EQUAL(seldec(boost::simd::One<T>(), boost::simd::Mone<T>()), boost::simd::Mtwo<T>());
  NT2_TEST_EQUAL(seldec(boost::simd::One<T>(), boost::simd::One<T>()), boost::simd::Zero<T>());
  NT2_TEST_EQUAL(seldec(boost::simd::One<T>(), boost::simd::Zero<T>()), boost::simd::Mone<T>());
}

NT2_TEST_CASE_TPL ( seldec_unsigned_int__2_0,  BOOST_SIMD_UNSIGNED_TYPES)
{
  using boost::simd::seldec;
  using boost::simd::tag::seldec_;
  typedef typename boost::dispatch::meta::call<seldec_(T, T)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS( r_t, wished_r_t );

  // specific values tests
  NT2_TEST_EQUAL(seldec(boost::simd::One<T>(), boost::simd::One<T>()), boost::simd::Zero<T>());
  NT2_TEST_EQUAL(seldec(boost::simd::One<T>(), boost::simd::Two<T>()), boost::simd::One<T>());
  NT2_TEST_EQUAL(seldec(boost::simd::One<T>(), boost::simd::Zero<T>()), boost::simd::Valmax<T>());
}

NT2_TEST_CASE_TPL( seldec_floating, BOOST_SIMD_REAL_TYPES)
{
  using boost::simd::seldec;
  using boost::simd::tag::seldec_;
  typedef typename boost::dispatch::meta::call<seldec_(T, T)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS( r_t, wished_r_t );

  // specific values tests
  NT2_TEST_EQUAL(seldec(boost::simd::One<T>(), boost::simd::One<T>()), boost::simd::Zero<T>());
  NT2_TEST_EQUAL(seldec(boost::simd::One<T>(), boost::simd::Two<T>()), boost::simd::One<T>());
  NT2_TEST_EQUAL(seldec(boost::simd::One<T>(), boost::simd::Zero<T>()), boost::simd::Mone<T>());
}


/*=============================================================================
    Copyright (c) 2017 Paul Fultz II
    lift.cpp
    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)
==============================================================================*/
#include "test.hpp"
#include <boost/hof/lift.hpp>
#include <boost/hof/function.hpp>
#include <boost/hof/detail/move.hpp>
#include <tuple>
#include <algorithm>

template<class T, class U>
constexpr T sum(T x, U y) BOOST_HOF_RETURNS_DEDUCE_NOEXCEPT(x+y)
{
    return x + y;
}

BOOST_HOF_LIFT_CLASS(max_f, std::max);
BOOST_HOF_LIFT_CLASS(sum_f, sum);

#if BOOST_HOF_HAS_NOEXCEPT_DEDUCTION
BOOST_HOF_TEST_CASE()
{
    static_assert(noexcept(sum_f()(1, 2)), "noexcept lift");
    static_assert(!noexcept(sum_f()(std::string(), std::string())), "noexcept lift");
}
#endif

BOOST_HOF_TEST_CASE()
{
    BOOST_HOF_TEST_CHECK(max_f()(3, 4) == std::max(3, 4));

    BOOST_HOF_TEST_CHECK(sum_f()(1, 2) == 3);
    BOOST_HOF_STATIC_TEST_CHECK(sum_f()(1, 2) == 3);
}

#if BOOST_HOF_HAS_GENERIC_LAMBDA
BOOST_HOF_TEST_CASE()
{
    auto my_max = BOOST_HOF_LIFT(std::max);
    BOOST_HOF_TEST_CHECK(my_max(3, 4) == std::max(3, 4));

    BOOST_HOF_TEST_CHECK(BOOST_HOF_LIFT(std::max)(3, 4) == std::max(3, 4));
    BOOST_HOF_TEST_CHECK(BOOST_HOF_LIFT(sum)(1, 2) == 3);
}

BOOST_HOF_STATIC_FUNCTION(psum) = BOOST_HOF_LIFT(sum);
BOOST_HOF_STATIC_FUNCTION(pmax) = BOOST_HOF_LIFT(std::max);

BOOST_HOF_TEST_CASE()
{
    BOOST_HOF_TEST_CHECK(pmax(3, 4) == std::max(3, 4));

    BOOST_HOF_TEST_CHECK(psum(1, 2) == 3);
}
#endif


//
// Created by huangkun on 2019/12/30.
//

#ifndef OPENGV2_FEATUREBASE_HPP
#define OPENGV2_FEATUREBASE_HPP

#include <memory>

#include <Eigen/Eigen>

#include <opengv2/landmark/LandmarkBase.hpp>

namespace opengv2 {
    class FeatureBase { // TODO: inherit ObservationBase
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        typedef std::shared_ptr<FeatureBase> Ptr;

        explicit FeatureBase(const Eigen::Ref<const Eigen::VectorXd> &loc) :
                locBackup(loc), landmark_(LandmarkBase::Ptr()), loc_(loc), bearingVector_(Eigen::VectorXd()) {}

        virtual ~FeatureBase() = default;

        inline LandmarkBase::Ptr landmark() const noexcept {
            return landmark_.lock();
        }

        /**
         * @note Warning: Not thread safe.
         */
        inline void setLandmark(const LandmarkBase::Ptr &lm) noexcept {
            landmark_ = lm;
        }

        inline const Eigen::VectorXd &location() const noexcept {
            return loc_;
        }

        /**
         * @note Warning: Not thread safe.
         */
        virtual inline void setLocation(const Eigen::Ref<const Eigen::VectorXd> &loc) noexcept {
            loc_ = loc;
        }

        inline const Eigen::VectorXd &bearingVector() noexcept {
            return bearingVector_;
        }

        /**
         * @note Warning: Not thread safe.
         */
        inline void setBearingVector(const Eigen::Ref<const Eigen::VectorXd> &bv) {
            bearingVector_ = bv;
        }

        Eigen::VectorXd locBackup; // backup

    protected:
        std::weak_ptr<LandmarkBase> landmark_;
        Eigen::VectorXd loc_;
        Eigen::VectorXd bearingVector_;
    };
}

#endif //OPENGV2_FEATUREBASE_HPP


// Boost.Range 2.0 Extension library
// via PStade Oven Library
//
// Copyright Akira Takahashi 2011.
// Copyright Shunsuke Sogame 2005-2007.
// Use, modification and distribution is subject to 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)

#include <iostream>
#include <boost/detail/lightweight_test.hpp>

#include <boost/range/iteration.hpp>

#include <vector>
#include <boost/assign/list_of.hpp>
#include <boost/range/adaptor/taken.hpp>
#include <boost/range/adaptor/dropped.hpp>
#include <boost/range/access/front.hpp>
#include <boost/range/algorithm/equal.hpp>

int next(int x)
{
    return x * 2;
}

int main()
{
    {
        const std::vector<int> expected = boost::assign::list_of(1)(2)(4)(8)(16);

        BOOST_TEST(boost::equal(
            boost::iteration(1, next) | boost::adaptors::taken(5),
            expected
        ));
    }
    {
        const int expected = 4;

        BOOST_TEST(
            (boost::iteration(1, next) | boost::adaptors::dropped(2) | boost::range::access::front)
            == expected
        );
    }

    return boost::report_errors();
}


#include "roi_size_statistics.hpp"

#include <csapex/model/node_modifier.h>
#include <csapex/msg/generic_vector_message.hpp>
#include <csapex/msg/io.h>
#include <csapex/param/parameter_factory.h>
#include <csapex/param/range_parameter.h>
#include <csapex/utility/register_apex_plugin.h>

#include <csapex_opencv/cv_mat_message.h>
#include <csapex_opencv/roi_message.h>
#include <cslibs_vision/utils/color_functions.hpp>
#include <cslibs_vision/utils/heatmap.hpp>

#include <boost/algorithm/clamp.hpp>
#include <fstream>

CSAPEX_REGISTER_CLASS(csapex::vision::ROISizeStatistics, csapex::Node)

using namespace csapex;
using namespace csapex::vision;
using namespace csapex::connection_types;

void ROISizeStatistics::Statistics::reset()
{
    count = 0;
    height = Accumulator<int>();
    width = Accumulator<int>();
    area = Accumulator<int>();
    ratio = Accumulator<float>();
}

void ROISizeStatistics::Statistics::update(const Roi& roi)
{
    ++count;
    height(roi.h());
    width(roi.w());
    area(roi.h() * roi.w());
    ratio(float(roi.w()) / roi.h());
}

std::string ROISizeStatistics::Statistics::format() const
{
    std::ostringstream os;
    os << "Count : " << count << '\n';
    os << "Width : " << formatAccumulator(width) << "\n";
    os << "Height: " << formatAccumulator(height) << "\n";
    os << "Area  : " << formatAccumulator(area) << "\n";
    os << "Ratio : " << formatAccumulator(ratio) << "\n";
    return os.str();
}

ROISizeStatistics::ROISizeStatistics() : max_width_(640), max_height_(480), histogram_bin_size_(16), bin_count_(1)
{
}

void ROISizeStatistics::setup(csapex::NodeModifier& node_modifier)
{
    in_rois_ = node_modifier.addMultiInput<GenericVectorMessage, RoiMessage>("ROIs");
    out_info_ = node_modifier.addOutput<std::string>("Info");
    out_histogram_ = node_modifier.addOutput<CvMatMessage>("Histogram");
}

void ROISizeStatistics::setupParameters(csapex::Parameterizable& parameters)
{
    parameters.addParameter(param::factory::declareTrigger("reset"), std::bind(&ROISizeStatistics::resetStats, this));
    parameters.addParameter(param::factory::declareTrigger("save"), std::bind(&ROISizeStatistics::saveStats, this));

    parameters.addParameter(param::factory::declareFileOutputPath("path", "", "*.txt"), output_path_);

    parameters.addParameter(param::factory::declareRange("max_width", 1, 1920, 640, 1), [this](csapex::param::Parameter* param) {
        max_width_ = param->as<int>();
        resetStats();
    });
    parameters.addParameter(param::factory::declareRange("max_height", 1, 1080, 480, 1), [this](csapex::param::Parameter* param) {
        max_height_ = param->as<int>();
        resetStats();
    });
    parameters.addParameter(param::factory::declareRange("histogram_bin_size", 1, 128, 16, 1), [this](csapex::param::Parameter* param) {
        histogram_bin_size_ = param->as<int>();
        resetStats();
    });

    auto bin_count_param = param::factory::declareRange("bin/count", 0, 64, 0, 1).build();
    auto enable_bin_count = [bin_count_param]() { return bin_count_param->as<int>() > 0; };
    parameters.addParameter(bin_count_param, [this](csapex::param::Parameter* param) {
        bin_count_ = param->as<int>();
        resetStats();
    });

    static const std::map<std::string, int> available_bin_types = {
        { "height", static_cast<int>(BinType::HEIGHT) },
        { "width", static_cast<int>(BinType::WIDTH) },
        { "area", static_cast<int>(BinType::AREA) },
    };
    parameters.addConditionalParameter(param::factory::declareParameterSet("bin/type", available_bin_types, static_cast<int>(BinType::WIDTH)), enable_bin_count,
                                       [this](csapex::param::Parameter* param) {
                                           bin_type_ = static_cast<BinType>(param->as<int>());
                                           resetStats();
                                       });
}

void ROISizeStatistics::process()
{
    CvMatMessage::Ptr hist_msg;

    if (msg::isMessage<RoiMessage>(in_rois_)) {
        RoiMessage::ConstPtr in_roi = msg::getMessage<RoiMessage>(in_rois_);
        updateStats(in_roi->value);

        hist_msg = std::make_shared<CvMatMessage>(enc::unknown, in_roi->frame_id, in_roi->stamp_micro_seconds);
    } else {
        std::shared_ptr<std::vector<RoiMessage> const> in_rois = msg::getMessage<GenericVectorMessage, RoiMessage>(in_rois_);
        for (const RoiMessage& roi_msg : *in_rois)
            updateStats(roi_msg.value);

        if (!in_rois->empty())
            hist_msg = std::make_shared<CvMatMessage>(enc::unknown, in_rois->front().frame_id, in_rois->front().stamp_micro_seconds);
        else
            hist_msg = std::make_shared<CvMatMessage>(enc::unknown, "", 0);
    }

    hist_msg->value = global_histogram_;

    msg::publish<std::string>(out_info_, formatStats());
    msg::publish(out_histogram_, hist_msg);
}

void ROISizeStatistics::resetStats()
{
    global_stats_.reset();
    {
        const int width = max_width_ / histogram_bin_size_;
        const int height = max_height_ / histogram_bin_size_;
        global_histogram_ = cv::Mat::zeros(cv::Size(width, height), CV_32SC1);
    }

    bin_stats_.clear();
    bin_stats_.resize(bin_count_);
}

void ROISizeStatistics::updateStats(const Roi& roi)
{
    global_stats_.update(roi);
    {
        const int x = roi.w() / histogram_bin_size_;
        const int y = roi.h() / histogram_bin_size_;
        global_histogram_.at<int32_t>(cv::Point(x, y)) += 1;
    }

    std::size_t bin_index = 0;
    switch (bin_type_) {
        case BinType::HEIGHT:
            bin_index = std::size_t(boost::algorithm::clamp(roi.h(), 0, max_height_ - 1) / (max_height_ / float(bin_count_)));
            break;
        case BinType::WIDTH:
            bin_index = std::size_t(boost::algorithm::clamp(roi.w(), 0, max_width_ - 1) / (max_width_ / float(bin_count_)));
            break;
        case BinType::AREA:
            bin_index = std::size_t(boost::algorithm::clamp(roi.w() * roi.h(), 0, (max_width_ * max_height_) - 1) / ((max_width_ * max_height_) / float(bin_count_)));
            break;
        default:
            throw std::runtime_error("Unhandled BinType");
    }

    if (bin_count_ != 0)
        bin_stats_[bin_index].update(roi);
}

void ROISizeStatistics::saveStats()
{
    if (output_path_.empty())
        return;

    std::ofstream file(output_path_);
    file << formatStats();

    {
        const auto idx = output_path_.find_last_of(".");
        const std::string hist_path = output_path_.substr(0, idx) + ".hist.pgm";
        cv::imwrite(hist_path, global_histogram_);

        const std::string heatmap_path = output_path_.substr(0, idx) + ".heatmap.png";
        cv::Mat heatmap(global_histogram_.rows, global_histogram_.cols, CV_32FC3, cv::Scalar::all(0));
        cv::Mat float_histogram;
        global_histogram_.convertTo(float_histogram, CV_32FC1);
        cslibs_vision::heatmap::renderHeatmap(float_histogram, heatmap, &cslibs_vision::color::bezierColor<cv::Vec3f>);
        cv::resize(heatmap, heatmap, cv::Size(max_width_, max_height_));
        cv::imwrite(heatmap_path, heatmap);
    }
}

std::string ROISizeStatistics::formatStats() const
{
    std::ostringstream os;
    os << "ROI Statistics\n";
    os << "==============\n";
    os << "Total:\n";
    os << global_stats_.format();

    for (std::size_t i = 0; i < static_cast<std::size_t>(bin_count_); ++i) {
        os << "\n";
        os << "Bin #" << i << " break: ";
        switch (bin_type_) {
            case BinType::HEIGHT:
                os << i * (max_height_ / bin_count_) << " - " << (i + 1) * (max_height_ / bin_count_) << " (height)";
                break;
            case BinType::WIDTH:
                os << i * (max_width_ / bin_count_) << " - " << (i + 1) * (max_width_ / bin_count_) << " (width)";
                break;
            case BinType::AREA:
                os << i * ((max_width_ * max_height_) / bin_count_) << " - " << (i + 1) * ((max_width_ * max_height_) / bin_count_) << " (area)";
                break;
            default:
                throw std::runtime_error("Unhandled BinType");
        }
        os << "\n";
        os << bin_stats_[i].format();
    }

    return os.str();
}


#include <stdexcept>
#include <boost\test\unit_test.hpp>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

using namespace std;

BOOST_AUTO_TEST_SUITE( TestFloatingPoint )

BOOST_AUTO_TEST_CASE( ShouldNotEqualWhen1Over3 )
{
	float a1 = (float)( 1.0/3.0 );
	double a2 = 1.0/3.0;

	BOOST_CHECK( (double)a1 != a2 );
}

BOOST_AUTO_TEST_CASE( TestPointer )
{
	float a = 1.0f;
	float* b = &a;

	BOOST_CHECK_EQUAL( a, *b);
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include <cstdint>
#include <string>
#include <vector>
#include <functional>
#include <map>

/*
 * Associate subprograms to --program argument.
 */

std::map<std::string, std::function<int(int argc, char *argv[])>> &programs();

/*
 * Global configuration and resources.
 * A shard is the portion of ensemble chain replications that the GPU owns.
 */

extern struct configuration {

	bool verbose;

	uint16_t chain_length, shard_copies;
	uint32_t copies_total, shard_elements;
	uint32_t kernel_batching, dump_interval;
	double dt;

	std::string dump_prefix;
	uint64_t steps = 0, time_offset = 0;
	double entropy_limit;
	enum {
		NONE = 0, INFORMATION, WT
	} entropy_limit_type = NONE;

	size_t sizeof_linenergies, sizeof_shard;
	std::vector<uint16_t> entropy_modes_indices;

} gconf;

/*
 * Command line parsing.
 */

#include <boost/program_options.hpp>

struct parse_cmdline {
	boost::program_options::options_description options;
	boost::program_options::variables_map vm;

	parse_cmdline(const std::string &name);

	void run(int argc, char *argv[]);

	/*
	 * Throw this to terminate gracefully and print the boost options.
	 */
	struct help_quit {
		const boost::program_options::options_description options;
	};
private:
	std::string initial_filename, entropymask_filename;
};

#include "utilities.hpp"
//for double2
#include <vector_types.h>

extern struct resources {
	double2 *shard_host = 0, *shard_gpu = 0;
	double *linenergies_host = 0, *linenergies_gpu = 0;

private:
	std::vector<double2> shard_buffer;
	std::vector<double, simd_allocator<double>> linenergies_buffer;
	friend struct parse_cmdline;
} gres;


/*-----------------------------------------------------------------------------+
Copyright (c) 2010-2010: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#ifndef BOOST_ICL_TYPE_TRAITS_IS_ASYMMETRIC_INTERVAL_HPP_JOFA_100327
#define BOOST_ICL_TYPE_TRAITS_IS_ASYMMETRIC_INTERVAL_HPP_JOFA_100327

#include <boost/icl/type_traits/is_interval.hpp>

namespace boost{ namespace icl
{

template <class Type> struct is_asymmetric_interval
{
    typedef is_asymmetric_interval<Type> type;
    BOOST_STATIC_CONSTANT(bool,
            value = (mpl::and_<
                        is_interval<Type>
                    ,   has_static_bounds<Type>
                    ,   has_asymmetric_bounds<Type>
                    >::value)
                );
};

template <class Type> struct is_continuous_asymmetric
{
    typedef is_continuous_asymmetric<Type> type;
    BOOST_STATIC_CONSTANT(bool,
            value = (mpl::and_<
                        is_asymmetric_interval<Type>
                    ,   is_continuous<typename domain_type_of<interval_traits<Type> >::type>
                    >::value)
                );
};

template <class Type> struct is_discrete_asymmetric
{
    typedef is_discrete_asymmetric<Type> type;
    BOOST_STATIC_CONSTANT(bool,
            value = (mpl::and_<
                        is_asymmetric_interval<Type>
                    ,   mpl::not_<is_continuous<typename domain_type_of<interval_traits<Type> >::type> >
                    >::value)
                );
};

}} // namespace boost icl

#endif


/*
LICENSE: see isogeometric_application/LICENSE.txt
*/

//
//   Project Name:        Kratos
//   Last modified by:    $Author: hbui $
//   Date:                $Date: Nov 11, 2017 $
//   Revision:            $Revision: 1.1 $
//
//

// System includes
#include <string>

// External includes
#include <boost/foreach.hpp>
#include <boost/python.hpp>
#include <boost/python/stl_iterator.hpp>
#include <boost/python/operators.hpp>

// Project includes
#include "includes/define.h"
#include "custom_utilities/control_point.h"
#include "custom_utilities/control_grid.h"
#include "custom_utilities/fespace.h"
#include "custom_utilities/grid_function.h"

namespace Kratos
{

namespace Python
{

using namespace boost::python;

template<class TGridFrunctionType>
typename TGridFrunctionType::FESpaceType::Pointer GridFunction_GetFESpace(TGridFrunctionType& rDummy)
{
    return rDummy.pFESpace();
}

template<class TGridFrunctionType>
void GridFunction_SetFESpace(TGridFrunctionType& rDummy, typename TGridFrunctionType::FESpaceType::Pointer pNewFESpace)
{
    rDummy.SetFESpace(pNewFESpace);
}

template<class TGridFrunctionType>
typename TGridFrunctionType::ControlGridType::Pointer GridFunction_GetControlGrid(TGridFrunctionType& rDummy)
{
    return rDummy.pControlGrid();
}

template<class TGridFrunctionType>
void GridFunction_SetControlGrid(TGridFrunctionType& rDummy, typename TGridFrunctionType::ControlGridType::Pointer pNewControlGrid)
{
    rDummy.SetControlGrid(pNewControlGrid);
}

template<class TGridFrunctionType>
typename TGridFrunctionType::DataType GridFunction_GetValue1(TGridFrunctionType& rDummy, const boost::python::list& xi)
{
    std::vector<double> xi_vec;
    typedef boost::python::stl_input_iterator<double> iterator_value_type;
    BOOST_FOREACH(const iterator_value_type::value_type& v, std::make_pair(iterator_value_type(xi), iterator_value_type() ) )
    {
        xi_vec.push_back(v);
    }

    return rDummy.GetValue(xi_vec);
}

template<class TGridFrunctionType>
typename TGridFrunctionType::DataType GridFunction_GetValue2(TGridFrunctionType& rDummy, const array_1d<double, 3>& xi)
{
    return rDummy.GetValue(xi);
}

template<class TGridFrunctionType>
boost::python::list GridFunction_GetDerivative1(TGridFrunctionType& rDummy, const boost::python::list& xi)
{
    std::vector<double> xi_vec;
    typedef boost::python::stl_input_iterator<double> iterator_value_type;
    BOOST_FOREACH(const iterator_value_type::value_type& v, std::make_pair(iterator_value_type(xi), iterator_value_type() ) )
    {
        xi_vec.push_back(v);
    }

    std::vector<typename TGridFrunctionType::DataType> derivatives;
    rDummy.GetDerivative(derivatives, xi_vec);

    boost::python::list results;
    for (std::size_t i = 0; i < derivatives.size(); ++i)
        results.append(derivatives[i]);

    return results;
}

template<class TGridFrunctionType>
boost::python::list GridFunction_GetDerivative2(TGridFrunctionType& rDummy, const array_1d<double, 3>& xi)
{
    std::vector<typename TGridFrunctionType::DataType> derivatives;
    rDummy.GetDerivative(derivatives, xi);

    boost::python::list results;
    for (std::size_t i = 0; i < derivatives.size(); ++i)
        results.append(derivatives[i]);

    return results;
}

template<class TGridFrunctionType, typename TCoordinatesType>
boost::python::list GridFunction_LocalCoordinates(TGridFrunctionType& rDummy,
        const typename TGridFrunctionType::DataType& v, const TCoordinatesType& xi0)
{
    TCoordinatesType xi = xi0;
    int stat = rDummy.LocalCoordinates(v, xi);
    // KRATOS_WATCH(v)
    // KRATOS_WATCH(stat)
    // KRATOS_WATCH(xi)
    boost::python::list output;
    output.append(stat);
    output.append(xi);
    return output;
}

///////////////////////////////////////////////////////

template<int TDim>
void IsogeometricApplication_AddGridFunctionsToPython()
{
    std::stringstream ss;

    ss.str(std::string());
    ss << "ControlPointGridFunction" << TDim << "D";
    typedef GridFunction<TDim, ControlPoint<double> > ControlPointGridFunctionType;
    class_<ControlPointGridFunctionType, typename ControlPointGridFunctionType::Pointer, boost::noncopyable>
    (ss.str().c_str(), init<typename FESpace<TDim>::Pointer, typename ControlGrid<ControlPoint<double> >::Pointer>())
    .add_property("FESpace", GridFunction_GetFESpace<ControlPointGridFunctionType>, GridFunction_SetFESpace<ControlPointGridFunctionType>)
    .add_property("ControlGrid", GridFunction_GetControlGrid<ControlPointGridFunctionType>, GridFunction_SetControlGrid<ControlPointGridFunctionType>)
    .def("GetValue", &GridFunction_GetValue1<ControlPointGridFunctionType>)
    .def("GetValue", &GridFunction_GetValue2<ControlPointGridFunctionType>)
    .def("GetDerivative", &GridFunction_GetDerivative1<ControlPointGridFunctionType>)
    .def("GetDerivative", &GridFunction_GetDerivative2<ControlPointGridFunctionType>)
    .def(self_ns::str(self))
    ;

    ss.str(std::string());
    ss << "DoubleGridFunction" << TDim << "D";
    typedef GridFunction<TDim, double> DoubleGridFunctionType;
    class_<DoubleGridFunctionType, typename DoubleGridFunctionType::Pointer, boost::noncopyable>
    (ss.str().c_str(), init<typename FESpace<TDim>::Pointer, typename ControlGrid<double>::Pointer>())
    .add_property("FESpace", GridFunction_GetFESpace<DoubleGridFunctionType>, GridFunction_SetFESpace<DoubleGridFunctionType>)
    .add_property("ControlGrid", GridFunction_GetControlGrid<DoubleGridFunctionType>, GridFunction_SetControlGrid<DoubleGridFunctionType>)
    .def("GetValue", &GridFunction_GetValue1<DoubleGridFunctionType>)
    .def("GetValue", &GridFunction_GetValue2<DoubleGridFunctionType>)
    .def("GetDerivative", &GridFunction_GetDerivative1<DoubleGridFunctionType>)
    .def("GetDerivative", &GridFunction_GetDerivative2<DoubleGridFunctionType>)
    .def(self_ns::str(self))
    ;

    ss.str(std::string());
    ss << "Array1DGridFunction" << TDim << "D";
    typedef GridFunction<TDim, array_1d<double, 3> > Array1DGridFunctionType;
    class_<Array1DGridFunctionType, typename Array1DGridFunctionType::Pointer, boost::noncopyable>
    (ss.str().c_str(), init<typename FESpace<TDim>::Pointer, typename ControlGrid<array_1d<double, 3> >::Pointer>())
    .add_property("FESpace", GridFunction_GetFESpace<Array1DGridFunctionType>, GridFunction_SetFESpace<Array1DGridFunctionType>)
    .add_property("ControlGrid", GridFunction_GetControlGrid<Array1DGridFunctionType>, GridFunction_SetControlGrid<Array1DGridFunctionType>)
    .def("GetValue", &GridFunction_GetValue1<Array1DGridFunctionType>)
    .def("GetValue", &GridFunction_GetValue2<Array1DGridFunctionType>)
    .def("GetDerivative", &GridFunction_GetDerivative1<Array1DGridFunctionType>)
    .def("GetDerivative", &GridFunction_GetDerivative2<Array1DGridFunctionType>)
    .def("LocalCoordinates", &GridFunction_LocalCoordinates<Array1DGridFunctionType, array_1d<double, 3> >)
    .def(self_ns::str(self))
    ;

    ss.str(std::string());
    ss << "VectorGridFunction" << TDim << "D";
    typedef GridFunction<TDim, Vector> VectorGridFunctionType;
    class_<VectorGridFunctionType, typename VectorGridFunctionType::Pointer, boost::noncopyable>
    (ss.str().c_str(), init<typename FESpace<TDim>::Pointer, typename ControlGrid<Vector>::Pointer>())
    .add_property("FESpace", GridFunction_GetFESpace<VectorGridFunctionType>, GridFunction_SetFESpace<VectorGridFunctionType>)
    .add_property("ControlGrid", GridFunction_GetControlGrid<VectorGridFunctionType>, GridFunction_SetControlGrid<VectorGridFunctionType>)
    .def("GetValue", &GridFunction_GetValue1<VectorGridFunctionType>)
    .def("GetValue", &GridFunction_GetValue2<VectorGridFunctionType>)
    .def("GetDerivative", &GridFunction_GetDerivative1<VectorGridFunctionType>)
    .def("GetDerivative", &GridFunction_GetDerivative2<VectorGridFunctionType>)
    .def("LocalCoordinates", &GridFunction_LocalCoordinates<VectorGridFunctionType, array_1d<double, 3> >)
    .def(self_ns::str(self))
    ;
}

}  // namespace Python.

} // Namespace Kratos


/**
 * @file origin.cpp
 * @brief Parse origin from xml string
 *
 * @author Levi Armstrong
 * @date September 1, 2019
 * @version TODO
 * @bug No known bugs
 *
 * @copyright Copyright (c) 2019, Southwest Research Institute
 *
 * @par License
 * Software License Agreement (Apache License)
 * @par
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * http://www.apache.org/licenses/LICENSE-2.0
 * @par
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <stdexcept>
#include <tesseract_common/utils.h>
#include <Eigen/Geometry>
#include <vector>
#include <boost/algorithm/string.hpp>
#include <tinyxml2.h>
TESSERACT_COMMON_IGNORE_WARNINGS_POP

#include <tesseract_urdf/origin.h>

Eigen::Isometry3d tesseract_urdf::parseOrigin(const tinyxml2::XMLElement* xml_element, int /*version*/)
{
  Eigen::Isometry3d origin = Eigen::Isometry3d::Identity();

  if (xml_element->Attribute("xyz") == nullptr && xml_element->Attribute("rpy") == nullptr &&
      xml_element->Attribute("wxyz") == nullptr)
    std::throw_with_nested(std::runtime_error("Origin: Error missing required attributes 'xyz' and 'rpy' and/or 'wxyz' "
                                              "for origin element!"));

  std::string xyz_string, rpy_string, wxyz_string;
  tinyxml2::XMLError status = tesseract_common::QueryStringAttribute(xml_element, "xyz", xyz_string);
  if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
    std::throw_with_nested(std::runtime_error("Origin: Failed to parse attribute 'xyz'!"));

  if (status != tinyxml2::XML_NO_ATTRIBUTE)
  {
    std::vector<std::string> tokens;
    boost::split(tokens, xyz_string, boost::is_any_of(" "), boost::token_compress_on);
    if (tokens.size() != 3 || !tesseract_common::isNumeric(tokens))
      std::throw_with_nested(std::runtime_error("Origin: Failed to parse attribute 'xyz' string!"));

    double x{ 0 }, y{ 0 }, z{ 0 };
    // No need to check return values because the tokens are verified above
    tesseract_common::toNumeric<double>(tokens[0], x);
    tesseract_common::toNumeric<double>(tokens[1], y);
    tesseract_common::toNumeric<double>(tokens[2], z);

    origin.translation() = Eigen::Vector3d(x, y, z);
  }

  if (xml_element->Attribute("wxyz") == nullptr)
  {
    status = tesseract_common::QueryStringAttribute(xml_element, "rpy", rpy_string);
    if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
      std::throw_with_nested(std::runtime_error("Origin: Failed to parse attribute 'rpy'!"));

    if (status != tinyxml2::XML_NO_ATTRIBUTE)
    {
      std::vector<std::string> tokens;
      boost::split(tokens, rpy_string, boost::is_any_of(" "), boost::token_compress_on);
      if (tokens.size() != 3 || !tesseract_common::isNumeric(tokens))
        std::throw_with_nested(std::runtime_error("Origin: Failed to parse attribute 'rpy' string!"));

      double r{ 0 }, p{ 0 }, y{ 0 };
      // No need to check return values because the tokens are verified above
      tesseract_common::toNumeric<double>(tokens[0], r);
      tesseract_common::toNumeric<double>(tokens[1], p);
      tesseract_common::toNumeric<double>(tokens[2], y);

      Eigen::AngleAxisd rollAngle(r, Eigen::Vector3d::UnitX());
      Eigen::AngleAxisd pitchAngle(p, Eigen::Vector3d::UnitY());
      Eigen::AngleAxisd yawAngle(y, Eigen::Vector3d::UnitZ());

      Eigen::Quaterniond rpy = yawAngle * pitchAngle * rollAngle;

      origin.linear() = rpy.toRotationMatrix();
    }
  }
  else
  {
    status = tesseract_common::QueryStringAttribute(xml_element, "wxyz", wxyz_string);
    if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
      std::throw_with_nested(std::runtime_error("Origin: Failed to parse attribute 'wxyz'!"));

    if (status != tinyxml2::XML_NO_ATTRIBUTE)
    {
      std::vector<std::string> tokens;
      boost::split(tokens, wxyz_string, boost::is_any_of(" "), boost::token_compress_on);
      if (tokens.size() != 4 || !tesseract_common::isNumeric(tokens))
        std::throw_with_nested(std::runtime_error("Origin: Failed to parse attribute 'wxyz' string!"));

      double qw{ 0 }, qx{ 0 }, qy{ 0 }, qz{ 0 };
      // No need to check return values because the tokens are verified above
      tesseract_common::toNumeric<double>(tokens[0], qw);
      tesseract_common::toNumeric<double>(tokens[1], qx);
      tesseract_common::toNumeric<double>(tokens[2], qy);
      tesseract_common::toNumeric<double>(tokens[3], qz);

      Eigen::Quaterniond q(qw, qx, qy, qz);
      q.normalize();

      origin.linear() = q.toRotationMatrix();
    }
  }
  return origin;
}


#include <boost/test/unit_test.hpp>

#include <scorum/chain/services/budgets.hpp>
#include <scorum/protocol/types.hpp>
#include <scorum/protocol/asset.hpp>

#include "defines.hpp"

namespace {

using namespace scorum::chain;

BOOST_AUTO_TEST_SUITE(per_block_evaluation_tests)

SCORUM_TEST_CASE(start_time_aligned_deadline_is_not_test)
{
    auto start = fc::time_point_sec(10);
    auto deadline = fc::time_point_sec(15);
    auto block_time = fc::time_point_sec(7);

    auto per_block = scorum::chain::detail::adv_calculate_per_block(start, deadline, block_time, ASSET_SCR(900));
    BOOST_CHECK_EQUAL(per_block.amount, 300u);
}

SCORUM_TEST_CASE(start_time_is_not_aligned_deadline_is_test)
{
    auto start = fc::time_point_sec(8);
    auto deadline = fc::time_point_sec(16);
    auto block_time = fc::time_point_sec(7);

    auto per_block = scorum::chain::detail::adv_calculate_per_block(start, deadline, block_time, ASSET_SCR(900));
    BOOST_CHECK_EQUAL(per_block.amount, 300u);
}

SCORUM_TEST_CASE(start_time_and_deadline_are_not_aligned_test)
{
    auto start = fc::time_point_sec(8);
    auto deadline = fc::time_point_sec(14);
    auto block_time = fc::time_point_sec(7);

    auto per_block = scorum::chain::detail::adv_calculate_per_block(start, deadline, block_time, ASSET_SCR(900));
    BOOST_CHECK_EQUAL(per_block.amount, 300u);
}

SCORUM_TEST_CASE(balance_do_not_divide_completely_test)
{
    auto start = fc::time_point_sec(10);
    auto deadline = fc::time_point_sec(19);
    auto block_time = fc::time_point_sec(7);

    auto per_block = scorum::chain::detail::adv_calculate_per_block(start, deadline, block_time, ASSET_SCR(90));
    BOOST_CHECK_EQUAL(per_block.amount, 22u);
}

SCORUM_TEST_CASE(start_time_equal_deadline_test)
{
    auto start = fc::time_point_sec(10);
    auto deadline = fc::time_point_sec(10);
    auto block_time = fc::time_point_sec(7);

    auto per_block = scorum::chain::detail::adv_calculate_per_block(start, deadline, block_time, ASSET_SCR(90));
    BOOST_CHECK_EQUAL(per_block.amount, 90u);
}

SCORUM_TEST_CASE(balance_is_too_small_to_fit_each_per_block_payment_test)
{
    auto start = fc::time_point_sec(10);
    auto deadline = fc::time_point_sec(300);
    auto block_time = fc::time_point_sec(7);

    auto per_block = scorum::chain::detail::adv_calculate_per_block(start, deadline, block_time, ASSET_SCR(90));
    BOOST_CHECK_EQUAL(per_block.amount, 1u);
}

BOOST_AUTO_TEST_SUITE_END()
}


#include <aikido/constraint/SequentialSampleable.hpp>

#include <Eigen/StdVector>
#include <aikido/constraint/CyclicSampleable.hpp>
#include <aikido/constraint/FiniteSampleable.hpp>
#include <aikido/statespace/Rn.hpp>
#include <aikido/statespace/StateSpace.hpp>
#include "../eigen_tests.hpp"

#include <gtest/gtest.h>

using aikido::statespace::R1;
using aikido::statespace::R2;
using aikido::constraint::CyclicSampleable;
using aikido::constraint::FiniteSampleable;
using aikido::constraint::ConstSampleablePtr;
using aikido::constraint::SequentialSampleable;
using aikido::constraint::SampleGenerator;
using State = aikido::statespace::StateSpace::State;

EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2d)

TEST(SequentialSampleableTest, ConstructorThrowsOnNullStateSpace)
{
  auto r1 = std::make_shared<R1>();

  auto s1 = r1->createState();
  s1.setValue(aikido::tests::make_vector(-2));

  std::vector<ConstSampleablePtr> sampleables;
  sampleables.emplace_back(std::make_shared<const FiniteSampleable>(r1, s1));

  EXPECT_THROW(
      SequentialSampleable(nullptr, sampleables), std::invalid_argument);
}

TEST(SequentialSampleableTest, ConstructorThrowsOnEmptySampleables)
{
  auto r1 = std::make_shared<R1>();

  auto s1 = r1->createState();
  s1.setValue(aikido::tests::make_vector(-2));

  std::vector<ConstSampleablePtr> sampleables;

  EXPECT_THROW(SequentialSampleable(r1, sampleables), std::invalid_argument);
}

TEST(SequentialSampleableTest, ConstructorThrowsOnNullSampleable)
{
  auto r1 = std::make_shared<R1>();

  auto s1 = r1->createState();
  s1.setValue(aikido::tests::make_vector(-2));

  std::vector<ConstSampleablePtr> sampleables;
  sampleables.emplace_back(nullptr);

  EXPECT_THROW(SequentialSampleable(r1, sampleables), std::invalid_argument);
}

TEST(SequentialSampleableTest, ConstructorThrowsOnStateSpaceMismatch)
{
  auto r1 = std::make_shared<R1>();
  auto r2 = std::make_shared<R2>();

  auto s1 = r1->createState();
  s1.setValue(aikido::tests::make_vector(-2));

  std::vector<ConstSampleablePtr> sampleables;
  sampleables.emplace_back(std::make_shared<const FiniteSampleable>(r1, s1));

  EXPECT_THROW(SequentialSampleable(r2, sampleables), std::invalid_argument);
}

TEST(SequentialSampleableTest, SingleSampleGenerator)
{
  auto r1 = std::make_shared<R1>();

  auto s1 = r1->createState();
  Eigen::VectorXd v = aikido::tests::make_vector(-2);
  s1.setValue(v);

  std::vector<ConstSampleablePtr> sampleables;
  sampleables.emplace_back(std::make_shared<const FiniteSampleable>(r1, s1));

  SequentialSampleable constraint(r1, sampleables);

  // Single-generator.
  std::unique_ptr<SampleGenerator> generator
      = constraint.createSampleGenerator();

  auto state = r1->createState();

  // Test the generator's behavior.
  EXPECT_TRUE(generator->canSample());
  EXPECT_EQ(generator->getNumSamples(), 1);

  generator->sample(state);
  EXPECT_TRUE(state.getValue().isApprox(v));

  EXPECT_FALSE(generator->canSample());
  EXPECT_EQ(generator->getNumSamples(), 0);
}

TEST(SequentialSampleableTest, MultipleSampleGenerators)
{
  auto r1 = std::make_shared<R1>();

  auto s1 = r1->createState();
  auto s2 = r1->createState();
  Eigen::VectorXd v1 = aikido::tests::make_vector(-2);
  Eigen::VectorXd v2 = aikido::tests::make_vector(-1);
  s1.setValue(v1);
  s2.setValue(v2);

  std::vector<ConstSampleablePtr> sampleables;
  sampleables.emplace_back(std::make_shared<const FiniteSampleable>(r1, s1));
  sampleables.emplace_back(
      std::make_shared<const CyclicSampleable>(
          std::make_shared<FiniteSampleable>(r1, s2)));

  SequentialSampleable constraint(r1, sampleables);

  // Multiple-generator.
  std::unique_ptr<SampleGenerator> generator
      = constraint.createSampleGenerator();

  auto state = r1->createState();

  // Test the generator's behavior.
  EXPECT_TRUE(generator->canSample());
  EXPECT_EQ(generator->getNumSamples(), SampleGenerator::NO_LIMIT);

  generator->sample(state);
  EXPECT_TRUE(state.getValue().isApprox(v1));

  EXPECT_TRUE(generator->canSample());
  EXPECT_EQ(generator->getNumSamples(), SampleGenerator::NO_LIMIT);

  generator->sample(state);
  EXPECT_TRUE(state.getValue().isApprox(v2));
}


#pragma once

#ifdef PY_BINDINGS
    #include <pybind11/pybind11.h>
    #include <pybind11/stl.h>
    #include <pybind11/iostream.h>
    #include <pybind11/functional.h>
    namespace py = pybind11;
#endif

// #include <boost/multiprecision/gmp.hpp>
// using BigInt = boost::multiprecision::mpz_int;
// namespace std {
//     inline BigInt abs(BigInt num) {
//         return boost::multiprecision::abs(num);
//     }
// }
// inline std::string toString(BigInt& num, int base = 10) {
//     num.str(base);
// }
#include <string_view>
#include <functional>

template <class T>
inline void hash_combine(std::size_t& seed, const T& v)
{
    std::hash<T> hasher;
    seed ^= hasher(v) + 0x9e3779b9 + (seed<<6) + (seed>>2);
}

#include <gmpxx.h>
using BigInt = mpz_class;

template<typename Out, typename In>
Out convertInt(In value) {
    return value;
}

template<>
inline int32_t convertInt(mpz_class value) {
    return value.get_si();
}

namespace std {
    inline BigInt abs(BigInt& num) {
        return ::abs(num);
    }

    template<>
    struct hash<mpz_class> {
        std::size_t operator()(const mpz_class& y) const {
            if (y.fits_slong_p()) {
                // this check is crucial to get the same result for
                // different int types, i.e., we want that the hash
                // value of BigInt(1) is the same as the hash value of
                // int32_t(1)
                static_assert(sizeof(long) == 8, "If long is not 64 "
                "bit then hashing bitween 64 bit integer and BigInt will "
                "not be identical.");
                return std::hash<long>()(y.get_si());
            }

            const mpz_srcptr x = y.get_mpz_t();

            size_t hash = 0;
            hash_combine(hash, x->_mp_size);

            size_t limb_size = abs(x->_mp_size);

            if (limb_size > 2) {
                hash_combine(hash, x->_mp_d[0]);
                hash_combine(hash, x->_mp_d[limb_size - 1]);
            } else {
                for (size_t i = 0; i < limb_size; i++) {
                    hash_combine(hash, x->_mp_d[i]);
                }
            }

            return hash;
        }
    };
}

inline std::string toString(BigInt& num, int base = 10) {
    return num.get_str(base);
}

#ifdef PY_BINDINGS
    namespace pybind11 { namespace detail {
        template <> struct type_caster<BigInt> {
        public:
            /**
             * This macro establishes the name 'BigInt' in
             * function signatures and declares a local variable
             * 'value' of type BigInt
             */
            PYBIND11_TYPE_CASTER(BigInt, _("BigInt"));

            /**
             * Conversion part 1 (Python->C++): convert a PyObject into a BigInt
             * instance or return false upon failure. The second argument
             * indicates whether implicit conversions should be applied.
             */
            bool load(handle src, bool) {
                /* Extract PyObject from handle */
                PyObject *source = src.ptr();
                /* Try converting into a Python integer value */
                PyObject *pyNumber = nullptr;
                PyObject *pyString = nullptr;

                bool error = false;

                pyNumber = PyNumber_Long(source);
                if (!pyNumber) {
                    error = true;
                } else {
                    // using base 10 because boost multiprecision does not
                    // seem to support sign for base 16
                    pyString = PyNumber_ToBase(pyNumber,10);
                    if (!pyString) {
                        error = true;
                    } else {
                        const char* string = PyUnicode_AsUTF8(pyString);
                        if (string) {
                            try {
                                value = BigInt(string);
                            } catch (const std::runtime_error& e) {
                                error = true;
                            }
                        }

                    }

                }

                error |= (PyErr_Occurred() != nullptr);
                if (pyNumber)
                    Py_DECREF(pyNumber);
                if (pyString)
                    Py_DECREF(pyString);
                return !error;
            }

            /**
             * Conversion part 2 (C++ -> Python): convert an BigInt instance into
             * a Python object. The second and third arguments are used to
             * indicate the return value policy and parent object (for
             * ``return_value_policy::reference_internal``) and are generally
             * ignored by implicit casters.
             */
            static handle cast(BigInt src, return_value_policy /* policy */, handle /* parent */) {
                std::string string = toString(src,10);
                PyObject *pyString = PyUnicode_FromString(string.c_str());
                PyObject *result = PyNumber_Long(pyString);
                Py_DECREF(pyString);
                return result;
            }
        };
    }} // namespace pybind11::detail
#endif // ifdef PY_BINDINGS


#include <cmath>
#include <cstdint>
#include <limits>

#include <pcl/common/common.h>
#include <pcl/conversions.h>
#include <pcl/point_cloud.h>

#include <pybind11/eigen.h>
#include <pybind11/numpy.h>
#include <pybind11/pybind11.h>

#include <boost/shared_ptr.hpp>

#include "point_cloud2_iterator.h"
#include "typedefs.h"
#include "util.h"

namespace pypcl {

namespace py = pybind11;

// computemeanandcovariancematrix
// demeanpointcloud
// getminmax3d
// getPointCloudAsEigen(PCLPointCloud2 in, Eigen::MatrixXf out)
// getEigenAsPointCloud(Eigen::MatrixXf in, PCLPointCloud2 out)
// PCA
// posesfrommatches

// removenan can be done with passthrough_filter

#if 0
// disparity to xyz
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud (new
                                              pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::RGB>::Ptr left_image (new
                                             pcl::PointCloud<pcl::RGB>);
// Fill left image cloud.
// pcl::DisparityMapConverter<pcl::PointXYZI> dmc;
// dmc.setBaseline (0.8387445f);
// dmc.setFocalLength (368.534700f);
// dmc.setImageCenterX (318.112200f);
// dmc.setImageCenterY (224.334900f);
// dmc.setDisparityThresholdMin(15.0f);
// // Left view of the scene.
// dmc.setImage (left_image);
// // Disparity map of the scene.
// dmc.loadDisparityMap ("disparity_map.txt", 640, 480);
// dmc.compute(*cloud);
#endif

#if 0
// disparity to dem
pcl::PointCloud<pcl::PointDEM>::Ptr cloud (new
                                               pcl::PointCloud<pcl::PointDEM>);
pcl::PointCloud<pcl::RGB>::Ptr left_image (new
                                             pcl::PointCloud<pcl::RGB>);
// Fill left image cloud.
// pcl::DigitalElevationMapBuilder demb;
// demb.setBaseline (0.8387445f);
// demb.setFocalLength (368.534700f);
// demb.setImageCenterX (318.112200f);
// demb.setImageCenterY (224.334900f);
// demb.setDisparityThresholdMin (15.0f);
// demb.setDisparityThresholdMax (80.0f);
// demb.setResolution (64, 32);
// // Left view of the scene.
// demb.loadImage (left_image);
// // Disparity map of the scene.
// demb.loadDisparityMap ("disparity_map.txt", 640, 480);
// demb.compute(*cloud);
#endif

// GroundPlaneComparator to beused with OrganizedConnectedComponentSegmentation
// TSDFVolume
// generalized icp
// voxelgridlabel(pointxyzrgbl)
// voxelgridcovariance (PointT) used for ndt3d
// medianfilter for organized xyz
// covariancesampling(pointT, pointNT)
// organizededgefronormals??
// PointRepresentation<PointT>::copyToFloatArray
// pclCorrespondence(int,int)
// getApproximateIndices(PointT)
// pointToPlaneDistance, projectPoint(to plane)
// extractEuclideanClusters(PointT)

#if 0
py::tuple pclpc2_minmax(const PCLPC2::Ptr pc) {
  std::vector<int> valid_ix;
  get_valid_xyz_indices(*pc, valid_ix);
  if (valid_ix.empty()) {
    return py::make_tuple(py::none, py::none);
  }

  PointCloud2ConstIterator<float> iter_x0(*pc, "x");
  auto iter_x = iter_x0 + valid_ix;
  Eigen::Vector4f minpt;
  Eigen::Vector4f maxpt;
  for (int i : valid_ix) {
    auto iter_x = iter_x0;
    iter_x += i;
    //py::print("i=", i, "iter_x[0] = ", iter_x[0]);
    Eigen::Vector4f p(iter_x[0],
                      iter_x[1],
                      iter_x[2],
                      0.);

  }
}
#endif

Eigen::Vector3f pclpc2_centroid(const PCLPC2::Ptr pc) {
  std::vector<int> valid_ix;
  get_valid_xyz_indices(*pc, valid_ix);
  if (valid_ix.empty()) {
    return Eigen::Vector3f(std::numeric_limits<float>::quiet_NaN(),
                           std::numeric_limits<float>::quiet_NaN(),
                           std::numeric_limits<float>::quiet_NaN());
  }
  float n = static_cast<float>(valid_ix.size());

  PointCloud2ConstIterator<float> iter_x0(*pc, "x");
  Eigen::Vector4f centroid = Eigen::Vector4f::Zero();
  for (int i : valid_ix) {
    auto iter_x = iter_x0;
    iter_x += i;
    // TODO use Map
    Eigen::Vector4f p(iter_x[0], iter_x[1], iter_x[2], 0.);
    centroid += (p / n);
  }
  return centroid.head<3>();
}

void export_common(py::module& m) {
  m.def("pclpc2_centroid", &pclpc2_centroid, py::arg("pc"));
}
}


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

/*
 * region-math.hpp
 *
 *  Created on: Dec 1, 2014
 *      Author: David A. Boyuka II
 */
#ifndef REGION_MATH_HPP_
#define REGION_MATH_HPP_

#include <vector>
#include <set>
#include <boost/variant.hpp>

#include "pique/indexing/binned-index-types.hpp"
#include "pique/region/region-encoding.hpp"
#include "pique/setops/setops.hpp"

namespace RegionMath {

using region_id_t = BinnedIndexTypes::region_id_t;

struct RegionTerm {
	RegionTerm(region_id_t rid) : rid(rid) {}
	const region_id_t rid;
};

struct UnaryOperatorTerm {
	UnaryOperatorTerm(UnarySetOperation op) : op(op) {}
	const UnarySetOperation op;
};

struct NAryOperatorTerm {
	NAryOperatorTerm(NArySetOperation op, int arity = 2) : op(op), arity(arity) {}
	const NArySetOperation op;
	const int arity;
};

using TermVariant = boost::variant< RegionTerm, UnaryOperatorTerm, NAryOperatorTerm >;

class RegionMath : public std::vector< TermVariant > {
public:
	static constexpr int REGION = 0;
	static constexpr int UNARY_OP = 1;
	static constexpr int NARY_OP = 2;

	RegionMath() {}
	RegionMath(region_id_t only) : std::vector< TermVariant >(1, RegionTerm(only)) {}

	RegionMath & push_region_range(region_id_t lb, region_id_t ub);
	RegionMath & push_region(region_id_t region_id);
	RegionMath & push_op(UnarySetOperation op);
	RegionMath & push_op(NArySetOperation op); // arity = all
	RegionMath & push_op(NArySetOperation op, int arity);
	RegionMath & push_region_op(region_id_t region_id, UnarySetOperation op); // push_region, push_op
	RegionMath & push_region_op(region_id_t region_id, NArySetOperation op); // push_region, push_op

	std::set< region_id_t > get_all_regions() const;

	std::string to_string();
};

}

#endif /* REGION_MATH_HPP_ */


// Boost.Geometry
// Unit Test

// Copyright (c) 2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to 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)

#include <geometry_test_common.hpp>

#include <boost/geometry/srs/projection.hpp>

namespace srs = bg::srs;
namespace par = bg::srs::par4;

int test_main(int, char* [])
{
    typedef par::proj<par::aea> proj;
    typedef par::ellps<par::clrk80> ellps;
    typedef par::datum<par::ire65> datum;
    typedef par::o_proj<par::tmerc> o_proj;
    typedef par::guam guam;

    /*BOOST_MPL_ASSERT_MSG((par::detail::is_proj<proj>::value),
                         PROJ, (proj));
    BOOST_MPL_ASSERT_MSG((!par::detail::is_proj<int>::value),
                         NOT_PROJ, (int));

    BOOST_MPL_ASSERT_MSG((par::detail::is_ellps<ellps>::value),
                         ELLPS, (ellps));
    BOOST_MPL_ASSERT_MSG((!par::detail::is_ellps<int>::value),
                         NOT_ELLPS, (int));

    BOOST_MPL_ASSERT_MSG((par::detail::is_datum<datum>::value),
                         DATUM, (datum));
    BOOST_MPL_ASSERT_MSG((!par::detail::is_datum<int>::value),
                         NOT_DATUM, (int));

    BOOST_MPL_ASSERT_MSG((par::detail::is_o_proj<o_proj>::value),
                         O_PROJ, (o_proj));
    BOOST_MPL_ASSERT_MSG((!par::detail::is_o_proj<int>::value),
                         NOT_O_PROJ, (int));

    BOOST_MPL_ASSERT_MSG((par::detail::is_guam<guam>::value),
                         GUAM, (guam));
    BOOST_MPL_ASSERT_MSG((!par::detail::is_guam<int>::value),
                         NOT_GUAM, (int));*/

    BOOST_MPL_ASSERT_MSG((par::detail::is_param_t<par::proj>::pred<proj>::value),
                         PROJ, (proj));
    BOOST_MPL_ASSERT_MSG((!par::detail::is_param_t<par::proj>::pred<int>::value),
                         NOT_PROJ, (int));

    BOOST_MPL_ASSERT_MSG((par::detail::is_param_t<par::ellps>::pred<ellps>::value),
                         ELLPS, (ellps));
    BOOST_MPL_ASSERT_MSG((!par::detail::is_param_t<par::ellps>::pred<int>::value),
                         NOT_ELLPS, (int));

    BOOST_MPL_ASSERT_MSG((par::detail::is_param_t<par::datum>::pred<datum>::value),
                         DATUM, (datum));
    BOOST_MPL_ASSERT_MSG((!par::detail::is_param_t<par::datum>::pred<int>::value),
                         NOT_DATUM, (int));

    BOOST_MPL_ASSERT_MSG((par::detail::is_param_t<par::o_proj>::pred<o_proj>::value),
                         O_PROJ, (o_proj));
    BOOST_MPL_ASSERT_MSG((!par::detail::is_param_t<par::o_proj>::pred<int>::value),
                         NOT_O_PROJ, (int));

    BOOST_MPL_ASSERT_MSG((par::detail::is_param<par::guam>::pred<guam>::value),
                         GUAM, (guam));
    BOOST_MPL_ASSERT_MSG((!par::detail::is_param<par::guam>::pred<int>::value),
                         NOT_GUAM, (int));

    typedef srs::static_proj4<proj, ellps, datum, o_proj, guam> params;
    typedef srs::static_proj4<proj, ellps> params_e;
    typedef srs::static_proj4<proj, datum> params_d;
    typedef srs::static_proj4<proj> params_0;

    BOOST_MPL_ASSERT_MSG((boost::is_same<par::detail::pick_proj_tag<params>::type, par::aea>::value),
                         PICK_PROJ, (params));
    BOOST_MPL_ASSERT_MSG((boost::is_same<par::detail::pick_ellps<params>::type::type, par::clrk80>::value),
                         PICK_ELLPS, (params));
    BOOST_MPL_ASSERT_MSG((boost::is_same<par::detail::pick_o_proj_tag<params>::type, par::tmerc>::value),
                         PICK_O_PROJ, (params));
    BOOST_MPL_ASSERT_MSG((boost::is_same<par::detail::pick_ellps<params_e>::type::type, par::clrk80>::value),
                         PICK_ELLPS_E, (params_e));
    BOOST_MPL_ASSERT_MSG((boost::is_same<par::detail::pick_ellps<params_d>::type::type, par::mod_airy>::value),
                         PICK_ELLPS_D, (params_d));
    //default ellps WGS84
    BOOST_MPL_ASSERT_MSG((boost::is_same<par::detail::pick_ellps<params_0>::type::type, par::WGS84>::value),
                         PICK_NO_ELLPS, (params_0));

    return 0;
}


#ifndef QST_SGD_HPP
#define QST_SGD_HPP

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <cassert>
#include <cmath>
#include <random>

namespace qst{

// Stochastic gradient descent
class Sgd{

    double eta_;                // Learning rate
    int npar_;                  // Number of parameters
    double l2reg_;              // L2 regularization
    Eigen::VectorXd deltaP_;    // Parameter change
    std::mt19937 rgen_;         // Random number

public:

    Sgd(Parameters &par):eta_(par.lr_),l2reg_(par.l2_){
        npar_=-1;
    }

    // Set the number of parameter
    void SetNpar(int npar){
        npar_=npar;
        deltaP_.resize(npar_);
    }

    // Update the parameters
    void Update(Eigen::VectorXd & pars){
        assert(npar_>0);

        std::uniform_real_distribution<double> distribution(0,1);
        for(int i=0;i<npar_;i++){
            pars(i)=pars(i) - (deltaP_(i)+l2reg_*pars(i))*eta_;
        }
    }

    // Read update from gradient calculator
    void getUpdates(const Eigen::MatrixXd & derLog){
        deltaP_ = derLog;
    }

    void Reset(){

    }
};

}

#endif


//////////////////////////////////////////////////////////////////////////////
//
// (C) Copyright Ion Gaztanaga 2015-2016.
// 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)
//
// See http://www.boost.org/libs/move for documentation.
//
//////////////////////////////////////////////////////////////////////////////

#include <cstdlib>   //std::srand
#include <iostream>  //std::cout

#include <boost/config.hpp>

#include <boost/move/unique_ptr.hpp>
#include <boost/move/algo/detail/merge_sort.hpp>

#include "order_type.hpp"
#include "random_shuffle.hpp"

#include <boost/move/algo/adaptive_merge.hpp>
#include <boost/move/core.hpp>

template<class T>
bool test_random_shuffled(std::size_t const element_count, std::size_t const num_keys, std::size_t const num_iter)
{
   boost::movelib::unique_ptr<T[]> elements(new T[element_count]);
   boost::movelib::unique_ptr<std::size_t[]> key_reps(new std::size_t[num_keys ? num_keys : element_count]);
   std::cout << "- - N: " << element_count << ", Keys: " << num_keys << ", It: " << num_iter << " \n";

   //Initialize keys
   for(std::size_t  i=0; i < element_count; ++i){
      std::size_t  key = num_keys ? (i % num_keys) : i;
      elements[i].key=key;
   }

   std::srand(0);

   for (std::size_t i = 0; i != num_iter; ++i)
   {
      ::random_shuffle(elements.get(), elements.get() + element_count);
      for(std::size_t i = 0; i < (num_keys ? num_keys : element_count); ++i){
         key_reps[i]=0;
      }
      for(std::size_t i = 0; i < element_count; ++i){
         elements[i].val = key_reps[elements[i].key]++;
      }

      boost::movelib::unique_ptr<char[]> buf(new char [sizeof(T)*(element_count-element_count/2)]);

      std::size_t const split = std::size_t(std::rand()) % element_count;
      boost::movelib::merge_sort(elements.get(), elements.get()+split, order_type_less(), (T*)buf.get());
      boost::movelib::merge_sort(elements.get()+split, elements.get()+element_count, order_type_less(), (T*)buf.get());

      boost::movelib::adaptive_merge(elements.get(), elements.get()+split, elements.get()+element_count, order_type_less());

      if (!is_order_type_ordered(elements.get(), element_count))
      {
         std::cout <<  "\n ERROR\n";
         throw int(0);
      }
   }
   return true;
}

int main()
{
   const std::size_t NIter = 100;
   test_random_shuffled<order_move_type>(10001, 65,   NIter);
   test_random_shuffled<order_move_type>(10001, 101,  NIter);
   test_random_shuffled<order_move_type>(10001, 1023, NIter);
   test_random_shuffled<order_move_type>(10001, 4095, NIter);
   test_random_shuffled<order_move_type>(10001, 0,    NIter);

   return 0;
}


/******************************************************************************\
* Author: Matthew Beauregard Smith                                             *
* Affiliation: The University of Texas at Austin                               *
* Department: Oden Institute and Institute for Cellular and Molecular Biology  *
* PI: Edward Marcotte                                                          *
* Project: Protein Fluorosequencing                                            *
\******************************************************************************/

// Boost unit test framework (recommended to be the first include):
#include <boost/test/unit_test.hpp>

// File under test:
#include "stuck-dye-state-vector.h"

namespace whatprot {

BOOST_AUTO_TEST_SUITE(hmm_suite)
BOOST_AUTO_TEST_SUITE(state_vector_suite)
BOOST_AUTO_TEST_SUITE(stuck_dye_state_vector_suite)

BOOST_AUTO_TEST_CASE(initialize_from_start_test) {
    StuckDyeStateVector sdsv;
    sdsv.initialize_from_start();
    BOOST_TEST(sdsv.dye == 1.0);
    BOOST_TEST(sdsv.no_dye == 0.0);
}

BOOST_AUTO_TEST_CASE(initialize_from_finish_test) {
    StuckDyeStateVector sdsv;
    sdsv.initialize_from_finish();
    BOOST_TEST(sdsv.dye == 1.0);
    BOOST_TEST(sdsv.no_dye == 1.0);
}

BOOST_AUTO_TEST_CASE(sum_test) {
    StuckDyeStateVector sdsv;
    sdsv.dye = 2.0;
    sdsv.no_dye = 2.0;
    BOOST_TEST(sdsv.sum() == 4.0);
}

BOOST_AUTO_TEST_CASE(source_test) {
    StuckDyeStateVector sdsv;
    sdsv.dye = 3.14;
    sdsv.no_dye = 2.12;
    BOOST_TEST(sdsv.source() == 3.14);
}

BOOST_AUTO_TEST_SUITE_END()  // stuck_dye_state_vector_suite
BOOST_AUTO_TEST_SUITE_END()  // state_vector_suite
BOOST_AUTO_TEST_SUITE_END()  // hmm_suite

}  // namespace whatprot


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_IS_INCLUDED_C_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IS_INCLUDED_C_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-reduction
    Function object implementing is_included_c capabilities

    Returns @ref True is only if all bits set in x are not set in y

    @par Semantic:

    For every parameters of types respectively T0, T1 of same bit size:

    @code
    logical<scalar_of_t<T0>> r = is_included_c(x,y);
    @endcode

    is similar to:

    @code
    logical<scalar_of_t<T0>> r = all(x&y == zero);
    @endcode

    @par Alias:
    @c testz, @c are_disjoint

  **/
  const boost::dispatch::functor<tag::is_included_c_> is_included_c = {};
} }
#endif

#include <boost/simd/function/scalar/is_included_c.hpp>
#include <boost/simd/function/simd/is_included_c.hpp>

#endif


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschrÃ¤nkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

int main(int , char** )
{

    typedef mtl::mat::parameters<mtl::row_major,
	mtl::index::c_index,
	mtl::non_fixed::dimensions,
	false, int> para;
    typedef mtl::compressed2D<double,para> matrix_type;
    matrix_type Ks;
    //
    mtl::io::matrix_market_istream ik("../../../../../branches/data/matrix_market/outK.mm");
    //
    ik >> Ks;
    //
    ik.close();
    std::cout << num_rows(Ks) << std::endl;
    std::cout << num_cols(Ks) << std::endl;
    std::cout << Ks.nnz() << std::endl;
    for(int k=0;k<10;k++)
        std::cout << Ks(k,k) << std::endl;

    if (num_rows(Ks) < 20)
	std::cout << Ks;

    return 0;
}


/*
Copyright 2013 Rogier van Dalen.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/

#ifndef MATH_DETAIL_IS_CLOSE_HPP
#define MATH_DETAIL_IS_CLOSE_HPP

#include <cmath>

#include <boost/utility/enable_if.hpp>

#include "./log-float_fwd.hpp"

namespace math { namespace detail {

template <class Type, class Tolerance, class Enable =
    typename boost::disable_if <is_log_float <Type>>::type>
inline bool is_close (
    Type const & left, Type const & right, Tolerance const & tolerance)
{
    using std::abs;
    if (left == right)
        return true;
    else {
        auto difference = abs (left - right);
        return difference <= tolerance * abs (left)
            && difference <= tolerance * abs (right);
    }
}

// log_float: compare exponents.
template <class Type, class Tolerance, class Enable =
    typename boost::enable_if <is_log_float <Type>>::type>
inline bool is_close (
    Type const & left, Type const & right, Tolerance const & tolerance,
    // Dummy argument to distinguish this overloads from the one above.
    void * = 0)
{ return is_close (left.exponent(), right.exponent(), tolerance); }

}} // namespace math::detail

#endif // MATH_DETAIL_IS_CLOSE_HPP


//  (C) Copyright John Maddock 2015.
//  Use, modification and distribution are subject to 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)

#ifndef BOOST_REMEZ_MULTIPRECISION_HPP
#define BOOST_REMEZ_MULTIPRECISION_HPP

#include <boost/multiprecision/cpp_bin_float.hpp>

#ifdef USE_NTL
#include <boost/math/bindings/rr.hpp>
namespace std {
   using boost::math::ntl::pow;
} // workaround for spirit parser.

typedef boost::math::ntl::RR mp_type;

inline void set_working_precision(int n)
{
   NTL::RR::SetPrecision(working_precision);
}

inline int get_working_precision()
{
   return mp_type::precision(working_precision);
}

inline void set_output_precision(int n)
{
   NTL::RR::SetOutputPrecision(n);
}

inline mp_type round_to_precision(mp_type m, int bits)
{
   return NTL::RoundToPrecision(m.value(), bits);
}

namespace boost {
   namespace math {
      namespace tools {

         template <>
         inline boost::multiprecision::cpp_bin_float_double_extended real_cast<boost::multiprecision::cpp_bin_float_double_extended, mp_type>(mp_type val)
         {
            unsigned p = NTL::RR::OutputPrecision();
            NTL::RR::SetOutputPrecision(20);
            boost::multiprecision::cpp_bin_float_double_extended r = boost::lexical_cast<boost::multiprecision::cpp_bin_float_double_extended>(val);
            NTL::RR::SetOutputPrecision(p);
            return r;
         }
         template <>
         inline boost::multiprecision::cpp_bin_float_quad real_cast<boost::multiprecision::cpp_bin_float_quad, mp_type>(mp_type val)
         {
            unsigned p = NTL::RR::OutputPrecision();
            NTL::RR::SetOutputPrecision(35);
            boost::multiprecision::cpp_bin_float_quad r = boost::lexical_cast<boost::multiprecision::cpp_bin_float_quad>(val);
            NTL::RR::SetOutputPrecision(p);
            return r;
         }

      }
   }
}

#elif defined(USE_MPFR_100)

#include <boost/multiprecision/mpfr.hpp>

typedef boost::multiprecision::mpfr_float_100 mp_type;

inline void set_working_precision(int n)
{
}

inline void set_output_precision(int n)
{
   std::cout << std::setprecision(n);
   std::cerr << std::setprecision(n);
}

inline mp_type round_to_precision(mp_type m, int bits)
{
   mpfr_prec_round(m.backend().data(), bits, MPFR_RNDN);
   return m;
}

inline int get_working_precision()
{
   return std::numeric_limits<mp_type>::digits;
}

namespace boost {
   namespace math {
      namespace tools {

         template <>
         inline boost::multiprecision::cpp_bin_float_double_extended real_cast<boost::multiprecision::cpp_bin_float_double_extended, mp_type>(mp_type val)
         {
            return boost::multiprecision::cpp_bin_float_double_extended(val);
         }
         template <>
         inline boost::multiprecision::cpp_bin_float_quad real_cast<boost::multiprecision::cpp_bin_float_quad, mp_type>(mp_type val)
         {
            return boost::multiprecision::cpp_bin_float_quad(val);
         }

      }
   }
}

#else

#include <boost/multiprecision/mpfr.hpp>

typedef boost::multiprecision::mpfr_float mp_type;

inline void set_working_precision(int n)
{
   boost::multiprecision::mpfr_float::default_precision(boost::multiprecision::detail::digits2_2_10(n));
}

inline void set_output_precision(int n)
{
   std::cout << std::setprecision(n);
   std::cerr << std::setprecision(n);
}

inline mp_type round_to_precision(mp_type m, int bits)
{
   mpfr_prec_round(m.backend().data(), bits, MPFR_RNDN);
   return m;
}

inline int get_working_precision()
{
   return mp_type::default_precision();
}

namespace boost {
   namespace math {
      namespace tools {

         template <>
         inline boost::multiprecision::cpp_bin_float_double_extended real_cast<boost::multiprecision::cpp_bin_float_double_extended, mp_type>(mp_type val)
         {
            return boost::multiprecision::cpp_bin_float_double_extended(val);
         }
         template <>
         inline boost::multiprecision::cpp_bin_float_quad real_cast<boost::multiprecision::cpp_bin_float_quad, mp_type>(mp_type val)
         {
            return boost::multiprecision::cpp_bin_float_quad(val);
         }

      }
   }
}

#endif

#endif // BOOST_REMEZ_MULTIPRECISION_HPP


//==================================================================================================
/*!
  @file

  @copyright 2012-2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_DEFINITION_PIO_3_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_DEFINITION_PIO_3_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/simd/detail/brigand.hpp>
#include <boost/simd/detail/dispatch.hpp>
#include <boost/simd/detail/constant_traits.hpp>
#include <boost/dispatch/function/make_callable.hpp>
#include <boost/dispatch/hierarchy/functions.hpp>
#include <boost/dispatch/as.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
    struct pio_3_ : boost::dispatch::constant_value_<pio_3_>
    {
      BOOST_DISPATCH_MAKE_CALLABLE(ext,pio_3_,boost::dispatch::constant_value_<pio_3_>);
      BOOST_SIMD_REGISTER_CONSTANT(0, 0X3F860A92LL, 0X3FF0C152382D7366ULL);
    };
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag,pio_3_);
  }

  namespace detail
  {
    BOOST_DISPATCH_CALLABLE_DEFINITION(tag::pio_3_,pio_3);
  }

  template<typename T> BOOST_FORCEINLINE auto Pio_3()
  BOOST_NOEXCEPT_DECLTYPE(detail::pio_3( boost::dispatch::as_<T>{}))
  {
    return detail::pio_3( boost::dispatch::as_<T>{} );
  }
} }

#endif