// File: match_utils.cpp
// Author: Francesco Giacomini
// Copyright (c) Members of the EGEE Collaboration. 2009.
// See http://www.eu-egee.org/partners/ for details on the copyright holders.

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

// $Id: match_utils.cpp,v 1.1.2.1.4.1 2009/06/18 08:33:29 fcapanni Exp $

#include "match_utils.h"
#include <cmath>
#include <functional>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_smallint.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/regex.hpp>
#include <boost/thread/xtime.hpp>
#include <classad_distribution.h>
#include "glite/wmsutils/classads/classad_utils.h"

namespace ca = glite::wmsutils::classads;

namespace glite {
namespace wms {
namespace manager {
namespace server {

template<typename Container, typename T>
T variance(Container const& c, T mean)
{
  T v = T();
  size_t n = 0;
  typename Container::const_iterator first = c.begin();
  typename Container::const_iterator const last = c.end();
  for ( ; first != last; ++first, ++n) {
    T t = *first - mean;
    v += t * t;
  }

  return n ? v / n : v;
}

class rank_greater_than
  : public std::unary_function<match_type, bool>
{
  double m_rank;
public:
  rank_greater_than(double rank)
    : m_rank(rank)
  {
  }
  bool operator()(match_type const& match)
  {
    return match.get<1>() > m_rank;
  }
};

class rank_less_than
  : public std::unary_function<match_type, bool>
{
  double m_rank;
public:
  rank_less_than(double rank)
    : m_rank(rank)
  {
  }
  bool operator()(match_type const& match)
  {
    return match.get<1>() < m_rank;
  }
};

matches_type::const_iterator
select_best_ce_max_rank(matches_type const& matches)
{
  assert(!matches.empty());
  matches_type::const_iterator const begin(matches.begin());

  double max_rank = begin->get<1>();
  matches_type::const_iterator it(
    find_if(begin, matches.end(), rank_less_than(max_rank))
  );

  boost::xtime current;
  boost::xtime_get(&current, boost::TIME_UTC);
  boost::minstd_rand generator(static_cast<unsigned int>(current.nsec));
  boost::uniform_smallint<size_t> distrib(0, distance(begin, it) - 1);
  boost::variate_generator<
    boost::minstd_rand,
    boost::uniform_smallint<size_t>
    > rand(generator, distrib);

  return begin + rand();
}

double get_p(double sum)
{
  boost::minstd_rand dist(std::time(0));
  boost::uniform_01<boost::minstd_rand> rand(dist);
  return rand() * sum;
}

matches_type::const_iterator
select_best_ce_stochastic(matches_type const& matches)
{
  assert(!matches.empty());

  std::vector<double> ranks;
  ranks.reserve(matches.size());
  double rank_sum = 0.;
  matches_type::const_iterator b = matches.begin();
  matches_type::const_iterator e = matches.end();
  for (; b != e; ++b) {
    double r = b->get<1>();
    ranks.push_back(r);
    rank_sum += r;
  }
  double rank_mean     = rank_sum / ranks.size();
  double rank_variance = variance(ranks, rank_mean);
  // We smooth rank values according to the following function:
  // f(x) = atan( V * (x - mean ) / dev ) + PI
  // Thanks to Alessio Gianelle for his usefull support and suggestions.
  static const double PI = std::atan(1.) * 4.;
  static const double V = PI;
  // Computing the variance and standard deviation of rank samples...
  double rank_deviation = rank_variance > 0 ? sqrt(rank_variance) : V;

  rank_sum = 0.;
  for (size_t r = 0; r < ranks.size(); ++r) {
    ranks[r] = atan(V * (ranks[r] - rank_mean) / rank_deviation) + PI;
    rank_sum += ranks[r];
  }

  boost::xtime current;
  boost::xtime_get(&current, boost::TIME_UTC);
  boost::minstd_rand generator(static_cast<unsigned int>(current.nsec));
  boost::uniform_01<boost::minstd_rand> rand(generator);
  double const p = rand() * rank_sum;

  double prob_sum = 0.;
  size_t i = 0;
  matches_type::const_iterator best = matches.begin();
  matches_type::const_iterator const matches_end = matches.end();
  do {
    prob_sum += ranks[i++];
    if (p < prob_sum) {
      break;
    }
  } while (++best != matches_end);

  return best;
}

matches_type::const_iterator
select_best_ce(matches_type const& matches, bool use_fuzzy_rank)
{
  assert(!matches.empty());

  if (use_fuzzy_rank) {
    return select_best_ce_stochastic(matches);
  } else {
    return select_best_ce_max_rank(matches);
  }
}

bool
fill_matches(
  classad::ClassAd const& match_response,
  matches_type& matches,
  bool include_brokerinfo,
  bool include_cream_resources
)
try {

  std::string reason(ca::evaluate_attribute(match_response, "reason"));
  if (reason != "ok") {
    throw MatchError(reason);
  }

  classad::ExprList const* const match_result(
    ca::evaluate_attribute(match_response, "match_result")
  );

  classad::ExprList::const_iterator it = match_result->begin();
  classad::ExprList::const_iterator const end = match_result->end();
  for ( ; it != end; ++it) {
    assert(ca::is_classad(*it));
    classad::ClassAd const& match(*static_cast<classad::ClassAd const*>(*it));

    std::string const ce_id(ca::evaluate_attribute(match, "ce_id"));
    {
      // if the ce is cream-based ignore it, because dagman is not able to
      // manage it
      static boost::regex const re(".+/([^-]+)-.+");
      boost::smatch pieces;
      std::string base;
      if (boost::regex_match(ce_id, pieces, re)) {
        base.assign(pieces[1].first, pieces[1].second);
      }
      if (base.empty() || (include_cream_resources==false && base == "cream")) {
        continue;
      }
    }
    double rank(ca::evaluate_attribute(match, "rank"));
    ClassAdPtr ad_ptr;
    if (include_brokerinfo) {
      classad::ClassAd const* brokerinfo(
        ca::evaluate_attribute(match, "brokerinfo")
      );
      ad_ptr.reset(static_cast<classad::ClassAd*>(brokerinfo->Copy()));
    }
    matches.push_back(
      boost::make_tuple(ce_id, rank, ad_ptr)
    );
  }

  return !matches.empty();

} catch (ca::InvalidValue&) {
  throw MatchError(
    "invalid format: " + ca::unparse_classad(match_response)
  );
}

}}}} // glite::wms::manager::server


#ifndef BOOSTRTREES_BOOSTGEOMETRYTYPES_HPP
#define BOOSTRTREES_BOOSTGEOMETRYTYPES_HPP

#include <vector>
#include <boost/foreach.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/algorithms/distance.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/index/rtree.hpp>

namespace rtrees {

// Helpful namespace shortcuts
namespace b = boost;
namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;

// Typedefs for points, polygons and multipolygons
typedef bg::model::d2::point_xy<double> point_t;
typedef bg::model::polygon <point_t> polygon_t;
typedef bg::model::multi_polygon <polygon_t> mpolygon_t;

// Typedefs for bbox
typedef bg::model::box <point_t> bbox;
typedef std::pair<point_t, long> value;

}

#endif //BOOSTRTREES_BOOSTGEOMETRYTYPES_HPP


///////////////////////////////////////////////////////////////
//  Copyright 2011 John Maddock. 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_

#ifdef _MSC_VER
#  define _SCL_SECURE_NO_WARNINGS
#endif

#include "test.hpp"

#if !defined(TEST_MPF_50) && !defined(TEST_MPF) && !defined(TEST_BACKEND) && !defined(TEST_MPZ) && \
   !defined(TEST_CPP_DEC_FLOAT) && !defined(TEST_MPFR) && !defined(TEST_MPFR_50) && !defined(TEST_MPQ) && \
   !defined(TEST_TOMMATH) && !defined(TEST_CPP_INT) && !defined(TEST_MPFI_50) &&!defined(TEST_FLOAT128) && !defined(TEST_CPP_BIN_FLOAT)
#  define TEST_MPF_50
#  define TEST_MPF
#  define TEST_BACKEND
#  define TEST_MPZ
#  define TEST_MPFR
#  define TEST_MPFR_50
#  define TEST_CPP_DEC_FLOAT
#  define TEST_MPQ
#  define TEST_TOMMATH
#  define TEST_CPP_INT
#  define TEST_MPFI_50
#  define TEST_FLOAT128
#  define TEST_CPP_BIN_FLOAT

#ifdef _MSC_VER
#pragma message("CAUTION!!: No backend type specified so testing everything.... this will take some time!!")
#endif
#ifdef __GNUC__
#pragma warning "CAUTION!!: No backend type specified so testing everything.... this will take some time!!"
#endif

#endif

#if defined(TEST_MPF_50) || defined(TEST_MPF) || defined(TEST_MPZ) || defined(TEST_MPQ)
#include <boost/multiprecision/gmp.hpp>
#endif
#ifdef TEST_BACKEND
#include <boost/multiprecision/concepts/mp_number_archetypes.hpp>
#endif
#ifdef TEST_CPP_DEC_FLOAT
#include <boost/multiprecision/cpp_dec_float.hpp>
#endif
#if defined(TEST_MPFR) || defined(TEST_MPFR_50)
#include <boost/multiprecision/mpfr.hpp>
#endif
#if defined(TEST_MPFI_50)
#include <boost/multiprecision/mpfi.hpp>
#endif
#ifdef TEST_TOMMATH
#include <boost/multiprecision/tommath.hpp>
#endif
#ifdef TEST_CPP_INT
#include <boost/multiprecision/cpp_int.hpp>
#endif
#ifdef TEST_FLOAT128
#include <boost/multiprecision/float128.hpp>
#endif
#ifdef TEST_CPP_BIN_FLOAT
#include <boost/multiprecision/cpp_bin_float.hpp>
#endif

#ifdef BOOST_MSVC
#pragma warning(disable:4127)
#endif

#define PRINT(x)\
   std::cout << BOOST_STRINGIZE(x) << " = " << std::numeric_limits<Number>::x << std::endl;

template <class Number>
void test_specific(const boost::mpl::int_<boost::multiprecision::number_kind_floating_point>&)
{
   Number minv, maxv;
   minv = (std::numeric_limits<Number>::min)();
   maxv = (std::numeric_limits<Number>::max)();
   BOOST_CHECK((boost::math::isnormal)(minv));
   BOOST_CHECK((boost::math::isnormal)(maxv));
   BOOST_CHECK((boost::math::isnormal)(log(minv)));
   BOOST_CHECK((boost::math::isnormal)(log(maxv)));
   BOOST_CHECK((boost::math::isnormal)(sqrt(minv)));
   BOOST_CHECK((boost::math::isnormal)(sqrt(maxv)));

   if(std::numeric_limits<Number>::is_specialized)
   {
      if(std::numeric_limits<Number>::has_quiet_NaN)
      {
         BOOST_TEST((boost::math::isnan)(std::numeric_limits<Number>::quiet_NaN()));
         BOOST_TEST(FP_NAN == (boost::math::fpclassify)(std::numeric_limits<Number>::quiet_NaN()));
         BOOST_TEST(!(boost::math::isfinite)(std::numeric_limits<Number>::quiet_NaN()));
         BOOST_TEST(!(boost::math::isnormal)(std::numeric_limits<Number>::quiet_NaN()));
         BOOST_TEST(!(boost::math::isinf)(std::numeric_limits<Number>::quiet_NaN()));
      }
      if(std::numeric_limits<Number>::has_signaling_NaN)
      {
         BOOST_TEST((boost::math::isnan)(std::numeric_limits<Number>::signaling_NaN()));
         BOOST_TEST(FP_NAN == (boost::math::fpclassify)(std::numeric_limits<Number>::signaling_NaN()));
         BOOST_TEST(!(boost::math::isfinite)(std::numeric_limits<Number>::signaling_NaN()));
         BOOST_TEST(!(boost::math::isnormal)(std::numeric_limits<Number>::signaling_NaN()));
         BOOST_TEST(!(boost::math::isinf)(std::numeric_limits<Number>::signaling_NaN()));
      }
      if(std::numeric_limits<Number>::has_infinity)
      {
         BOOST_TEST((boost::math::isinf)(std::numeric_limits<Number>::infinity()));
         BOOST_TEST(FP_INFINITE == (boost::math::fpclassify)(std::numeric_limits<Number>::infinity()));
         BOOST_TEST(!(boost::math::isfinite)(std::numeric_limits<Number>::infinity()));
         BOOST_TEST(!(boost::math::isnormal)(std::numeric_limits<Number>::infinity()));
         BOOST_TEST(!(boost::math::isnan)(std::numeric_limits<Number>::infinity()));
      }
      if(std::numeric_limits<Number>::has_denorm)
      {
         BOOST_TEST(FP_SUBNORMAL == (boost::math::fpclassify)(std::numeric_limits<Number>::denorm_min()));
         BOOST_TEST((boost::math::isfinite)(std::numeric_limits<Number>::denorm_min()));
         BOOST_TEST(!(boost::math::isnormal)(std::numeric_limits<Number>::denorm_min()));
         BOOST_TEST(!(boost::math::isinf)(std::numeric_limits<Number>::denorm_min()));
         BOOST_TEST(!(boost::math::isnan)(std::numeric_limits<Number>::denorm_min()));
      }
   }
   Number n = 0;
   BOOST_TEST((boost::math::fpclassify)(n) == FP_ZERO);
   BOOST_TEST((boost::math::isfinite)(n));
   BOOST_TEST(!(boost::math::isnormal)(n));
   BOOST_TEST(!(boost::math::isinf)(n));
   BOOST_TEST(!(boost::math::isnan)(n));
   n = 2;
   BOOST_TEST((boost::math::fpclassify)(n) == FP_NORMAL);
   BOOST_TEST((boost::math::isfinite)(n));
   BOOST_TEST((boost::math::isnormal)(n));
   BOOST_TEST(!(boost::math::isinf)(n));
   BOOST_TEST(!(boost::math::isnan)(n));

   if(std::numeric_limits<Number>::round_style == std::round_to_nearest)
   {
      BOOST_CHECK_EQUAL(std::numeric_limits<Number>::round_error(), 0.5);
   }
   else if(std::numeric_limits<Number>::round_style != std::round_indeterminate)
   {
      // Round error is 1.0:
      BOOST_CHECK_EQUAL(std::numeric_limits<Number>::round_error(), 1);
   }
   else
   {
      // Round error is presumably somewhere between 0.5 and 1:
      BOOST_CHECK((std::numeric_limits<Number>::round_error() <= 1) && (std::numeric_limits<Number>::round_error() >= 0.5));
   }
}

template <class Number>
void test_specific(const boost::mpl::int_<boost::multiprecision::number_kind_integer>&)
{
   if(std::numeric_limits<Number>::is_modulo)
   {
      if(!std::numeric_limits<Number>::is_signed)
      {
         BOOST_TEST(1 + (std::numeric_limits<Number>::max)() == 0);
         BOOST_TEST(--Number(0) == (std::numeric_limits<Number>::max)());
      }
   }
}

template <class Number, class T>
void test_specific(const T&)
{
}

template <class Number>
void test()
{
   //
   // Note really a test just yet, but we can at least print out all the values:
   //
   std::cout << "numeric_limits values for type " << typeid(Number).name() << std::endl;

   PRINT(is_specialized);
   if(std::numeric_limits<Number>::is_integer)
   {
      std::cout << std::hex << std::showbase;
   }
   std::cout << "max()" << " = " << (std::numeric_limits<Number>::max)() << std::endl;
   if(std::numeric_limits<Number>::is_integer)
   {
      std::cout << std::dec;
   }
   std::cout << "max()" << " = " << (std::numeric_limits<Number>::max)() << std::endl;
   std::cout << "min()" << " = " << (std::numeric_limits<Number>::min)() << std::endl;
#ifndef BOOST_NO_CXX11_NUMERIC_LIMITS
   PRINT(lowest());
#endif
   PRINT(digits);
   PRINT(digits10);
#ifndef BOOST_NO_CXX11_NUMERIC_LIMITS
   PRINT(max_digits10);
#endif
   PRINT(is_signed);
   PRINT(is_integer);
   PRINT(is_exact);
   PRINT(radix);
   PRINT(epsilon());
   PRINT(round_error());
   PRINT(min_exponent);
   PRINT(min_exponent10);
   PRINT(max_exponent);
   PRINT(max_exponent10);
   PRINT(has_infinity);
   PRINT(has_quiet_NaN);
   PRINT(has_signaling_NaN);
   PRINT(has_denorm);
   PRINT(has_denorm_loss);
   PRINT(infinity());
   PRINT(quiet_NaN());
   PRINT(signaling_NaN());
   PRINT(denorm_min());
   PRINT(is_iec559);
   PRINT(is_bounded);
   PRINT(is_modulo);
   PRINT(traps);
   PRINT(tinyness_before);
   PRINT(round_style);

   typedef typename boost::mpl::if_c<
      std::numeric_limits<Number>::is_specialized,
      typename boost::multiprecision::number_category<Number>::type,
      boost::mpl::int_<500> // not a number type
   >::type fp_test_type;

   test_specific<Number>(fp_test_type());
}

int main()
{
#ifdef TEST_BACKEND
   test<boost::multiprecision::number<boost::multiprecision::concepts::number_backend_float_architype> >();
#endif
#ifdef TEST_MPF_50
   test<boost::multiprecision::mpf_float_50>();
#endif
#ifdef TEST_MPF
   boost::multiprecision::mpf_float::default_precision(1000);
   /*
   boost::multiprecision::mpf_float r;
   r.precision(50);
   BOOST_TEST(r.precision() >= 50);
   */
   BOOST_TEST(boost::multiprecision::mpf_float::default_precision() == 1000);
   test<boost::multiprecision::mpf_float>();
#endif
#ifdef TEST_MPZ
   test<boost::multiprecision::mpz_int>();
#endif
#ifdef TEST_MPQ
   test<boost::multiprecision::mpq_rational>();
#endif
#ifdef TEST_CPP_DEC_FLOAT
   test<boost::multiprecision::cpp_dec_float_50>();
   test<boost::multiprecision::cpp_dec_float_100>();
#endif
#ifdef TEST_MPFR
   test<boost::multiprecision::mpfr_float>();
#endif
#ifdef TEST_MPFR_50
   test<boost::multiprecision::mpfr_float_50>();
#endif
#ifdef TEST_MPFI_50
   test<boost::multiprecision::mpfi_float_50>();
   test<boost::multiprecision::mpfi_float>();
#endif
#ifdef TEST_TOMMATH
   test<boost::multiprecision::tom_int>();
#endif
#ifdef TEST_CPP_INT
   test<boost::multiprecision::cpp_int>();
   test<boost::multiprecision::int256_t>();
   test<boost::multiprecision::uint512_t>();
   test<boost::multiprecision::number<boost::multiprecision::cpp_int_backend<200, 200, boost::multiprecision::unsigned_magnitude, boost::multiprecision::checked, void> > >();
   test<boost::multiprecision::number<boost::multiprecision::cpp_int_backend<70, 70, boost::multiprecision::signed_magnitude, boost::multiprecision::unchecked, void> > >();
#endif
#ifdef TEST_FLOAT128
   test<boost::multiprecision::float128>();
#endif
#ifdef TEST_CPP_BIN_FLOAT
   test<boost::multiprecision::cpp_bin_float_50>();
#endif
   return boost::report_errors();
}


/**
   @file
   @author Shin'ichiro Nakaoka
*/

#include "MultiSE3Seq.h"
#include "PlainSeqFormatLoader.h"
#include "ValueTree.h"
#include "YAMLWriter.h"
#include "EigenUtil.h"
#include <boost/format.hpp>
#include "gettext.h"

using namespace std;
using namespace cnoid;
using boost::format;

MultiSE3Seq::MultiSE3Seq()
    : MultiSE3Seq::BaseSeqType("MultiSE3Seq")
{

}

MultiSE3Seq::MultiSE3Seq(int numFrames, int numParts)
    : MultiSE3Seq::BaseSeqType("MultiSE3Seq", numFrames, numParts)
{

}

MultiSE3Seq::MultiSE3Seq(const MultiSE3Seq& org)
    : MultiSE3Seq::BaseSeqType(org)
{

}

AbstractSeqPtr MultiSE3Seq::cloneSeq() const
{
    return boost::make_shared<MultiSE3Seq>(*this);
}

MultiSE3Seq::~MultiSE3Seq()
{

}

bool MultiSE3Seq::loadPlainMatrixFormat(const std::string& filename)
{
    clearSeqMessage();
    PlainSeqFileLoader loader;

    if(!loader.load(filename)){
        addSeqMessage(loader.errorMessage());
        return false;
    }

    int n = loader.numParts();
    if(n < 12 || (n % 12) != 0){
        addSeqMessage(filename +
                      "does not have elements in multiple of twelve "
                      "(each 3 for position vectors, 9 for attitde matrices)");
        return false;
    }
    int m = n / 12;

    setDimension(loader.numFrames(), m);
    setTimeStep(loader.timeStep());

    int f = 0;
    Part base = part(0);
    for(PlainSeqFileLoader::iterator it = loader.begin(); it != loader.end(); ++it){
        vector<double>& data = *it;
        int i = 0;
        Frame frame = MultiSE3Seq::frame(f++);
        for(int j=0; j < m; ++j){
            SE3& x = frame[j];
            x.translation() << data[i++], data[i++], data[i++];
            Matrix3 R;
            R <<
                data[i++], data[i++], data[i++],
                data[i++], data[i++], data[i++],
                data[i++], data[i++], data[i++];
            x.rotation() = R;
        }
    }

    return true;
}

bool MultiSE3Seq::saveTopPartAsPlainMatrixFormat(const std::string& filename)
{
    clearSeqMessage();
    boost::format f("%1$.4f");
    const int nFrames = numFrames();

    if(nFrames > 0 && numParts() > 0){

        ofstream os(filename.c_str());
        if(!os){
            addSeqMessage(filename + " cannot be opened.");
            return false;
        }

        const double r = frameRate();

        Part base = part(0);
        for(int i=0; i < nFrames; ++i){
            os << (f % (i / r));
            const SE3& x = base[i];
            for(int j=0; j < 3; ++j){
                os << " " << x.translation()[j];
            }
            Matrix3 R(x.rotation());
            for(int j=0; j < 3; ++j){
                for(int k=0; k < 3; ++k){
                    double m = R(j, k);
                    if(fabs(m) < 1.0e-14){
                        m = 0.0;
                    }
                    os << " " << m;
                }
            }
            os << " 0 0 0 0 0 0"; // velocity elements (dv, omega)
            os << "\n";
        }

        return true;
    }

    return false;
}

static inline void writeSE3(YAMLWriter& writer, const SE3& value)
{
    writer.startFlowStyleListing();

    const Vector3& p = value.translation();
    writer.putScalar(p.x());
    writer.putScalar(p.y());
    writer.putScalar(p.z());

    const Quat& q = value.rotation();
    writer.putScalar(q.w());
    writer.putScalar(q.x());
    writer.putScalar(q.y());
    writer.putScalar(q.z());

    writer.endListing();
}

bool MultiSE3Seq::doWriteSeq(YAMLWriter& writer)
{
    if(BaseSeqType::doWriteSeq(writer)){

        writer.putKeyValue("format", "XYZQWQXQYQZ");

        writer.putKey("frames");
        writer.startListing();
        const int m = numParts();
        const int n = numFrames();
        for(int i=0; i < n; ++i){
            Frame f = frame(i);
            writer.startFlowStyleListing();
            for(int j=0; j < m; ++j){
                writeSE3(writer, f[j]);
            }
            writer.endListing();
        }
        writer.endListing();

        return true;
    }
    return false;
}

bool MultiSE3Seq::doReadSeq(const Mapping& archive)
{
    if(BaseSeqType::doReadSeq(archive)){
        const string& type = archive["type"].toString();
        if(type == seqType() || type == "MultiSe3Seq" || type == "MultiAffine3Seq"){
            string formatString = archive["format"].toString();
            const Listing& values = *archive.findListing("frames");
            if(values.isValid()){
                const int nParts = archive["numParts"].toInt();
                const int nFrames = values.size();
                setDimension(nFrames, nParts);
                if(formatString == "XYZQWQXQYQZ"){
                    readPosQuatSeq(nParts, nFrames, values, true);
                } else if(formatString == "XYZQXQYQZQW"){
                    readPosQuatSeq(nParts, nFrames, values, false);
                } else if(formatString == "XYZRPY"){
                    readPosRpySeq(nParts, nFrames, values);
                } else {
                    addSeqMessage(
                        str(format(("Unknown format \"%1%\" cannot be loaded into MultiSE3Seq."))
                            % formatString));
                    return false;
                }
            }
        }
        return true;
    }
    return false;
}

void MultiSE3Seq::readPosQuatSeq(int nParts, int nFrames, const Listing& values, bool isWfirst)
{
    for(int i=0; i < nFrames; ++i){
        const Listing& frameNode = *values[i].toListing();
        Frame f = frame(i);
        const int n = std::min(frameNode.size(), nParts);
        for(int j=0; j < n; ++j){
            const Listing& node = *frameNode[j].toListing();
            SE3& x = f[j];
            if(node.size() == 7){
                x.translation() << node[0].toDouble(), node[1].toDouble(), node[2].toDouble();
                if(isWfirst){
                    x.rotation() = Quat(node[3].toDouble(),
                                        node[4].toDouble(), node[5].toDouble(), node[6].toDouble());
                } else {
                    x.rotation() = Quat(node[6].toDouble(),
                                        node[3].toDouble(), node[4].toDouble(), node[5].toDouble());
                }
            }
        }
    }
}

void MultiSE3Seq::readPosRpySeq(int nParts, int nFrames, const Listing& values)
{
    for(int i=0; i < nFrames; ++i){
        const Listing& frameNode = *values[i].toListing();
        Frame f = frame(i);
        const int n = std::min(frameNode.size(), nParts);
        for(int j=0; j < n; ++j){
            const Listing& node = *frameNode[j].toListing();
            if(node.size() == 6){
                SE3& x = f[j];
                x.translation() << node[0].toDouble(), node[1].toDouble(), node[2].toDouble();
                const double r = node[3].toDouble();
                const double p = node[4].toDouble();
                const double y = node[5].toDouble();
                x.rotation() = rotFromRpy(r, p, y);
            }
        }
    }
}


//
// Created by yche on 12/24/17.
//
#include <cstdlib>

#include <chrono>
#include <iostream>

#include <boost/program_options.hpp>

#include "../yche_refactor/bflpmc_yche.h"
#include "../yche_refactor/simrank.h"

void test_BFLPMC(string data_name, double c, double epsilon, double delta, int x, int y) {
    string path = get_edge_list_path(data_name);
    GraphYche g(path);

    NodePair q{x, y};
    BFLPMC bflpmc(data_name, g, c, epsilon, delta);

    TruthSim ts(data_name, g, c, epsilon);
    auto max_err = 0.0;
    auto min_err = 1.0;
    auto failure_count = 0;
    auto start = std::chrono::high_resolution_clock::now();
    for (auto i = 0; i < 2000; i++) {
        double result = bflpmc.query_one2one(q);
        auto cur_err = abs(result - ts.sim(x, y));
        max_err = max(max_err, cur_err);
        min_err = min(min_err, cur_err);
        if (cur_err > 0.01) {
            cout << result << " ," << ts.sim(x, y) << endl;
            failure_count++;
        }
    }
    auto end = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> elapsed = end - start;

    cout << "failure count:" << failure_count << endl;
    cout << "max err:" << max_err << endl;
    cout << "min err:" << min_err << endl;
}

int main(int argc, char *argv[]) {
    string data_name(argv[1]);
    double c = 0.6;
    double epsilon = 0.01;
    double delta = 0.01;

    int x = atoi(argv[2]), y = atoi(argv[3]);
    test_BFLPMC(data_name, c, epsilon, delta, x, y);
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    testRot3.cpp
 * @brief   Unit tests for Rot3 class, Quaternion specific
 * @author  Alireza Fathi
 */

#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h>

#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/lieProxies.h>

#include <boost/math/constants/constants.hpp>

#include <CppUnitLite/TestHarness.h>

#ifdef GTSAM_USE_QUATERNIONS

// No quaternion only tests

#endif

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


//optical flow

#include <stdio.h>
#include <opencv2/opencv.hpp>
#include <opencv2/core/mat.hpp>
#include <opencv2/video/tracking.hpp>
#include <boost/program_options.hpp>
#include <exception>
#include <iostream>

using std::exception;

// TO DO:
// create 2 module
// - 1 find optical flow between two images
// - 2 find optical flow in video stream
// - seperate header and cpp

namespace po = boost::program_options;

int main(int argc, char** argv )
{
    std::string image1, image2;
    std::string video;
    std::string mode;

    try {

    po::options_description desc("Allowed options");
    desc.add_options()
        ("help ", "produce help message")
        ("mode ",        po::value<std::string>(&mode)->default_value("image"), "select optical flow mode")
        ("video ",       po::value<std::string>(&video)->default_value("null"),  "video path")
        ("image_1",      po::value<std::string>(&image1)->default_value("null"), "image 1")
        ("image_2 ",     po::value<std::string>(&image2)->default_value("null"), "image 2");

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify( vm);

    if (vm.count("help")) {
        std::cout << desc << "\n";
        return 0;
    }

    catch(exception& e) {
        std::cerr << "error: " << e.what() << "\n";
        return 1;
    }
    catch(...) {
        std::cerr << "Exception of unknown type!\n";
    return 1;
    }

    if(mode=="image")
    {
         //first frame
    cv::Mat image;
    image = cv::imread( image1, 1 );
    if ( !image.data )
    {
        printf("No image data \n");
        return -1;
    }

    //second frame
    cv::Mat imageNext;
    imageNext = cv::imread( image2, 1 );
    if ( !image.data )
    {
        printf("No image data \n");
        return -1;
    }

    cv::Mat grayImage;
    cv::Mat grayImage_1;

    cv::cvtColor(image, grayImage, cv::COLOR_RGB2GRAY);
    cv::cvtColor(imageNext, grayImage_1, cv::COLOR_RGB2GRAY);
    cv::namedWindow("Base Image", cv::WINDOW_AUTOSIZE );
    cv::namedWindow("Next Image", cv::WINDOW_AUTOSIZE );
    cv::namedWindow("Base Corners", cv::WINDOW_AUTOSIZE );
    cv::namedWindow("Next Image Corners", cv::WINDOW_AUTOSIZE );
    cv::imshow("Base Image", image);
    cv::imshow("Next Image", imageNext);

    cv::RNG rng(12345);
    std::vector<cv::Point2f> corners;
    std::vector<cv::Point2f> corners_1;
    //find counter on first frame
    //grayimage=first frame
    cv::goodFeaturesToTrack(grayImage, corners, 10, 0.2, 3, cv::noArray(), 5, 5);

    std::cout << "number of corners detected: " << corners << std::endl;
    int radius=7;
    cv::Mat copy = imageNext.clone();
    for(size_t i=0; i<corners.size(); i++){
        cv::circle(image, corners[i],radius,cv::Scalar(124,252,0), cv::FILLED);
    }

    cv::goodFeaturesToTrack(grayImage_1, corners_1, 10, 0.2, 3, cv::noArray(), 5, 5);
    for(size_t i=0; i<corners_1.size(); i++){
        cv::circle(imageNext, corners_1[i],radius,cv::Scalar(124,252,0), cv::FILLED);
    }

    //cv::imshow("Gray Image", grayImage);
    cv::imshow("Base Corners", image);
    cv::imshow("Next Image Corners", imageNext);

    //calculate optical flow
    std::vector<uchar> status;
    std::vector<float> err;
    cv::TermCriteria criteria = cv::TermCriteria((cv::TermCriteria::COUNT) + (cv::TermCriteria::EPS), 10, 0.03);
    cv::calcOpticalFlowPyrLK(grayImage, grayImage_1, corners, corners_1, status, err, cv::Size(5,5), 2, criteria);
    cv::Mat old_frame = copy.clone();
    cv::Mat mask = cv::Mat::zeros(old_frame.size(), old_frame.type());

    std::vector<cv::Point2f> good_new;

     for(uint i = 0; i < corners.size(); i++)
        {
            // Select good points
                good_new.push_back(corners_1[i]);
                // draw the tracks
                cv::line(mask,corners_1[i], corners[i], cv::Scalar(124,252,0), 5);
                cv::circle(copy, corners_1[i], 5, cv::Scalar(124,252,0), -1);

        }

        cv::Mat img;
        add(copy, mask, img);
        imshow("Frame", img);

    cv::waitKey(0);

    }
    else
    {
        std::cout << "selected mode " << mode << std::endl;
    }

    return 0;
}


#include <iostream>
#include <mutex>
#include <boost/thread/thread.hpp>
#include <boost/thread/tss.hpp>

#define LOOP 1000000
#define BENCH_RUNS 5

using namespace std;

template <typename T=int>
struct node{
    T payload;
    node * next;
};

template <typename V=int>
class gBag {
    private:
    //global state
    node<V> * globalHead=NULL;

    //class auxiliars
    mutex m;

    public:
    bool strongLookup(const V value) {
        m.lock();
        node<V>* tempNode=globalHead;
        while(tempNode != NULL){
            if(tempNode->payload == value ) {
             m.unlock();
             return true;
            }
            tempNode=tempNode->next;
        }
        m.unlock();

        return false;
    }

    void strongAdd(const V value){
        node<V>* tempNode = new node<V>;
        tempNode->payload = value;

        m.lock();
        tempNode->next = globalHead;
        globalHead = tempNode;
        m.unlock();

    }

   ////////////////////////////BENCHMARKING PURPOSES
    int globalCount() {
        int i=0;
        node<V> * tempNode=globalHead;
        while(tempNode != NULL){
            i++;
            tempNode=tempNode->next;
        }
        return i;
    }

    void cleanLinkedList(){
        node<V>* tempNode = globalHead;
        while( tempNode != NULL ) {
            node<V>* next = tempNode->next;
            delete tempNode;
            tempNode = next;
        }
        globalHead = NULL;
    }

    void printLinkedList(){
        m.lock();
        node<V>* tempNode = globalHead;
        cout << "LIST: ";
        while( tempNode != NULL ) {
            cout << tempNode->payload << "--> ";
            node<V>* next = tempNode->next;
            tempNode = next;
        }
        cout << endl;
        m.unlock();
    }

    void reset(){
        cleanLinkedList();
    }
};

gBag<int> mdt;
vector<int> NTHREADS;
std::atomic<int> threadCount;
void work(int operationsPerThread){
  threadCount++;
  int localCount = threadCount;
  int startNumber = operationsPerThread * (localCount-1);
  int endNumber = startNumber + operationsPerThread;

  for (int i=startNumber; i < endNumber; i++){
    mdt.strongAdd(i);

    if(i%50000 == 0){
      mdt.strongLookup(LOOP/2);
    }
  }
}

void benchmark(){
    using namespace std::chrono;
    for(int k = 0; k < NTHREADS.size(); k++){
      std::list<double> times;
      std::list<double> elementCount;
      std::list<double> throughs;

      for(int i= 0; i< BENCH_RUNS; i++){
        steady_clock::time_point t1 = steady_clock::now();

        boost::thread_group threads;
        int operationsPerThread = LOOP/NTHREADS[k];
        for (int a=0; a < NTHREADS[k]; a++){
          threads.create_thread(boost::bind(work,  boost::cref(operationsPerThread)));
        }

        threads.join_all();
        threadCount=0;

        steady_clock::time_point t2 = steady_clock::now();

        duration<double> ti = duration_cast<duration<double>>(t2 - t1);

        times.push_back(ti.count());
        elementCount.push_back(mdt.globalCount());
        throughs.push_back(mdt.globalCount()/ti.count());

        mdt.reset();
      }

      double sumTimes = 0;
      for(double t: times){
        sumTimes += t;
      }

      double finalTime = sumTimes/times.size();
      cout << fixed;

      double sumCounters = 0;
      for(double c: elementCount){
        sumCounters += c;
      }
      double finalElemCount = (int)(sumCounters/elementCount.size());

      double sumThroughs = 0;
      for(double th: throughs){
        sumThroughs += th;
      }
      double finalThroughs = sumThroughs/throughs.size();

    //   cout << "FINAL TIME: " << finalTime << endl;
    //   cout << (int)finalElemCount << "," << (int)finalThroughs << "," << NTHREADS[k] << endl;
    //   cout << endl;

    cout << NTHREADS[k] << "," << (int)finalThroughs << endl;    }
}

int main(int argc, char** argv){
    for(int i=1; i<argc; i++){
      NTHREADS.push_back(atoi(argv[i]));
    }

    benchmark();
    return 0;
}


#pragma once

#include <memory>
#include <Eigen/Eigen>

#include "Shader.hpp"

namespace LRR
{
  namespace Rendering
  {
    using namespace Eigen;

    struct Scanline
    {
      int min, max;
    };

    enum class ScanlineLineTarget
    {
      Min,
      Max
    };

    class ScanlineBuffer final
    {
    private:
      using ScanlineT = Scanline;
    public:
      ScanlineBuffer() = delete;
      ScanlineBuffer(int height);
      ScanlineBuffer(ScanlineBuffer const &other);
      ScanlineBuffer(ScanlineBuffer && other);

      inline int Height() const {
        return mHeight;
      }

      inline ScanlineT const &Scanline(int y) const {
        return mScanlines[y];
      }

      inline void Scanline(int y, ScanlineT const &scanline) {
        mScanlines[y] = scanline;
      }

      //void DrawLine(int x1, int y1, int x2, int y2, ScanlineLineTarget target);
      void DrawLine(Vector2i const &top, Vector2i const &bottom,
                    ScanlineLineTarget target);

      static ScanlineBuffer Rectangle(int width, int height);
      static ScanlineBuffer Triangle(Vector2i const &a, Vector2i const &b,
                                     Vector2i const &c);
    private:
      static void SortTriangleVertices(Vector2i &a, Vector2i &b,
                                       Vector2i &c);

      static float TriangleHandedness(Vector2i const &b, Vector2i const &c);

      int const mHeight;
      std::unique_ptr<ScanlineT[]> mScanlines;
    };
  }
}


#include <boost/mpl/assert.hpp>
#include <nt2/sdk/config/types.hpp>
#include <nt2/sdk/meta/downgrade.hpp>
#include <boost/type_traits/is_same.hpp>

using boost::is_same;

int main()
{
  BOOST_MPL_ASSERT(( is_same<nt2::uint8_t             , nt2::meta::downgrade<uint16_t>::type>                     ));
  BOOST_MPL_ASSERT(( is_same<nt2::int8_t              , nt2::meta::downgrade<uint8_t, signed>::type>              ));
  BOOST_MPL_ASSERT(( is_same<float                    , nt2::meta::downgrade<double>::type>                        ));
}


//=======================================================================
// Copyright (c) 2013 Piotr Smulewicz
//
// 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)
//=======================================================================
/**
 * @file scheduling_jobs_with_deadlines_on_a_single_machine_test.cpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2013-09-06
 */
#include "test_utils/logger.hpp"
#include "test_utils/test_result_check.hpp"

#include "paal/greedy/scheduling_jobs_with_deadlines_on_a_single_machine/scheduling_jobs_with_deadlines_on_a_single_machine.hpp"
#include "paal/utils/irange.hpp"

#include <boost/test/unit_test.hpp>

#include <vector>

BOOST_AUTO_TEST_CASE(scheduling_jobs_with_deadlines_on_a_single_machine) {
    // sample data
    using Time = double;
    std::vector<Time> time = { 2.1, 3.1, 4.1, 5.1, 6.1, 7.1, 8.1 };
    std::vector<Time> relase = { 1, 2, 3, 4, 5, 6, 7 };
    std::vector<Time> due_date = { -1, 0, -2, -3, -4, -5, -6 };
    double BEST_DELAY = 36.7;

    auto jobs = paal::irange(time.size());
    std::vector<std::pair<decltype(jobs)::iterator, Time>> jobs_to_start_dates;

    Time delay =
        paal::greedy::scheduling_jobs_with_deadlines_on_a_single_machine(
            jobs.begin(), jobs.end(), paal::utils::make_array_to_functor(time),
            paal::utils::make_array_to_functor(relase),
            paal::utils::make_array_to_functor(due_date),
            back_inserter(jobs_to_start_dates));
    Time max_delay = 0;
    int jobId;
    for (auto job_start_time : jobs_to_start_dates) {
        Time start_time = job_start_time.second;
        jobId = (*(job_start_time.first));
        LOGLN("Job " << jobId << " Start time: " << start_time);
        if ((start_time + time[jobId] - due_date[jobId]) > max_delay)
            max_delay = start_time + time[jobId] - due_date[jobId];
    }
    // print result
    check_result(delay, BEST_DELAY, 2);
    BOOST_CHECK_EQUAL(delay, max_delay);
}


/* Copyright 2016 kunming.xie
 *
 * 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.
 */

#include <boost/mpl/vector_c.hpp>

typedef boost::mpl::vector_c<int, 1, 0, 0, 0, 0, 0, 0> mass;
typedef boost::mpl::vector_c<int, 0, 1, 0, 0, 0, 0, 0> length;

template <class T, class Dimensions>
class quantity
{
public:
    explicit quantity(T x)
        : m_value (x)
    {}

    T value() const { return m_value; }

private:
    T m_value;
};

void test()
{
    quantity<float, mass> m(2.0);
}


//============================================================================
// Name        : main.cpp
// Author      : Olu Adebari
// Description : Process the captured video game frame, analyse and then send data over to Python caller program
//============================================================================

#include <iostream>
#include <vector>
#include <chrono>
#include <future>
#include <memory>
#include <string>
#include <opencv2/opencv.hpp>

#include "SeperateObjects.h"
#include "SendDataToPython.h"
#include "OCR.h"
#include "RegionOfInterest.h"
#include "BinaryImage.h"
#include "FeatureExtraction.h"
#include "RecordProcessedImage.h"
#include "ConvertToBinaryImage.h"

#include <boost/python.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include <boost/python/suite/indexing/map_indexing_suite.hpp>

boost::python::dict processImageFrame()
{

	auto save_current_frame_with_bounded_boxes = true;
	auto save_current_frame = true;
	auto save_binary_image_of_current_frame = true;
	auto save_regions_of_interest_from_current_frame_binary_image = true;
	auto save_regions_of_interest_from_current_frame_binary_image_features_points = true;

	static uint32_t starting_frame_number = 1;
	uint32_t Number_of_frames_to_record = 1000;

	auto image_processing_start_time = std::chrono::high_resolution_clock::now();

	//read current video_game frame
	cv::Mat img = cv::imread("../game_vision/current_game_frame/current_game_frame.jpg");

	//get words in frame
	OCR capture_words_in_image;
	auto words_and_coordinates = std::async(std::launch::async,std::bind(&OCR::GetWordsAndLocations, capture_words_in_image,std::ref(img)));

	//convert to grayscale
	cv::Mat gray = cv::Mat::zeros(img.size(), img.type());
	cv::cvtColor(img, gray, cv::COLOR_RGB2GRAY);

	//smooth image
	blur(gray, gray, cv::Size(3,3));

	cv::Mat image_with_bounded_boxes_drawn_on = img.clone();

	//create binary images
	std::vector< std::shared_ptr<BinaryImage> > binary_images;
	binary_images.emplace_back(std::make_shared<BinaryImage> (1, ConvertToBinaryImage().convertToBinary(gray,img)));
	binary_images.emplace_back(std::make_shared<BinaryImage> (2, ConvertToBinaryImage().convertToBinaryInverse(gray,img)));

	//extract regions of interest within an image
	SeperateObjects seperate_regions_of_interest (gray,image_with_bounded_boxes_drawn_on);
	auto get_binary_image_1_ROI_objects  = std::async(std::launch::async,std::bind(&SeperateObjects::BoundBox, &seperate_regions_of_interest,binary_images[0] ,save_regions_of_interest_from_current_frame_binary_image));
	auto get_binary_image_2_ROI_objects = std::async(std::launch::async,std::bind(&SeperateObjects::BoundBox, &seperate_regions_of_interest,binary_images[1] ,save_regions_of_interest_from_current_frame_binary_image));

	get_binary_image_1_ROI_objects.wait();
	get_binary_image_2_ROI_objects.wait();

	//extract feature points for each region of interest
	using surf_OCL = cv::xfeatures2d::SURF;

	FeatureExtraction <surf_OCL> features_of_objects;

	auto get_feature_points_for_binary_1_ROI_objects = std::async(std::launch::async,std::bind(&FeatureExtraction <surf_OCL>::extractFeaturePoints,
											&features_of_objects,binary_images[0],save_regions_of_interest_from_current_frame_binary_image_features_points));

	auto get_feature_points_for_binary_2_ROI_objects = std::async(std::launch::async,std::bind(&FeatureExtraction <surf_OCL>::extractFeaturePoints,
											&features_of_objects,binary_images[1], save_regions_of_interest_from_current_frame_binary_image_features_points));

	get_feature_points_for_binary_1_ROI_objects.wait();
	get_feature_points_for_binary_2_ROI_objects.wait();

	//Optional code: record frames with bounded boxes drawn on into their own directories.
	std::string current_frame_file_name = "Image"+std::to_string(starting_frame_number)+".jpg";
	RecordProcessedImage saveProcessedImages;
	if (starting_frame_number >= Number_of_frames_to_record){
		starting_frame_number = 1;
	}

	if(save_current_frame_with_bounded_boxes){
		auto recordBoundBoxAsync =  std::async(std::launch::async,std::bind(&RecordProcessedImage::saveImage, saveProcessedImages,std::ref(image_with_bounded_boxes_drawn_on),"../game_vision/cloudbank_images/Frame/bounded_boxes/",current_frame_file_name ));
	}

	if (save_current_frame){
		auto recordCurrentFrameAsync = std::async(std::launch::async,std::bind(&RecordProcessedImage::saveImage, saveProcessedImages,std::ref(img),"../game_vision/cloudbank_images/Frame/Unprocessed/",current_frame_file_name));

	}
	if (save_binary_image_of_current_frame){
		for (auto const & binary_image : binary_images){
			auto recordCurrentFrameAsync = std::async(std::launch::async,std::bind(&RecordProcessedImage::saveImage, saveProcessedImages,binary_image->getBinaryImage(),"../game_vision/cloudbank_images/Frame/Binary/binaryImage_ID_"+std::to_string(binary_image->getID()),current_frame_file_name));

		}

	}
	++starting_frame_number;
	auto image_processing_end_time = std::chrono::high_resolution_clock::now();
	auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(image_processing_end_time - image_processing_start_time ).count();
	std::cout<< "this program took about " << duration<< " milliseconds to process the image" << std::endl;

	words_and_coordinates.wait();

	//send extracted image frame in python form into calling python process
	SendDataToPython python_features_of_objects;
	boost::python::dict send_to_python_Object_info= python_features_of_objects.SendObjectInformationToDict(binary_images, words_and_coordinates.get());

	return send_to_python_Object_info;

}

int main()
{

	Py_Initialize();
	processImageFrame();

	return 0;

}

BOOST_PYTHON_MODULE(opencv)
{
	 def("ProcessGameFrame", processImageFrame);
}


#pragma once

#include <Eigen/Core>
#include <polysolve/LinearSolver.hpp>

#include <constants.hpp>
#include <solvers/optimization_solver.hpp>
#include <utils/not_implemented_error.hpp>

namespace ipc::rigid {

enum ConvergenceCriteria {
    VELOCITY, ///< Change in position of a newton direction
    ENERGY    ///< Change in energy of a newton direction
};

NLOHMANN_JSON_SERIALIZE_ENUM(
    ConvergenceCriteria, { { VELOCITY, "velocity" }, { ENERGY, "energy" } });

class NewtonSolver : public virtual OptimizationSolver {
public:
    NewtonSolver();
    virtual ~NewtonSolver() = default;

    /// Initialize the state of the solver using the settings saved in JSON
    virtual void settings(const nlohmann::json& params) override;
    /// Export the state of the solver using the settings saved in JSON
    virtual nlohmann::json settings() const override;

    /// An identifier for the solver class
    static std::string solver_name() { return "newton_solver"; }
    /// An identifier for this solver
    virtual std::string name() const override
    {
        return NewtonSolver::solver_name();
    }

    /// Initialize the solver with a problem to solve
    virtual void set_problem(OptimizationProblem& problem) override
    {
        this->problem_ptr = &problem;
    }

    /// Initialize the solver state for a new solve
    virtual void init_solve(const Eigen::VectorXd& x0) override;

    /**
     * @brief Perform Newton's Method to minimize the objective, \f$f(x)\f$,
     * of the problem unconstrained.
     *
     * @param[in] problem  The optimization problem to minimize
     *                     unconstrained.
     *
     * @return The results of the optimization including the minimizer,
     * minimum, and if the optimization was successful.
     */
    virtual OptimizationResults solve(const Eigen::VectorXd& x0) override;

    /// Perform a single step of solving the optimization problem
    virtual OptimizationResults step_solve() override
    {
        throw NotImplementedError(
            "Taking a single newton step is not implemented!");
    };

    /**
     * @brief Solve for the Newton direction
     *        (\f$\Delta x = -H^{-1} \nabla f \f$).
     *
     * @param[in]  gradient  Gradient of the objective function.
     * @param[in]  hessian   Hessian of the objective function.
     * @param[out] delta_x   Output newton direction.
     * @param[in]  make_psd  If delta_x is not a descent direction, then
     * make the hessian positive semi-definite.
     *
     * @return Returns true if the solve was successful.
     */
    virtual bool compute_direction(
        const Eigen::VectorXd& gradient,
        const Eigen::SparseMatrix<double>& hessian,
        Eigen::VectorXd& delta_x,
        bool make_psd = false);

    virtual bool compute_regularized_direction(
        double& fx,
        Eigen::VectorXd& gradient,
        Eigen::SparseMatrix<double>& hessian,
        Eigen::VectorXd& delta_x,
        double& coeff);

    virtual std::string stats_string() const override;
    virtual nlohmann::json stats() const override;

    int max_iterations;

protected:
    virtual bool converged();

    virtual void post_step_update();

    virtual bool line_search(
        const Eigen::VectorXd& x,
        const Eigen::VectorXd& dir,
        const double fx,
        const Eigen::VectorXd& grad_fx,
        double& step_length);

    virtual double line_search_lower_bound() const
    {
        return m_line_search_lower_bound;
    }

    /// @brief Pointer to the problem to solve.
    OptimizationProblem* problem_ptr;

    int iteration_number; ///< @brief The current iteration number.
    ConvergenceCriteria convergence_criteria;

    double m_line_search_lower_bound; ///< @brief Line search lower bound

    double energy_conv_tol;        ///< @brief Energy convergence tolerance
    double velocity_conv_tol;      ///< @brief Velocity convergence tolerance
    bool is_velocity_conv_tol_abs; ///< @brief Absolute velocity tol
    bool is_energy_converged;

    // State variables
    Eigen::VectorXd x, x_prev;
    Eigen::VectorXd gradient, gradient_free;
    Eigen::VectorXd direction, direction_free;
    Eigen::VectorXd grad_direction; ///< Gradient with fixed DoF set to zero
    Eigen::SparseMatrix<double> hessian, hessian_free;

    // Linear solver pointer
    std::unique_ptr<polysolve::LinearSolver> linear_solver;
    nlohmann::json linear_solver_settings;

private:
    void reset_stats();

    size_t num_fx = 0;
    size_t num_grad_fx = 0;
    size_t num_hessian_fx = 0;
    size_t num_collision_check = 0;
    size_t ls_iterations = 0;
    size_t newton_iterations = 0;
    size_t num_newton_ls_fails = 0;
    size_t num_grad_ls_fails = 0;
    size_t regularization_iterations = 0;
};

/**
 * @brief Make the matrix positive definite (\f$x^T A x > 0\$).
 *
 * @param A The matrix to make positive definite.
 *
 * @return The scale of the update to the diagonal.
 */
double make_matrix_positive_definite(Eigen::SparseMatrix<double>& A);

/**
 * @brief Log values along a search direction.
 *
 * @param[in] x                 Starting point for the line search.
 * @param[in] dir               Direction to search along.
 * @param[in] f_and_gradf       Function of x to sample with gradient.
 */
void sample_search_direction(
    const Eigen::VectorXd& x,
    const Eigen::VectorXd& dir,
    const std::function<double(const Eigen::VectorXd&, Eigen::VectorXd&)>&
        f_and_gradf,
    double max_step);

} // namespace ipc::rigid


#include <stdlib.h>
#include <sstream>
#include <string>
#include <iostream>
#include <vector>
#include <cmath>
#include <ros/ros.h>
#include <ros/package.h>
#include <Eigen/Dense>
#include "pcl_ros/point_cloud.h"
#include <object_assembly_msgs/Point2D.h>
#include <object_assembly_msgs/Points2D.h>
#include <object_assembly_msgs/PointCloudArray.h>
#include <object_assembly_msgs/FetchPCLsGivenLocations.h>
#include <object_assembly_msgs/FetchPCLs.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <tf/transform_broadcaster.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/pcd_io.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/registration/icp.h>
#include <Eigen/Geometry>
#include <geometry_msgs/PoseArray.h>

class PoseInitializerNode {

public:
    PoseInitializerNode(std::vector<std::string> file_paths, int num_objects, std::string background_removal_service, std::string frame_id);

    void hmp_callback(const object_assembly_msgs::Points2D& points);

    void colour_tracker_callback(const object_assembly_msgs::Points2D& points);

    void calculate_icp(object_assembly_msgs::PointCloudArray &pcl_array);

    void publish();

private:
    int num_objects_;
    int num_random_rotations_ = 4;
    std::string frame_id_;
    std::string background_removal_service_;
    ros::NodeHandle node_handle_;
    ros::Publisher initial_poses_publisher_;
    std::vector<pcl::PointCloud<pcl::PointXYZ>> file_meshes_;
    std::vector<tf::Transform> initial_poses_;

};


#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/cstdint.hpp>
#include <iostream>

int main()
{
  boost::posix_time::ptime start = boost::posix_time::microsec_clock::local_time();

  boost::uint64_t sum = 0;
  for (int i = 0; i < 1000000000; ++i)
    sum += i;

  boost::posix_time::ptime end = boost::posix_time::microsec_clock::local_time();
  std::cout << end - start << std::endl;

  std::cout << sum << std::endl;
}

Boost/The Boost C++ Libraries/src/6.5.1/main.cpp


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          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
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_COMMON_TOUINTS_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_COMMON_TOUINTS_HPP_INCLUDED

#include <boost/simd/arithmetic/functions/touints.hpp>
#include <boost/simd/include/functions/simd/is_ngez.hpp>
#include <boost/simd/include/functions/simd/if_zero_else.hpp>
#include <boost/simd/include/functions/simd/touint.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/is_greater_equal.hpp>
#include <boost/simd/include/functions/simd/saturate.hpp>
#include <boost/simd/include/functions/simd/splat.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( touints_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<int_<A0>,X>))
                                    )
  {
    typedef typename dispatch::meta::as_integer<A0, unsigned>::type result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      return bitwise_cast<result_type>(saturate<result_type>(a0));
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( touints_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<uint_<A0>,X>))
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0) const
    {
      return a0;
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( touints_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_< floating_<A0>,X >))
                                    )
  {
    typedef typename dispatch::meta::as_integer<A0, unsigned>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename meta::scalar_of<result_type>::type sr_t;
      static const A0 Vax = splat<A0>(boost::simd::Valmax<sr_t>());
      return if_zero_else(boost::simd::is_ngez(a0),
                          if_else(boost::simd::ge(a0, Vax), Valmax<result_type>(),
                                  touint(a0)
                                 )
                         );

    }
  };

} } }

#endif