text:
#include "TcpClientBase.h"

#include "OnConnectionChangedParam.h"
#include "OnMessageReceivedParam.h"

#include "SocketSetting.h"

#include "StringAlgo.h"
#include "NetworkHelpers.h"
#include "RAII.h"

#include "ThreadingSafety.h"

#include "Network/NetworkConstants.cs"
#include "Network/NetworkMessageType.cs"
#include "Network/NetworkApplication.cs"

#include <boost/asio/read_until.hpp>
#include <boost/asio/connect.hpp>
#include <boost/asio/ip/address.hpp>

void Storm::TCPClientBasePrivateLogic::setup(Traits::SocketType &/*socket*/)
{
	/*socket.set_option(boost::asio::socket_base::keep_alive{ true });
	socket.set_option(boost::asio::socket_base::reuse_address{ true });*/
}

void Storm::TCPClientBasePrivateLogic::connect(Traits::NetworkService &/*ioService*/, Traits::SocketType &socket, const Storm::SocketSetting &settings)
{
	Traits::TcpType::endpoint endpt{
		boost::asio::ip::address::from_string(settings.getIPStr()),
		settings._port
	};

	socket.async_connect(endpt, [this, &socket](const boost::system::error_code &ec)
	{
		if (!ec)
		{
			try
			{
				this->notifyStartAuthentication();

				this->doAuthentication(socket);
				return;
			}
			catch (const Storm::Exception &e)
			{
				LOG_ERROR <<
					"Storm::Exception catched while connecting : " << e.what() << ".\n" << e.stackTrace()
					;
			}
			catch (const std::exception &e)
			{
				LOG_ERROR << "std::exception catched while connecting : " << e.what();
			}
		}

		this->disconnectLogicCall();
	});
}

void Storm::TCPClientBasePrivateLogic::disconnect(Traits::NetworkService &/*ioService*/, Traits::SocketType &socket)
{
	socket.close();
}

void Storm::TCPClientBasePrivateLogic::doAuthentication(Traits::SocketType &socket)
{
	Storm::EndPointIdentifier applicationIdentifier;

	const auto remoteEndpoint = socket.remote_endpoint();
	applicationIdentifier._ipAddress = remoteEndpoint.address().to_v4().to_string();

	_temporaryRead.clear();
	boost::asio::async_read_until(socket, boost::asio::dynamic_buffer(_temporaryRead), Storm::NetworkConstants::k_endOfMessageCommand,
		[this, appIdentifier = std::move(applicationIdentifier)](const boost::system::error_code &ec, const std::size_t) mutable
	{
		auto disconnectRaiiObject = Storm::makeLazyRAIIObject([this]()
		{
			_temporaryRead.clear();
			this->disconnectLogicCall();
		});

		if (ec)
		{
			return;
		}

		std::string authenticatedMsgReceived = std::move(_temporaryRead);

		const std::size_t authenticateSize =
			Storm::NetworkConstants::k_networkSeparator.size() * 4 +
			sizeof(Storm::NetworkConstants::k_magicKeyword) +
			sizeof(uint8_t) + // ApplicationType
			sizeof(uint32_t) + // PID
			sizeof(uint64_t) + // Version
			Storm::NetworkConstants::k_endOfMessageCommand.size()
			;

		if (authenticatedMsgReceived.size() != authenticateSize)
		{
			LOG_ERROR << "Wrong authentication message size received from " << appIdentifier._ipAddress << "!";
			return;
		}

		std::vector<std::string_view> authenticatedTokens;

		Storm::StringAlgo::split(authenticatedTokens, authenticatedMsgReceived, Storm::StringAlgo::makeSplitPredicate(Storm::NetworkConstants::k_networkSeparator));

		if (authenticatedTokens.size() == 5)
		{
			const uint32_t magicKeyword = Storm::NetworkHelpers::fromNetwork<uint32_t>(authenticatedTokens[0]);
			if (magicKeyword != Storm::NetworkConstants::k_magicKeyword)
			{
				LOG_ERROR << "Wrong magic word received received from " << appIdentifier._ipAddress << ". Value received was " << magicKeyword;
				return;
			}

			if (authenticatedTokens[4] != Storm::NetworkConstants::k_endOfMessageCommand)
			{
				LOG_ERROR << "Authentication message received from received from " << appIdentifier._ipAddress << " did not finish by an end of message. Value received was " << authenticatedTokens[4];
				return;
			}

			appIdentifier._connectedApplication = static_cast<Storm::NetworkApplication>(Storm::NetworkHelpers::fromNetwork<uint8_t>(authenticatedTokens[1]));
			appIdentifier._pid = Storm::NetworkHelpers::fromNetwork<uint32_t>(authenticatedTokens[2]);

			if (authenticatedTokens[3].empty())
			{
				LOG_ERROR << "We didn't received a network version. The authentication message received from " << appIdentifier._ipAddress << " is invalid!";
				return;
			}

			Storm::OnConnectionStateChangedParam param;
			param._applicationId = appIdentifier;

			std::size_t versionSize = 0;
			for (; authenticatedTokens[3][versionSize] != '\0'; ++versionSize);
			param._applicationVersion = std::string{ authenticatedTokens[3].data(), versionSize };

			param._connected = true;

			this->notifyOnConnectionChanged(std::move(param));

			// The only way to not disconnect is to successfully come here.
			_temporaryRead.clear();
			disconnectRaiiObject.release();
		}
		else
		{
			LOG_ERROR << "Authentication failed because of a wrong number of arguments received from " << appIdentifier._ipAddress << "!";
		}
	});

	std::string authenticationMsgToSend = Storm::NetworkHelpers::makeAuthenticationMsg();
	const std::size_t authenticationMsgLength = authenticationMsgToSend.size();
	auto buffer = boost::asio::buffer(authenticationMsgToSend.data(), authenticationMsgLength);
	socket.async_send(buffer, [this, aliveKeeper = std::move(authenticationMsgToSend), authenticationMsgLength](const boost::system::error_code &ec, const std::size_t byteWrote)
	{
		if (!ec && byteWrote == authenticationMsgLength)
		{
			LOG_DEBUG << "Authenticated ourself successfully!";
		}
		else
		{
			LOG_ERROR << "We weren't able to authenticate ourself!";
			this->disconnectLogicCall();
		}
	});
}

void Storm::TCPClientBasePrivateLogic::sendMessage(Traits::SocketType &socket, std::string &&msg, const Storm::NetworkMessageType messageType)
{
	Storm::NetworkHelpers::prepareMsg(msg, messageType);
	socket.send(boost::asio::buffer(msg.data(), msg.size()));
}

void Storm::TCPClientBasePrivateLogic::sendAsyncMessage(Traits::SocketType &socket, std::string &&msg, const Storm::NetworkMessageType messageType, std::size_t byteWroteSince /*= 0*/)
{
	if (byteWroteSince == 0)
	{
		Storm::NetworkHelpers::prepareMsg(msg, messageType);
	}

	const auto sendBuffer = boost::asio::buffer(msg.data() + byteWroteSince, msg.size());

	socket.async_send(
		sendBuffer,
		[this, &socket, messageType, currentMsg = std::move(msg), byteWroteSince](const boost::system::error_code &ec, const std::size_t byteWrote) mutable
	{
		if (!ec)
		{
			byteWroteSince += byteWrote;
			if (byteWroteSince != currentMsg.size())
			{
				this->sendAsyncMessage(socket, std::move(currentMsg), messageType, byteWroteSince);
			}
		}
		else
		{
			this->disconnectLogicCall();
		}
	});
}

void Storm::TCPClientBasePrivateLogic::startRead(Traits::SocketType &socket)
{
	if (this->queryIsConnected())
	{
		boost::asio::async_read_until(socket, boost::asio::dynamic_buffer(_temporaryRead2), Storm::NetworkConstants::k_endOfMessageCommand,
			[this, &socket](const boost::system::error_code &ec, const std::size_t /*byteRead*/)
		{
			if (!ec)
			{
				auto raiiRestartRead = Storm::makeLazyRAIIObject([this, &socket]()
				{
					this->startRead(socket);
				});

				std::vector<Storm::OnMessageReceivedParam> params;
				if (Storm::NetworkHelpers::parseMsg(_temporaryRead2, params))
				{
					for (Storm::OnMessageReceivedParam &param : params)
					{
						this->notifyOnMessageReceived(std::move(param));
					}
				}
			}
			else
			{
				_temporaryRead2.clear();
				this->disconnectLogicCall();
			}
		});
	}
}

void Storm::TCPClientBasePrivateLogic::definitiveStop(Traits::SocketType &socket, const bool connected)
{
	// This is thread safe, so no need to lock anything.
	try
	{
		socket.cancel();
	}
	catch (const std::exception &ex)
	{
		LOG_ERROR << "Cannot cancel socket : " << ex.what();
	}

	try
	{
		if (connected)
		{
			socket.shutdown(boost::asio::socket_base::shutdown_type::shutdown_both);
		}
		socket.close();
	}
	catch (const boost::system::system_error &error)
	{
		LOG_ERROR << "Cannot close socket due to system error: '" << error.what() << "'. Error code was " << error.code();
	}
}

void Storm::TCPClientBasePrivateLogic::onConnectionChanged(const Storm::OnConnectionStateChangedParam &param, Storm::ConnectionStatus &connectedFlag, Storm::EndPointIdentifier &outApplicationId)
{
	assert(Storm::isNetworkThread() && "This method should only be called from Network thread!");

	if (param._connected)
	{
		connectedFlag = Storm::ConnectionStatus::Connected;
		if (param._applicationVersion != Storm::NetworkConstants::k_networkVersion)
		{
			Storm::throwException<Storm::Exception>(
				"Application's version to connect with is mismatching with our own.\n"
				"Ours : " + Storm::toStdString(Storm::NetworkConstants::k_networkVersion) + "."
				"Theirs : " + Storm::toStdString(param._applicationVersion) + ".\n"
				"We cannot accept the connection until both versions are the same!"
			);
		}

		outApplicationId = param._applicationId;
	}
	else
	{
		connectedFlag = Storm::ConnectionStatus::NotConnected;

		outApplicationId._connectedApplication = Storm::NetworkApplication::Unknown;
		outApplicationId._ipAddress.clear();
		outApplicationId._pid = 0;
	}
}

bool Storm::TCPClientBasePrivateLogic::onMessageReceived(const Storm::OnMessageReceivedParam &param)
{
	assert(Storm::isNetworkThread() && "This method should only be called from Network thread!");
	_temporaryRead2.clear();

	return param._messageType != Storm::NetworkMessageType::Ping;
}

max_stars_repo_name: SunlayGGX/Storm

max_issues_repo_name: SunlayGGX/Storm

max_forks_repo_name: SunlayGGX/Storm


text:
// Copyright Abel Sinkovics (abel@sinkovics.hu) 2013.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <mpllibs/metamonad/value_to_stream.hpp>
#include <mpllibs/metamonad/tmp_value.hpp>

#include <boost/test/unit_test.hpp>

#include <boost/mpl/assert.hpp>

#include <string>
#include <iostream>
#include <sstream>

using mpllibs::metamonad::tmp_value;

namespace
{
  struct with_const_value : tmp_value<with_const_value>
  {
    static const int value;
  };

  const int with_const_value::value = 1;

  struct with_enum_value : tmp_value<with_enum_value>
  {
    enum { value = 2 };
  };

  struct with_get_value : tmp_value<with_get_value>
  {
    static int get_value() { return 3; }
  };

  struct with_value_and_get_value : tmp_value<with_value_and_get_value>
  {
    static int get_value() { return 4; }
    enum { value = 5 };
  };

  struct without_value : tmp_value<without_value> {};

  template <class T>
  std::string to_string()
  {
    std::ostringstream s;
    mpllibs::metamonad::value_to_stream<T>::run(s);
    return s.str();
  }
}

BOOST_AUTO_TEST_CASE(test_value_to_stream)
{
  BOOST_CHECK_EQUAL("1", to_string<with_const_value>());
  BOOST_CHECK_EQUAL("2", to_string<with_enum_value>());
  BOOST_CHECK_EQUAL("3", to_string<with_get_value>());
  BOOST_CHECK_EQUAL("4", to_string<with_value_and_get_value>());
  BOOST_CHECK_EQUAL("???", to_string<without_value>());
}

ext: cpp


text:
//
// Created by SashiRin on 2019-02-09.
//

#ifndef SASHI_TINY_HTTP_RESPONSE_HPP
#define SASHI_TINY_HTTP_RESPONSE_HPP

#include <vector>
#include <string>
#include <boost/asio.hpp>
#include "utility.hpp"
#include "status_code.hpp"

namespace sashi_tiny_http {

    class Response {
    public:
        StatusCode status = StatusCode::unknown;

        std::string status_line;

        std::vector<HeaderItem> headers;

        std::string content;

        std::vector<boost::asio::const_buffer> ConvertToBuffers();

        static Response StockReply(StatusCode status);

    };

}

#endif //SASHI_TINY_HTTP_RESPONSE_HPP

ext: hpp


text:
/**
 * Copyright Soramitsu Co., Ltd. All Rights Reserved.
 * SPDX-License-Identifier: Apache-2.0
 */

#ifndef KAGOME_BLOCK_TREE_IMPL_HPP
#define KAGOME_BLOCK_TREE_IMPL_HPP

#include "blockchain/block_tree.hpp"

#include <boost/optional.hpp>
#include <functional>
#include <memory>
#include <queue>
#include <unordered_set>

#include "blockchain/block_header_repository.hpp"
#include "blockchain/block_storage.hpp"
#include "blockchain/impl/common.hpp"
#include "common/logger.hpp"
#include "crypto/hasher.hpp"
#include "network/extrinsic_observer.hpp"
#include "transaction_pool/transaction_pool.hpp"

namespace kagome::blockchain {
  /**
   * Block tree implementation
   */
  class BlockTreeImpl : public BlockTree {
    /**
     * In-memory light representation of the tree, used for efficiency and usage
     * convenience - we would only ask the database for some info, when directly
     * requested
     */
    struct TreeNode : public std::enable_shared_from_this<TreeNode> {
      TreeNode(primitives::BlockHash hash,
               primitives::BlockNumber depth,
               const std::shared_ptr<TreeNode> &parent,
               bool finalized = false);

      primitives::BlockHash block_hash;
      primitives::BlockNumber depth;

      std::weak_ptr<TreeNode> parent;

      bool finalized;

      std::vector<std::shared_ptr<TreeNode>> children{};

      /**
       * Get a node of the tree, containing block with the specified hash, if it
       * can be found
       */
      std::shared_ptr<TreeNode> getByHash(const primitives::BlockHash &hash);

      bool operator==(const TreeNode &other) const;
      bool operator!=(const TreeNode &other) const;
    };

    /**
     * Useful information about the tree & blocks it contains to make some of
     * the operations faster
     */
    struct TreeMeta {
      explicit TreeMeta(TreeNode &subtree_root_node);

      TreeMeta(std::unordered_set<primitives::BlockHash> leaves,
               TreeNode &deepest_leaf,
               TreeNode &last_finalized);

      std::unordered_set<primitives::BlockHash> leaves;
      std::reference_wrapper<TreeNode> deepest_leaf;

      std::reference_wrapper<TreeNode> last_finalized;
    };

   public:
    enum class Error {
      // target block number is past the given maximum number
      TARGET_IS_PAST_MAX = 1,
      // block resides on a dead fork
      BLOCK_ON_DEAD_END,
      // block exists in chain but not found when following all
      // leaves backwards.
      BLOCK_NOT_FOUND
    };

    /**
     * Create an instance of block tree
     * @param header_repo - block headers repository
     * @param storage - block storage for the tree to be put in
     * @param last_finalized_block - last finalized block, from which the tree
     * is going to grow
     * @param hasher - pointer to the hasher
     * @return ptr to the created instance or error
     */
    static outcome::result<std::shared_ptr<BlockTreeImpl>> create(
        std::shared_ptr<BlockHeaderRepository> header_repo,
        std::shared_ptr<BlockStorage> storage,
        const primitives::BlockId &last_finalized_block,
        std::shared_ptr<network::ExtrinsicObserver> extrinsic_observer,
        std::shared_ptr<crypto::Hasher> hasher);

    ~BlockTreeImpl() override = default;

    outcome::result<primitives::BlockHeader> getBlockHeader(
        const primitives::BlockId &block) const override;

    outcome::result<primitives::BlockBody> getBlockBody(
        const primitives::BlockId &block) const override;

    outcome::result<primitives::Justification> getBlockJustification(
        const primitives::BlockId &block) const override;

    outcome::result<void> addBlockHeader(
        const primitives::BlockHeader &header) override;

    outcome::result<void> addBlock(const primitives::Block &block) override;

    outcome::result<void> addBlockBody(
        primitives::BlockNumber block_number,
        const primitives::BlockHash &block_hash,
        const primitives::BlockBody &body) override;

    outcome::result<void> finalize(
        const primitives::BlockHash &block,
        const primitives::Justification &justification) override;

    BlockHashVecRes getChainByBlock(
        const primitives::BlockHash &block) override;

    BlockHashVecRes getChainByBlock(const primitives::BlockHash &block,
                                    bool ascending,
                                    uint64_t maximum) override;
    BlockHashVecRes getChainByBlocks(
        const primitives::BlockHash &top_block,
        const primitives::BlockHash &bottom_block) override;

    BlockHashVecRes longestPath() override;

    primitives::BlockInfo deepestLeaf() const override;

    outcome::result<primitives::BlockInfo> getBestContaining(
        const primitives::BlockHash &target_hash,
        const boost::optional<primitives::BlockNumber> &max_number)
        const override;

    std::vector<primitives::BlockHash> getLeaves() const override;

    BlockHashVecRes getChildren(const primitives::BlockHash &block) override;

    primitives::BlockInfo getLastFinalized() const override;

   private:
    /**
     * Private constructor, so that instances are created only through the
     * factory method
     */
    BlockTreeImpl(
        std::shared_ptr<BlockHeaderRepository> header_repo,
        std::shared_ptr<BlockStorage> storage,
        std::shared_ptr<TreeNode> tree,
        std::shared_ptr<TreeMeta> meta,
        std::shared_ptr<network::ExtrinsicObserver> extrinsic_observer,
        std::shared_ptr<crypto::Hasher> hasher);

    /**
     * Walks the chain backwards starting from \param start until the current
     * block number is less or equal than \param limit
     */
    outcome::result<primitives::BlockHash> walkBackUntilLess(
        const primitives::BlockHash &start,
        const primitives::BlockNumber &limit) const;

    /**
     * @returns the tree leaves sorted by their depth
     */
    std::vector<primitives::BlockHash> getLeavesSorted() const;

    static void collectDescendants(
        std::shared_ptr<TreeNode> node,
        std::vector<std::pair<primitives::BlockHash, primitives::BlockNumber>>
            &container);

    outcome::result<void> prune(
        const std::shared_ptr<TreeNode> &lastFinalizedNode);

    std::shared_ptr<BlockHeaderRepository> header_repo_;
    std::shared_ptr<BlockStorage> storage_;

    std::shared_ptr<TreeNode> tree_;
    std::shared_ptr<TreeMeta> tree_meta_;

    std::shared_ptr<network::ExtrinsicObserver> extrinsic_observer_;

    std::shared_ptr<crypto::Hasher> hasher_;
    common::Logger log_ = common::createLogger("BlockTreeImpl");
  };
}  // namespace kagome::blockchain

OUTCOME_HPP_DECLARE_ERROR(kagome::blockchain, BlockTreeImpl::Error);

#endif  // KAGOME_BLOCK_TREE_IMPL_HPP

max_stars_repo_name: iceseer/kagome

max_issues_repo_name: iceseer/kagome

max_forks_repo_name: iceseer/kagome


text:
#include "MultiKalmanBallLocator.h"

#include <Eigen/Core>
#include "Tools/Association.h"

MultiKalmanBallLocator::MultiKalmanBallLocator()
{
    // Modify number of models
    DEBUG_REQUEST_REGISTER("MultiKalmanBallLocator:remove_all_models",     "remove all models",                                                             false);
    DEBUG_REQUEST_REGISTER("MultiKalmanBallLocator:allow_just_one_model",  "allows only one model to be generated (all updates are applied to that model)", false);

    // Debug Drawings
    DEBUG_REQUEST_REGISTER("MultiKalmanBallLocator:draw_real_ball_percept",          "draw the real incomming ball percept",                               false);
    DEBUG_REQUEST_REGISTER("MultiKalmanBallLocator:draw_ball_on_field_before",       "draw the modelled ball on the field before prediction and update",   false);
    DEBUG_REQUEST_REGISTER("MultiKalmanBallLocator:draw_ball_on_field",              "draw the modelled ball on the field before update",                  false);
    DEBUG_REQUEST_REGISTER("MultiKalmanBallLocator:draw_ball_on_field_after",        "draw the modelled ball on the field after prediction and update",    false);
    DEBUG_REQUEST_REGISTER("MultiKalmanBallLocator:draw_assignment",                 "draws the assignment of the ball percept to the filter",             false);
    DEBUG_REQUEST_REGISTER("MultiKalmanBallLocator:draw_final_ball",                 "draws the final i.e. best model",                                    false);
    DEBUG_REQUEST_REGISTER("MultiKalmanBallLocator:draw_final_ball_postion_at_rest", "draws the final i.e. best model's rest position",                    false);
    DEBUG_REQUEST_REGISTER("MultiKalmanBallLocator:draw_covariance_ellipse",         "draws the ellipses representing the covariances",                    false);
    DEBUG_REQUEST_REGISTER("MultiKalmanBallLocator:draw_last_known_ball",            "draws the last known ball", 	                                   false);

    // Plotting Related Debug Requests
    DEBUG_REQUEST_REGISTER("MultiKalmanBallLocator:plot_prediction_error",     "plots the prediction errors in x (horizontal angle) and y (vertical angle)", false);

    // Update Association Function Debug Requests
    DEBUG_REQUEST_REGISTER("MultiKalmanBallLocator:UpdateAssociationFunction:useEuclid",            "minimize Euclidian distance in measurement space",                                false);
    DEBUG_REQUEST_REGISTER("MultiKalmanBallLocator:UpdateAssociationFunction:useMahalanobis",       "minimize Mahalanobis distance in measurement space (no common covarince matrix)", false);
    DEBUG_REQUEST_REGISTER("MultiKalmanBallLocator:UpdateAssociationFunction:useMaximumLikelihood", "maximize likelihood of measurement in measurement space ",                        true );

    // Parameter Related Debug Requests
    DEBUG_REQUEST_REGISTER("MultiKalmanBallLocator:reloadParameters",          "reloads the kalman filter parameters from the kfParameter object", false);

    DEBUG_REQUEST_REGISTER("MultiKalmanBallLocator:draw_trust_the_ball", "..", false);

    h.ballRadius = getFieldInfo().ballRadius;

    updateAssociationFunction = &likelihood;

    getDebugParameterList().add(&params);

    filter.reserve(10);  // set capacity for improved performance

    reloadParameters();
}

MultiKalmanBallLocator::~MultiKalmanBallLocator()
{
    getDebugParameterList().remove(&params);
}

void MultiKalmanBallLocator::execute()
{
    // allways reset the model first
    getBallModel().reset();

    // HACK: no updates in ready or when lifted
    if(getPlayerInfo().robotState == PlayerInfo::ready || getBodyState().isLiftedUp) {
      filter.clear();
      return;
    }

    DEBUG_REQUEST("MultiKalmanBallLocator:remove_all_models",
        filter.clear();
    );

    // delete some filter if they are too bad
    // NOTE: make sure at least one filter remains, so filter is not empty
    Filters::iterator iter = filter.begin();
    while(iter != filter.end() && filter.size() > 1) {
      // TODO: here we make a linear approximation depending n the distance - does it make problems? Can we have a better solution?
      double distance = Vector2d(iter->getState()(0), iter->getState()(2)).abs();
      double threshold_radius = params.area95Threshold_radius.factor * distance + params.area95Threshold_radius.offset;
      // HACK: we need ballSeenFilter here because the other condition didn't seem to work as expected
      // compare area of the position uncertainty ellipse with a circle representing the maximum uncertainty a ball can have
      // Note: pi is avoided on both sides of the inequality
      if(!iter->ballSeenFilter.value() && (*iter).getEllipseLocation().major * (*iter).getEllipseLocation().minor > threshold_radius*threshold_radius){
          iter = filter.erase(iter);
      } else {
          ++iter;
      }
    }

    // apply odometry on the filter state, to keep it in the robot's local coordinate system
    for(Filters::iterator iter = filter.begin(); iter != filter.end(); ++iter) {
        applyOdometryOnFilterState(*iter);
    }

    doDebugRequestBeforPredictionAndUpdate();

    // prediction
    double dt = getFrameInfo().getTimeInSeconds() - lastFrameInfo.getTimeInSeconds();
    ASSERT(dt > 0);
    for(Filters::iterator iter = filter.begin(); iter != filter.end(); ++iter) {
      predict(*iter, dt);
    }

    doDebugRequestBeforUpdate();

    // sensor update
    if(params.association.use_normal) {
        updateByPerceptsNormal();
    } else if(params.association.use_cool) {
        updateByPerceptsCool();
    } else if(params.association.use_greedy) {
        // need to handle bottom and top percepts independently because both cameras can observe the same ball
        updateByPerceptsGreedy(CameraInfo::Bottom);
        updateByPerceptsGreedy(CameraInfo::Top);
    }

    // NOTE: (Heinrich) update the "ball seen" values
    for(Filters::iterator iter = filter.begin(); iter != filter.end(); ++iter) {
      bool updated = iter->getLastUpdateFrame().getFrameNumber() == getFrameInfo().getFrameNumber();
      (*iter).ballSeenFilter.setParameter(params.g0, params.g1);
      (*iter).ballSeenFilter.update(updated, 0.3, 0.7);
    }

    // estimate the best model
    if(params.use_covariance_based_selection) {
        bestModel = selectBestModelBasedOnCovariance();
    } else {
        bestModel = selectBestModel();
    }

    // fill the ball model representation
    if (bestModel != filter.end()) {
      provideBallModel(*bestModel);
    }

    doDebugRequest();

    lastFrameInfo     = getFrameInfo();
    lastRobotOdometry = getOdometryData();
}

void MultiKalmanBallLocator::updateByPerceptsNormal()
{
  // measurement
    for(MultiBallPercept::ConstABPIterator iter = getMultiBallPercept().begin(); iter != getMultiBallPercept().end(); iter++) {

        Eigen::Vector2d z;
        Vector2d p;

        // set correct camera matrix and info in functional
        if((*iter).cameraId == CameraInfo::Bottom)
        {
            //PLOT("MultiKalmanBallLocator:Measurement:Bottom:horizontal", z(0));
            //PLOT("MultiKalmanBallLocator:Measurement:Bottom:vertical",   z(1));

            h.camMat  = getCameraMatrix();
            h.camInfo = getCameraInfo();
        }
        else
        {
            //PLOT("MultiKalmanBallLocator:Measurement:Top:horizontal", z(0));
            //PLOT("MultiKalmanBallLocator:Measurement:Top:vertical",   z(1));

            h.camMat  = getCameraMatrixTop();
            h.camInfo = getCameraInfoTop();
        }

        // tansform measurement into angles
        Vector2d angles = CameraGeometry::pixelToAngles(h.camInfo,(*iter).centerInImage.x,(*iter).centerInImage.y);
        z << angles.x, angles.y;

        // needed if a new filter has to be created
        p = (*iter).positionOnField;

        // find best matching filter
        updateAssociationFunction->determineBestPredictor(filter, z, h);

        Filters::iterator bestPredictor = updateAssociationFunction->getBestPredictor();

        bool allowJustOneModel = false;
        DEBUG_REQUEST("MultiKalmanBallLocator:allow_just_one_model",
            allowJustOneModel = true;
        );

        // if no suitable filter found create a new one
        if((!allowJustOneModel && !updateAssociationFunction->inRange()) || bestPredictor == filter.end())
        {
            Eigen::Vector4d newState;
            newState << p.x, 0, p.y, 0;
            filter.push_back(BallHypothesis(getFrameInfo(), newState, processNoiseStdSingleDimension, measurementNoiseCovariances, initialStateStdSingleDimension));
        }
        else
        {
            DEBUG_REQUEST("MultiKalmanBallLocator:draw_assignment",
                FIELD_DRAWING_CONTEXT;
                PEN("FF0000", 10);
                const Eigen::Vector4d state = (*bestPredictor).getState();
                LINE(p.x,p.y,state(0),state(2));
                TEXT_DRAWING((p.x+state(0))/2,(p.y+state(2))/2,updateAssociationFunction->getScore());
            );

            // debug stuff -> should be in a DEBUG_REQUEST
            DEBUG_REQUEST("MultiKalmanBallLocator:plot_prediction_error",
                Eigen::Vector2d prediction_error;
                prediction_error = z - (*bestPredictor).getStateInMeasurementSpace(h);

                PLOT("MultiKalmanBallLocator:Innovation:x", prediction_error(0));
                PLOT("MultiKalmanBallLocator:Innovation:y", prediction_error(1));
            );

            (*bestPredictor).update(z,h,getFrameInfo());
        }
    }// end for
}

/*
 * Perform a greedy 1:1-matching of filters (hypothesis) and measurements (ball percept).
 * The association depends on the horizontal and vertical angles of the percept and
 * the hypothesis in the image. Each combination is assigned a score. The combination
 * with the best score is choosen and the filter updated with the measurement.
 * After that the second best and so on...
 */
void MultiKalmanBallLocator::updateByPerceptsGreedy(CameraInfo::CameraID camera)
{
    // set correct camera matrix and info in functional
    if(camera == CameraInfo::Bottom) {
        h.camMat  = getCameraMatrix();
        h.camInfo = getCameraInfo();
    } else {
        h.camMat  = getCameraMatrixTop();
        h.camInfo = getCameraInfoTop();
    }

    // phase 1: filter percepts
    std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > measurements;
    std::vector<Vector2d> positions;

    for(const MultiBallPercept::BallPercept& percept: getMultiBallPercept().getPercepts()){
        Eigen::Vector2d z;

        if(percept.cameraId == camera){
            // transform measurement into angles
            Vector2d angles = CameraGeometry::pixelToAngles(h.camInfo, percept.centerInImage.x, percept.centerInImage.y);
            z << angles.x, angles.y;
            measurements.push_back(z);

            // needed if a new filter has to be created
            positions.push_back(percept.positionOnField);
        }
    }

    // abort if there are no measurements for the current camera
    if(measurements.empty()) return;

    // if no filter exists, skip directly to phase 4
    if(!filter.empty())
    {
        // phase 2: calculate #measurement x #filter score-matrix
        // Note: it depends on the association function whether a low or a high score is better
        Eigen::MatrixXd scores = Eigen::MatrixXd::Zero(measurements.size(), filter.size());
        for(size_t i = 0; i < measurements.size(); ++i){
            for(size_t j = 0; j < filter.size(); ++j){
                scores(i,j) = updateAssociationFunction->associationScore(filter[j], measurements[i], h);
            }
        }

        // create index vector for filters
        std::vector<int> filterIndices;
        for(size_t i = 0; i < filter.size(); ++i){
            filterIndices.push_back((int)i);
        }

        // phase 3: greedyly update each filter by best matching percept
        while(!(filterIndices.empty() || measurements.empty())){
            // phase 3.1: find location of best entry
            // which is depending on the updateAssociationFunction a global maximum or global minimum
            Eigen::Index maxColIdx, minColIdx, maxRowIdx, minRowIdx, bestColIdx, bestRowIdx;
            double max = scores.maxCoeff(&maxRowIdx, &maxColIdx);
            double min = scores.minCoeff(&minRowIdx, &minColIdx);

            if(updateAssociationFunction->better(min, max)){
                bestColIdx = minColIdx;
                bestRowIdx = minRowIdx;
            } else {
                bestColIdx = maxColIdx;
                bestRowIdx = maxRowIdx;
            }

            // phase 3.2: update filter
            bool allowJustOneModel = false;
            DEBUG_REQUEST("MultiKalmanBallLocator:allow_just_one_model",
                allowJustOneModel = true;
            );

            // if the score is not in the valid range continue with phase 4
            // because no other possible matching could be better than the current one
            if(!allowJustOneModel && !updateAssociationFunction->inRange(scores(bestRowIdx,bestColIdx))) {
                break;
            } else {
                DEBUG_REQUEST("MultiKalmanBallLocator:draw_assignment",
                    FIELD_DRAWING_CONTEXT;
                    PEN("FF0000", 10);
                    const Eigen::Vector4d state = filter[filterIndices[bestColIdx]].getState();
                    LINE(positions[bestRowIdx].x, positions[bestRowIdx].y, state(0), state(2));
                    TEXT_DRAWING((positions[bestRowIdx].x + state(0)) / 2, (positions[bestRowIdx].y + state(2)) / 2, scores(bestRowIdx,bestColIdx));
                );

                DEBUG_REQUEST("MultiKalmanBallLocator:plot_prediction_error",
                    Eigen::Vector2d prediction_error;
                    prediction_error = measurements[bestRowIdx] - filter[filterIndices[bestColIdx]].getStateInMeasurementSpace(h);

                    PLOT("MultiKalmanBallLocator:Innovation:x", prediction_error(0));
                    PLOT("MultiKalmanBallLocator:Innovation:y", prediction_error(1));
                );

                filter[filterIndices[bestColIdx]].update(measurements[bestRowIdx], h, getFrameInfo());
            }

            // phase 3.3: remove matching pair of hypothesis and measurement
            // from the set of matchable options, that means:
            // - remove bestColIdx from filterIndices
            // - remove the measurement and its position from measurements and positions
            // - remove their corresponding row and column in the score matrix
            filterIndices.erase(filterIndices.begin() + bestColIdx);
            measurements.erase(measurements.begin() + bestRowIdx);
            positions.erase(positions.begin() + bestRowIdx);

            Eigen::Index numRowsAfterResize = scores.rows() - 1;
            Eigen::Index numColsAfterResize = scores.cols() - 1;
            Eigen::Index numRowsAfterBestRow = numRowsAfterResize - bestRowIdx;
            Eigen::Index numColsAfterBestCol = numColsAfterResize - bestColIdx;

            // delete the row by moving all rows after the best row one row up and resize the matrix accordingly
            if(bestRowIdx < numRowsAfterResize)
                scores.block(bestRowIdx, 0, numRowsAfterBestRow, scores.cols()) = scores.bottomRows(numRowsAfterBestRow).eval();
            scores.conservativeResize(numRowsAfterResize, scores.cols());

            // delete the column by moving all columns right of the best column one column to the left and resize the matrix accordingly
            if(bestColIdx < numColsAfterResize)
                scores.block(0, bestColIdx, numRowsAfterResize, numColsAfterBestCol) = scores.rightCols(numColsAfterBestCol).eval();
            scores.conservativeResize(numRowsAfterResize, numColsAfterResize);
        }
    }

    // phase 4: create new filter for each percept which couldn't be associated with a filter
    // Diskussion: vielleicht filter erstellen wenn keine da ist und dann die zweite beobachtung dazu matchen ...
    for(const Vector2d& p : positions){
        Eigen::Vector4d newState;
        newState << p.x, 0, p.y, 0;
        // add BallHypothesis to filter vector
        filter.emplace_back(getFrameInfo(), newState, processNoiseStdSingleDimension, measurementNoiseCovariances, initialStateStdSingleDimension);
    }

    // TODO maybe add possibility to merge filters
    // TODO maybe add possibility to apply update of observation to every filter in a bayesian fashion
}

void MultiKalmanBallLocator::updateByPerceptsCool()
{
  // update by percepts
  // A ~ goal posts
  // B ~ hypotheses
  Assoziation sensorAssoziation(static_cast<int>(getMultiBallPercept().getPercepts().size()), static_cast<int>(filter.size()));

  int i = 0;
  for(MultiBallPercept::ConstABPIterator iter = getMultiBallPercept().begin(); iter != getMultiBallPercept().end(); iter++)
  {
      // set correct camera matrix and info in functional
      if((*iter).cameraId == CameraInfo::Bottom) {
          h.camMat  = getCameraMatrix();
          h.camInfo = getCameraInfo();
      } else {
          h.camMat  = getCameraMatrixTop();
          h.camInfo = getCameraInfoTop();
      }

      // tansform measurement into angles
      Vector2d angles = CameraGeometry::pixelToAngles(h.camInfo,(*iter).centerInImage.x,(*iter).centerInImage.y);
      Eigen::Vector2d z;
      z << angles.x, angles.y;

      int x = 0;
      for(Filters::iterator iter = filter.begin(); iter != filter.end(); ++iter)
      {
        double confidence = updateAssociationFunction->associationScore(*iter, z, h);

        // store best association for measurement
        if(confidence > updateAssociationFunction->getThreshold() &&
          confidence > sensorAssoziation.getW4A(i) &&
          confidence > sensorAssoziation.getW4B(x))
        {
          sensorAssoziation.addAssociation(i, x, confidence);
        }
        x++;
      }
      i++;
  }

  i = 0;
  for(MultiBallPercept::ConstABPIterator iter = getMultiBallPercept().begin(); iter != getMultiBallPercept().end(); iter++)
  {
    int x = sensorAssoziation.getB4A(i); // get hypothesis for measurement
    if (x != -1)
    {
      // set correct camera matrix and info in functional
      if((*iter).cameraId == CameraInfo::Bottom) {
          h.camMat  = getCameraMatrix();
          h.camInfo = getCameraInfo();
      } else {
          h.camMat  = getCameraMatrixTop();
          h.camInfo = getCameraInfoTop();
      }

      // tansform measurement into angles
      Vector2d angles = CameraGeometry::pixelToAngles(h.camInfo,(*iter).centerInImage.x,(*iter).centerInImage.y);
      Eigen::Vector2d z;
      z << angles.x, angles.y;

      filter[x].update(z,h,getFrameInfo());
    }
    else
    {
      Vector2d p = (*iter).positionOnField;
      Eigen::Vector4d newState;
      newState << p.x, 0, p.y, 0;
      filter.push_back(BallHypothesis(getFrameInfo(), newState, processNoiseStdSingleDimension, measurementNoiseCovariances, initialStateStdSingleDimension));
    }
    i++;
  }

}

void MultiKalmanBallLocator::predict(ExtendedKalmanFilter4d& filter, double dt) const
{
    /*
        rolling resistance = rolling resitance coefficient * normal force
        F_R = d / R * F_N

        F_N = g * weight
        g = 9.81
        weight = ballMass

        deceleration = F_R/ballMass
    */

    const Eigen::Vector4d& x = filter.getState();
    Eigen::Vector2d vel; // control vector
    vel <<  x(1), x(3);
    double abs_velocity = vel.norm();

    double time_until_vel_zero = 0;

    if(abs_velocity > epsilon){
        time_until_vel_zero = -abs_velocity/getFieldInfo().ballDeceleration;
    }

    if(time_until_vel_zero < epsilon)
    {
        filter.predict(Eigen::Vector2d::Zero(),dt);
    } else {
        // ballDeceleration is negative so the deceleration will be in opposite direction of current velocity
        Eigen::Vector2d u = vel.normalized() * getFieldInfo().ballDeceleration;
        if(time_until_vel_zero >= dt) {
            filter.predict(u,dt);
        } else {
            filter.predict(u, time_until_vel_zero);
            dt -= time_until_vel_zero;
            filter.predict(Eigen::Vector2d::Zero(), dt);
        }
    }

    return;
}

void MultiKalmanBallLocator::applyOdometryOnFilterState(ExtendedKalmanFilter4d& filter)
{
    const Eigen::Vector4d& x = filter.getState();
    Pose2D odometryDelta = lastRobotOdometry - getOdometryData();

    Eigen::Vector4d newStateX;
    newStateX << x;

    // construct rotation matrix
    double s = sin(odometryDelta.getAngle());
    double c = cos(odometryDelta.getAngle());

    Eigen::Matrix4d rotation;
    rotation << c, 0, -s,  0,
                0, c,  0, -s,
                s, 0,  c,  0,
                0, s,  0,  c;

    newStateX = rotation*newStateX;

    // translate the location of the filters state (velocity is relative so translating it doesn't make sense)
    newStateX(0) = newStateX(0) + odometryDelta.translation.x;
    newStateX(2) = newStateX(2) + odometryDelta.translation.y;

    // rotate P (translation doesn't affect the covariances)
    Eigen::Matrix4d P = filter.getProcessCovariance();

    Eigen::Matrix4d new_P = rotation * P * rotation.transpose();

    filter.setState(newStateX);
    filter.setCovarianceOfState(new_P);
}

// TODO: returns the first model as best model even if it is "not seen"
//		 it might be better to return an invalid iterator and handle this case outside
//		 handling this better might make last_known_ball symbol obsolete
MultiKalmanBallLocator::Filters::const_iterator MultiKalmanBallLocator::selectBestModel() const
{
  // find the best model for the ball: closest hypothesis that is "known"
  Filters::const_iterator bestModel = filter.end();
  double minDistance = 0;

  for(Filters::const_iterator iter = filter.begin(); iter != filter.end(); ++iter) {
    double distance = Vector2d(iter->getState()(0), iter->getState()(2)).abs();
    if( bestModel == filter.end() ||
       (iter->ballSeenFilter.value() && !bestModel->ballSeenFilter.value()) ||
       (iter->ballSeenFilter.value() == bestModel->ballSeenFilter.value() && distance < minDistance))
    {
      bestModel = iter;
      minDistance = distance;
    }
  }

  return bestModel;
}

MultiKalmanBallLocator::Filters::const_iterator MultiKalmanBallLocator::selectBestModelBasedOnCovariance() const
{
  Filters::const_iterator bestModel = filter.end();
  double value = 0;

  // find the best seen model for the ball based on covariance
  for(Filters::const_iterator iter = filter.begin(); iter != filter.end(); ++iter) {
      double temp = iter->getEllipseLocation().major * iter->getEllipseLocation().minor * Math::pi;
      if(bestModel == filter.end()
         || (iter->ballSeenFilter.value() && !bestModel->ballSeenFilter.value())
         || (iter->ballSeenFilter.value() == bestModel->ballSeenFilter.value() && temp < value)) {
          bestModel = iter;
          value = temp;
      }
  }

  return bestModel;
}

void MultiKalmanBallLocator::provideBallModel(const BallHypothesis& model)
{
  getBallModel().valid = true;
  getBallModel().knows = model.ballSeenFilter.value();
  getBallModel().setFrameInfoWhenBallWasSeen(model.getLastUpdateFrame());

  // set ball model representation
  const Eigen::Vector4d& x = model.getState();
  getBallModel().position.x = x(0);
  getBallModel().position.y = x(2);
  getBallModel().speed.x = x(1);
  getBallModel().speed.y = x(3);

  // transform ball model into feet coordinates
  const Pose3D& lFoot = getKinematicChain().theLinks[KinematicChain::LFoot].M;
  const Pose3D& rFoot = getKinematicChain().theLinks[KinematicChain::RFoot].M;
  Vector2d ballLeftFoot  = lFoot.projectXY()/getBallModel().position;
  Vector2d ballRightFoot = rFoot.projectXY()/getBallModel().position;

  //set preview ball model representation
  getBallModel().positionPreview = getMotionStatus().plannedMotion.hip / getBallModel().position;
  getBallModel().positionPreviewInLFoot = getMotionStatus().plannedMotion.lFoot / ballLeftFoot;
  getBallModel().positionPreviewInRFoot = getMotionStatus().plannedMotion.rFoot / ballRightFoot;

  // determine rest position
  Eigen::Vector2d vel;
  vel <<  x(1), x(3);
  double abs_velocity = vel.norm();

  BallHypothesis modelCopy(model);
  if(abs_velocity > epsilon){
      double time_until_vel_zero = -abs_velocity / getFieldInfo().ballDeceleration;
      Eigen::Vector2d u = vel.normalized() * getFieldInfo().ballDeceleration;
      modelCopy.predict(u, time_until_vel_zero);

      DEBUG_REQUEST("MultiKalmanBallLocator:draw_final_ball_postion_at_rest",
          FIELD_DRAWING_CONTEXT;
          PEN(Color(Color::black), 20);
          CIRCLE(modelCopy.getState()(0), modelCopy.getState()(2), getFieldInfo().ballRadius-10);
      );
  }

  // set position at rest
  getBallModel().position_at_rest.x = modelCopy.getState()(0);
  getBallModel().position_at_rest.y = modelCopy.getState()(2);

  // update last known ball
  if(getBallModel().knows) {
    getBallModel().last_known_ball = getBallModel().position;
  } else {
    // need to update odometry by hand ...
    Pose2D odometryDelta = lastRobotOdometry - getOdometryData();
    getBallModel().last_known_ball = odometryDelta * getBallModel().last_known_ball;
  }

  // some final debug stuff
  PLOT("MultiKalmanBallLocator:ballSeenFilter", model.ballSeenFilter.floatValue());
}

void MultiKalmanBallLocator::doDebugRequestBeforPredictionAndUpdate()
{
    DEBUG_REQUEST("MultiKalmanBallLocator:draw_ball_on_field_before",
        drawFiltersOnField();
    );

    DEBUG_REQUEST("MultiKalmanBallLocator:UpdateAssociationFunction:useEuclid",
        updateAssociationFunction = &euclid;
    );

    DEBUG_REQUEST("MultiKalmanBallLocator:UpdateAssociationFunction:useMahalanobis",
        updateAssociationFunction = &mahalanobis;
    );

    DEBUG_REQUEST("MultiKalmanBallLocator:UpdateAssociationFunction:useMaximumLikelihood",
        updateAssociationFunction = &likelihood;
    );
}

void MultiKalmanBallLocator::doDebugRequestBeforUpdate()
{
    DEBUG_REQUEST("MultiKalmanBallLocator:draw_ball_on_field",
        drawFiltersOnField();
    );
}

void MultiKalmanBallLocator::doDebugRequest()
{
    //PLOT("MultiKalmanBallLocator:ModelIsValid", getBallModel().valid);

    //to check correctness of the prediction
    DEBUG_REQUEST("MultiKalmanBallLocator:draw_real_ball_percept",
      if(getMultiBallPercept().wasSeen()) {
        FIELD_DRAWING_CONTEXT;
        PEN("FF0000", 10);
        for(MultiBallPercept::ConstABPIterator iter = getMultiBallPercept().begin(); iter != getMultiBallPercept().end(); iter++) {
            CIRCLE((*iter).positionOnField.x, (*iter).positionOnField.y, getFieldInfo().ballRadius-5);
        }
      }
    );

    DEBUG_REQUEST("MultiKalmanBallLocator:draw_ball_on_field_after",
        drawFiltersOnField();
    );

    DEBUG_REQUEST("MultiKalmanBallLocator:draw_final_ball",
        FIELD_DRAWING_CONTEXT;
        PEN("FF0000", 10);
        CIRCLE( getBallModel().position.x, getBallModel().position.y, getFieldInfo().ballRadius-10);
    );

    DEBUG_REQUEST("MultiKalmanBallLocator:draw_last_known_ball",
        FIELD_DRAWING_CONTEXT;
        PEN("EF871E", 10);
        CIRCLE(getBallModel().last_known_ball.x, getBallModel().last_known_ball.y, getFieldInfo().ballRadius-10);
    );

    DEBUG_REQUEST("MultiKalmanBallLocator:reloadParameters",
        reloadParameters();
    );

    DEBUG_REQUEST("MultiKalmanBallLocator:draw_trust_the_ball",
        FIELD_DRAWING_CONTEXT;
        for(Filters::const_iterator iter = filter.begin(); iter != filter.end(); iter++)
        {
          if((*iter).ballSeenFilter.value()) {
            PEN("00FF00", 10);
          } else {
            PEN("FF0000", 10);
          }

          const Eigen::Vector4d& state = (*iter).getState();
          CIRCLE( state(0), state(2), (*iter).ballSeenFilter.value()*1000);
        }
    );
}

void MultiKalmanBallLocator::drawFilter(const BallHypothesis& bh, const Color& model_color, Color cov_loc_color, Color cov_vel_color) const
{
    bool draw_covariances = false;
    DEBUG_REQUEST("MultiKalmanBallLocator:draw_covariance_ellipse",
        draw_covariances = true;
    );

    if(!draw_covariances) {
        cov_loc_color[cov_loc_color.Alpha] = 0;
        cov_vel_color[cov_vel_color.Alpha] = 0;
    }

    PEN(model_color.toString(),20);

    const Eigen::Vector4d& state = bh.getState();

    CIRCLE( state(0), state(2), getFieldInfo().ballRadius-10);
    ARROW( state(0), state(2),
           state(0)+state(1),
           state(2)+state(3));

    PEN(cov_loc_color.toString(), 20);
    const Ellipse2d& ellipse_loc = bh.getEllipseLocation();
    OVAL_ROTATED(state(0),
                 state(2),
                 ellipse_loc.minor,
                 ellipse_loc.major,
                 ellipse_loc.angle);

    PEN(cov_vel_color, 20);
    const Ellipse2d& ellipse_vel = bh.getEllipseVelocity();
    OVAL_ROTATED(state(0)+state(1),
                 state(2)+state(3),
                 ellipse_vel.minor,
                 ellipse_vel.major,
                 ellipse_vel.angle);

    if (!draw_covariances) {
      cov_loc_color[cov_loc_color.Alpha] = 1;
      cov_vel_color[cov_vel_color.Alpha] = 1;
    }
}

void MultiKalmanBallLocator::drawFiltersOnField() const
{
    FIELD_DRAWING_CONTEXT;

    Color cov_loc_color("00FFFF");  // cyan
    Color cov_vel_color("FF00FF");  // pink
    Color model_color;

    for(Filters::const_iterator iter = filter.begin(); iter != filter.end(); iter++) {
        if(getBallModel().valid) {
            if((*iter).getLastUpdateFrame().getTime() == getFrameInfo().getTime()) {
                if(bestModel == iter)
                    model_color = "99FF00";  // green
                else
                    model_color = "FF9900";  // orange
            } else {
                    model_color = "0099FF";  // light blue
            }
        } else {
            model_color = "999999";  // grey
        }

        drawFilter(*iter, model_color, cov_loc_color, cov_vel_color);
    }
}

void MultiKalmanBallLocator::reloadParameters()
{
    // parameters for initializing new filters
    processNoiseStdSingleDimension << params.processNoiseStdQ00, params.processNoiseStdQ01,
                                      params.processNoiseStdQ10, params.processNoiseStdQ11;

    measurementNoiseCovariances << params.measurementNoiseR00, params.measurementNoiseR10,
                                   params.measurementNoiseR10, params.measurementNoiseR11;

    initialStateStdSingleDimension << params.initialStateStdP00, params.initialStateStdP01,
                                      params.initialStateStdP10, params.initialStateStdP11;

    // UAF thresholds
    euclid.setThreshold(params.euclidThreshold);
    mahalanobis.setThreshold(params.mahalanobisThreshold);
    likelihood.setThreshold(params.maximumLikelihoodThreshold);

    // update existing filters with new process and measurement noise
    Eigen::Matrix2d processNoiseCovariancesSingleDimension;
    processNoiseCovariancesSingleDimension = processNoiseStdSingleDimension.cwiseProduct(processNoiseStdSingleDimension);

    for(Filters::iterator iter = filter.begin(); iter != filter.end(); iter++){
        (*iter).setCovarianceOfProcessNoise(processNoiseCovariancesSingleDimension);
        (*iter).setCovarianceOfMeasurementNoise(measurementNoiseCovariances);
    }
}

max_stars_repo_name: BerlinUnited/NaoTH

max_issues_repo_name: BerlinUnited/NaoTH

max_forks_repo_name: BerlinUnited/NaoTH


text:
// Distributed under the MIT License.
// See LICENSE.txt for details.

/// \file
/// Defines helper functions for converting between `DataVector`s and boost
/// quaternions.

#pragma once

#include <boost/math/quaternion.hpp>

#include "DataStructures/DataVector.hpp"

/// \cond
namespace gsl {
template <class T>
class not_null;
}  // namespace gsl
/// \endcond

/// Convert a `boost::math::quaternion` to a `DataVector`
DataVector quaternion_to_datavector(
    const boost::math::quaternion<double>& input) noexcept;

/// \brief Convert a `DataVector` to a `boost::math::quaternion`
///
/// \details To convert to a quaternion, a `DataVector` must have either 3 or 4
/// components. If it has 3 components, the quaternion will be constructed with
/// 0 scalar part while the vector part is the `DataVector`. If the `DataVector`
/// has 4 components, the quaternion is just the `DataVector` itself.
boost::math::quaternion<double> datavector_to_quaternion(
    const DataVector& input) noexcept;

/// Normalize a `boost::math::quaternion`
void normalize_quaternion(
    gsl::not_null<boost::math::quaternion<double>*> input) noexcept;


text:
#pragma once
#include "WorkSimulator.hpp"

#include <chrono>
#include <future>
#include <iostream>
#include <mutex>
#include <sstream>
#include <string>

#include <boost/program_options.hpp>
struct ExecutionResult {
  std::chrono::high_resolution_clock::time_point time_point;
  std::string data;
};

struct PrintableResult {
  static std::string column_names() {
    std::stringstream ss;
    ss << "Time[ns]"
          "\tThreadID"
          "\tIteration"
          "\tIndex"
          // "\tRandom Sum"
          ;
    return ss.str();
  }
  ExecutionResult execution_result;
  std::chrono::high_resolution_clock::time_point start_time_point;

  friend std::ostream &operator<<(std::ostream &os,
                                  const PrintableResult &exec);
};
std::ostream &operator<<(std::ostream &os, const PrintableResult &exec) {
  os << std::chrono::duration_cast<std::chrono::nanoseconds>(
            exec.execution_result.time_point - exec.start_time_point)
            .count()
     << exec.execution_result.data;
  return os;
}
template <typename Result> struct Executable {
  std::promise<Result> promise;
  template <typename... Args> void operator()(Args &&... args) {
    promise.set_value(Result{std::forward<Args>(args)...});
  }
};
template <> struct Executable<ExecutionResult> {
  long long index;
  std::promise<ExecutionResult> promise;
  void operator()(long long counter) {
    std::stringstream ss;
    ss << '\t' << std::hex << std::this_thread::get_id() << std::dec;
    ss << '\t' << counter;
    ss << '\t' << index;
    // ss << '\t' << WorkSimulator{}.work<int8_t>();
    promise.set_value(
        ExecutionResult{std::chrono::high_resolution_clock::now(), ss.str()});
  }
};

using CountingExecutable = Executable<ExecutionResult>;

template <typename DerivedExecutor> struct BaseExecutor {
  void operator()() { static_cast<DerivedExecutor &> (*this)(); }
  void emplace(CountingExecutable *new_work) {
    static_cast<DerivedExecutor &>(*this).emplace(new_work);
  }
  void stop() { static_cast<DerivedExecutor &>(*this).stop(); }
  virtual ~BaseExecutor() = default;
};

template <typename Executor> struct CountBenchmark {
  BaseExecutor<Executor> &executor;
  std::mutex &cout_mutex;
  std::ostream &ostream;
  void operator()(int num_dispatches = 2048) {
    auto start = std::chrono::high_resolution_clock::now();
    for (int i = 0; i < num_dispatches; ++i) {
      CountingExecutable executable{i};
      auto future = executable.promise.get_future();
      executor.emplace(&executable);
      auto res = future.get();
      // Print results under mutex
      std::lock_guard guard{cout_mutex};
      ostream << PrintableResult{res, start} << std::endl;
    }
  }
};
template <typename Executor, typename Duration> struct DurationBenchmark {
  using Clock = std::chrono::high_resolution_clock;

  BaseExecutor<Executor> &executor;
  std::mutex &cout_mutex;
  std::ostream &ostream;
  Duration duration;
  bool elapsed(const Clock::time_point &start_time) {
    return Clock::now() < start_time + duration;
  }
  void operator()() {
    auto start = Clock::now();
    for (int i = 0; elapsed(start); ++i) {
      CountingExecutable executable{i};
      auto future = executable.promise.get_future();
      executor.emplace(&executable);
      auto res = future.get();
      // Print results under mutex
      std::lock_guard guard{cout_mutex};
      ostream << PrintableResult{res, start} << std::endl;
    }
  }
};

template <typename ExecutorImplementation> struct BenchmarkRunner {
  std::unique_ptr<BaseExecutor<ExecutorImplementation>> executor;
  std::mutex cout_mutex{};
  std::thread executor_thread{[&executor = *executor] { executor(); }};
  ~BenchmarkRunner() {
    executor->stop();
    executor_thread.join();
  }
  void operator()(int count) {
    std::cout << PrintableResult::column_names() << std::endl;
    {
      using namespace std::chrono_literals;
      auto num_threads = std::thread::hardware_concurrency() - 1;
      auto work_per_thread = std::div(count, num_threads);
      auto rem = work_per_thread.rem;
      std::vector<std::thread> threads(num_threads);
      for (auto &th : threads)
        th = std::thread(CountBenchmark<ExecutorImplementation>{*executor,
                                                                cout_mutex,
                                                                std::cout},
                         work_per_thread.quot + ((--rem >= 0) ? 1 : 0));
      for (auto &th : threads)
        th.join();
    }
  }
  void operator()(std::chrono::seconds sec) {
    std::cout << PrintableResult::column_names() << std::endl;
    {
      using namespace std::chrono_literals;
      std::vector<std::thread> threads(std::thread::hardware_concurrency() - 1);
      for (auto &th : threads)
        th = std::thread(
            DurationBenchmark<ExecutorImplementation, std::chrono::nanoseconds>{
                *executor, cout_mutex, std::cout, sec});
      for (auto &th : threads)
        th.join();
    }
  }
};

template <typename ExecuterImplementation> struct CmdlineParser {
  int operator()(int &argc, char **&argv) {
    namespace po = boost::program_options;
    po::options_description desc("Benchmark options");
    desc.add_options()("help", "produce help message")(
        "timer", po::value<int>(),
        "run time in seconds")("count", po::value<int>(), "iterations count");
    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 EXIT_SUCCESS;
    }
    BenchmarkRunner<ExecuterImplementation> benchmark_runner{
        std::make_unique<ExecuterImplementation>()};
    if (vm.count("timer")) {
      benchmark_runner(std::chrono::seconds(vm["timer"].as<int>()));
      return EXIT_SUCCESS;
    } else if (vm.count("count")) {
      benchmark_runner(vm["count"].as<int>());
      return EXIT_SUCCESS;
    } else {
      std::cout << desc << "\n";
      return EXIT_FAILURE;
    }
  }
};

max_stars_repo_name: janekzieleznicki/cpp-samples

max_issues_repo_name: janekzieleznicki/cpp-samples

max_forks_repo_name: janekzieleznicki/cpp-samples