<commit_before>#pragma once
#include <algorithm>
#include <vector>
#include <boost/serialization/access.hpp>
#include <boost/serialization/vector.hpp>
#include "serialization/traits.hpp"
#include "sort.hpp"

namespace spn {
	//! ソート済みベクタ
	/*! 内部をソートされた状態に保つため要素の編集は基本的に不可
		もし要素を編集した場合は必ずre_sort()を呼ぶ */
	template <class T, class Pred=std::less<>, class CT=std::vector<T>>
	class AssocVec : public boost::serialization::traits<AssocVec<T,Pred,CT>,
								boost::serialization::object_serializable,
								boost::serialization::track_selectively>
	{
		private:
			using Vec = CT;
			Vec		_vec;

			friend class boost::serialization::access;
			template <class Archive>
			void serialize(Archive& ar, const unsigned int /*ver*/) {
				ar & BOOST_SERIALIZATION_NVP(_vec);
			}

		protected:
			using VecItrC = typename Vec::const_iterator;
			VecItrC _cbegin() const { return _vec.cbegin(); }
			VecItrC _cend() const { return _vec.cend(); }

		public:
			using value_type = T;
			using cmp_type = Pred;

			AssocVec() = default;
			AssocVec(AssocVec&&) = default;
			AssocVec(const Vec& src): _vec(src) {
				std::sort(_vec.begin(), _vec.end());
			}
			AssocVec(Vec&& src): _vec(std::forward<Vec>(src)) {
				std::sort(_vec.begin(), _vec.end());
			}

			bool operator == (const AssocVec& av) const {
				return _vec == av._vec;
			}
			bool operator != (const AssocVec& av) const {
				return _vec != av._vec;
			}
			//! 要素を追加
			/*! \return 挿入されたソート後のインデックス */
			template <class T2>
			int insert(T2&& t) {
				T obj(std::forward<T2>(t));
				auto itrE = _vec.end();
				auto itr = _vec.begin();
				Pred pred;
				while(itr!=itrE && pred(*itr, obj))
					++itr;
				if(itr == itrE) {
					_vec.emplace_back(std::move(obj));
					itr = _vec.end()-1;
				} else
					itr = _vec.emplace(itr, std::move(obj));
				return itr - _vec.begin();
			}
			template <class... Ts>
			int emplace(Ts&&... ts) {
				return insert(value_type(std::forward<Ts>(ts)...));
			}
			//! 外部から要素の書き換えを行った際にソートし直す
			void re_sort() {
				// 前回と大体同じような並び順になっている事を想定し、単純挿入ソート
				insertion_sort(_vec.begin(), _vec.end(), Pred());
			}

			void pop_front() {
				_vec.erase(_vec.begin());
			}
			//! pop_frontと同じだが、消去される要素の値を返す
			T pop_frontR() {
				T ret(std::move(_vec.front()));
				_vec.erase(_vec.begin());
				return std::move(ret);
			}
			void pop_back() { _vec.pop_back(); }
			//! pop_backと同じだが、消去される要素の値を返す
			T pop_backR() {
				T ret(std::move(_vec.back()));
				_vec.pop_back();
				return std::move(ret);
			}
			void erase(int n) {
				_vec.erase(_vec.begin()+n);
			}
			void erase(typename Vec::iterator itr) {
				_vec.erase(itr);
			}
			void erase(typename Vec::const_iterator itr) {
				_vec.erase(itr);
			}
			const T& back() const { return _vec.back(); }
			const T& front() const { return _vec.front(); }

			size_t size() const { return _vec.size(); }
			void swap(AssocVec& v) noexcept {
				std::swap(_vec, v._vec);
			}
			const T& operator [](int n) const {
				return _vec[n];
			}
			typename Vec::const_iterator cbegin() const {
				return _vec.cbegin();
			}
			typename Vec::const_iterator cend() const {
				return _vec.cend();
			}
			bool empty() const {
				return _vec.empty();
			}
			void clear() {
				_vec.clear();
			}
	};
	//! std::pairのfirstをless比較
	template <class CMP>
	struct CmpFirst {
		template <class K, class T>
		bool operator()(const std::pair<K,T>& t0, const std::pair<K,T>& t1) const {
			return CMP()(t0.first, t1.first);
		}
	};
	//! キー付きソート済みベクタ
	/*! キーは固定のため、値の編集が可能 */
	template <class K, class T, class Pred=std::less<>, class CT=std::vector<std::pair<K,T>>>
	class AssocVecK : public AssocVec<std::pair<K,T>, CmpFirst<Pred>, CT> {
		public:
			using value_type = std::pair<K,T>;
			using Entry = value_type;
			using cmp_type = Pred;
			using ASV = AssocVec<value_type, CmpFirst<Pred>, CT>;

			T& back() {
				const Entry& ent = const_cast<ASV*>(this)->back();
				return const_cast<Entry&>(ent).second;
			}
			T& front() {
				const Entry& ent = const_cast<ASV*>(this)->front();
				return const_cast<Entry&>(ent).second;
			}
			T& operator [](int n) {
				const Entry& ent = (*const_cast<ASV*>(this))[n];
				return const_cast<Entry&>(ent).second;
			}
			const K& getKey(int n) const {
				return ASV::operator [](n).first;
			}

			typename ASV::VecItrC cbegin() const {
				return ASV::_cbegin();
			}
			typename ASV::VecItrC cend() const {
				return ASV::_cend();
			}
	};
}

<commit_msg>AssocVec: erase()の戻り値を変更<commit_after>#pragma once
#include <algorithm>
#include <vector>
#include <boost/serialization/access.hpp>
#include <boost/serialization/vector.hpp>
#include "serialization/traits.hpp"
#include "sort.hpp"

namespace spn {
	//! ソート済みベクタ
	/*! 内部をソートされた状態に保つため要素の編集は基本的に不可
		もし要素を編集した場合は必ずre_sort()を呼ぶ */
	template <class T, class Pred=std::less<>, class CT=std::vector<T>>
	class AssocVec : public boost::serialization::traits<AssocVec<T,Pred,CT>,
								boost::serialization::object_serializable,
								boost::serialization::track_selectively>
	{
		private:
			using Vec = CT;
			Vec		_vec;

			friend class boost::serialization::access;
			template <class Archive>
			void serialize(Archive& ar, const unsigned int /*ver*/) {
				ar & BOOST_SERIALIZATION_NVP(_vec);
			}

		protected:
			using VecItrC = typename Vec::const_iterator;
			VecItrC _cbegin() const { return _vec.cbegin(); }
			VecItrC _cend() const { return _vec.cend(); }

		public:
			using value_type = T;
			using cmp_type = Pred;

			AssocVec() = default;
			AssocVec(AssocVec&&) = default;
			AssocVec(const Vec& src): _vec(src) {
				std::sort(_vec.begin(), _vec.end());
			}
			AssocVec(Vec&& src): _vec(std::forward<Vec>(src)) {
				std::sort(_vec.begin(), _vec.end());
			}

			bool operator == (const AssocVec& av) const {
				return _vec == av._vec;
			}
			bool operator != (const AssocVec& av) const {
				return _vec != av._vec;
			}
			//! 要素を追加
			/*! \return 挿入されたソート後のインデックス */
			template <class T2>
			int insert(T2&& t) {
				T obj(std::forward<T2>(t));
				auto itrE = _vec.end();
				auto itr = _vec.begin();
				Pred pred;
				while(itr!=itrE && pred(*itr, obj))
					++itr;
				if(itr == itrE) {
					_vec.emplace_back(std::move(obj));
					itr = _vec.end()-1;
				} else
					itr = _vec.emplace(itr, std::move(obj));
				return itr - _vec.begin();
			}
			template <class... Ts>
			int emplace(Ts&&... ts) {
				return insert(value_type(std::forward<Ts>(ts)...));
			}
			//! 外部から要素の書き換えを行った際にソートし直す
			void re_sort() {
				// 前回と大体同じような並び順になっている事を想定し、単純挿入ソート
				insertion_sort(_vec.begin(), _vec.end(), Pred());
			}

			void pop_front() {
				_vec.erase(_vec.begin());
			}
			//! pop_frontと同じだが、消去される要素の値を返す
			T pop_frontR() {
				T ret(std::move(_vec.front()));
				_vec.erase(_vec.begin());
				return std::move(ret);
			}
			void pop_back() { _vec.pop_back(); }
			//! pop_backと同じだが、消去される要素の値を返す
			T pop_backR() {
				T ret(std::move(_vec.back()));
				_vec.pop_back();
				return std::move(ret);
			}
			typename Vec::const_iterator erase(int n) {
				return _vec.erase(_vec.cbegin()+n);
			}
			typename Vec::iterator erase(typename Vec::iterator itr) {
				return _vec.erase(itr);
			}
			typename Vec::const_iterator erase(typename Vec::const_iterator itr) {
				return _vec.erase(itr);
			}
			const T& back() const { return _vec.back(); }
			const T& front() const { return _vec.front(); }

			size_t size() const { return _vec.size(); }
			void swap(AssocVec& v) noexcept {
				std::swap(_vec, v._vec);
			}
			const T& operator [](int n) const {
				return _vec[n];
			}
			typename Vec::const_iterator cbegin() const {
				return _vec.cbegin();
			}
			typename Vec::const_iterator cend() const {
				return _vec.cend();
			}
			bool empty() const {
				return _vec.empty();
			}
			void clear() {
				_vec.clear();
			}
	};
	//! std::pairのfirstをless比較
	template <class CMP>
	struct CmpFirst {
		template <class K, class T>
		bool operator()(const std::pair<K,T>& t0, const std::pair<K,T>& t1) const {
			return CMP()(t0.first, t1.first);
		}
	};
	//! キー付きソート済みベクタ
	/*! キーは固定のため、値の編集が可能 */
	template <class K, class T, class Pred=std::less<>, class CT=std::vector<std::pair<K,T>>>
	class AssocVecK : public AssocVec<std::pair<K,T>, CmpFirst<Pred>, CT> {
		public:
			using value_type = std::pair<K,T>;
			using Entry = value_type;
			using cmp_type = Pred;
			using ASV = AssocVec<value_type, CmpFirst<Pred>, CT>;

			T& back() {
				const Entry& ent = const_cast<ASV*>(this)->back();
				return const_cast<Entry&>(ent).second;
			}
			T& front() {
				const Entry& ent = const_cast<ASV*>(this)->front();
				return const_cast<Entry&>(ent).second;
			}
			T& operator [](int n) {
				const Entry& ent = (*const_cast<ASV*>(this))[n];
				return const_cast<Entry&>(ent).second;
			}
			const K& getKey(int n) const {
				return ASV::operator [](n).first;
			}

			typename ASV::VecItrC cbegin() const {
				return ASV::_cbegin();
			}
			typename ASV::VecItrC cend() const {
				return ASV::_cend();
			}
	};
}

<|endoftext|>


<commit_before>/************************************************
 *  gen.hpp
 *  ESA++
 *
 *  Copyright (c) 2014, Chi-En Wu
 *  Distributed under The BSD 3-Clause License
 ************************************************/

#ifndef ESAPP_GEN_HPP_
#define ESAPP_GEN_HPP_

#include <iterator>
#include <type_traits>

namespace esapp
{

/************************************************
 * Declaration: class generator_iterator
 ************************************************/

template <typename I, typename G>
class generator_iterator
    : public std::iterator<std::input_iterator_tag,
                           typename std::result_of<G(I &, I const &)>::type>
{
private: // Private Type(s)
    typedef std::iterator<
        std::input_iterator_tag,
        typename std::result_of<G(I &, I const &)>::type
    > base;

public: // Public Type(s)
    typedef typename base::iterator_category iterator_category;
    typedef typename base::value_type value_type;
    typedef typename base::reference reference;
    typedef typename base::pointer pointer;
    typedef typename base::difference_type difference_type;

    typedef I iterator;
    typedef G generator;

public: // Public Method(s)
    generator_iterator(iterator const &begin, iterator const &end,
                       generator const &g);
    template <typename Initializer>
    generator_iterator(iterator const &begin, iterator const &end,
                       generator const &g, Initializer const &init);
    generator_iterator(generator_iterator const &it);

    generator_iterator begin(void) const;
    generator_iterator end(void) const;

    generator_iterator &operator++(void);
    generator_iterator operator++(int);
    value_type &operator*(void);
    value_type *operator->(void) const;
    bool operator==(generator_iterator const &it) const;
    bool operator!=(generator_iterator const &it) const;

private: // Private Property(ies)
    iterator begin_, end_;
    generator g_;
    value_type val_;
    bool has_val_;
}; // class generator_iterator

/************************************************
 * Implementation: class generator_iterator
 ************************************************/

template <typename I, typename G>
inline generator_iterator<I, G>::generator_iterator(iterator const &begin,
                                                    iterator const &end,
                                                    generator const &g)
    : begin_(begin), end_(end), g_(g)
{
    operator++();
}

template <typename I, typename G>
template <typename Initializer>
inline generator_iterator<I, G>::generator_iterator(iterator const &begin,
                                                    iterator const &end,
                                                    generator const &g,
                                                    Initializer const &init)
    : begin_(begin), end_(end), g_(g)
{
    init(begin_, end_);
    operator++();
}

template <typename I, typename G>
inline generator_iterator<I, G>::generator_iterator(generator_iterator const &it)
    : begin_(it.begin_), end_(it.end_), g_(it.g_),
      val_(it.val_), has_val_(it.has_val_)
{
    // do nothing
}

template <typename I, typename G>
inline typename generator_iterator<I, G>::generator_iterator
generator_iterator<I, G>::begin(void) const
{
    return *this;
}

template <typename I, typename G>
inline typename generator_iterator<I, G>::generator_iterator
generator_iterator<I, G>::end(void) const
{
    return generator_iterator(end_, end_, g_);
}

template <typename I, typename G>
inline typename generator_iterator<I, G>::generator_iterator &
generator_iterator<I, G>::operator++(void)
{
    has_val_ = begin_ != end_;
    val_ = has_val_ ? g_(begin_, end_) : decltype(val_)();

    return *this;
}

template <typename I, typename G>
inline typename generator_iterator<I, G>::generator_iterator
generator_iterator<I, G>::operator++(int)
{
    generator_iterator it(*this);
    operator++();
    return it;
}

template <typename I, typename G>
inline typename generator_iterator<I, G>::value_type &
generator_iterator<I, G>::operator*(void)
{
    return val_;
}

template <typename I, typename G>
inline typename generator_iterator<I, G>::value_type *
generator_iterator<I, G>::operator->(void) const
{
    return &val_;
}

template <typename I, typename G>
inline bool generator_iterator<I, G>::operator==(generator_iterator const &it) const
{
    return begin_ == it.begin_ && end_ == it.end_ && g_ == it.g_ &&
           val_ == it.val_ && has_val_ == it.has_val_;
}

template <typename I, typename G>
inline bool generator_iterator<I, G>::operator!=(generator_iterator const &it) const
{
    return !(*this == it);
}

} // namespace esapp

#endif // ESAPP_GEN_HPP_
<commit_msg>Correct reference and pointer type for generator<commit_after>/************************************************
 *  gen.hpp
 *  ESA++
 *
 *  Copyright (c) 2014, Chi-En Wu
 *  Distributed under The BSD 3-Clause License
 ************************************************/

#ifndef ESAPP_GEN_HPP_
#define ESAPP_GEN_HPP_

#include <iterator>
#include <type_traits>

namespace esapp
{

/************************************************
 * Declaration: class generator_iterator
 ************************************************/

template <typename I, typename G>
class generator_iterator
    : public std::iterator<
        std::input_iterator_tag,
        typename std::result_of<G(I &, I const &)>::type,
        ptrdiff_t,
        typename std::add_pointer<
            typename std::add_const<
                typename std::result_of<G(I &, I const &)>::type
            >::type
        >::type,
        typename std::add_lvalue_reference<
            typename std::add_const<
                typename std::result_of<G(I &, I const &)>::type
            >::type
        >::type
    >
{
private: // Private Type(s)
    typedef std::iterator<
        std::input_iterator_tag,
        typename std::result_of<G(I &, I const &)>::type,
        ptrdiff_t,
        typename std::add_pointer<
            typename std::add_const<
                typename std::result_of<G(I &, I const &)>::type
            >::type
        >::type,
        typename std::add_lvalue_reference<
            typename std::add_const<
                typename std::result_of<G(I &, I const &)>::type
            >::type
        >::type
    > base;

public: // Public Type(s)
    typedef typename base::iterator_category iterator_category;
    typedef typename base::value_type value_type;
    typedef typename base::reference reference;
    typedef typename base::pointer pointer;
    typedef typename base::difference_type difference_type;

    typedef I iterator;
    typedef G generator;

public: // Public Method(s)
    generator_iterator(iterator const &begin, iterator const &end,
                       generator const &g);
    template <typename Initializer>
    generator_iterator(iterator const &begin, iterator const &end,
                       generator const &g, Initializer const &init);
    generator_iterator(generator_iterator const &it);

    generator_iterator begin(void) const;
    generator_iterator end(void) const;

    generator_iterator &operator++(void);
    generator_iterator operator++(int);
    reference operator*(void) const;
    pointer operator->(void) const;
    bool operator==(generator_iterator const &it) const;
    bool operator!=(generator_iterator const &it) const;

private: // Private Property(ies)
    iterator begin_, end_;
    generator g_;
    value_type val_;
    bool has_val_;
}; // class generator_iterator

/************************************************
 * Implementation: class generator_iterator
 ************************************************/

template <typename I, typename G>
inline generator_iterator<I, G>::generator_iterator(iterator const &begin,
                                                    iterator const &end,
                                                    generator const &g)
    : begin_(begin), end_(end), g_(g)
{
    operator++();
}

template <typename I, typename G>
template <typename Initializer>
inline generator_iterator<I, G>::generator_iterator(iterator const &begin,
                                                    iterator const &end,
                                                    generator const &g,
                                                    Initializer const &init)
    : begin_(begin), end_(end), g_(g)
{
    init(begin_, end_);
    operator++();
}

template <typename I, typename G>
inline generator_iterator<I, G>::generator_iterator(generator_iterator const &it)
    : begin_(it.begin_), end_(it.end_), g_(it.g_),
      val_(it.val_), has_val_(it.has_val_)
{
    // do nothing
}

template <typename I, typename G>
inline typename generator_iterator<I, G>::generator_iterator
generator_iterator<I, G>::begin(void) const
{
    return *this;
}

template <typename I, typename G>
inline typename generator_iterator<I, G>::generator_iterator
generator_iterator<I, G>::end(void) const
{
    return generator_iterator(end_, end_, g_);
}

template <typename I, typename G>
inline typename generator_iterator<I, G>::generator_iterator &
generator_iterator<I, G>::operator++(void)
{
    has_val_ = begin_ != end_;
    val_ = has_val_ ? g_(begin_, end_) : decltype(val_)();

    return *this;
}

template <typename I, typename G>
inline typename generator_iterator<I, G>::generator_iterator
generator_iterator<I, G>::operator++(int)
{
    generator_iterator it(*this);
    operator++();
    return it;
}

template <typename I, typename G>
inline typename generator_iterator<I, G>::reference
generator_iterator<I, G>::operator*(void) const
{
    return val_;
}

template <typename I, typename G>
inline typename generator_iterator<I, G>::pointer
generator_iterator<I, G>::operator->(void) const
{
    return &val_;
}

template <typename I, typename G>
inline bool generator_iterator<I, G>::operator==(generator_iterator const &it) const
{
    return begin_ == it.begin_ && end_ == it.end_ && g_ == it.g_ &&
           val_ == it.val_ && has_val_ == it.has_val_;
}

template <typename I, typename G>
inline bool generator_iterator<I, G>::operator!=(generator_iterator const &it) const
{
    return !(*this == it);
}

} // namespace esapp

#endif // ESAPP_GEN_HPP_
<|endoftext|>


<commit_before><commit_msg>Implement functions to return the primary display dimensions and the number of displays connected.<commit_after><|endoftext|>


<commit_before>#include "../include/mcmc.h"

#include "../include/source.h"
#include "../include/lens.h"
#include "../include/model.h"
#include "../include/image_processing.h"
#include "../include/grid_maintenance.h"
#include "../include/map_images.h"

#include <cmath>
#include <ctime>

namespace
{
	template<typename It>
	static double likelihood(It first, It last, const PixelData& data)
	{
		const PixelMap& image = data.image();
		const PixelMap& noise = data.noise();
		const PixelMask& mask = data.mask();
		double norm = data.getNormalization();

		double chi2 = 0;

		if(mask.valid())
		{
			for(std::size_t i = 0, n = mask.size(); i < n; ++i)
			{
				std::size_t k = mask[i];
				double model = 0;
				for(It j = first; j != last; ++j)
					model += (*j)[k];

				double diff = norm*(model - image[k]);
				double sigma2 = norm*model + (norm*noise[k])*(norm*noise[k]);

				chi2 += (diff*diff)/sigma2 + std::log(sigma2);
			}
		}
		else
		{
			for(std::size_t i = 0, n = image.size(); i < n; ++i)
			{
				double model = 0;
				for(It j = first; j != last; ++j)
					model += (*j)[i];

				double diff = norm*(model - image[i]);
				double sigma2 = norm*model + (norm*noise[i])*(norm*noise[i]);

				chi2 += (diff*diff)/sigma2 + std::log(sigma2);
			}
		}

		return -0.5*chi2;
	}
}

MCMC::MCMC(Lens& l, Sky& s, const PixelData& d)
: lens(l), sky(s), data(d)
{
}

std::vector<RawData> MCMC::run(std::size_t n, double step, long* seed)
{
	std::size_t num_sources = sky.getNsources();

	// the Markov chain
	std::vector<RawData> chain;

	// the number of steps in the chain
	chain.reserve(n+1);

	// put initial parameters in chain
	chain.push_back(RawData());
	//lens.serialize(chain.back());
	sky.serialize(chain.back());

	// the image properties
	double center[2];
	center[0] = data.getCenter()[0];
	center[1] = data.getCenter()[1];
	double range = data.getRange();
	double resolution = data.getResolution();
	std::size_t npixels = data.getNpixels();

	// buffer for images
	ImageInfo images[MAX_N_IMAGES];
	int image_count;

	// create pixmaps for the individual sources
	std::vector<PixelMap> pixmaps(num_sources);

	// the likelihood for each source
	for(std::size_t s = 0; s < num_sources; ++s)
	{
		// get current source
		Source& source = sky.getSource(s);

		// check if source plane has changed
		if(lens.getSourceZ() != source.getZ())
			lens.ResetSourcePlane(source.getZ(), false);

		// create a grid
		Grid grid(&lens, GRID_POINTS, center, range);

		// render model
		map_images(&lens, &source, &grid, &image_count, images, MAX_N_IMAGES, source.getRadius(), 0.1*source.getRadius(), 0, EachImage, true, false, true);

		// create pixmap for images
		pixmaps[s] = PixelMap(center, npixels, resolution);
		pixmaps[s].AddImages(images, image_count);
	}

	// calculate current likelihood
	double Lx = likelihood(pixmaps.begin(), pixmaps.end(), data);

	// run n complete cycles through all sources
	for(std::size_t i = 0; i < n; ++i)
	{
		// randomize each source in turn
		for(std::size_t s = 0; s < num_sources; ++s)
		{
			// get current source
			Source& source = sky.getSource(s);

			// check if source plane has changed
			if(lens.getSourceZ() != source.getZ())
				lens.ResetSourcePlane(source.getZ(), false);

			// get source parameters
			RawData d;
			source.serialize(d);

			// randomize source
			source.randomize(step, seed);

			// create a grid
			Grid grid(&lens, GRID_POINTS, center, range);

			// render model
			map_images(&lens, &source, &grid, &image_count, images, MAX_N_IMAGES, source.getRadius(), 0.1*source.getRadius(), 0, EachImage, true, false, true);

			// create pixmap for images
			PixelMap pixmap(center, npixels, resolution);
			pixmap.AddImages(images, image_count);

			// swap pixmap into array to calculate likelihood
			swap(pixmaps[s], pixmap);

			// calculate candidate likelihood
			double Ly = likelihood(pixmaps.begin(), pixmaps.end(), data);

			// probability of accepting a candidate point
			if(Ly < Lx && std::exp(Ly - Lx) < ran2(seed))
			{
				// not accepted, restore parameters
				source.unserialize(d);

				// restore pixmap
				swap(pixmaps[s], pixmap);
			}
			else
			{
				// accepted, candidate likelihood becomes current
				Lx = Ly;
			}
		}

		// TODO: randomize lens

		// add parameters to chain after complete cycle
		chain.push_back(RawData());
		//lens.serialize(chain.back());
		sky.serialize(chain.back());
	}

	// chain is complete
	return chain;
}
<commit_msg>First randomization of lens.<commit_after>#include "../include/mcmc.h"

#include "../include/source.h"
#include "../include/lens.h"
#include "../include/model.h"
#include "../include/image_processing.h"
#include "../include/grid_maintenance.h"
#include "../include/map_images.h"

#include <cmath>
#include <ctime>

namespace
{
	template<typename It>
	static double likelihood(It first, It last, const PixelData& data)
	{
		const PixelMap& image = data.image();
		const PixelMap& noise = data.noise();
		const PixelMask& mask = data.mask();
		double norm = data.getNormalization();

		double chi2 = 0;

		if(mask.valid())
		{
			for(std::size_t i = 0, n = mask.size(); i < n; ++i)
			{
				std::size_t k = mask[i];
				double model = 0;
				for(It j = first; j != last; ++j)
					model += (*j)[k];

				double diff = norm*(model - image[k]);
				double sigma2 = norm*model + (norm*noise[k])*(norm*noise[k]);

				chi2 += (diff*diff)/sigma2 + std::log(sigma2);
			}
		}
		else
		{
			for(std::size_t i = 0, n = image.size(); i < n; ++i)
			{
				double model = 0;
				for(It j = first; j != last; ++j)
					model += (*j)[i];

				double diff = norm*(model - image[i]);
				double sigma2 = norm*model + (norm*noise[i])*(norm*noise[i]);

				chi2 += (diff*diff)/sigma2 + std::log(sigma2);
			}
		}

		return -0.5*chi2;
	}
}

MCMC::MCMC(Lens& l, Sky& s, const PixelData& d)
: lens(l), sky(s), data(d)
{
}

std::vector<RawData> MCMC::run(std::size_t n, double step, long* seed)
{
	std::size_t num_sources = sky.getNsources();

	// the Markov chain
	std::vector<RawData> chain;

	// the number of steps in the chain
	chain.reserve(n+1);

	// put initial parameters in chain
	chain.push_back(RawData());
	LensHalo* halo = lens.getMainHalo(0);
	halo->serialize(chain.back());
	sky.serialize(chain.back());

	// the image properties
	double center[2];
	center[0] = data.getCenter()[0];
	center[1] = data.getCenter()[1];
	double range = data.getRange();
	double resolution = data.getResolution();
	std::size_t npixels = data.getNpixels();

	// buffer for images
	ImageInfo images[MAX_N_IMAGES];
	int image_count;

	// create pixmaps for the individual sources
	std::vector<PixelMap> pixmaps(num_sources);

	// the likelihood for each source
	for(std::size_t s = 0; s < num_sources; ++s)
	{
		// get current source
		Source& source = sky.getSource(s);

		// check if source plane has changed
		if(lens.getSourceZ() != source.getZ())
			lens.ResetSourcePlane(source.getZ(), false);

		// create a grid
		Grid grid(&lens, GRID_POINTS, center, range);

		// render model
		map_images(&lens, &source, &grid, &image_count, images, MAX_N_IMAGES, source.getRadius(), 0.1*source.getRadius(), 0, EachImage, true, false, true);

		// create pixmap for images
		pixmaps[s] = PixelMap(center, npixels, resolution);
		pixmaps[s].AddImages(images, image_count);
	}

	// calculate current likelihood
	double Lx = likelihood(pixmaps.begin(), pixmaps.end(), data);

	// run n complete cycles through all sources
	for(std::size_t i = 0; i < n; ++i)
	{
		// randomize each source in turn
		for(std::size_t s = 0; s < num_sources; ++s)
		{
			// get current source
			Source& source = sky.getSource(s);

			// check if source plane has changed
			if(lens.getSourceZ() != source.getZ())
				lens.ResetSourcePlane(source.getZ(), false);

			// get source parameters
			RawData d;
			source.serialize(d);

			// randomize source
			source.randomize(step, seed);

			// create a grid
			Grid grid(&lens, GRID_POINTS, center, range);

			// render model
			map_images(&lens, &source, &grid, &image_count, images, MAX_N_IMAGES, source.getRadius(), 0.1*source.getRadius(), 0, EachImage, true, false, true);

			// create pixmap for images
			PixelMap pixmap(center, npixels, resolution);
			pixmap.AddImages(images, image_count);

			// swap pixmap into array to calculate likelihood
			swap(pixmaps[s], pixmap);

			// calculate candidate likelihood
			double Ly = likelihood(pixmaps.begin(), pixmaps.end(), data);

			// probability of accepting a candidate point
			if(Ly < Lx && std::exp(Ly - Lx) < ran2(seed))
			{
				// not accepted, restore parameters
				source.unserialize(d);

				// restore pixmap
				swap(pixmaps[s], pixmap);
			}
			else
			{
				// accepted, candidate likelihood becomes current
				Lx = Ly;
			}
		}

		// get halo parameters
		RawData d;
		halo->serialize(d);

		// randomize halo
		halo->randomize(step, seed);

		// create new pixmaps for the individual sources
		std::vector<PixelMap> pixmaps_new(num_sources);

		for(std::size_t s = 0; s < num_sources; ++s)
		{
			// get current source
			Source& source = sky.getSource(s);

			// check if source plane has changed
			if(lens.getSourceZ() != source.getZ())
				lens.ResetSourcePlane(source.getZ(), false);

			// create a grid
			Grid grid(&lens, GRID_POINTS, center, range);

			// render model
			map_images(&lens, &source, &grid, &image_count, images, MAX_N_IMAGES, source.getRadius(), 0.1*source.getRadius(), 0, EachImage, true, false, true);

			// create pixmap for images
			pixmaps_new[s] = PixelMap(center, npixels, resolution);
			pixmaps_new[s].AddImages(images, image_count);
		}

		// calculate candidate likelihood
		double Ly = likelihood(pixmaps_new.begin(), pixmaps_new.end(), data);

		// probability of accepting a candidate point
		if(Ly < Lx && std::exp(Ly - Lx) < ran2(seed))
		{
			// not accepted, restore parameters
			halo->unserialize(d);
		}
		else
		{
			// accepted, candidate likelihood becomes current
			Lx = Ly;
			for(std::size_t s = 0; s < num_sources; ++s)
			{
				pixmaps[s] = pixmaps_new[s];
			}
		}

		// add parameters to chain after complete cycle
		chain.push_back(RawData());
		halo->serialize(chain.back());
		sky.serialize(chain.back());
	}

	// chain is complete
	return chain;
}
<|endoftext|>


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

/**
 * @file
 **/

#include "modules/planning/scenarios/side_pass/side_pass_stage.h"

#include <algorithm>
#include <vector>

#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/proto/pnc_point.pb.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/common/speed_profile_generator.h"

namespace apollo {
namespace planning {
namespace scenario {
namespace side_pass {

using apollo::common::TrajectoryPoint;

constexpr double kExtraMarginforStopOnWaitPointStage = 3.0;

/*
 * @brief:
 * STAGE: SidePassApproachObstacle
 */
Stage::StageStatus SidePassApproachObstacle::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  bool plan_ok = PlanningOnReferenceLine(planning_start_point, frame);
  if (!plan_ok) {
    AERROR << "Stage " << Name() << " error: "
           << "planning on reference line failed.";
    return Stage::ERROR;
  }
  const ReferenceLineInfo& reference_line_info =
      frame->reference_line_info().front();
  double adc_velocity = frame->vehicle_state().linear_velocity();
  double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();
  const PathDecision& path_decision = reference_line_info.path_decision();

  double front_obstacle_distance = 1000;
  for (const auto* obstacle : path_decision.obstacles().Items()) {
    if (obstacle->IsVirtual()) {
      continue;
    }

    bool is_on_road = reference_line_info.reference_line().HasOverlap(
        obstacle->PerceptionBoundingBox());
    if (!is_on_road) {
      continue;
    }

    const auto& obstacle_sl = obstacle->PerceptionSLBoundary();
    if (obstacle_sl.end_s() <= reference_line_info.AdcSlBoundary().start_s()) {
      continue;
    }

    double distance = obstacle_sl.start_s() - adc_front_edge_s;
    if (distance < front_obstacle_distance) {
      front_obstacle_distance = distance;
    }
  }

  if ((front_obstacle_distance) < 0) {
    AERROR << "Stage " << Name() << " error: "
           << "front obstacle has wrong position.";
    return Stage::ERROR;
  }
  // TODO(all): stage params need to be in config file
  double max_stop_velocity = 1.0e-5;
  double min_stop_obstacle_distance = 4.0;

  if (adc_velocity < max_stop_velocity &&
      front_obstacle_distance > min_stop_obstacle_distance) {
    next_stage_ = ScenarioConfig::SIDE_PASS_GENERATE_PATH;
    return Stage::FINISHED;
  }
  return Stage::RUNNING;
}

/*
 * @brief:
 * STAGE: SidePassGeneratePath
 */
Stage::StageStatus SidePassGeneratePath::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  if (PlanningOnReferenceLine(planning_start_point, frame)) {
    next_stage_ = ScenarioConfig::SIDE_PASS_STOP_ON_WAITPOINT;
    return Stage::FINISHED;
  } else {
    return Stage::ERROR;
  }
}

/*
 * @brief:
 * STAGE: SidePassDetectSafety
 */

Stage::StageStatus SidePassDetectSafety::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  if (PlanningOnReferenceLine(planning_start_point, frame)) {
    return Stage::ERROR;
  }
  bool is_safe = true;
  const ReferenceLineInfo& reference_line_info =
      frame->reference_line_info().front();
  double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();

  const PathDecision& path_decision =
      frame->reference_line_info().front().path_decision();
  for (const auto* obstacle : path_decision.obstacles().Items()) {
    if (obstacle->IsVirtual() &&
        obstacle->PerceptionSLBoundary().start_s() >= adc_front_edge_s) {
      is_safe = false;
      break;
    }
  }
  if (is_safe) {
    next_stage_ = ScenarioConfig::SIDE_PASS_PASS_OBSTACLE;
    return Stage::FINISHED;
  }
  return Stage::RUNNING;
}

/*
 * @brief:
 * STAGE: SidePassPassObstacle
 */
Stage::StageStatus SidePassPassObstacle::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  bool plan_ok = PlanningOnReferenceLine(planning_start_point, frame);
  if (!plan_ok) {
    return Stage::ERROR;
  }
  const auto& reference_line_info = frame->reference_line_info().front();
  const SLBoundary& adc_sl_boundary = reference_line_info.AdcSlBoundary();
  const auto& end_point =
      reference_line_info.path_data().discretized_path().EndPoint();
  Vec2d last_xy_point(end_point.x(), end_point.y());
  // get s of last point on path
  common::SLPoint sl_point;
  if (!reference_line_info.reference_line().XYToSL(last_xy_point, &sl_point)) {
    AERROR << "Fail to transfer cartesian point to frenet point.";
    return Stage::ERROR;
  }

  if (adc_sl_boundary.end_s() > sl_point.s() - 1.0) {
    next_stage_ = ScenarioConfig::NO_STAGE;
    return Stage::FINISHED;
  }
  return Stage::RUNNING;
}

}  // namespace side_pass
}  // namespace scenario
}  // namespace planning
}  // namespace apollo
<commit_msg>Planning: record path_data in side_pass.<commit_after>/******************************************************************************
 * Copyright 2018 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

/**
 * @file
 **/

#include "modules/planning/scenarios/side_pass/side_pass_stage.h"

#include <algorithm>
#include <vector>

#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/proto/pnc_point.pb.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/common/speed_profile_generator.h"

namespace apollo {
namespace planning {
namespace scenario {
namespace side_pass {

using apollo::common::TrajectoryPoint;

constexpr double kExtraMarginforStopOnWaitPointStage = 3.0;

/*
 * @brief:
 * STAGE: SidePassApproachObstacle
 */
Stage::StageStatus SidePassApproachObstacle::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  bool plan_ok = PlanningOnReferenceLine(planning_start_point, frame);
  if (!plan_ok) {
    AERROR << "Stage " << Name() << " error: "
           << "planning on reference line failed.";
    return Stage::ERROR;
  }
  const ReferenceLineInfo& reference_line_info =
      frame->reference_line_info().front();
  double adc_velocity = frame->vehicle_state().linear_velocity();
  double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();
  const PathDecision& path_decision = reference_line_info.path_decision();

  double front_obstacle_distance = 1000;
  for (const auto* obstacle : path_decision.obstacles().Items()) {
    if (obstacle->IsVirtual()) {
      continue;
    }

    bool is_on_road = reference_line_info.reference_line().HasOverlap(
        obstacle->PerceptionBoundingBox());
    if (!is_on_road) {
      continue;
    }

    const auto& obstacle_sl = obstacle->PerceptionSLBoundary();
    if (obstacle_sl.end_s() <= reference_line_info.AdcSlBoundary().start_s()) {
      continue;
    }

    double distance = obstacle_sl.start_s() - adc_front_edge_s;
    if (distance < front_obstacle_distance) {
      front_obstacle_distance = distance;
    }
  }

  if ((front_obstacle_distance) < 0) {
    AERROR << "Stage " << Name() << " error: "
           << "front obstacle has wrong position.";
    return Stage::ERROR;
  }
  // TODO(all): stage params need to be in config file
  double max_stop_velocity = 1.0e-5;
  double min_stop_obstacle_distance = 4.0;

  if (adc_velocity < max_stop_velocity &&
      front_obstacle_distance > min_stop_obstacle_distance) {
    next_stage_ = ScenarioConfig::SIDE_PASS_GENERATE_PATH;
    return Stage::FINISHED;
  }
  return Stage::RUNNING;
}

/*
 * @brief:
 * STAGE: SidePassGeneratePath
 */
Stage::StageStatus SidePassGeneratePath::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  if (PlanningOnReferenceLine(planning_start_point, frame)) {
    GetContext()->path_data_ = frame->reference_line_info().front().path_data();
    next_stage_ = ScenarioConfig::SIDE_PASS_STOP_ON_WAITPOINT;
    return Stage::FINISHED;
  } else {
    return Stage::ERROR;
  }
}

/*
 * @brief:
 * STAGE: SidePassDetectSafety
 */

Stage::StageStatus SidePassDetectSafety::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  if (PlanningOnReferenceLine(planning_start_point, frame)) {
    return Stage::ERROR;
  }
  bool is_safe = true;
  const ReferenceLineInfo& reference_line_info =
      frame->reference_line_info().front();
  double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();

  const PathDecision& path_decision =
      frame->reference_line_info().front().path_decision();
  for (const auto* obstacle : path_decision.obstacles().Items()) {
    if (obstacle->IsVirtual() &&
        obstacle->PerceptionSLBoundary().start_s() >= adc_front_edge_s) {
      is_safe = false;
      break;
    }
  }
  if (is_safe) {
    next_stage_ = ScenarioConfig::SIDE_PASS_PASS_OBSTACLE;
    return Stage::FINISHED;
  }
  return Stage::RUNNING;
}

/*
 * @brief:
 * STAGE: SidePassPassObstacle
 */
Stage::StageStatus SidePassPassObstacle::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  bool plan_ok = PlanningOnReferenceLine(planning_start_point, frame);
  if (!plan_ok) {
    return Stage::ERROR;
  }
  const auto& reference_line_info = frame->reference_line_info().front();
  const SLBoundary& adc_sl_boundary = reference_line_info.AdcSlBoundary();
  const auto& end_point =
      reference_line_info.path_data().discretized_path().EndPoint();
  Vec2d last_xy_point(end_point.x(), end_point.y());
  // get s of last point on path
  common::SLPoint sl_point;
  if (!reference_line_info.reference_line().XYToSL(last_xy_point, &sl_point)) {
    AERROR << "Fail to transfer cartesian point to frenet point.";
    return Stage::ERROR;
  }

  if (adc_sl_boundary.end_s() > sl_point.s() - 1.0) {
    next_stage_ = ScenarioConfig::NO_STAGE;
    return Stage::FINISHED;
  }
  return Stage::RUNNING;
}

}  // namespace side_pass
}  // namespace scenario
}  // namespace planning
}  // namespace apollo
<|endoftext|>


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

/**
 * @file
 **/

#include "modules/planning/scenarios/side_pass/side_pass_stage.h"

#include <algorithm>
#include <vector>

#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/proto/pnc_point.pb.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/common/speed_profile_generator.h"

namespace apollo {
namespace planning {
namespace scenario {
namespace side_pass {

using apollo::common::TrajectoryPoint;

constexpr double kExtraMarginforStopOnWaitPointStage = 3.0;

/*
 * @brief:
 * STAGE: SidePassApproachObstacle
 */
Stage::StageStatus SidePassApproachObstacle::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  bool plan_ok = PlanningOnReferenceLine(planning_start_point, frame);
  if (!plan_ok) {
    AERROR << "Stage " << Name() << " error: "
           << "planning on reference line failed.";
    return Stage::ERROR;
  }
  const ReferenceLineInfo& reference_line_info =
      frame->reference_line_info().front();
  double adc_velocity = frame->vehicle_state().linear_velocity();
  double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();
  const PathDecision& path_decision = reference_line_info.path_decision();

  double front_obstacle_distance = 1000;
  for (const auto* obstacle : path_decision.obstacles().Items()) {
    if (obstacle->IsVirtual()) {
      continue;
    }

    bool is_on_road = reference_line_info.reference_line().HasOverlap(
        obstacle->PerceptionBoundingBox());
    if (!is_on_road) {
      continue;
    }

    const auto& obstacle_sl = obstacle->PerceptionSLBoundary();
    if (obstacle_sl.end_s() <= reference_line_info.AdcSlBoundary().start_s()) {
      continue;
    }

    double distance = obstacle_sl.start_s() - adc_front_edge_s;
    if (distance < front_obstacle_distance) {
      front_obstacle_distance = distance;
    }
  }

  if ((front_obstacle_distance) < 0) {
    AERROR << "Stage " << Name() << " error: "
           << "front obstacle has wrong position.";
    return Stage::ERROR;
  }
  // TODO(all): stage params need to be in config file
  double max_stop_velocity = 1.0e-5;
  double min_stop_obstacle_distance = 4.0;

  if (adc_velocity < max_stop_velocity &&
      front_obstacle_distance > min_stop_obstacle_distance) {
    next_stage_ = ScenarioConfig::SIDE_PASS_GENERATE_PATH;
    return Stage::FINISHED;
  }
  return Stage::RUNNING;
}

/*
 * @brief:
 * STAGE: SidePassGeneratePath
 */
Stage::StageStatus SidePassGeneratePath::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  if (PlanningOnReferenceLine(planning_start_point, frame)) {
    GetContext()->path_data_ = frame->reference_line_info().front().path_data();
    if (GetContext()->path_data_.discretized_path().Length() > 0.0) {
      next_stage_ = ScenarioConfig::SIDE_PASS_STOP_ON_WAITPOINT;
      return Stage::FINISHED;
    } else {
      return Stage::RUNNING;
    }
  }
  return Stage::ERROR;
}

/*
 * @brief:
 * STAGE: SidePassDetectSafety
 */

Stage::StageStatus SidePassDetectSafety::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  const auto& reference_line_info = frame->reference_line_info().front();
  bool update_success = GetContext()->path_data_.UpdateFrenetFramePath(
      &reference_line_info.reference_line());
  if (!update_success) {
    AERROR << "Fail to update path_data.";
    return Stage::ERROR;
  }

  const auto adc_frenet_frame_point_ =
      reference_line_info.reference_line().GetFrenetPoint(
          frame->PlanningStartPoint());

  bool trim_success = GetContext()->path_data_.LeftTrimWithRefS(
      adc_frenet_frame_point_.s(), adc_frenet_frame_point_.l());
  if (!trim_success) {
    AERROR << "Fail to trim path_data. adc_frenet_frame_point: "
           << adc_frenet_frame_point_.ShortDebugString();
    return Stage::ERROR;
  }

  auto& rfl_info = frame->mutable_reference_line_info()->front();
  *(rfl_info.mutable_path_data()) = GetContext()->path_data_;

  const auto& path_points =
      rfl_info.path_data().discretized_path().path_points();
  auto* debug_path =
      rfl_info.mutable_debug()->mutable_planning_data()->add_path();

  debug_path->set_name("DpPolyPathOptimizer");
  debug_path->mutable_path_point()->CopyFrom(
      {path_points.begin(), path_points.end()});

  if (!PlanningOnReferenceLine(planning_start_point, frame)) {
    return Stage::ERROR;
  }
  bool is_safe = true;
  double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();

  const PathDecision& path_decision =
      frame->reference_line_info().front().path_decision();
  for (const auto* obstacle : path_decision.obstacles().Items()) {
    // TODO(All): check according to neighbor lane.
    if (obstacle->IsVirtual() && obstacle->Id().substr(0, 3) == "SP_" &&
        obstacle->PerceptionSLBoundary().start_s() >= adc_front_edge_s) {
      is_safe = false;
      break;
    }
  }
  if (is_safe) {
    next_stage_ = ScenarioConfig::SIDE_PASS_PASS_OBSTACLE;
    return Stage::FINISHED;
  }
  return Stage::RUNNING;
}

/*
 * @brief:
 * STAGE: SidePassPassObstacle
 */
Stage::StageStatus SidePassPassObstacle::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  const auto& reference_line_info = frame->reference_line_info().front();
  bool update_success = GetContext()->path_data_.UpdateFrenetFramePath(
      &reference_line_info.reference_line());
  if (!update_success) {
    AERROR << "Fail to update path_data.";
    return Stage::ERROR;
  }

  const auto adc_frenet_frame_point_ =
      reference_line_info.reference_line().GetFrenetPoint(
          frame->PlanningStartPoint());

  bool trim_success = GetContext()->path_data_.LeftTrimWithRefS(
      adc_frenet_frame_point_.s(), adc_frenet_frame_point_.l());
  if (!trim_success) {
    AERROR << "Fail to trim path_data. adc_frenet_frame_point: "
           << adc_frenet_frame_point_.ShortDebugString();
    return Stage::ERROR;
  }

  auto& rfl_info = frame->mutable_reference_line_info()->front();
  *(rfl_info.mutable_path_data()) = GetContext()->path_data_;

  const auto& path_points =
      rfl_info.path_data().discretized_path().path_points();
  auto* debug_path =
      rfl_info.mutable_debug()->mutable_planning_data()->add_path();

  // TODO(All):
  // Have to use DpPolyPathOptimizer to show in dreamview. Need change to
  // correct name.
  debug_path->set_name("DpPolyPathOptimizer");
  debug_path->mutable_path_point()->CopyFrom(
      {path_points.begin(), path_points.end()});

  bool plan_ok = PlanningOnReferenceLine(planning_start_point, frame);
  if (!plan_ok) {
    AERROR << "Fail to plan on reference line.";
    return Stage::ERROR;
  }

  const SLBoundary& adc_sl_boundary = reference_line_info.AdcSlBoundary();
  const auto& end_point =
      reference_line_info.path_data().discretized_path().EndPoint();
  Vec2d last_xy_point(end_point.x(), end_point.y());
  // get s of last point on path
  common::SLPoint sl_point;
  if (!reference_line_info.reference_line().XYToSL(last_xy_point, &sl_point)) {
    AERROR << "Fail to transfer cartesian point to frenet point.";
    return Stage::ERROR;
  }

  if (adc_sl_boundary.end_s() > sl_point.s() - 1.0) {
    next_stage_ = ScenarioConfig::NO_STAGE;
    return Stage::FINISHED;
  }
  return Stage::RUNNING;
}

}  // namespace side_pass
}  // namespace scenario
}  // namespace planning
}  // namespace apollo
<commit_msg>planning: fixed side pass.<commit_after>/******************************************************************************
 * Copyright 2018 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

/**
 * @file
 **/

#include "modules/planning/scenarios/side_pass/side_pass_stage.h"

#include <algorithm>
#include <vector>

#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/proto/pnc_point.pb.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/common/speed_profile_generator.h"

namespace apollo {
namespace planning {
namespace scenario {
namespace side_pass {

using apollo::common::TrajectoryPoint;

constexpr double kExtraMarginforStopOnWaitPointStage = 3.0;

/*
 * @brief:
 * STAGE: SidePassApproachObstacle
 */
Stage::StageStatus SidePassApproachObstacle::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  bool plan_ok = PlanningOnReferenceLine(planning_start_point, frame);
  if (!plan_ok) {
    AERROR << "Stage " << Name() << " error: "
           << "planning on reference line failed.";
    return Stage::ERROR;
  }
  const ReferenceLineInfo& reference_line_info =
      frame->reference_line_info().front();
  double adc_velocity = frame->vehicle_state().linear_velocity();
  double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();
  const PathDecision& path_decision = reference_line_info.path_decision();

  double front_obstacle_distance = 1000;
  for (const auto* obstacle : path_decision.obstacles().Items()) {
    if (obstacle->IsVirtual()) {
      continue;
    }

    bool is_on_road = reference_line_info.reference_line().HasOverlap(
        obstacle->PerceptionBoundingBox());
    if (!is_on_road) {
      continue;
    }

    const auto& obstacle_sl = obstacle->PerceptionSLBoundary();
    if (obstacle_sl.end_s() <= reference_line_info.AdcSlBoundary().start_s()) {
      continue;
    }

    double distance = obstacle_sl.start_s() - adc_front_edge_s;
    if (distance < front_obstacle_distance) {
      front_obstacle_distance = distance;
    }
  }

  if ((front_obstacle_distance) < 0) {
    AERROR << "Stage " << Name() << " error: "
           << "front obstacle has wrong position.";
    return Stage::ERROR;
  }
  // TODO(all): stage params need to be in config file
  double max_stop_velocity = 1.0e-5;
  double min_stop_obstacle_distance = 4.0;

  if (adc_velocity < max_stop_velocity &&
      front_obstacle_distance > min_stop_obstacle_distance) {
    next_stage_ = ScenarioConfig::SIDE_PASS_GENERATE_PATH;
    return Stage::FINISHED;
  }
  return Stage::RUNNING;
}

/*
 * @brief:
 * STAGE: SidePassGeneratePath
 */
Stage::StageStatus SidePassGeneratePath::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  if (PlanningOnReferenceLine(planning_start_point, frame)) {
    GetContext()->path_data_ = frame->reference_line_info().front().path_data();
    if (frame->reference_line_info().front().TrajectoryLength() > 0.0) {
      next_stage_ = ScenarioConfig::SIDE_PASS_STOP_ON_WAITPOINT;
      return Stage::FINISHED;
    } else {
      return Stage::RUNNING;
    }
  }
  return Stage::ERROR;
}

/*
 * @brief:
 * STAGE: SidePassDetectSafety
 */

Stage::StageStatus SidePassDetectSafety::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  const auto& reference_line_info = frame->reference_line_info().front();
  bool update_success = GetContext()->path_data_.UpdateFrenetFramePath(
      &reference_line_info.reference_line());
  if (!update_success) {
    AERROR << "Fail to update path_data.";
    return Stage::ERROR;
  }

  const auto adc_frenet_frame_point_ =
      reference_line_info.reference_line().GetFrenetPoint(
          frame->PlanningStartPoint());

  bool trim_success = GetContext()->path_data_.LeftTrimWithRefS(
      adc_frenet_frame_point_.s(), adc_frenet_frame_point_.l());
  if (!trim_success) {
    AERROR << "Fail to trim path_data. adc_frenet_frame_point: "
           << adc_frenet_frame_point_.ShortDebugString();
    return Stage::ERROR;
  }

  auto& rfl_info = frame->mutable_reference_line_info()->front();
  *(rfl_info.mutable_path_data()) = GetContext()->path_data_;

  const auto& path_points =
      rfl_info.path_data().discretized_path().path_points();
  auto* debug_path =
      rfl_info.mutable_debug()->mutable_planning_data()->add_path();

  debug_path->set_name("DpPolyPathOptimizer");
  debug_path->mutable_path_point()->CopyFrom(
      {path_points.begin(), path_points.end()});

  if (!PlanningOnReferenceLine(planning_start_point, frame)) {
    return Stage::ERROR;
  }
  bool is_safe = true;
  double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();

  const PathDecision& path_decision =
      frame->reference_line_info().front().path_decision();
  for (const auto* obstacle : path_decision.obstacles().Items()) {
    // TODO(All): check according to neighbor lane.
    if (obstacle->IsVirtual() && obstacle->Id().substr(0, 3) == "SP_" &&
        obstacle->PerceptionSLBoundary().start_s() >= adc_front_edge_s) {
      is_safe = false;
      break;
    }
  }
  if (is_safe) {
    next_stage_ = ScenarioConfig::SIDE_PASS_PASS_OBSTACLE;
    return Stage::FINISHED;
  }
  return Stage::RUNNING;
}

/*
 * @brief:
 * STAGE: SidePassPassObstacle
 */
Stage::StageStatus SidePassPassObstacle::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  const auto& reference_line_info = frame->reference_line_info().front();
  bool update_success = GetContext()->path_data_.UpdateFrenetFramePath(
      &reference_line_info.reference_line());
  if (!update_success) {
    AERROR << "Fail to update path_data.";
    return Stage::ERROR;
  }

  const auto adc_frenet_frame_point_ =
      reference_line_info.reference_line().GetFrenetPoint(
          frame->PlanningStartPoint());

  bool trim_success = GetContext()->path_data_.LeftTrimWithRefS(
      adc_frenet_frame_point_.s(), adc_frenet_frame_point_.l());
  if (!trim_success) {
    AERROR << "Fail to trim path_data. adc_frenet_frame_point: "
           << adc_frenet_frame_point_.ShortDebugString();
    return Stage::ERROR;
  }

  auto& rfl_info = frame->mutable_reference_line_info()->front();
  *(rfl_info.mutable_path_data()) = GetContext()->path_data_;

  const auto& path_points =
      rfl_info.path_data().discretized_path().path_points();
  auto* debug_path =
      rfl_info.mutable_debug()->mutable_planning_data()->add_path();

  // TODO(All):
  // Have to use DpPolyPathOptimizer to show in dreamview. Need change to
  // correct name.
  debug_path->set_name("DpPolyPathOptimizer");
  debug_path->mutable_path_point()->CopyFrom(
      {path_points.begin(), path_points.end()});

  bool plan_ok = PlanningOnReferenceLine(planning_start_point, frame);
  if (!plan_ok) {
    AERROR << "Fail to plan on reference line.";
    return Stage::ERROR;
  }

  const SLBoundary& adc_sl_boundary = reference_line_info.AdcSlBoundary();
  const auto& end_point =
      reference_line_info.path_data().discretized_path().EndPoint();
  Vec2d last_xy_point(end_point.x(), end_point.y());
  // get s of last point on path
  common::SLPoint sl_point;
  if (!reference_line_info.reference_line().XYToSL(last_xy_point, &sl_point)) {
    AERROR << "Fail to transfer cartesian point to frenet point.";
    return Stage::ERROR;
  }

  if (adc_sl_boundary.end_s() > sl_point.s() - 1.0) {
    next_stage_ = ScenarioConfig::NO_STAGE;
    return Stage::FINISHED;
  }
  return Stage::RUNNING;
}

}  // namespace side_pass
}  // namespace scenario
}  // namespace planning
}  // namespace apollo
<|endoftext|>


<commit_before>#include <mapnik/timer.hpp>
#include <mapnik/util/file_io.hpp>
#include "vector_tile_backend_pbf.hpp"
#include "vector_tile_processor.hpp"

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"
#pragma GCC diagnostic ignored "-Wsign-conversion"
#include "vector_tile.pb.h"
#pragma GCC diagnostic pop

#include <iostream>
#include <fstream>

// mapnik
#include <mapnik/box2d.hpp>
#include <mapnik/datasource_cache.hpp>
#include <mapnik/global.hpp>
#include <mapnik/well_known_srs.hpp>
#include <mapnik/json/geometry_parser.hpp>
#include <mapnik/memory_datasource.hpp>
#include <mapnik/feature_factory.hpp>
#include <mapnik/map.hpp>
#include <mapnik/layer.hpp>
#include <mapnik/request.hpp>

#include <protozero/pbf_reader.hpp>

typedef mapnik::vector_tile_impl::backend_pbf backend_type;
typedef mapnik::vector_tile_impl::processor<backend_type> renderer_type;

void from_pixels(double shift, double & x, double & y)
{
    double b = shift/2.0;
    x = (x - b)/(shift/360.0);
    double g = (y - b)/-(shift/(2 * M_PI));
    y = mapnik::R2D * (2.0 * std::atan(std::exp(g)) - mapnik::M_PI_by2);
}

void xyz(int tile_size,
    int x,
    int y,
    int z,
    double & minx,
    double & miny,
    double & maxx,
    double & maxy)
{
    minx = x * tile_size;
    miny = (y + 1.0) * tile_size;
    maxx = (x + 1.0) * tile_size;
    maxy = y * tile_size;
    double shift = std::pow(2.0,z) * tile_size;
    from_pixels(shift,minx,miny);
    from_pixels(shift,maxx,maxy);
    mapnik::lonlat2merc(&minx,&miny,1);
    mapnik::lonlat2merc(&maxx,&maxy,1);
}

int main(int argc, char** argv)
{
    try
    {

        if (argc < 4)
        {
            std::clog << "usage: vtile-encode /path/to/geometry.geojson z x y [iterations]\n";
            return -1;
        }
        std::string geojson_file(argv[1]);

        int z = std::stoi(argv[2]);
        int x = std::stoi(argv[3]);
        int y = std::stoi(argv[4]);

        std::size_t iterations = 1000;
        std::string output_path = "";
        if (argc > 5)
        {
            for (int i = 5; i < argc; i++)
            {
                std::string flag = argv[i];
                if (flag == "-i")
                {
                    iterations = std::stoi(argv[i + 1]);
                }
                if (flag == "-o")
                {
                    output_path = argv[i + 1];
                }
            }
        }

        std::clog << "z:" << z << " x:" << x << " y:" << y <<  " iterations:" << iterations << std::endl;

        mapnik::datasource_cache::instance().register_datasources(MAPNIK_PLUGINDIR);
        mapnik::parameters params_1;
        params_1["type"] = "geojson";
        params_1["file"] = geojson_file;
        params_1["cache_features"] = "false";
        auto ds_1 = mapnik::datasource_cache::instance().create(params_1);

        // Query
        mapnik::box2d<double> query_ext(std::numeric_limits<double>::min(),
                                        std::numeric_limits<double>::min(),
                                        std::numeric_limits<double>::max(),
                                        std::numeric_limits<double>::max());
        mapnik::query q(query_ext);
        auto json_fs = ds_1->features(q);

        // Create memory datasource from geojson
        mapnik::parameters params;
        params["type"] = "memory";
        auto ds = std::make_shared<mapnik::memory_datasource>(params);

        if (!json_fs)
        {
            // No features in geojson so lets try to process it differently as
            // it might be a partial geojson and we can use from_geojson
            mapnik::util::file input(geojson_file);
            if (!input.open())
            {
                std::clog << "failed to open " << geojson_file << std::endl;
                return -1;
            }
            mapnik::geometry::geometry<double> geom;
            std::string json_string(input.data().get(), input.size());
            if (!mapnik::json::from_geojson(json_string, geom))
            {
                std::clog << "failed to parse geojson" << std::endl;
                return -1;
            }
            mapnik::context_ptr ctx = std::make_shared<mapnik::context_type>();
            mapnik::feature_ptr feature(mapnik::feature_factory::create(ctx,1));
            feature->set_geometry(std::move(geom));
            ds->push(feature);
        }
        else
        {
            mapnik::feature_ptr feature = json_fs->next();

            while (feature)
            {
                ds->push(feature);
                feature = json_fs->next();
            }
        }

        // Create tile
        unsigned tile_size = 256;

        double minx,miny,maxx,maxy;
        xyz(tile_size, x, y, z, minx, miny, maxx, maxy);
        mapnik::box2d<double> bbox(minx,miny,maxx,maxy);

        // Create a fresh map to render into a tile
        mapnik::Map map(tile_size,tile_size,"+init=epsg:3857");
        mapnik::layer lyr("layer","+init=epsg:4326");
        lyr.set_datasource(ds);
        map.add_layer(lyr);
        map.zoom_to_box(bbox);

        // Output buffer
        std::string output_buffer;
        std::size_t expected_size;
        {
            vector_tile::Tile tile;
            mapnik::vector_tile_impl::backend_pbf backend(tile, 16);
            mapnik::request m_req(tile_size,tile_size,bbox);
            renderer_type ren(backend,map,m_req);
            ren.apply();
            tile.SerializeToString(&output_buffer);
            expected_size = output_buffer.size();
        }

        {
            mapnik::progress_timer __stats__(std::clog, std::string("encode tile: ") + geojson_file);
            for (std::size_t i = 0; i < iterations; ++i)
            {
                vector_tile::Tile tile;
                mapnik::vector_tile_impl::backend_pbf backend(tile, 16);
                mapnik::request m_req(tile_size,tile_size,bbox);
                renderer_type ren(backend,map,m_req,1.0,0.0,0.0,0.1,true);
                ren.set_simplify_distance(5.0);
                ren.set_fill_type(mapnik::vector_tile_impl::positive_fill);
                ren.apply();
                std::string buffer;
                tile.SerializeToString(&buffer);
                assert(buffer.size() == expected_size);
            }
        }

        if (output_path != "")
        {
            std::ofstream out(output_path);
            out << output_buffer;
        }
    }
    catch (std::exception const& ex)
    {
        std::clog << "error: " << ex.what() << std::endl;
        return -1;
    }
    return 0;
}
<commit_msg>fixed bug in vtile encoding<commit_after>#include <mapnik/timer.hpp>
#include <mapnik/util/file_io.hpp>
#include "vector_tile_backend_pbf.hpp"
#include "vector_tile_processor.hpp"

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"
#pragma GCC diagnostic ignored "-Wsign-conversion"
#include "vector_tile.pb.h"
#pragma GCC diagnostic pop

#include <iostream>
#include <fstream>

// mapnik
#include <mapnik/box2d.hpp>
#include <mapnik/datasource_cache.hpp>
#include <mapnik/global.hpp>
#include <mapnik/well_known_srs.hpp>
#include <mapnik/json/geometry_parser.hpp>
#include <mapnik/memory_datasource.hpp>
#include <mapnik/feature_factory.hpp>
#include <mapnik/map.hpp>
#include <mapnik/layer.hpp>
#include <mapnik/request.hpp>

#include <protozero/pbf_reader.hpp>

typedef mapnik::vector_tile_impl::backend_pbf backend_type;
typedef mapnik::vector_tile_impl::processor<backend_type> renderer_type;

void from_pixels(double shift, double & x, double & y)
{
    double b = shift/2.0;
    x = (x - b)/(shift/360.0);
    double g = (y - b)/-(shift/(2 * M_PI));
    y = mapnik::R2D * (2.0 * std::atan(std::exp(g)) - mapnik::M_PI_by2);
}

void xyz(int tile_size,
    int x,
    int y,
    int z,
    double & minx,
    double & miny,
    double & maxx,
    double & maxy)
{
    minx = x * tile_size;
    miny = (y + 1.0) * tile_size;
    maxx = (x + 1.0) * tile_size;
    maxy = y * tile_size;
    double shift = std::pow(2.0,z) * tile_size;
    from_pixels(shift,minx,miny);
    from_pixels(shift,maxx,maxy);
    mapnik::lonlat2merc(&minx,&miny,1);
    mapnik::lonlat2merc(&maxx,&maxy,1);
}

int main(int argc, char** argv)
{
    try
    {

        if (argc < 4)
        {
            std::clog << "usage: vtile-encode /path/to/geometry.geojson z x y [iterations]\n";
            return -1;
        }
        std::string geojson_file(argv[1]);

        int z = std::stoi(argv[2]);
        int x = std::stoi(argv[3]);
        int y = std::stoi(argv[4]);

        std::size_t iterations = 1000;
        std::string output_path = "";
        if (argc > 5)
        {
            for (int i = 5; i < argc; i++)
            {
                std::string flag = argv[i];
                if (flag == "-i")
                {
                    iterations = std::stoi(argv[i + 1]);
                }
                if (flag == "-o")
                {
                    output_path = argv[i + 1];
                }
            }
        }

        std::clog << "z:" << z << " x:" << x << " y:" << y <<  " iterations:" << iterations << std::endl;

        mapnik::datasource_cache::instance().register_datasources(MAPNIK_PLUGINDIR);
        mapnik::parameters params_1;
        params_1["type"] = "geojson";
        params_1["file"] = geojson_file;
        params_1["cache_features"] = "false";
        auto ds_1 = mapnik::datasource_cache::instance().create(params_1);

        // Query
        mapnik::box2d<double> query_ext(std::numeric_limits<double>::min(),
                                        std::numeric_limits<double>::min(),
                                        std::numeric_limits<double>::max(),
                                        std::numeric_limits<double>::max());
        mapnik::query q(query_ext);
        auto json_fs = ds_1->features(q);

        // Create memory datasource from geojson
        mapnik::parameters params;
        params["type"] = "memory";
        auto ds = std::make_shared<mapnik::memory_datasource>(params);

        if (!json_fs)
        {
            // No features in geojson so lets try to process it differently as
            // it might be a partial geojson and we can use from_geojson
            mapnik::util::file input(geojson_file);
            if (!input.open())
            {
                std::clog << "failed to open " << geojson_file << std::endl;
                return -1;
            }
            mapnik::geometry::geometry<double> geom;
            std::string json_string(input.data().get(), input.size());
            if (!mapnik::json::from_geojson(json_string, geom))
            {
                std::clog << "failed to parse geojson" << std::endl;
                return -1;
            }
            mapnik::context_ptr ctx = std::make_shared<mapnik::context_type>();
            mapnik::feature_ptr feature(mapnik::feature_factory::create(ctx,1));
            feature->set_geometry(std::move(geom));
            ds->push(feature);
        }
        else
        {
            mapnik::feature_ptr feature = json_fs->next();

            while (feature)
            {
                ds->push(feature);
                feature = json_fs->next();
            }
        }

        // Create tile
        unsigned tile_size = 256;

        double minx,miny,maxx,maxy;
        xyz(tile_size, x, y, z, minx, miny, maxx, maxy);
        mapnik::box2d<double> bbox(minx,miny,maxx,maxy);

        // Create a fresh map to render into a tile
        mapnik::Map map(tile_size,tile_size,"+init=epsg:3857");
        mapnik::layer lyr("layer","+init=epsg:4326");
        lyr.set_datasource(ds);
        map.add_layer(lyr);
        map.zoom_to_box(bbox);

        // Output buffer
        std::string output_buffer;
        std::size_t expected_size;
        {
            vector_tile::Tile tile;
            mapnik::vector_tile_impl::backend_pbf backend(tile, 16);
            mapnik::request m_req(tile_size,tile_size,bbox);
            renderer_type ren(backend,map,m_req,1.0,0.0,0.0,0.1,true);
            ren.set_simplify_distance(5.0);
            ren.set_fill_type(mapnik::vector_tile_impl::positive_fill);
            ren.apply();
            tile.SerializeToString(&output_buffer);
            expected_size = output_buffer.size();
        }

        {
            mapnik::progress_timer __stats__(std::clog, std::string("encode tile: ") + geojson_file);
            for (std::size_t i = 0; i < iterations; ++i)
            {
                vector_tile::Tile tile;
                mapnik::vector_tile_impl::backend_pbf backend(tile, 16);
                mapnik::request m_req(tile_size,tile_size,bbox);
                renderer_type ren(backend,map,m_req,1.0,0.0,0.0,0.1,true);
                ren.set_simplify_distance(5.0);
                ren.set_fill_type(mapnik::vector_tile_impl::positive_fill);
                ren.apply();
                std::string buffer;
                tile.SerializeToString(&buffer);
                assert(buffer.size() == expected_size);
            }
        }

        if (output_path != "")
        {
            std::ofstream out(output_path);
            out << output_buffer;
        }
    }
    catch (std::exception const& ex)
    {
        std::clog << "error: " << ex.what() << std::endl;
        return -1;
    }
    return 0;
}
<|endoftext|>


<commit_before>// Config file test for MUON spectormeter
// Remember to define the directory and option
// gAlice->SetConfigFunction("Config('$HOME','box');");

void Config(char directory[100]="", char option[6]="param")
{
  //
  // Config file for MUON test
  //

  //=====================================================================
  //  Libraries required by geant321
  gSystem->Load("libgeant321.so");
  new TGeant3TGeo("C++ Interface to Geant3");
  //=======================================================================
  //  Create the output file
  Text_t filename[100];
  sprintf(filename,"%sgalice.root",directory);
  cout << ">>> Output file is " << filename << endl;
  cout << ">>> Config.C: Creating Run Loader ..."<<endl;
  AliRunLoader* rl=0x0;
  rl = AliRunLoader::Open(
	filename, AliConfig::GetDefaultEventFolderName(), "recreate");
  if (rl == 0x0) {
    gAlice->Fatal("Config.C","Can not instatiate the Run Loader");
    return;
  }
  rl->SetCompressionLevel(2);
  rl->SetNumberOfEventsPerFile(100);
  gAlice->SetRunLoader(rl);

  //AliLog::SetModuleDebugLevel("MUON", 1);

  //=======================================================================
  // Set External decayer
  TVirtualMCDecayer *decayer = new AliDecayerPythia();
  decayer->SetForceDecay(kAll);
  decayer->Init();
  gMC->SetExternalDecayer(decayer);

  //
  //=======================================================================
  // ******* GEANT STEERING parameters FOR ALICE SIMULATION *******
    gMC->SetProcess("DCAY",1);
    gMC->SetProcess("PAIR",1);
    gMC->SetProcess("COMP",1);
    gMC->SetProcess("PHOT",1);
    gMC->SetProcess("PFIS",0);
    gMC->SetProcess("DRAY",0);
    gMC->SetProcess("ANNI",1);
    gMC->SetProcess("BREM",1);
    gMC->SetProcess("MUNU",1);
    gMC->SetProcess("CKOV",1);
    gMC->SetProcess("HADR",1);
    gMC->SetProcess("LOSS",2);
    gMC->SetProcess("MULS",1);
    gMC->SetProcess("RAYL",1);

    Float_t cut = 1.e-3;        // 1MeV cut by default
    Float_t tofmax = 1.e10;

    gMC->SetCut("CUTGAM", cut);
    gMC->SetCut("CUTELE", cut);
    gMC->SetCut("CUTNEU", cut);
    gMC->SetCut("CUTHAD", cut);
    gMC->SetCut("CUTMUO", cut);
    gMC->SetCut("BCUTE",  cut);
    gMC->SetCut("BCUTM",  cut);
    gMC->SetCut("DCUTE",  cut);
    gMC->SetCut("DCUTM",  cut);
    gMC->SetCut("PPCUTM", cut);
    gMC->SetCut("TOFMAX", tofmax);
  //
  //=======================================================================
  // ************* STEERING parameters FOR ALICE SIMULATION **************
  // Chamber positions
  // From AliMUONConstants class we get :
  //   Position Z (along beam) of the chambers (in cm)
  //        (from AliMUONConstants class):
  //    533.5,  546.5,  678.5, 693.5,  964.0, 986.0, 1251.5, 1278.5,
  //   1416.5, 1443.5,  1610, 1625.,  1710., 1725.
  //   Internal Radius (in cm)
  //     36.4,  46.2,  66.0,  80.,  80., 100., 100.
  //   External Radius (in cm)
  //    183.,  245.,  395.,  560., 563., 850., 900.
  //=======================================================================
  if (!strcmp(option,"box")) {
    AliGenBox * gener = new AliGenBox(1);
    gener->SetMomentumRange(20.,20.1);
    gener->SetPhiRange(0., 360.);
    gener->SetThetaRange(171.000,178.001);
    gener->SetPart(13);           // Muons
    gener->SetOrigin(0.,0., 0.);  //vertex position
    gener->SetSigma(0.0, 0.0, 0.0);         //Sigma in (X,Y,Z) (cm) on IP position
  }
  if (!strcmp(option,"gun")) {
    //*********************************************
    // Example for Fixed Particle Gun             *
    //*********************************************
    AliGenFixed *gener = new AliGenFixed(ntracks);
    gener->SetMomentum(10);
    gener->SetPhiRange(0.);
    gener->SetThetaRange(0.);
    gener->SetOrigin(30,30,1200);//vertex position
    gener->SetPart(13);          //GEANT particle type  13 is muons
  }
  if (!strcmp(option,"scan")) {
    AliGenScan *gener = new AliGenScan(-1);
    gener->SetMomentumRange(10,10);
    gener->SetPhiRange(0, 0);
    gener->SetThetaRange(-180, -180);
    //vertex position
    //gener->SetSigma(1,1,0);           //Sigma in (X,Y,Z) (cm) on IP position
    gener->SetPart(kRootino);
    gener->SetRange(100, -300., 300., 100, -300., 300., 1, 2000, 2000);
  }
  if (!strcmp(option,"param")) {
    //*******************************************************
    // Example for J/psi or Upsilon Production from  Parameterisation *
    //*******************************************************
    AliGenParam *gener = new AliGenParam(1, AliGenMUONlib::kUpsilon);
    gener->SetMomentumRange(0,999);
    gener->SetPtRange(0,100.);
    gener->SetPhiRange(0., 360.);
    gener->SetCutOnChild(1);
    gener->SetChildPhiRange(0.,360.);
    gener->SetChildThetaRange(171.0,178.0);
    gener->SetOrigin(0,0,0);          //vertex position    gener->SetSigma(0,0,0);           //Sigma in (X,Y,Z) (cm) on IP position
    gener->SetForceDecay(kDiMuon);
    gener->SetTrackingFlag(1);
    gener->Init();
  }
  //=============================================================
  // Field (L3 0.4 T)
  AliMagFMaps* field = new AliMagFMaps("Maps","Maps", 1, 1., 10., AliMagFMaps::k4kG);
  gAlice->SetField(field);

  //=================== Alice BODY parameters =============================
  AliBODY *BODY = new AliBODY("BODY","Alice envelop");
  //=================== ABSO parameters ============================
  AliABSO *ABSO = new AliABSOv0("ABSO", "Muon Absorber");
  //=================== DIPO parameters ============================
  AliDIPO *DIPO = new AliDIPOv2("DIPO", "Dipole version 2");
  //================== HALL parameters ============================
  AliHALL *HALL = new AliHALL("HALL", "Alice Hall");
  //=================== PIPE parameters ============================
  AliPIPE *PIPE = new AliPIPEv0("PIPE", "Beam Pipe");
  //=================== SHIL parameters ============================
  AliSHIL *SHIL = new AliSHILv2("SHIL", "Shielding Version 2");
  //=================== MUON Subsystem ===========================
  cout << ">>> Config.C: Creating AliMUONv1 ..."<<endl;

  // New MUONv1 version (geometry defined via builders)
  //
  //AliMUON *MUON = new AliMUONv1("MUON", "default");
  //
  AliMUON *MUON = new AliMUONv1("MUON", "FactoryV3"); // New segmentation slats
  //
  //AliMUON *MUON = new AliMUONv1("MUON", "FactoryV4"); // New segmentation trigger
  //
  //Version below to get digitizer making a decalibration.
  //The 4-th parameter should be put to digitizer:NewDigitizerNewTrigger
  //to test latest and greatest trigger code (from Rachid) as well.
  //
  //AliMUON *MUON = new AliMUONv1("MUON", "FactoryV4",
  //                              "sdigitizer:AliMUONSDigitizerV2",
  //                              "digitizer:NewDigitizerOldTrigger");
  //AliMUON *MUON = new AliMUONv1("MUON", "FactoryV4",
  //                              "sdigitizer:AliMUONSDigitizerV2",
  //                              "digitizer:NewDigitizerNewTrigger");

  // If SetAlign, the detection elements transformations
  // are taken from the input file and not from the code
  // MUON->SetAlign("transform.dat");

  // To generate and read scaler trigger events in rawdata
  // MUON->SetTriggerScalerEvent();

  MUON->AddGeometryBuilder(new AliMUONSt1GeometryBuilderV2(MUON));
  MUON->AddGeometryBuilder(new AliMUONSt2GeometryBuilderV2(MUON));
  MUON->AddGeometryBuilder(new AliMUONSlatGeometryBuilder(MUON));
  MUON->AddGeometryBuilder(new AliMUONTriggerGeometryBuilder(MUON));
}

Float_t EtaToTheta(Float_t arg){
  return (180./TMath::Pi())*2.*atan(exp(-arg));
}
<commit_msg>Changing the default way to create AliMUON object. (Laurent)<commit_after>// Config file test for MUON spectormeter
// Remember to define the directory and option
// gAlice->SetConfigFunction("Config('$HOME','box');");

void Config(char directory[100]="", char option[6]="param")
{
  //
  // Config file for MUON test
  //

  //=====================================================================
  //  Libraries required by geant321
  gSystem->Load("libgeant321.so");
  new TGeant3TGeo("C++ Interface to Geant3");
  //=======================================================================
  //  Create the output file
  Text_t filename[100];
  sprintf(filename,"%sgalice.root",directory);
  cout << ">>> Output file is " << filename << endl;
  cout << ">>> Config.C: Creating Run Loader ..."<<endl;
  AliRunLoader* rl=0x0;
  rl = AliRunLoader::Open(
	filename, AliConfig::GetDefaultEventFolderName(), "recreate");
  if (rl == 0x0) {
    gAlice->Fatal("Config.C","Can not instatiate the Run Loader");
    return;
  }
  rl->SetCompressionLevel(2);
  rl->SetNumberOfEventsPerFile(100);
  gAlice->SetRunLoader(rl);

  //AliLog::SetModuleDebugLevel("MUON", 1);

  //=======================================================================
  // Set External decayer
  TVirtualMCDecayer *decayer = new AliDecayerPythia();
  decayer->SetForceDecay(kAll);
  decayer->Init();
  gMC->SetExternalDecayer(decayer);

  //
  //=======================================================================
  // ******* GEANT STEERING parameters FOR ALICE SIMULATION *******
    gMC->SetProcess("DCAY",1);
    gMC->SetProcess("PAIR",1);
    gMC->SetProcess("COMP",1);
    gMC->SetProcess("PHOT",1);
    gMC->SetProcess("PFIS",0);
    gMC->SetProcess("DRAY",0);
    gMC->SetProcess("ANNI",1);
    gMC->SetProcess("BREM",1);
    gMC->SetProcess("MUNU",1);
    gMC->SetProcess("CKOV",1);
    gMC->SetProcess("HADR",1);
    gMC->SetProcess("LOSS",2);
    gMC->SetProcess("MULS",1);
    gMC->SetProcess("RAYL",1);

    Float_t cut = 1.e-3;        // 1MeV cut by default
    Float_t tofmax = 1.e10;

    gMC->SetCut("CUTGAM", cut);
    gMC->SetCut("CUTELE", cut);
    gMC->SetCut("CUTNEU", cut);
    gMC->SetCut("CUTHAD", cut);
    gMC->SetCut("CUTMUO", cut);
    gMC->SetCut("BCUTE",  cut);
    gMC->SetCut("BCUTM",  cut);
    gMC->SetCut("DCUTE",  cut);
    gMC->SetCut("DCUTM",  cut);
    gMC->SetCut("PPCUTM", cut);
    gMC->SetCut("TOFMAX", tofmax);
  //
  //=======================================================================
  // ************* STEERING parameters FOR ALICE SIMULATION **************
  // Chamber positions
  // From AliMUONConstants class we get :
  //   Position Z (along beam) of the chambers (in cm)
  //        (from AliMUONConstants class):
  //    533.5,  546.5,  678.5, 693.5,  964.0, 986.0, 1251.5, 1278.5,
  //   1416.5, 1443.5,  1610, 1625.,  1710., 1725.
  //   Internal Radius (in cm)
  //     36.4,  46.2,  66.0,  80.,  80., 100., 100.
  //   External Radius (in cm)
  //    183.,  245.,  395.,  560., 563., 850., 900.
  //=======================================================================
  if (!strcmp(option,"box")) {
    AliGenBox * gener = new AliGenBox(1);
    gener->SetMomentumRange(20.,20.1);
    gener->SetPhiRange(0., 360.);
    gener->SetThetaRange(171.000,178.001);
    gener->SetPart(13);           // Muons
    gener->SetOrigin(0.,0., 0.);  //vertex position
    gener->SetSigma(0.0, 0.0, 0.0);         //Sigma in (X,Y,Z) (cm) on IP position
  }
  if (!strcmp(option,"gun")) {
    //*********************************************
    // Example for Fixed Particle Gun             *
    //*********************************************
    AliGenFixed *gener = new AliGenFixed(ntracks);
    gener->SetMomentum(10);
    gener->SetPhiRange(0.);
    gener->SetThetaRange(0.);
    gener->SetOrigin(30,30,1200);//vertex position
    gener->SetPart(13);          //GEANT particle type  13 is muons
  }
  if (!strcmp(option,"scan")) {
    AliGenScan *gener = new AliGenScan(-1);
    gener->SetMomentumRange(10,10);
    gener->SetPhiRange(0, 0);
    gener->SetThetaRange(-180, -180);
    //vertex position
    //gener->SetSigma(1,1,0);           //Sigma in (X,Y,Z) (cm) on IP position
    gener->SetPart(kRootino);
    gener->SetRange(100, -300., 300., 100, -300., 300., 1, 2000, 2000);
  }
  if (!strcmp(option,"param")) {
    //*******************************************************
    // Example for J/psi or Upsilon Production from  Parameterisation *
    //*******************************************************
    AliGenParam *gener = new AliGenParam(1, AliGenMUONlib::kUpsilon);
    gener->SetMomentumRange(0,999);
    gener->SetPtRange(0,100.);
    gener->SetPhiRange(0., 360.);
    gener->SetCutOnChild(1);
    gener->SetChildPhiRange(0.,360.);
    gener->SetChildThetaRange(171.0,178.0);
    gener->SetOrigin(0,0,0);          //vertex position    gener->SetSigma(0,0,0);           //Sigma in (X,Y,Z) (cm) on IP position
    gener->SetForceDecay(kDiMuon);
    gener->SetTrackingFlag(1);
    gener->Init();
  }
  //=============================================================
  // Field (L3 0.4 T)
  AliMagFMaps* field = new AliMagFMaps("Maps","Maps", 1, 1., 10., AliMagFMaps::k4kG);
  gAlice->SetField(field);

  //=================== Alice BODY parameters =============================
  AliBODY *BODY = new AliBODY("BODY","Alice envelop");
  //=================== ABSO parameters ============================
  AliABSO *ABSO = new AliABSOv0("ABSO", "Muon Absorber");
  //=================== DIPO parameters ============================
  AliDIPO *DIPO = new AliDIPOv2("DIPO", "Dipole version 2");
  //================== HALL parameters ============================
  AliHALL *HALL = new AliHALL("HALL", "Alice Hall");
  //=================== PIPE parameters ============================
  AliPIPE *PIPE = new AliPIPEv0("PIPE", "Beam Pipe");
  //=================== SHIL parameters ============================
  AliSHIL *SHIL = new AliSHILv2("SHIL", "Shielding Version 2");

  //=================== MUON Subsystem ===========================
  cout << ">>> Config.C: Creating AliMUONv1 ..."<<endl;

  // With the following compact ctor, what you get (in case you wonder...) is :
  //
  // - "FactoryV4", that is all stations using new segmentations/mapping
  // - "sdigitizer:AliMUONSDigitizerV2", performing decalibration
  // - "digitizer:NewDigitizerOldTrigger" <=> digitizer=AliMUONDigitizerV3,
  //    using the "old" trigger code, performing calibration
  //
  AliMUON *MUON = new AliMUONv1("MUON");

  //To get old behavior (which usage is no longer supported), please use :
  //
  //  AliMUON* MUON = new AliMUONv1("MUON","FactoryV3",
  //                                "AliMUONSDigitizerv1",
  //                                "AliMUONDigitizerv2");
  //
  //To get brand new trigger code, please use :
  //
  // AliMUON *MUON = new AliMUONv1("MUON", "FactoryV4",
  //                              "sdigitizer:AliMUONSDigitizerV2",
  //                              "digitizer:NewDigitizerNewTrigger");

  // If SetAlign, the detection elements transformations
  // are taken from the input file and not from the code
  // MUON->SetAlign("transform.dat");

  // To generate and read scaler trigger events in rawdata
  // MUON->SetTriggerScalerEvent();

  // If you want to play with builders, first reset the geometry builder,
  // and then add yours.
  //  MUON->ResetGeometryBuilder();
  //  MUON->AddGeometryBuilder(new AliMUONSt1GeometryBuilderV2(MUON));
  //  MUON->AddGeometryBuilder(new AliMUONSt2GeometryBuilderV2(MUON));
  //  MUON->AddGeometryBuilder(new AliMUONSlatGeometryBuilder(MUON));
  //  MUON->AddGeometryBuilder(new AliMUONTriggerGeometryBuilder(MUON));
}

Float_t EtaToTheta(Float_t arg){
  return (180./TMath::Pi())*2.*atan(exp(-arg));
}
<|endoftext|>