text:
// Copyright John Maddock, 2021
// 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/tools/config.hpp>

#if !defined(BOOST_MATH_NO_THREAD_LOCAL_WITH_NON_TRIVIAL_TYPES) && defined(BOOST_HAS_THREADS)

#include <vector>
#include <atomic>
#include <thread>
#include <random>
#include <algorithm>
#include <iostream>

std::atomic<int> counter{ 0 };

void thread_proc()
{
   static thread_local std::vector<double> list;

   std::default_random_engine rnd;
   std::uniform_real_distribution<> dist;

   for (unsigned i = 0; i < 1000; ++i)
   {
      list.push_back(dist(rnd));
      ++counter;
   }
   std::sort(list.begin(), list.end());
}

int main()
{
   std::thread f1(thread_proc);
   std::thread f2(thread_proc);
   std::thread f3(thread_proc);
   std::thread f4(thread_proc);
   std::thread f5(thread_proc);
   std::thread f6(thread_proc);

   f1.join();
   f2.join();
   f3.join();
   f4.join();
   f5.join();
   f6.join();

   std::cout << "Counter value was: " << counter << std::endl;

   return counter - 6000;
}

#else

int main() { return 0; }

#endif

max_stars_repo_name: oleg-alexandrov/math

max_issues_repo_name: oleg-alexandrov/math

max_forks_repo_name: oleg-alexandrov/math


text:
/**
 * @file   protocol/tcp/socket/src/write_one.cpp
 * @brief  Class definition of protocol::tcp::socket::WriteOne
 */

#include <boost/asio/write.hpp>
#include <boost/bind.hpp>

#include <protocol/service/singleton.hpp>
#include <protocol/tcp/socket/write_one.hpp>
#include <protocol/tcp/socket/buffer.hpp>

#include <boost/asio/coroutine.hpp>
#include <boost/asio/yield.hpp>

namespace protocol {
namespace tcp {
namespace socket {

namespace ba = boost::asio;
namespace bs = boost::system;
using std::vector;

#define BIND(x) boost::bind(&WriteOne::x, shared_from_this())               ///< Helper bind to member method
#define BIND2(x, y, z) boost::bind(&WriteOne::x, shared_from_this(), y, z)  ///< Helper bind to member method

write_one::Ptr WriteOne::Start(
    socket::sock::Ptr sock, buffer::Ptr buffer, const Callback& on_done, const size_t timeout_ms) {
  write_one::Ptr new_(new WriteOne(sock, buffer, on_done));
  new_->Start(timeout_ms);
  return new_;
}

WriteOne::WriteOne(socket::sock::Ptr sock, buffer::Ptr buffer, const Callback& on_done)
  : stopped_(false), sock_(sock), write_buffer_(buffer), on_done_callback_(on_done) {
  if (!sock_) throw std::invalid_argument("Socket can't be null");
  deadline_ = std::shared_ptr<ba::deadline_timer>(new ba::deadline_timer(sock_->get_io_service()));
}

void WriteOne::Start(const size_t timeout_ms) {
  if (timeout_ms) {
    deadline_->expires_from_now(boost::posix_time::milliseconds(timeout_ms));
    deadline_->async_wait(BIND(OnTimeout));
  }
  operator()(bs::error_code(), 0);
}

void WriteOne::Stop() {
  if (!stopped_) {
    stopped_ = true;
  }
  deadline_->cancel();
}

const sock::Ptr& WriteOne::GetSock() const {
  return sock_;
}

void WriteOne::operator()(bs::error_code ec = bs::error_code(), std::size_t = 0) {
  if (stopped_) {
    return;
  }

  if (ec) {
    on_done_callback_(ec, shared_from_this());
  } else {
    reenter(this) {
      for (;;) {
        yield ba::async_write(*sock_, **write_buffer_, BIND2(operator(), _1, _2));
        yield service::singleton::Instance().post(boost::bind(on_done_callback_, ec, shared_from_this()));
      }
    }
  }
}

void WriteOne::OnTimeout() {
  if (stopped_) {
    return;
  }

  if (deadline_->expires_at() <= ba::deadline_timer::traits_type::now()) {
    Stop();
    on_done_callback_(bs::error_code(bs::errc::timed_out, bs::system_category()), shared_from_this());
  }
}

}  // namespace socket
}  // namespace tcp
}  // namespace protocol

ext: cpp

max_stars_repo_name: SergioSoldado/Protocol

max_issues_repo_name: SergioSoldado/Protocol

max_forks_repo_name: SergioSoldado/Protocol


text:
#pragma once

#include "layer.hpp"
#include <Eigen/Dense>

namespace
{
	using namespace nn;

	uint32_t conv_size(uint32_t input, uint32_t kernel_width, uint32_t padding, uint32_t stride)
	{
		return (input - kernel_width + 2 * padding) / stride + 1;
	}

	layer::MatrixType conv_helper(const layer::MatrixType& input,
		const layer::MatrixType& kernel,
		uint32_t start_row, uint32_t start_col,
		uint32_t end_row, uint32_t end_col,
		uint32_t stride,
		bool normalize)
	{
		layer::MatrixType output = layer::MatrixType::Zero(end_row - start_row, end_col - start_col);
		const auto KSizeX = kernel.rows();
		const auto KSizeY = kernel.cols();

		for (auto row = start_row; row < end_row; row += stride)
			for (auto col = start_col; col < end_col; col += stride)
				output(row - start_row, col - start_col) = input.block(row - KSizeX / 2, col - KSizeY / 2, KSizeX, KSizeY).cwiseProduct(kernel).sum();

		if (normalize)
			return output / kernel.sum();
		else
			return output;
	}

	layer::MatrixType zero_pad(const layer::MatrixType& input, uint32_t size)
	{
		layer::MatrixType result = layer::MatrixType::Zero(input.rows() + 2 * size, input.cols() + 2 * size);
		result.block(size, size, input.rows(), input.cols()) = input;
		return result;
	}
}

namespace nn
{
	layer::MatrixType conv(const layer::MatrixType& input, const layer::MatrixType& kernel, uint32_t stride, bool zeroPad = false, bool normalize = false)
	{
		const auto kernel_half_width = kernel.cols() / 2;
		const auto kernel_half_height = kernel.rows() / 2;
		if (!zeroPad)
		{
			const auto result_width = conv_size(input.cols(), kernel.cols(), 0, stride);
			const auto result_height = conv_size(input.rows(), kernel.rows(), 0, stride);
			const auto conv_start_col = (input.cols() - result_width) / 2;
			const auto conv_start_row = (input.rows() - result_height) / 2;
			const auto conv_end_col = conv_start_col + result_width;
			const auto conv_end_row = conv_start_row + result_height;
			return conv_helper(input, kernel, conv_start_row, conv_start_col, conv_end_row, conv_end_col, stride, normalize);
		}
		else
		{
			return conv_helper(zero_pad(input, kernel_half_width),
				kernel, kernel_half_width, kernel_half_height,
				kernel_half_width + input.cols(),
				kernel_half_height + input.rows(),
				stride,
				normalize);
		}
	}

}

ext: hpp

max_stars_repo_name: dmitryduka/nn

max_issues_repo_name: dmitryduka/nn

max_forks_repo_name: dmitryduka/nn


text:
/**
 *   Copyright (C) 2012 ciere consulting, ciere.com
 *   Copyright (C) 2012 Jeroen Habraken
 *   Copyright (C) 2011, 2012 Object Modeling Designs
 *
 *   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)
 *
 */

#ifndef CIERE_JSON_VALUE_HPP
#define CIERE_JSON_VALUE_HPP

#include <string>
#include <boost/variant.hpp>
#include <boost/exception/all.hpp>
#include <boost/iterator/transform_iterator.hpp>
#include <boost/iterator/iterator_facade.hpp>
#include <boost/container/stable_vector.hpp>
#include <boost/spirit/include/support_extended_variant.hpp>
#include <boost/mpl/or.hpp>
#include <boost/type_traits/is_enum.hpp>
#include <boost/type_traits/is_floating_point.hpp>
#include <boost/type_traits/is_integral.hpp>
#include <boost/utility/enable_if.hpp>

namespace ciere { namespace json
{

   // ---------------------------------------------------
   // exceptions
   // ---------------------------------------------------

   struct not_container_type : virtual boost::exception, virtual std::exception {};
   struct not_object_type    : virtual boost::exception, virtual std::exception {};
   struct not_array_type     : virtual boost::exception, virtual std::exception {};
   struct not_number_type    : virtual boost::exception, virtual std::exception {};
   struct get_as_error       : virtual boost::exception, virtual std::exception {};

   // ---------------------------------------------------
   // ---------------------------------------------------

   // ------------------- ast types --------------------
   //
   typedef std::string                                  string_t;
   typedef double                                       double_t;
   typedef double                                       float_t;
   typedef int64_t                                      int_t;
   typedef bool                                         bool_t;
   struct                                               null_t
   {};
   class                                                value;
   typedef std::map<std::string, value>                 object_t;
   typedef std::pair<std::string, value>                object_member_t;
   typedef boost::container::stable_vector<value>       array_t;

   // nulls always compare
   inline bool operator==(null_t,null_t) { return true;  }
   inline bool operator!=(null_t,null_t) { return false; }

   inline std::ostream & operator<<(std::ostream & out, null_t)
   { out << "null"; return out; }

   /**
    *  possible types being stored in the json::value
    */
   enum value_types
   {
      string_type,
      double_type,
      int_type,
      bool_type,
      null_type,
      value_type,
      object_type,
      array_type
   };

   /**
    *
    *
    */
   class value
      : public boost::spirit::extended_variant<
                   null_t
                 , bool_t
                 , string_t
                 , int_t
                 , double_t
                 , object_t
                 , array_t      >
   {
      public:
         // -------------------------------------------------------------------------------
         // -------------------------------------------------------------------------------
         value(null_t val = null_t())     : base_type(val) {}
         value(bool_t val)                : base_type(val) {}
         value(string_t const & val)      : base_type(val) {}
         value(char const * val)          : base_type((string_t(val))) {}
         value(object_t const & val)      : base_type(val) {}
         value(array_t const & val)       : base_type(val) {}
         value(value const & rhs)         : base_type(rhs.get_ast()) {}

         template< typename T >
         value(T val,
               typename boost::enable_if<boost::is_floating_point<T> >::type* = 0)
            : base_type( (double_t(val)) ) {}

         template< typename T >
         value(T val,
               typename boost::enable_if<
                   boost::mpl::or_<
                        boost::is_integral<T>
                      , boost::is_enum<T>
                      >
                  >::type* = 0)
            : base_type( (int_t(val)) ) {}

         // -------------------------------------------------------------------------------
         // -------------------------------------------------------------------------------

         /**
          *  Returns the type currently being stored
          */
         value_types type() const;

         // -------------------------------------------------------------------------------
         //    iterator helpers
         // -------------------------------------------------------------------------------
         /**
          *  type returned from an object iterator
          */
         struct member
         {
            member(object_t::value_type & m) : member_pair(m) {}

            std::string         name()  const { return member_pair.first; }
            json::value &       value()       { return member_pair.second; }
            json::value const & value() const { return member_pair.second; }

            private:
               object_t::value_type & member_pair;
         };

         struct const_member
         {
            const_member(object_t::value_type & m)       : member_pair(m) {}
            const_member(object_t::value_type const & m) : member_pair(m) {}

            std::string         name()  const { return member_pair.first; }
            const json::value & value() const { return member_pair.second; }

            private:
               object_t::value_type const & member_pair;
         };

         // -------------------------------------------------------------------------------
         //    iterator types
         // -------------------------------------------------------------------------------

      private:
         struct make_json_value;
         struct make_json_member;

      public:
         typedef boost::transform_iterator<  make_json_member
                                           , object_t::iterator
                                           , member >                      object_iterator;

         typedef boost::transform_iterator<  make_json_member
                                           , object_t::const_iterator
                                           , const_member >                const_object_iterator;

         typedef boost::transform_iterator<  make_json_value
                                           , array_t::iterator
                                           , value & >                     array_iterator;

         typedef boost::transform_iterator<  make_json_value
                                           , array_t::const_iterator
                                           , value const & >               const_array_iterator;

         // -------------------------------------------------------------------------------
         //    iterator access
         // -------------------------------------------------------------------------------

         /**
          *  Returns an iterator pointing to the fist member in the object if the value type
          *  is object_type. Otherwise, throw not_object_type.
          */
         object_iterator       begin_object();
         const_object_iterator begin_object() const;

         /**
          *  Returns an iterator that can be compared to determine the end of members in an object. If
          *  the value type is not an object then throw not_object_type.
          */
         object_iterator       end_object();
         const_object_iterator end_object() const;

         /**
          *  Returns an iterator pointing to the fist member in the array if the value type
          *  is array_type. Otherwise, throw not_array_type.
          */
         array_iterator       begin_array();
         const_array_iterator begin_array() const;

         /**
          *  Returns an iterator that can be compared to determine the end of members in an array. If the
          *  value type is not array_type throw not_array_type.
          */
         array_iterator       end_array();
         const_array_iterator end_array() const;

         // -------------------------------------------------------------------------------
         // -------------------------------------------------------------------------------

         // -------------------------------------------------------------------------------
         //    array handling
         // -------------------------------------------------------------------------------

         /**
          *  Add compatible type to the end of the array
          */
         template< typename T >
         value & add(T v);
         value & add(char const * v);

         /**
          *  Add a compatible type to the end of the array, functor style.
          */
         template< typename T >
         value & operator()(T v);

         /**
          *  Add compatible type to the end of the array, stl style-ish
          *  Actually returns a reference the newly added value.
          */
         template< typename T >
         value & push_back(T v);
         value & push_back(char const * v);

         /**
          *  Access array element stored at index value. Will throw out_of_range exception
          *  if index is not a valid index for the container.
          */
         value &       at(size_t index);
         value const & at(size_t index) const;

         /**
          *  Access array element stored at index value. If the array length is less than
          *  index, the array will be expanded to index + 1 length and additional
          *  locations will be filled with null_type.
          */
         value &       operator[](int index);
         value const & operator[](int index) const;

         /**
          *  Erase removes the value at index.
          */
         value & erase(int index);

         /**
          *  Returns the number of elements stored if the value is holding an array or
          *  object type. If it is not an array or object type the method throws
          *  ciere::json::not_array_type
          */
         std::size_t length() const;

         // -------------------------------------------------------------------------------
         // -------------------------------------------------------------------------------

         // -------------------------------------------------------------------------------
         //    object handling
         // -------------------------------------------------------------------------------

         /**
          *  Set the member name to value v.
          *  If member name already exists, its value will be replaced.
          *
          *  If the value object is not an object type it will become one.
          */
         template< typename T >
         value & set(string_t const & name, T v);

         // template can be ambigous for some types ... lets get specific
         value & set(string_t const & name, char const * v);

         /**
          *  Functor style setting. The first value is the name of the member to add.
          *  the second value is the value to be set.
          */
         template< typename T >
         value & operator()(string_t const & name, T v);

         /**
          *  Tests if the object contains the specified named object.
          */
         bool has_key(std::string const & key) const;

         /**
          *  Access the object's member with the specified name.
          *  If no member exists, one will be created with a default value
          *  of null_t.
          */
         value &       operator[](char const * v);
         value &       operator[](string_t const & v);
         value const & operator[](char const * v) const;
         value const & operator[](string_t const & v) const;

         value & erase(char const * v){ return erase( (string_t(v)) ); }
         value & erase(string_t const & v);

         // -------------------------------------------------------------------------------
         // -------------------------------------------------------------------------------

         /**
          *  Extract the actual value based on the type T. If the json::value
          *  does not contain type T, the method will throw.
          */
         template< typename T >
         T get();

         template< typename T >
         const T get() const;

         /**
          *  Extract the contained value as if it is of type T. If the
          *  contained type is not of type T, the value will be converted
          *  if it can. If no conversion can be found, it will throw.
          */
         template< typename T >
         T get_as();

         template< typename T >
         const T get_as() const;

         // -------------------------------------------------------------------------------
         //    conversion
         // -------------------------------------------------------------------------------

         operator string_t() const;

         // -------------------------------------------------------------------------------
         // -------------------------------------------------------------------------------

         base_type::variant_type       & get_ast()       { return base_type::get(); }
         base_type::variant_type const & get_ast() const { return base_type::get(); }
   };

   /**
    *  Constructs a value containing an array type
    */
   value array();

   /**
    *  Constructs a value containing an object type
    */
   value object();

   bool operator==(value const & a, value const & b);
   bool operator!=(value const & a, value const & b);
   bool operator<(value const & a, value const & b);

}}

#include "detail/value_impl.hpp"

#endif  // CIERE_JSON_VALUE_HPP

max_stars_repo_name: Abce/boost


text:
//
// Created by matthew on 1/11/20.
//

#include <boost/test/unit_test.hpp>
#include "unit.hh"
#include "weapon.hh"

using namespace srpg;

struct Globals {
  Globals()
      : mercenary(CoreStatSpread(), MovementType::Infantry, UnitAttribute::None, WeaponType::Sword),
      myrmidon(CoreStatSpread(3, 2, 1, 0, 1, 2, 3), MovementType::Infantry, UnitAttribute::None, WeaponType::Sword) {
    myrmidon_bonuses = myrmidon.stat_bonuses;
    merc_bases = CoreStatSpread(20, 7, 4, 0, 0, 5, 9);
  }

  static std::unique_ptr<InventoryItem> get_potion() {
    return std::make_unique<InventoryItem>(CoreStatSpread(), [](Unit& u) { u.offset_hp(10); });
  }

  CoreStatSpread myrmidon_bonuses;
  CoreStatSpread merc_bases;
  UnitClass mercenary;
  UnitClass myrmidon;
};

BOOST_FIXTURE_TEST_SUITE(unit, Globals)

BOOST_AUTO_TEST_CASE(attributes) {
  Unit x(merc_bases, CoreStatSpread(), UnitAttribute::Draconic | UnitAttribute::Winged, mercenary, 2);

  BOOST_CHECK(x.has_attributes(UnitAttribute::Draconic));
  BOOST_CHECK(x.has_attributes(UnitAttribute::Winged));
  BOOST_CHECK(x.has_attributes(UnitAttribute::Draconic | UnitAttribute::Winged));

  BOOST_CHECK(!x.has_attributes(UnitAttribute::Beastial));
  BOOST_CHECK(!x.has_attributes(UnitAttribute::Armored));
  BOOST_CHECK(!x.has_attributes(UnitAttribute::Draconic | UnitAttribute::Beastial));
  BOOST_CHECK(!x.has_attributes(UnitAttribute::Draconic | UnitAttribute::Winged | UnitAttribute::Beastial));
}

BOOST_AUTO_TEST_CASE(null_attributes) {
  Unit x(merc_bases, CoreStatSpread(), UnitAttribute::None, mercenary, 4);

  BOOST_CHECK(x.has_attributes(UnitAttribute::None));

  BOOST_CHECK(!x.has_attributes(UnitAttribute::Draconic));
  BOOST_CHECK(!x.has_attributes(UnitAttribute::Winged));
  BOOST_CHECK(!x.has_attributes(UnitAttribute::Beastial));
  BOOST_CHECK(!x.has_attributes(UnitAttribute::Armored));
}

BOOST_AUTO_TEST_CASE(stat_minimums) {
  Unit x(CoreStatSpread(10, 10, 10, 10, 10, 10, 10), CoreStatSpread(), UnitAttribute::Winged, mercenary, 11);
  x.buff(CoreStatSpread(-255, -255, -255, -255, -255, -255, -255));

  BOOST_CHECK(CoreStatSpread(1, 0, 0, 0, 0, 0, 0) == x.stats());
}

BOOST_AUTO_TEST_CASE(stat_buffing) {
  Unit x(CoreStatSpread(10, 10, 10, 10, 10, 10, 10), CoreStatSpread(), UnitAttribute::Winged, mercenary, 11);
  x.buff(CoreStatSpread(5, 5, 5, 5, 5, 5, 5));

  BOOST_CHECK(CoreStatSpread(15, 15, 15, 15, 15, 15, 15) == x.stats());
}

BOOST_AUTO_TEST_CASE(hp_reduction) {
  Unit x(merc_bases, CoreStatSpread(), UnitAttribute::None, mercenary, 4);

  BOOST_CHECK(merc_bases.hp == x.remaining_hp());

  x.offset_hp(-6);

  BOOST_CHECK_EQUAL(merc_bases.hp, x.stats().hp);
  BOOST_CHECK_EQUAL(merc_bases.hp - 6, x.remaining_hp());
}

BOOST_AUTO_TEST_CASE(hp_restoration) {
  Unit x(merc_bases, CoreStatSpread(), UnitAttribute::None, mercenary, 4);

  BOOST_CHECK_EQUAL(merc_bases.hp, x.remaining_hp());

  x.offset_hp(12);

  BOOST_CHECK_EQUAL(merc_bases.hp, x.stats().hp);
  BOOST_CHECK_EQUAL(merc_bases.hp, x.remaining_hp());

  x.offset_hp(-6);
  x.offset_hp(4);

  BOOST_CHECK(merc_bases.hp == x.stats().hp);
  BOOST_CHECK(merc_bases.hp - 2 == x.remaining_hp());
}

BOOST_AUTO_TEST_CASE(no_item_by_default) {
  Unit x(merc_bases, CoreStatSpread(), UnitAttribute::None, mercenary, 4);

  BOOST_CHECK(!x.held_item());
}

BOOST_AUTO_TEST_CASE(granting_an_item) {
  Unit x(merc_bases, CoreStatSpread(), UnitAttribute::None, mercenary, 4);
  x.give_item(get_potion());

  // Both f and potion should be invalid.
  BOOST_REQUIRE(x.held_item());

  // As a result, the result must be checked independently.
  x.offset_hp(-15);

  x.held_item().value().get().on_use(x);

  // I could check for reduced durability here, but have decided to put that off with related tests.

  BOOST_CHECK(merc_bases.hp - 5 == x.remaining_hp());
}

BOOST_AUTO_TEST_CASE(autoleveling) {
  Unit x(merc_bases, CoreStatSpread(90, 45, 50, 50, 45, 50, 50), UnitAttribute::None, mercenary, 8);

  BOOST_CHECK_EQUAL(1, x.level());
  BOOST_CHECK_EQUAL(0, x.exp());

  bool failed = false;
  x.give_exp(EXP_PER_LEVEL, failed);

  BOOST_REQUIRE(!failed);
  BOOST_CHECK_EQUAL(2, x.level());

  // No growths are high enough to level anything yet.
  BOOST_CHECK(merc_bases == x.stats());

  x.give_exp(EXP_PER_LEVEL, failed);
  BOOST_REQUIRE(!failed);
  BOOST_CHECK_EQUAL(3, x.level());

  BOOST_CHECK(merc_bases + CoreStatSpread(1, 0, 1, 1, 0, 1, 1) == x.stats());
}

BOOST_AUTO_TEST_CASE(unequipped_by_default) {
  Unit x(merc_bases, CoreStatSpread(90, 45, 50, 50, 45, 50, 50), UnitAttribute::None, mercenary, 8);

  BOOST_CHECK(!x.is_equipped());
  BOOST_CHECK(!x.held_item());
}

BOOST_AUTO_TEST_CASE(equip_fails_when_item_not_present) {
  Unit x(merc_bases, CoreStatSpread(), UnitAttribute::None, mercenary, 8);

  BOOST_REQUIRE(!x.held_item());

  BOOST_CHECK(!x.equip());
  BOOST_REQUIRE(!x.held_item());
}

BOOST_AUTO_TEST_CASE(equipping_a_non_equippable_item_fails) {
  Unit x(merc_bases, CoreStatSpread(), UnitAttribute::None, mercenary, 8);

  // Note that the potion is not an equipable item, although it may be held and used.
  x.give_item(get_potion());

  // Ensure that it doesn't equip on a give operation.
  BOOST_CHECK(!x.equipped_weapon());

  // Ensure that the item may not be manually equipped.
  BOOST_CHECK(!x.equip());

  BOOST_CHECK(!x.equipped_weapon());

  BOOST_REQUIRE(x.held_item());
}

BOOST_AUTO_TEST_CASE(able_to_equip_an_eqiupable_item) {
  Unit x(merc_bases, CoreStatSpread(), UnitAttribute::None, mercenary, 8);
  auto w = std::make_unique<Weapon>(WeaponType::Sword, UnitAttribute::None, 1, 1, 1, 1, 1, false);

  // TODO(matthew-c21): w seems to be getting sliced, and I'm not entirely sure where.
  x.give_item(std::move(w));
  // x.give_item(std::move(w));

  BOOST_CHECK(x.held_item());
  BOOST_CHECK(!x.equipped_weapon());
  BOOST_CHECK(!x.is_equipped());
  BOOST_CHECK(x.equip());
  BOOST_CHECK(x.is_equipped());
}

// TODO(matthew-c21): Add tests for units being (un)able to equip weapons of certain ranks or type based on class.

BOOST_AUTO_TEST_CASE(units_die_at_zero_hp) {
  Unit x(merc_bases, CoreStatSpread(), UnitAttribute::None, mercenary, 8);

  BOOST_REQUIRE(!x.dead());

  x.offset_hp(-merc_bases.hp / 2);

  BOOST_CHECK(!x.dead());

  x.offset_hp(-merc_bases.hp / 2 - 1);

  BOOST_CHECK(x.dead());
}

BOOST_AUTO_TEST_CASE(stats_correct_after_class_bonus) {
  Unit merc1(merc_bases, CoreStatSpread(), UnitAttribute::None, mercenary, 3);
  Unit merc2(merc_bases, CoreStatSpread(), UnitAttribute::None, myrmidon, 3);

  BOOST_CHECK(merc_bases == merc1.stats());
  BOOST_CHECK(merc_bases + myrmidon_bonuses == merc2.stats());
}

BOOST_AUTO_TEST_SUITE_END()


text:
#include "extractor/guidance/constants.hpp"
#include "extractor/guidance/intersection_generator.hpp"
#include "extractor/guidance/toolkit.hpp"

#include <algorithm>
#include <iomanip>
#include <iterator>
#include <limits>
#include <unordered_set>
#include <utility>

#include <boost/range/algorithm/count_if.hpp>

namespace osrm
{
namespace extractor
{
namespace guidance
{

IntersectionGenerator::IntersectionGenerator(
    const util::NodeBasedDynamicGraph &node_based_graph,
    const RestrictionMap &restriction_map,
    const std::unordered_set<NodeID> &barrier_nodes,
    const std::vector<QueryNode> &node_info_list,
    const CompressedEdgeContainer &compressed_edge_container)
    : node_based_graph(node_based_graph), restriction_map(restriction_map),
      barrier_nodes(barrier_nodes), node_info_list(node_info_list),
      compressed_edge_container(compressed_edge_container)
{
}

Intersection IntersectionGenerator::operator()(const NodeID from_node, const EdgeID via_eid) const
{
    auto intersection = GetConnectedRoads(from_node, via_eid);
    const auto node_at_intersection = node_based_graph.GetTarget(via_eid);
    return AdjustForJoiningRoads(
        node_at_intersection, MergeSegregatedRoads(node_at_intersection, std::move(intersection)));
}

//                                               a
//                                               |
//                                               |
//                                               v
// For an intersection from_node --via_edi--> turn_node ----> c
//                                               ^
//                                               |
//                                               |
//                                               b
// This functions returns _all_ turns as if the graph was undirected.
// That means we not only get (from_node, turn_node, c) in the above example
// but also (from_node, turn_node, a), (from_node, turn_node, b). These turns are
// marked as invalid and only needed for intersection classification.
Intersection IntersectionGenerator::GetConnectedRoads(const NodeID from_node,
                                                      const EdgeID via_eid) const
{
    Intersection intersection;
    const NodeID turn_node = node_based_graph.GetTarget(via_eid);
    const NodeID only_restriction_to_node = [&]() {
        // If only restrictions refer to invalid ways somewhere far away, we rather ignore the
        // restriction than to not route over the intersection at all.
        const auto only_restriction_to_node =
            restriction_map.CheckForEmanatingIsOnlyTurn(from_node, turn_node);
        if (only_restriction_to_node != SPECIAL_NODEID)
        {
            // check if we can find an edge in the edge-rage
            for (const auto onto_edge : node_based_graph.GetAdjacentEdgeRange(turn_node))
                if (only_restriction_to_node == node_based_graph.GetTarget(onto_edge))
                    return only_restriction_to_node;
        }
        // Ignore broken only restrictions.
        return SPECIAL_NODEID;
    }();
    const bool is_barrier_node = barrier_nodes.find(turn_node) != barrier_nodes.end();

    bool has_uturn_edge = false;
    bool uturn_could_be_valid = false;
    for (const EdgeID onto_edge : node_based_graph.GetAdjacentEdgeRange(turn_node))
    {
        BOOST_ASSERT(onto_edge != SPECIAL_EDGEID);
        const NodeID to_node = node_based_graph.GetTarget(onto_edge);

        bool turn_is_valid =
            // reverse edges are never valid turns because the resulting turn would look like this:
            // from_node --via_edge--> turn_node <--onto_edge-- to_node
            // however we need this for capture intersection shape for incoming one-ways
            !node_based_graph.GetEdgeData(onto_edge).reversed &&
            // we are not turning over a barrier
            (!is_barrier_node || from_node == to_node) &&
            // We are at an only_-restriction but not at the right turn.
            (only_restriction_to_node == SPECIAL_NODEID || to_node == only_restriction_to_node) &&
            // the turn is not restricted
            !restriction_map.CheckIfTurnIsRestricted(from_node, turn_node, to_node);

        auto angle = 0.;
        if (from_node == to_node)
        {
            uturn_could_be_valid = turn_is_valid;
            if (turn_is_valid && !is_barrier_node)
            {
                // we only add u-turns for dead-end streets.
                if (node_based_graph.GetOutDegree(turn_node) > 1)
                {
                    auto number_of_emmiting_bidirectional_edges = 0;
                    for (auto edge : node_based_graph.GetAdjacentEdgeRange(turn_node))
                    {
                        auto target = node_based_graph.GetTarget(edge);
                        auto reverse_edge = node_based_graph.FindEdge(target, turn_node);
                        BOOST_ASSERT(reverse_edge != SPECIAL_EDGEID);
                        if (!node_based_graph.GetEdgeData(reverse_edge).reversed)
                        {
                            ++number_of_emmiting_bidirectional_edges;
                        }
                    }
                    // is a dead-end, only possible road is to go back
                    turn_is_valid = number_of_emmiting_bidirectional_edges <= 1;
                }
            }
            has_uturn_edge = true;
            BOOST_ASSERT(angle >= 0. && angle < std::numeric_limits<double>::epsilon());
        }
        else
        {
            // unpack first node of second segment if packed
            const auto first_coordinate = getRepresentativeCoordinate(
                from_node, turn_node, via_eid, INVERT, compressed_edge_container, node_info_list);
            const auto third_coordinate = getRepresentativeCoordinate(
                turn_node, to_node, onto_edge, !INVERT, compressed_edge_container, node_info_list);
            angle = util::coordinate_calculation::computeAngle(
                first_coordinate, node_info_list[turn_node], third_coordinate);
            if (std::abs(angle) < std::numeric_limits<double>::epsilon())
                has_uturn_edge = true;
        }

        intersection.push_back(
            ConnectedRoad(TurnOperation{onto_edge,
                                        angle,
                                        {TurnType::Invalid, DirectionModifier::UTurn},
                                        INVALID_LANE_DATAID},
                          turn_is_valid));
    }

    // We hit the case of a street leading into nothing-ness. Since the code here assumes that this
    // will never happen we add an artificial invalid uturn in this case.
    if (!has_uturn_edge)
    {
        intersection.push_back(
            {TurnOperation{
                 via_eid, 0., {TurnType::Invalid, DirectionModifier::UTurn}, INVALID_LANE_DATAID},
             false});
    }

    const auto ByAngle = [](const ConnectedRoad &first, const ConnectedRoad second) {
        return first.turn.angle < second.turn.angle;
    };
    std::sort(std::begin(intersection), std::end(intersection), ByAngle);

    BOOST_ASSERT(intersection[0].turn.angle >= 0. &&
                 intersection[0].turn.angle < std::numeric_limits<double>::epsilon());

    const auto valid_count =
        boost::count_if(intersection, [](const ConnectedRoad &road) { return road.entry_allowed; });
    if (0 == valid_count && uturn_could_be_valid)
    {
        // after intersections sorting by angles, find the u-turn with (from_node == to_node)
        // that was inserted together with setting uturn_could_be_valid flag
        std::size_t self_u_turn = 0;
        while (self_u_turn < intersection.size() &&
               intersection[self_u_turn].turn.angle < std::numeric_limits<double>::epsilon() &&
               from_node != node_based_graph.GetTarget(intersection[self_u_turn].turn.eid))
        {
            ++self_u_turn;
        }

        BOOST_ASSERT(from_node == node_based_graph.GetTarget(intersection[self_u_turn].turn.eid));
        intersection[self_u_turn].entry_allowed = true;
    }

    return intersection;
}

// Checks for mergability of two ways that represent the same intersection. For further information
// see interface documentation in header.
bool IntersectionGenerator::CanMerge(const NodeID node_at_intersection,
                                     const Intersection &intersection,
                                     std::size_t first_index,
                                     std::size_t second_index) const
{
    const auto &first_data = node_based_graph.GetEdgeData(intersection[first_index].turn.eid);
    const auto &second_data = node_based_graph.GetEdgeData(intersection[second_index].turn.eid);

    // only merge named ids
    if (first_data.name_id == EMPTY_NAMEID)
        return false;

    // need to be same name
    if (first_data.name_id != second_data.name_id)
        return false;

    // compatibility is required
    if (first_data.travel_mode != second_data.travel_mode)
        return false;
    if (first_data.road_classification != second_data.road_classification)
        return false;

    // may not be on a roundabout
    if (first_data.roundabout || second_data.roundabout)
        return false;

    // exactly one of them has to be reversed
    if (first_data.reversed == second_data.reversed)
        return false;

    // one of them needs to be invalid
    if (intersection[first_index].entry_allowed && intersection[second_index].entry_allowed)
        return false;

    // mergeable if the angle is not too big
    const auto angle_between = angularDeviation(intersection[first_index].turn.angle,
                                                intersection[second_index].turn.angle);

    const auto coordinate_at_in_edge =
        getRepresentativeCoordinate(node_at_intersection,
                                    node_based_graph.GetTarget(intersection[0].turn.eid),
                                    intersection[0].turn.eid,
                                    false,
                                    compressed_edge_container,
                                    node_info_list);
    const auto coordinate_at_intersection = node_info_list[node_at_intersection];

    const auto isValidYArm = [this,
                              intersection,
                              coordinate_at_in_edge,
                              coordinate_at_intersection,
                              node_at_intersection](const std::size_t index,
                                                    const std::size_t other_index) {
        const auto GetActualTarget = [&](const std::size_t index) {
            EdgeID last_in_edge_id;
            GetActualNextIntersection(
                node_at_intersection, intersection[index].turn.eid, nullptr, &last_in_edge_id);
            return node_based_graph.GetTarget(last_in_edge_id);
        };

        const auto target_id = GetActualTarget(index);
        const auto other_target_id = GetActualTarget(other_index);
        if (target_id == node_at_intersection || other_target_id == node_at_intersection)
            return false;

        const auto coordinate_at_target = node_info_list[target_id];
        const auto coordinate_at_other_target = node_info_list[other_target_id];

        const auto turn_angle = util::coordinate_calculation::computeAngle(
            coordinate_at_in_edge, coordinate_at_intersection, coordinate_at_target);
        const auto other_turn_angle = util::coordinate_calculation::computeAngle(
            coordinate_at_in_edge, coordinate_at_intersection, coordinate_at_other_target);
        const double distance_to_target = util::coordinate_calculation::haversineDistance(
            coordinate_at_intersection, coordinate_at_target);

        const constexpr double MAX_COLLAPSE_DISTANCE = 30;
        if (distance_to_target < MAX_COLLAPSE_DISTANCE)
            return false;

        const bool becomes_narrower =
            angularDeviation(turn_angle, other_turn_angle) < NARROW_TURN_ANGLE &&
            angularDeviation(turn_angle, other_turn_angle) <
                angularDeviation(intersection[index].turn.angle,
                                 intersection[other_index].turn.angle);

        return becomes_narrower;
    };

    const bool is_y_arm_first = isValidYArm(first_index, second_index);
    const bool is_y_arm_second = isValidYArm(second_index, first_index);

    // Only merge valid y-arms
    if (!is_y_arm_first || !is_y_arm_second)
        return false;

    if (angle_between < 60)
        return true;

    // Finally, we also allow merging if all streets offer the same name, it is only three roads and
    // the angle is not fully extreme:
    if (intersection.size() != 3)
        return false;

    // since we have an intersection of size three now, there is only one index we are not looking
    // at right now. The final index in the intersection is calculated next:
    const std::size_t third_index = [first_index, second_index]() {
        if (first_index == 0)
            return second_index == 2 ? 1 : 2;
        else if (first_index == 1)
            return second_index == 2 ? 0 : 2;
        else
            return second_index == 1 ? 0 : 1;
    }();

    // needs to be same road coming in
    if (node_based_graph.GetEdgeData(intersection[third_index].turn.eid).name_id !=
        first_data.name_id)
        return false;

    // we only allow collapsing of a Y like fork. So the angle to the third index has to be
    // roughly equal:
    const auto y_angle_difference =
        angularDeviation(angularDeviation(intersection[third_index].turn.angle,
                                          intersection[first_index].turn.angle),
                         angularDeviation(intersection[third_index].turn.angle,
                                          intersection[second_index].turn.angle));

    // Allow larger angles if its three roads only of the same name
    // This is a heuristic and might need to be revised.
    const bool assume_y_intersection =
        angle_between < 100 && y_angle_difference < FUZZY_ANGLE_DIFFERENCE;
    return assume_y_intersection;
}

/*
 * Segregated Roads often merge onto a single intersection.
 * While technically representing different roads, they are
 * often looked at as a single road.
 * Due to the merging, turn Angles seem off, wenn we compute them from the
 * initial positions.
 *
 *         b<b<b<b(1)<b<b<b
 * aaaaa-b
 *         b>b>b>b(2)>b>b>b
 *
 * Would be seen as a slight turn going fro a to (2). A Sharp turn going from
 * (1) to (2).
 *
 * In cases like these, we megre this segregated roads into a single road to
 * end up with a case like:
 *
 * aaaaa-bbbbbb
 *
 * for the turn representation.
 * Anything containing the first u-turn in a merge affects all other angles
 * and is handled separately from all others.
 */
Intersection IntersectionGenerator::MergeSegregatedRoads(const NodeID intersection_node,
                                                         Intersection intersection) const
{
    const auto getRight = [&](std::size_t index) {
        return (index + intersection.size() - 1) % intersection.size();
    };

    const auto merge = [](const ConnectedRoad &first,
                          const ConnectedRoad &second) -> ConnectedRoad {
        if (!first.entry_allowed)
        {
            ConnectedRoad result = second;
            result.turn.angle = (first.turn.angle + second.turn.angle) / 2;
            if (first.turn.angle - second.turn.angle > 180)
                result.turn.angle += 180;
            if (result.turn.angle > 360)
                result.turn.angle -= 360;

            return result;
        }
        else
        {
            BOOST_ASSERT(!second.entry_allowed);
            ConnectedRoad result = first;
            result.turn.angle = (first.turn.angle + second.turn.angle) / 2;

            if (first.turn.angle - second.turn.angle > 180)
                result.turn.angle += 180;
            if (result.turn.angle > 360)
                result.turn.angle -= 360;

            return result;
        }
    };
    if (intersection.size() <= 1)
        return intersection;

    const bool is_connected_to_roundabout = [this, &intersection]() {
        for (const auto &road : intersection)
        {
            if (node_based_graph.GetEdgeData(road.turn.eid).roundabout)
                return true;
        }
        return false;
    }();

    // check for merges including the basic u-turn
    // these result in an adjustment of all other angles. This is due to how these angles are
    // perceived. Considering the following example:
    //
    //   c   b
    //     Y
    //     a
    //
    // coming from a to b (given a road that splits at the fork into two one-ways), the turn is not
    // considered as a turn but rather as going straight.
    // Now if we look at the situation merging:
    //
    //  a     b
    //    \ /
    // e - + - d
    //     |
    //     c
    //
    // With a,b representing the same road, the intersection itself represents a classif for way
    // intersection so we handle it like
    //
    //   (a),b
    //      |
    // e -  + - d
    //      |
    //      c
    //
    // To be able to consider this adjusted representation down the line, we merge some roads.
    // If the merge occurs at the u-turn edge, we need to adjust all angles, though, since they are
    // with respect to the now changed perceived location of a. If we move (a) to the left, we add
    // the difference to all angles. Otherwise we subtract it.
    bool merged_first = false;
    // these result in an adjustment of all other angles
    if (CanMerge(intersection_node, intersection, 0, intersection.size() - 1))
    {
        merged_first = true;
        // moving `a` to the left
        const double correction_factor =
            (360 - intersection[intersection.size() - 1].turn.angle) / 2;
        for (std::size_t i = 1; i + 1 < intersection.size(); ++i)
            intersection[i].turn.angle += correction_factor;

        // FIXME if we have a left-sided country, we need to switch this off and enable it below
        intersection[0] = merge(intersection.front(), intersection.back());
        intersection[0].turn.angle = 0;

        intersection.pop_back();
    }
    else if (CanMerge(intersection_node, intersection, 0, 1))
    {
        merged_first = true;
        // moving `a` to the right
        const double correction_factor = (intersection[1].turn.angle) / 2;
        for (std::size_t i = 2; i < intersection.size(); ++i)
            intersection[i].turn.angle -= correction_factor;
        intersection[0] = merge(intersection[0], intersection[1]);
        intersection[0].turn.angle = 0;
        intersection.erase(intersection.begin() + 1);
    }

    if (merged_first && is_connected_to_roundabout)
    {
        /*
         * We are merging a u-turn against the direction of a roundabout
         *
         *     -----------> roundabout
         *        /    \
         *     out      in
         *
         * These cases have to be disabled, even if they are not forbidden specifically by a
         * relation
         */
        intersection[0].entry_allowed = false;
    }

    // a merge including the first u-turn requres an adjustment of the turn angles
    // therefore these are handled prior to this step
    for (std::size_t index = 2; index < intersection.size(); ++index)
    {
        if (CanMerge(intersection_node, intersection, index, getRight(index)))
        {
            intersection[getRight(index)] =
                merge(intersection[getRight(index)], intersection[index]);
            intersection.erase(intersection.begin() + index);
            --index;
        }
    }

    const auto ByAngle = [](const ConnectedRoad &first, const ConnectedRoad second) {
        return first.turn.angle < second.turn.angle;
    };
    std::sort(std::begin(intersection), std::end(intersection), ByAngle);
    return intersection;
}

// OSM can have some very steep angles for joining roads. Considering the following intersection:
//        x
//        |
//        v __________c
//       /
// a ---d
//       \ __________b
//
// with c->d as a oneway
// and d->b as a oneway, the turn von x->d is actually a turn from x->a. So when looking at the
// intersection coming from x, we want to interpret the situation as
//           x
//           |
// a __ d __ v__________c
//      |
//      |_______________b
//
// Where we see the turn to `d` as a right turn, rather than going straight.
// We do this by adjusting the local turn angle at `x` to turn onto `d` to be reflective of this
// situation, where `v` would be the node at the intersection.
Intersection IntersectionGenerator::AdjustForJoiningRoads(const NodeID node_at_intersection,
                                                          Intersection intersection) const
{
    // nothing to do for dead ends
    if (intersection.size() <= 1)
        return intersection;

    const util::Coordinate coordinate_at_intersection = node_info_list[node_at_intersection];
    // never adjust u-turns
    for (std::size_t index = 1; index < intersection.size(); ++index)
    {
        auto &road = intersection[index];
        // to find out about the above situation, we need to look at the next intersection (at d in
        // the example). If the initial road can be merged to the left/right, we are about to adjust
        // the angle.
        const auto next_intersection_along_road =
            GetConnectedRoads(node_at_intersection, road.turn.eid);
        if (next_intersection_along_road.size() <= 1)
            continue;

        const auto node_at_next_intersection = node_based_graph.GetTarget(road.turn.eid);
        const util::Coordinate coordinate_at_next_intersection =
            node_info_list[node_at_next_intersection];
        if (util::coordinate_calculation::haversineDistance(coordinate_at_intersection,
                                                            coordinate_at_next_intersection) > 30)
            continue;

        const auto adjustAngle = [](double angle, double offset) {
            angle += offset;
            if (angle > 360)
                return angle - 360.;
            else if (angle < 0)
                return angle + 360.;
            return angle;
        };

        // check if the u-turn edge at the next intersection could be merged to the left/right. If
        // this is the case and the road is not far away (see previous distance check), if
        // influences the perceived angle.
        if (CanMerge(node_at_next_intersection, next_intersection_along_road, 0, 1))
        {
            const auto offset = 0.5 * angularDeviation(next_intersection_along_road[0].turn.angle,
                                                       next_intersection_along_road[1].turn.angle);
            // at the target intersection, we merge to the right, so we need to shift the current
            // angle to the left
            road.turn.angle = adjustAngle(road.turn.angle, offset);
        }
        else if (CanMerge(node_at_next_intersection,
                          next_intersection_along_road,
                          0,
                          next_intersection_along_road.size() - 1))
        {
            const auto offset =
                0.5 * angularDeviation(
                          next_intersection_along_road[0].turn.angle,
                          next_intersection_along_road[next_intersection_along_road.size() - 1]
                              .turn.angle);
            // at the target intersection, we merge to the left, so we need to shift the current
            // angle to the right
            road.turn.angle = adjustAngle(road.turn.angle, -offset);
        }
    }
    return intersection;
}

Intersection
IntersectionGenerator::GetActualNextIntersection(const NodeID starting_node,
                                                 const EdgeID via_edge,
                                                 NodeID *resulting_from_node = nullptr,
                                                 EdgeID *resulting_via_edge = nullptr) const
{
    // This function skips over traffic lights/graph compression issues and similar to find the next
    // actual intersection
    Intersection result = GetConnectedRoads(starting_node, via_edge);

    // Skip over stuff that has not been compressed due to barriers/parallel edges
    NodeID node_at_intersection = starting_node;
    EdgeID incoming_edge = via_edge;

    // to prevent endless loops
    const auto termination_node = node_based_graph.GetTarget(via_edge);

    // using a maximum lookahead, we make sure not to end up in some form of loop
    std::unordered_set<NodeID> visited_nodes;
    while (visited_nodes.count(node_at_intersection) == 0 &&
           (result.size() == 2 &&
            node_based_graph.GetEdgeData(via_edge).IsCompatibleTo(
                node_based_graph.GetEdgeData(result[1].turn.eid))))
    {
        visited_nodes.insert(node_at_intersection);
        node_at_intersection = node_based_graph.GetTarget(incoming_edge);
        incoming_edge = result[1].turn.eid;
        result = GetConnectedRoads(node_at_intersection, incoming_edge);

        // When looping back to the original node, we obviously are in a loop. Stop there.
        if (termination_node == node_based_graph.GetTarget(incoming_edge))
            break;
    }

    // return output if requested
    if (resulting_from_node)
        *resulting_from_node = node_at_intersection;
    if (resulting_via_edge)
        *resulting_via_edge = incoming_edge;

    return result;
}

} // namespace guidance
} // namespace extractor
} // namespace osrm

max_stars_repo_name: shiyuan/osrm

max_issues_repo_name: zummach/osrm

max_forks_repo_name: dingchunda/osrm-backend


text:
#include <ros/ros.h>
#include <ros/package.h>
#include <stdio.h>
#include <unistd.h>

#include <string>
#include <vector>
#include <unordered_map>
#include <iostream>
#include <memory>
#include <tuple>
#include <set>
#include <sstream>
#include <thread>
#include <atomic>
#include <mutex>
#include <future>
#include <chrono>
#include <exception>

#include <tbb/tbb.h>

#include <opencv2/videoio/videoio.hpp>
#include <opencv2/videoio/videoio_c.h>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include "openface_ros/ActionUnit.h"
#include "openface_ros/Face.h"
#include "openface_ros/Faces.h"

#include <sensor_msgs/Image.h>

#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>

#include <openface/LandmarkCoreIncludes.h>
#include <openface/Face_utils.h>
#include <openface/FaceAnalyser.h>
#include <openface/GazeEstimation.h>

#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>

#include <Eigen/Dense>

using namespace std;
using namespace ros;
using namespace cv;

namespace
{
  static geometry_msgs::Quaternion toQuaternion(double pitch, double roll, double yaw)
  {
    double t0 = std::cos(yaw * 0.5f);
    double t1 = std::sin(yaw * 0.5f);
    double t2 = std::cos(roll * 0.5f);
    double t3 = std::sin(roll * 0.5f);
    double t4 = std::cos(pitch * 0.5f);
    double t5 = std::sin(pitch * 0.5f);

    geometry_msgs::Quaternion q;
    q.w = t0 * t2 * t4 + t1 * t3 * t5;
    q.x = t0 * t3 * t4 - t1 * t2 * t5;
    q.y = t0 * t2 * t5 + t1 * t3 * t4;
    q.z = t1 * t2 * t4 - t0 * t3 * t5;
    return q;
  }

  static geometry_msgs::Quaternion operator *(const geometry_msgs::Quaternion &a, const geometry_msgs::Quaternion &b)
  {
    geometry_msgs::Quaternion q;

    q.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z;  // 1
    q.x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y;  // i
    q.y = a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x;  // j
    q.z = a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w;  // k
    return q;
  }

  void non_overlaping_detections(const vector<LandmarkDetector::CLNF *> &clnf_models, vector<Rect_<double>> &face_detections)
  {
    if(face_detections.empty()) return;

    for(size_t model = 0; model < clnf_models.size(); ++model)
    {
      // See if the detections intersect
      cv::Rect_<double> model_rect = clnf_models[model]->GetBoundingBox();

      for(int detection = face_detections.size() - 1; detection >= 0; --detection)
      {
        double intersection_area = (model_rect & face_detections[detection]).area();
        double union_area = model_rect.area() + face_detections[detection].area() - 2 * intersection_area;

        // If the model is already tracking what we're detecting ignore the detection, this is determined by amount of overlap
        if(intersection_area / union_area <= 0.5) continue;
        face_detections.erase(face_detections.begin() + detection);
      }
    }
  }

  vector<ssize_t> redundant_detections(const vector<LandmarkDetector::CLNF *> &clnf_models)
  {
    vector<ssize_t> ret(clnf_models.size(), -1);
    for(size_t modeli = 0; modeli < clnf_models.size(); ++modeli)
    {
      if(ret[modeli] >= 0) continue;
      // See if the detections intersect
      const Rect_<double> modeli_rect = clnf_models[modeli]->GetBoundingBox();

      for(size_t modelj = 0; modelj < clnf_models.size(); ++modelj)
      {
        if(modeli == modelj || ret[modelj] >= 0) continue;

        const Rect_<double> modelj_rect = clnf_models[modelj]->GetBoundingBox();

        double intersection_area = (modeli_rect & modelj_rect).area();
        double union_area = modeli_rect.area() + modelj_rect.area() - 2 * intersection_area;

        // If the model is already tracking what we're detecting ignore the detection, this is determined by amount of overlap
        if(intersection_area / union_area <= 0.5) continue;

        ret[modelj] = modeli;
      }
    }

    return ret;
  }

}

namespace openface_ros
{

  class OpenFaceRos
  {
  public:
    OpenFaceRos(NodeHandle &nh)
      : nh_(nh)
      , it_(nh_)
    {
      NodeHandle pnh("~");
      if(!pnh.getParam("image_topic", image_topic_)) throw invalid_argument("Expected ~image_topic parameter");

      const auto base_path = package::getPath("openface_ros");

      pnh.param<string>("clnf_model_path", clnf_model_path_, base_path + "/" + model_params_.model_location);
      pnh.param<string>("tri_model_path", tri_model_path_, base_path + "/model/tris_68_full.txt");
      pnh.param<string>("au_model_path", au_model_path_, base_path + "/model/AU_predictors/AU_all_best.txt");
      pnh.param<string>("haar_model_path", haar_model_path_, base_path + "/model/classifiers/haarcascade_frontalface_alt.xml");
      pnh.param<bool>("publish_viz", publish_viz_, false);

      int max_faces = 0;
      pnh.param<int>("max_faces", max_faces, 3);
      if(max_faces <= 0) throw invalid_argument("~max_faces must be > 0");

      max_faces_ = max_faces;

      camera_sub_ = it_.subscribeCamera(image_topic_, 1, &OpenFaceRos::process_incoming_, this);
      faces_pub_ = nh_.advertise<Faces>("faces", 1000);
      if(publish_viz_) viz_pub_ = it_.advertise("openface/viz", 1);
      init_openface_();
    }

    ~OpenFaceRos()
    {
    }

  private:
    void init_openface_()
    {
      model_params_.track_gaze = true;

      LandmarkDetector::CLNF clnf(clnf_model_path_);
      clnf.face_detector_HAAR.load(haar_model_path_);
	    clnf.face_detector_location = haar_model_path_;

      FaceAnalysis::FaceAnalyser face_analyser(vector<Vec3d>(), 0.7, 112, 112, au_model_path_, tri_model_path_);

      actives_ = vector<bool>(max_faces_, false);
      clnfs_ = vector<LandmarkDetector::CLNF>(max_faces_, clnf);
      face_analysers_ = vector<FaceAnalysis::FaceAnalyser>(max_faces_, face_analyser);

      ROS_INFO("OpenFace initialized!");
    }

    void process_incoming_(const sensor_msgs::ImageConstPtr &img, const sensor_msgs::CameraInfoConstPtr &cam)
    {
      cv_bridge::CvImagePtr cv_ptr;
      try
      {
        cv_ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8);
      }
      catch(const cv_bridge::Exception &e)
      {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
      }

      double fx = cam->K[0];
      double fy = cam->K[4];
      double cx = cam->K[2];
      double cy = cam->K[5];

      if(fx == 0 || fy == 0)
      {
        fx = 500.0 * cv_ptr->image.cols / 640.0;
        fy = 500.0 * cv_ptr->image.rows / 480.0;
        fx = (fx + fy) / 2.0;
        fy = fx;
      }

      if(cx == 0) cx = cv_ptr->image.cols / 2.0;
      if(cy == 0) cy = cv_ptr->image.rows / 2.0;

      vector<Rect_<double>> face_detections;

      if(cam->header.seq % 30 == 0)
      {
        LandmarkDetector::DetectFaces(face_detections, cv_ptr->image, clnfs_[0].face_detector_HAAR);
        vector<LandmarkDetector::CLNF *> active_clnfs;
        for(unsigned i = 0; i < max_faces_; ++i)
        {
          if(!actives_[i]) continue;
          active_clnfs.push_back(&clnfs_[i]);
        }

        non_overlaping_detections(active_clnfs, face_detections);
        ROS_INFO("new face detections %lu", face_detections.size());

      }

      vector<tbb::atomic<bool>> face_detections_used(face_detections.size(), false);
      tbb::parallel_for(0, (int)clnfs_.size(), [&](int i){
				if(clnfs_[i].failures_in_a_row > 4)
				{
					actives_[i] = false;
					clnfs_[i].Reset();
        }
				if(!actives_[i])
				{
					for(size_t j = 0; j < face_detections.size(); ++j)
					{
						// if it was not taken by another tracker take it (if it is false swap it to true and enter detection, this makes it parallel safe)
						if(face_detections_used[j].compare_and_swap(true, false)) continue;

            ROS_INFO("Allocating CLNF on face_detection %ld", j);

            clnfs_[i].Reset();

            // This ensures that a wider window is used for the initial landmark localisation
            clnfs_[i].detection_success = false;
            LandmarkDetector::DetectLandmarksInVideo(cv_ptr->image, face_detections[j], clnfs_[i], model_params_);
            actives_[i] = true;
            break;
					}
				}
				else
				{
					LandmarkDetector::DetectLandmarksInVideo(cv_ptr->image, clnfs_[i], model_params_);
				}
			});

      vector<LandmarkDetector::CLNF *> active_clnfs;
      for(unsigned i = 0; i < max_faces_; ++i)
      {
        if(!actives_[i]) continue;
        active_clnfs.push_back(&clnfs_[i]);
      }

      const auto redundancies = redundant_detections(active_clnfs);

      for(size_t i = 0; i < redundancies.size(); ++i)
      {
        if(redundancies[i] < 0) continue;

        cout << "Detected redundant CLNF at " << i << endl;
        actives_[i] = false;
        clnfs_[i].Reset();
      }

      Faces faces;
      for(unsigned i = 0; i < max_faces_; ++i)
      {
        if(!actives_[i]) continue;

        auto &clnf = clnfs_[i];
        auto &face_analyser = face_analysers_[i];

        Face face;
        face.header.frame_id = img->header.frame_id;
        face.header.stamp = Time::now();
        if(model_params_.track_gaze && clnf.eye_model)
        {
          Point3f left(0, 0, -1);
          Point3f right(0, 0, -1);
          FaceAnalysis::EstimateGaze(clnf, left, fx, fy, cx, cy, true);
          FaceAnalysis::EstimateGaze(clnf, right, fx, fy, cx, cy, false);

          face.left_gaze.x = left.x;
          face.left_gaze.y = left.y;
          face.left_gaze.z = left.z;

          face.right_gaze.x = right.x;
          face.right_gaze.y = right.y;
          face.right_gaze.z = right.z;
        }

        const auto head_pose = LandmarkDetector::GetCorrectedPoseWorld(clnf, fx, fy, cx, cy);
        face.head_pose.position.x = head_pose[0];
        face.head_pose.position.y = head_pose[1];
        face.head_pose.position.z = head_pose[2];
        const auto head_orientation = toQuaternion(head_pose[3], head_pose[5], head_pose[4]);

        face.head_pose.orientation = toQuaternion(M_PI / 2, 0, M_PI / 2);
        face.head_pose.orientation = face.head_pose.orientation * head_orientation;

        // tf
        {
          geometry_msgs::TransformStamped transform;
          transform.header = face.header;
          stringstream out;
          out << "head" << i;
          transform.child_frame_id = out.str();
          transform.transform.translation.x = face.head_pose.position.x / 1000.0;
          transform.transform.translation.y = face.head_pose.position.y / 1000.0;
          transform.transform.translation.z = face.head_pose.position.z / 1000.0;
          transform.transform.rotation = face.head_pose.orientation;
          tf_br_.sendTransform(transform);
        }

        if(clnf.tracking_initialised)
        {
          const auto &landmarks = clnf.detected_landmarks;
          for(unsigned i = 0; i < clnf.pdm.NumberOfPoints(); ++i)
          {
            geometry_msgs::Point p;
            p.x = landmarks.at<double>(i);
            p.y = landmarks.at<double>(clnf.pdm.NumberOfPoints() + i);
            face.landmarks_2d.push_back(p);
          }

          cv::Mat_<double> shape_3d = clnf.GetShape(fx, fy, cx, cy);
          for(unsigned i = 0; i < clnf.pdm.NumberOfPoints(); ++i)
          {
            geometry_msgs::Point p;
            p.x = shape_3d.at<double>(i);
            p.y = shape_3d.at<double>(clnf.pdm.NumberOfPoints() + i);
            p.z = shape_3d.at<double>(clnf.pdm.NumberOfPoints() * 2 + i);
            face.landmarks_3d.push_back(p);
          }
        }

        vector<std::pair<std::string, double>> aus_reg;
        vector<std::pair<std::string, double>> aus_class;
        tie(aus_reg, aus_class) = face_analyser.PredictStaticAUs(cv_ptr->image, clnf);

        unordered_map<string, ActionUnit> aus;
        for(const auto &au_reg : aus_reg)
        {
          auto it = aus.find(get<0>(au_reg));
          if(it == aus.end())
          {
            ActionUnit u;
            u.name = get<0>(au_reg);
            u.intensity = get<1>(au_reg);
            aus.insert({ get<0>(au_reg), u});
            continue;
          }

          it->second.intensity = get<1>(au_reg);
        }

        for(const auto &au_class : aus_class)
        {
          auto it = aus.find(get<0>(au_class));
          if(it == aus.end())
          {
            ActionUnit u;
            u.name = get<0>(au_class);
            u.presence = get<1>(au_class);
            aus.insert({ get<0>(au_class), u});
            continue;
          }

          it->second.presence = get<1>(au_class);
        }

        for(const auto &au : aus) face.action_units.push_back(get<1>(au));

        Point min(100000, 100000);
        Point max(0, 0);
        for(const auto &p : face.landmarks_2d)
        {
          if(p.x < min.x) min.x = p.x;
          if(p.y < min.y) min.y = p.y;
          if(p.x > max.x) max.x = p.x;
          if(p.y > max.y) max.y = p.y;
        }

        faces.faces.push_back(face);
      }

      faces_pub_.publish(faces);

      if(publish_viz_)
      {
        cv_bridge::CvImagePtr viz_ptr;
        try
        {
          viz_ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::BGR8);
        }
        catch(const cv_bridge::Exception &e)
        {
          ROS_ERROR("cv_bridge viz exception: %s", e.what());
          return;
        }
        decltype(viz_ptr->image) viz_img = viz_ptr->image.clone();

        for(unsigned i = 0; i < faces.faces.size(); ++i)
        {
          const auto &face = faces.faces[i];
          auto &clnf = clnfs_[i];
          for(const auto &p : face.landmarks_2d)
          {
            circle(viz_img, Point(p.x, p.y), 3, Scalar(255, 0, 0), -1);
          }

          if(model_params_.track_gaze && clnf.eye_model)
          {
            const Point3f left(face.left_gaze.x, face.left_gaze.y, face.left_gaze.z);
            const Point3f right(face.right_gaze.x, face.right_gaze.y, face.right_gaze.z);
            FaceAnalysis::DrawGaze(viz_img, clnf, left, right, fx, fy, cx, cy);
          }
        }

        auto viz_msg = cv_bridge::CvImage(img->header, "bgr8", viz_img).toImageMsg();
        viz_pub_.publish(viz_msg);
      }
    }

    tf2_ros::TransformBroadcaster tf_br_;

    LandmarkDetector::FaceModelParameters model_params_;
    vector<bool> actives_;
    vector<LandmarkDetector::CLNF> clnfs_;
    vector<FaceAnalysis::FaceAnalyser> face_analysers_;

    string image_topic_;
    string clnf_model_path_;
    string tri_model_path_;
    string au_model_path_;
    string haar_model_path_;
    unsigned max_faces_;

    NodeHandle nh_;
    image_transport::ImageTransport it_;
    image_transport::CameraSubscriber camera_sub_;
    Publisher faces_pub_;

    bool publish_viz_;
    image_transport::Publisher viz_pub_;
  };
}

int main(int argc, char *argv[])
{
  init(argc, argv, "openface_ros");

  using namespace openface_ros;

  NodeHandle nh;

  try
  {
    OpenFaceRos openface_(nh);
    spin();
  }
  catch(const exception &e)
  {
    ROS_FATAL("%s", e.what());
    return EXIT_FAILURE;
  }

  return EXIT_SUCCESS;
}


text:
/*
 * Copyright (C) 2005 National Association of REALTORS(R)
 * Portions Copyright (C) 2014 Real Estate Standards Organziation
 *
 * All rights reserved.
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use, copy,
 * modify, merge, publish, distribute, and/or sell copies of the
 * Software, and to permit persons to whom the Software is furnished
 * to do so, provided that the above copyright notice(s) and this
 * permission notice appear in all copies of the Software and that
 * both the above copyright notice(s) and this permission notice
 * appear in supporting documentation.
 */

#include <iostream>
#include <boost/lexical_cast.hpp>
#include "librets/GetObjectRequest.h"
#include "librets/RetsHttpRequest.h"
#include "librets/RetsVersion.h"
#include "librets/util.h"

using namespace librets;
using namespace librets::util;
using std::string;
using boost::lexical_cast;

typedef GetObjectRequest CLASS;

const char * CLASS::PHOTO_TYPE = "Photo";
const char * CLASS::PLAT_TYPE = "Plat";
const char * CLASS::VIDEO_TYPE = "Video";
const char * CLASS::AUDIO_TYPE = "Audio";
const char * CLASS::THUMBNAIL_TYPE = "Thumbnail";
const char * CLASS::MAP_TYPE = "Map";
const char * CLASS::VRIMAGE_TYPE = "VRImage";

GetObjectRequest::GetObjectRequest(string resource, string type)
{
    mResource = resource;
    mType = type;
    mLocation = false;
    mHasDefaultObjectKeyAndId = false;
    mDefaultObjectKey = "";
    mDefaultObjectId = -1;
    mIgnoreMalformedHeaders = false;
}

GetObjectRequest::~GetObjectRequest()
{
}

bool GetObjectRequest::GetLocation() const
{
    return mLocation;
}

void GetObjectRequest::SetLocation(bool location)
{
    mLocation = location;
}

void GetObjectRequest::SetObjectData(string objectData)
{
    mObjectData = objectData;
}

void GetObjectRequest::AddObject(string resourceEntity, int objectId)
{
    StringVectorPtr objectIds = GetObjectIds(resourceEntity);
    objectIds->push_back(lexical_cast<string>(objectId));
    UpdateDefaultObjectKeyAndId();
}

void GetObjectRequest::AddAllObjects(string resourceEntity)
{
    StringVectorPtr objectIds = GetObjectIds(resourceEntity);
    objectIds->push_back("*");
    UpdateDefaultObjectKeyAndId();
}

StringVectorPtr GetObjectRequest::GetObjectIds(string resourceEntity)
{
    ObjectList::iterator i = mObjectList.find(resourceEntity);
    if (i != mObjectList.end())
    {
        return i->second;
    }
    else
    {
        StringVectorPtr objectIds(new StringVector());
        mObjectList[resourceEntity] = objectIds;
        return objectIds;
    }
}

RetsHttpRequestPtr GetObjectRequest::CreateHttpRequest(RetsVersion retsVersion) const
{
    RetsHttpRequestPtr httpRequest(new RetsHttpRequest());
    PrepareHttpRequest(httpRequest, retsVersion);
    return httpRequest;
}

void GetObjectRequest::PrepareHttpRequest(RetsHttpRequestPtr httpRequest, RetsVersion retsVersion) const
{
    httpRequest->SetQueryParameter("Resource", mResource);
    httpRequest->SetQueryParameter("Type", mType);
    httpRequest->SetQueryParameter("Location", mLocation ? "1" : "0");
    httpRequest->SetMethod(RetsHttpRequest::GET);
    StringVector ids;
    ObjectList::const_iterator i;
    for (i = mObjectList.begin(); i != mObjectList.end(); i++)
    {
        string id;
        id.append(i->first);
        id.append(":");
        StringVectorPtr objectIds = i->second;
        id.append(join(*objectIds, ":"));
        ids.push_back(id);
    }

    httpRequest->SetQueryParameter("ID", join(ids, ","));
    if (retsVersion >= RETS_1_8)
    {
        if (!mObjectData.empty())
        {
            httpRequest->SetQueryParameter("ObjectData", mObjectData);
        }
    }

    /*
     * For Objects, if we are debugging and logging, we want to disable that because
     * most of this data is binary. We may want to make this user controlled at some point
     * as an option. In that case, RetsHttpRequest::SetLogging() can be used.
     */
    httpRequest->SetNoLogging();
}

void GetObjectRequest::UpdateDefaultObjectKeyAndId()
{
    if (mObjectList.size() == 1)
    {
        ObjectList::const_iterator i = mObjectList.begin();
        StringVectorPtr objectIds = i->second;
        if (objectIds->size() == 1)
        {
            string objectId = objectIds->at(0);
            if (objectId !=  "*")
            {
                mHasDefaultObjectKeyAndId = true;
                mDefaultObjectKey = i->first;
                try
                {
                    mDefaultObjectId = lexical_cast<int>(objectId);
                    return;
                }
                catch (std::exception &e)
                {
                    // Fall through and return -1 for the default.
                }
            }
        }
    }
    mHasDefaultObjectKeyAndId = false;
    mDefaultObjectKey = "";
    mDefaultObjectId = -1;
}

bool GetObjectRequest::HasDefaultObjectKeyAndId() const
{
    return mHasDefaultObjectKeyAndId;
}

string GetObjectRequest::GetDefaultObjectKey() const
{
    return mDefaultObjectKey;
}

int GetObjectRequest::GetDefaultObjectId() const
{
    return mDefaultObjectId;
}

bool GetObjectRequest::GetIgnoreMalformedHeaders() const
{
    return mIgnoreMalformedHeaders;
}

void GetObjectRequest::SetIgnoreMalformedHeaders(bool ignore)
{
    mIgnoreMalformedHeaders = ignore;
}

max_stars_repo_name: SMG-Digital/libRETS

max_issues_repo_name: SMG-Digital/libRETS

max_forks_repo_name: SMG-Digital/libRETS