///////////////////////////////////////////////////////////////////////////////
// chi_square_summand.hpp                                                    //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_PEARSON_CHISQ_INDEPENDENCE_AUX_CHI_SQUARE_SUMMAND_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_PEARSON_CHISQ_INDEPENDENCE_AUX_CHI_SQUARE_SUMMAND_HPP_ER_2010
#include <cmath>
#include <boost/mpl/size.hpp>
#include <boost/mpl/detail/wrapper.hpp>
#include <boost/mpl/vector/vector10.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/for_each.hpp>
#include <boost/numeric/conversion/converter.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/cells/count_matching.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/pearson_chisq/common/chi_square_summand.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace contingency_table{
namespace pearson_chi_square_statistic{
namespace independence_between_aux{

    template<typename T1,typename Keys,typename AccSet>
    struct chi_square_summand
    {

        typedef typename AccSet::weight_type size_;
        typedef typename boost::mpl::size<Keys>::type keys_count_;

        template<typename N>
        chi_square_summand(const AccSet& a, const N& total_count)
            : acc( a ) , denom(
                pow( total_count, keys_count_::value - 1)
            ){}

        template<typename C>
        T1 operator()(const C& cell)const
        {
            namespace ct = contingency_table;
            typedef boost::mpl::detail::wrapper<
                 boost::mpl::vector1< boost::mpl::_ >
            > op_;
            typedef boost::numeric::converter<T1,size_> conv_;
            size_ n = 1;
        	boost::mpl::for_each<Keys,op_>(
                ct::make_fun_count_matcher(acc, cell.first, prod_fun( n ) )
            );
            return pearson_chi_square_statistic::chi_square_summand<T1>(
                conv_::convert( n ) / conv_::convert( denom ),
                cell.second
            );
        }

        struct prod_fun{

            prod_fun(size_& n) : n_( n ){}

            template<typename T>
            void operator()(const T& k)const{ this->n_ *= k; }

            mutable size_& n_;
        };

        const AccSet& acc;
        size_ denom;

    };

}// independence_between_aux
}// pearson_chi_square_statistic
}// contingency_table
}// detail
}// statistics
}// boost

#endif


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#ifndef BOOST_HANA_TEST_LAWS_ITERABLE_HPP
#define BOOST_HANA_TEST_LAWS_ITERABLE_HPP

#include <boost/hana/any_of.hpp>
#include <boost/hana/assert.hpp>
#include <boost/hana/at.hpp>
#include <boost/hana/back.hpp>
#include <boost/hana/bool.hpp>
#include <boost/hana/concept/comparable.hpp>
#include <boost/hana/concept/foldable.hpp>
#include <boost/hana/concept/sequence.hpp>
#include <boost/hana/core/make.hpp>
#include <boost/hana/core/to.hpp>
#include <boost/hana/core/when.hpp>
#include <boost/hana/drop_front.hpp>
#include <boost/hana/drop_front_exactly.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/eval_if.hpp>
#include <boost/hana/find_if.hpp>
#include <boost/hana/for_each.hpp>
#include <boost/hana/front.hpp>
#include <boost/hana/functional/always.hpp>
#include <boost/hana/functional/capture.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/is_empty.hpp>
#include <boost/hana/lazy.hpp>
#include <boost/hana/length.hpp>
#include <boost/hana/minus.hpp>
#include <boost/hana/not.hpp>
#include <boost/hana/optional.hpp>
#include <boost/hana/range.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/base.hpp>

namespace boost { namespace hana { namespace test {
    template <typename It, typename = hana::when<true>>
    struct TestIterable : TestIterable<It, laws> {
        using TestIterable<It, laws>::TestIterable;
    };

    template <typename It>
    struct TestIterable<It, laws> {
        template <typename Xs>
        TestIterable(Xs xs) {
            hana::for_each(xs, [](auto xs) {
                static_assert(Iterable<decltype(xs)>{}, "");

                BOOST_HANA_CONSTANT_CHECK(
                    hana::is_empty(xs) ^iff^ hana::is_empty(hana::to<tuple_tag>(xs))
                );

                only_when_(hana::not_(hana::is_empty(xs)), hana::make_lazy([](auto xs) {
                    BOOST_HANA_CHECK(hana::equal(
                        hana::front(xs),
                        hana::front(hana::to<tuple_tag>(xs))
                    ));

                    BOOST_HANA_CHECK(hana::equal(
                        hana::to<tuple_tag>(hana::drop_front_exactly(xs)),
                        hana::drop_front_exactly(hana::to<tuple_tag>(xs))
                    ));

                    // methods
                    // back(xs) == at(xs, length(xs)-1)
                    BOOST_HANA_CHECK(hana::equal(
                        hana::back(xs),
                        hana::at(xs, hana::minus(hana::length(xs), hana::size_c<1>))
                    ));

                })(xs));

                // drop_front(xs, 0) == xs
                BOOST_HANA_CHECK(hana::equal(
                    hana::drop_front(xs, size_c<0>),
                    xs
                ));

                // at(xs, n) == front(drop_front(xs, n))
                hana::for_each(hana::make_range(size_c<0>, hana::length(xs)),
                hana::capture(xs)([](auto xs, auto n) {
                    BOOST_HANA_CHECK(hana::equal(
                        hana::at(xs, n),
                        hana::front(hana::drop_front(xs, n))
                    ));
                }));

                // Searchable
                hana::eval_if(hana::is_empty(xs),
                    hana::make_lazy([](auto xs) {
                        BOOST_HANA_CONSTANT_CHECK(
                            hana::not_(hana::any_of(xs, hana::always(true_c)))
                        );

                        BOOST_HANA_CONSTANT_CHECK(hana::equal(
                            hana::find_if(xs, hana::always(true_c)),
                            nothing
                        ));
                    })(xs),
                    hana::make_lazy([](auto xs) {
                        BOOST_HANA_CHECK(
                            hana::any_of(xs, hana::always(true_c))
                        );
                        BOOST_HANA_CHECK(
                            hana::not_(hana::any_of(xs, hana::always(false_c)))
                        );

                        BOOST_HANA_CHECK(hana::equal(
                            hana::find_if(xs, hana::always(true_c)),
                            hana::just(hana::front(xs))
                        ));
                    })(xs)
                );

            });
        }
    };

    template <typename S>
    struct TestIterable<S, when<Sequence<S>::value>>
        : TestIterable<S, laws>
    {
        template <int i>
        using x = ct_eq<i>;

        template <int i = 0>
        struct invalid { };

        struct undefined { };

        template <typename Xs>
        TestIterable(Xs xs) : TestIterable<S, laws>{xs} {
            constexpr auto list = make<S>;

            //////////////////////////////////////////////////////////////////
            // front
            //////////////////////////////////////////////////////////////////
            BOOST_HANA_CONSTANT_CHECK(equal(
                front(list(x<0>{})),
                x<0>{}
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                front(list(x<0>{}, invalid<>{})),
                x<0>{}
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                front(list(x<0>{}, invalid<1>{}, invalid<2>{})),
                x<0>{}
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                front(list(1)), 1
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                front(list(1, '2')), 1
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                front(list(1, '2', 3.3)), 1
            ));

            //////////////////////////////////////////////////////////////////
            // back
            //////////////////////////////////////////////////////////////////
            BOOST_HANA_CONSTANT_CHECK(equal(
                back(list(x<0>{})),
                x<0>{}
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                back(list(invalid<0>{}, x<1>{})),
                x<1>{}
            ));
            BOOST_HANA_CONSTANT_CHECK(equal(
                back(list(invalid<0>{}, invalid<1>{}, x<2>{})),
                x<2>{}
            ));

            BOOST_HANA_CONSTEXPR_CHECK(equal(
                back(list(1)), 1
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                back(list(1, '2')), '2'
            ));
            BOOST_HANA_CONSTEXPR_CHECK(equal(
                back(list(1, '2', 3.3)), 3.3
            ));
        }
    };
}}} // end namespace boost::hana::test

#endif // !BOOST_HANA_TEST_LAWS_ITERABLE_HPP


/**
 * \file libs/numeric/ublasx/test/triu.cpp
 *
 * \brief Test suite for the upper-triangular view operation.
 *
 * Copyright (c) 2011, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompwhiching file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublasx/detail/debug.hpp>
#include <boost/numeric/ublasx/operation/triu.hpp>
#include <complex>
#include <cstddef>
#include "libs/numeric/ublasx/test/utils.hpp"

namespace ublas = ::boost::numeric::ublas;
namespace ublasx = ::boost::numeric::ublasx;

static const double tol = 1e-5;

BOOST_UBLASX_TEST_DEF( real_square_matrix_row_major_keq0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real - Square - Row Major - k == 0" );

	typedef double value_type;
	typedef ublas::matrix<value_type,ublas::row_major> matrix_type;

	const ::std::size_t n(4);

	matrix_type A(n,n, value_type(1));

	matrix_type X;
	matrix_type E;

	X = ublasx::triu(A);
	E = ublasx::triangular_matrix<value_type,ublas::upper>(n,n);
	E(0,0) = E(0,1) = E(0,2) = E(0,3)
		   = E(1,1) = E(1,2) = E(1,3)
					= E(2,2) = E(2,3)
							 = E(3,3)
							 = value_type(1);

	BOOST_UBLASX_DEBUG_TRACE( "Input Matrix A=" << A );
	BOOST_UBLASX_DEBUG_TRACE( "triu(A)=" << X );
	BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, n, n, tol );
}

BOOST_UBLASX_TEST_DEF( real_square_matrix_col_major_keq0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real - Square - Column Major - k == 0" );

	typedef double value_type;
	typedef ublas::matrix<value_type,ublas::column_major> matrix_type;

	const ::std::size_t n(4);

	matrix_type A(n,n, value_type(1));

	matrix_type X;
	matrix_type E;

	X = ublasx::triu(A);
	E = ublasx::triangular_matrix<value_type,ublas::upper>(n,n);
	E(0,0) = E(0,1) = E(0,2) = E(0,3)
		   = E(1,1) = E(1,2) = E(1,3)
					= E(2,2) = E(2,3)
							 = E(3,3)
							 = value_type(1);

	BOOST_UBLASX_DEBUG_TRACE( "Input Matrix A=" << A );
	BOOST_UBLASX_DEBUG_TRACE( "triu(A)=" << X );
	BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, n, n, tol );
}

BOOST_UBLASX_TEST_DEF( real_square_matrix_row_major_kgt0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real - Square - Row Major - k > 0" );

	typedef double value_type;
	typedef ublas::matrix<value_type,ublas::row_major> matrix_type;

	const ::std::size_t n(4);

	matrix_type A(n,n, value_type(1));

	matrix_type X;
	matrix_type E;

	E = ublasx::triangular_matrix<value_type,ublas::upper>(n,n);
	E(0,0) = E(0,1) = E(0,2) = E(0,3)
		   = E(1,1) = E(1,2) = E(1,3)
					= E(2,2) = E(2,3)
							 = E(3,3)
							 = value_type(1);

	for (::std::size_t k = 1; k < n; ++k)
	{
		X = ublasx::triu(A, k);

		for (::std::size_t i = 0; i <= (n-k); ++i)
		{
			E(i,i+k-1) = value_type(0);
		}

		//BOOST_UBLASX_DEBUG_TRACE( "E=" << E );
		BOOST_UBLASX_DEBUG_TRACE( "triu(A," << k << ")=" << X );
		BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, n, n, tol );
	}
}

BOOST_UBLASX_TEST_DEF( real_square_matrix_col_major_kgt0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real - Square - Column Major - k > 0" );

	typedef double value_type;
	typedef ublas::matrix<value_type,ublas::column_major> matrix_type;

	const ::std::size_t n(4);

	matrix_type A(n,n, value_type(1));

	matrix_type X;
	matrix_type E;

	E = ublasx::triangular_matrix<value_type,ublas::upper>(n,n);
	E(0,0) = E(0,1) = E(0,2) = E(0,3)
		   = E(1,1) = E(1,2) = E(1,3)
					= E(2,2) = E(2,3)
							 = E(3,3)
							 = value_type(1);

	for (::std::size_t k = 1; k < n; ++k)
	{
		X = ublasx::triu(A, k);

		for (::std::size_t i = 0; i <= (n-k); ++i)
		{
			E(i,i+k-1) = value_type(0);
		}

		//BOOST_UBLASX_DEBUG_TRACE( "E=" << E );
		BOOST_UBLASX_DEBUG_TRACE( "triu(A," << k << ")=" << X );
		BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, n, n, tol );
	}
}

BOOST_UBLASX_TEST_DEF( real_square_matrix_row_major_klt0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real - Square - Row Major - k < 0" );

	typedef double value_type;
	typedef ublas::matrix<value_type,ublas::row_major> matrix_type;

	const ::std::size_t n(4);

	matrix_type A(n,n, value_type(1));

	matrix_type X;
	matrix_type E;

	E = ublasx::scalar_matrix<value_type>(n, n, value_type(1));

	for (::std::ptrdiff_t k = n-1; k >= 0; --k)
	{
		X = ublasx::triu(A, -k);

		for (::std::size_t i = k+1; i < n; ++i)
		{
			E(i,i-k-1) = value_type(0);
		}

		//BOOST_UBLASX_DEBUG_TRACE( "E=" << E );
		BOOST_UBLASX_DEBUG_TRACE( "triu(A," << -k << ")=" << X );
		BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, n, n, tol );
	}
}

BOOST_UBLASX_TEST_DEF( real_square_matrix_col_major_klt0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real - Square - Column Major - k < 0" );

	typedef double value_type;
	typedef ublas::matrix<value_type,ublas::column_major> matrix_type;

	const ::std::size_t n(4);

	matrix_type A(n,n, value_type(1));

	matrix_type X;
	matrix_type E;

	E = ublasx::scalar_matrix<value_type>(n, n, value_type(1));

	for (::std::ptrdiff_t k = n-1; k >= 0; --k)
	{
		X = ublasx::triu(A, -k);

		for (::std::size_t i = k+1; i < n; ++i)
		{
			E(i,i-k-1) = value_type(0);
		}

		//BOOST_UBLASX_DEBUG_TRACE( "E=" << E );
		BOOST_UBLASX_DEBUG_TRACE( "triu(A," << -k << ")=" << X );
		BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, n, n, tol );
	}
}

BOOST_UBLASX_TEST_DEF( real_horizontal_matrix_row_major_keq0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real - Horizontal - Row Major - k == 0" );

	typedef double value_type;
	typedef ublas::matrix<value_type,ublas::row_major> matrix_type;

	const ::std::size_t nr(4);
	const ::std::size_t nc(6);

	matrix_type A(nr,nc, value_type(1));

	matrix_type X;
	matrix_type E;

	X = ublasx::triu(A);
	E = ublasx::triangular_matrix<value_type,ublas::upper>(nr,nc);
	E(0,0) = E(0,1) = E(0,2) = E(0,3) = E(0,4) = E(0,5)
		   = E(1,1) = E(1,2) = E(1,3) = E(1,4) = E(1,5)
					= E(2,2) = E(2,3) = E(2,4) = E(2,5)
							 = E(3,3) = E(3,4) = E(3,5)
							 = value_type(1);

	BOOST_UBLASX_DEBUG_TRACE( "Input Matrix A=" << A );
	BOOST_UBLASX_DEBUG_TRACE( "triu(A)=" << X );
	BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, nr, nc, tol );
}

BOOST_UBLASX_TEST_DEF( real_horizontal_matrix_col_major_keq0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real - Horizontal - Column Major - k == 0" );

	typedef double value_type;
	typedef ublas::matrix<value_type,ublas::column_major> matrix_type;

	const ::std::size_t nr(4);
	const ::std::size_t nc(6);

	matrix_type A(nr,nc, value_type(1));

	matrix_type X;
	matrix_type E;

	X = ublasx::triu(A);
	E = ublasx::triangular_matrix<value_type,ublas::upper>(nr,nc);
	E(0,0) = E(0,1) = E(0,2) = E(0,3) = E(0,4) = E(0,5)
		   = E(1,1) = E(1,2) = E(1,3) = E(1,4) = E(1,5)
					= E(2,2) = E(2,3) = E(2,4) = E(2,5)
							 = E(3,3) = E(3,4) = E(3,5)
							 = value_type(1);

	BOOST_UBLASX_DEBUG_TRACE( "Input Matrix A=" << A );
	BOOST_UBLASX_DEBUG_TRACE( "triu(A)=" << X );
	BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, nr, nc, tol );
}

BOOST_UBLASX_TEST_DEF( real_horizontal_matrix_row_major_kgt0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real - Horizontal - Row Major - k > 0" );

	typedef double value_type;
	typedef ublas::matrix<value_type,ublas::row_major> matrix_type;

	const ::std::size_t nr(4);
	const ::std::size_t nc(6);

	matrix_type A(nr,nc, value_type(1));

	matrix_type X;
	matrix_type E;

	E = ublasx::triangular_matrix<value_type,ublas::upper>(nr,nc);
	E(0,0) = E(0,1) = E(0,2) = E(0,3) = E(0,4) = E(0,5)
		   = E(1,1) = E(1,2) = E(1,3) = E(1,4) = E(1,5)
					= E(2,2) = E(2,3) = E(2,4) = E(2,5)
							 = E(3,3) = E(3,4) = E(3,5)
							 = value_type(1);

	for (::std::size_t k = 1; k < nc; ++k)
	{
		X = ublasx::triu(A, k);

		for (::std::size_t i = 0; i <= ::std::min(nc-k,nr-1); ++i)
		{
			E(i,i+k-1) = value_type(0);
		}

		//BOOST_UBLASX_DEBUG_TRACE( "E=" << E );
		BOOST_UBLASX_DEBUG_TRACE( "triu(A," << k << ")=" << X );
		BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, nr, nc, tol );
	}
}

BOOST_UBLASX_TEST_DEF( real_horizontal_matrix_col_major_kgt0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real - Horizontal - Column Major - k > 0" );

	typedef double value_type;
	typedef ublas::matrix<value_type,ublas::column_major> matrix_type;

	const ::std::size_t nr(4);
	const ::std::size_t nc(6);

	matrix_type A(nr,nc, value_type(1));

	matrix_type X;
	matrix_type E;

	E = ublasx::triangular_matrix<value_type,ublas::upper>(nr,nc);
	E(0,0) = E(0,1) = E(0,2) = E(0,3) = E(0,4) = E(0,5)
		   = E(1,1) = E(1,2) = E(1,3) = E(1,4) = E(1,5)
					= E(2,2) = E(2,3) = E(2,4) = E(2,5)
							 = E(3,3) = E(3,4) = E(3,5)
							 = value_type(1);

	for (::std::size_t k = 1; k < nc; ++k)
	{
		X = ublasx::triu(A, k);

		for (::std::size_t i = 0; i <= ::std::min(nc-k,nr-1); ++i)
		{
			E(i,i+k-1) = value_type(0);
		}

		//BOOST_UBLASX_DEBUG_TRACE( "E=" << E );
		BOOST_UBLASX_DEBUG_TRACE( "triu(A," << k << ")=" << X );
		BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, nr, nc, tol );
	}
}

BOOST_UBLASX_TEST_DEF( real_horizontal_matrix_row_major_klt0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real - Horizontal - Row Major - k < 0" );

	typedef double value_type;
	typedef ublas::matrix<value_type,ublas::row_major> matrix_type;

	const ::std::size_t nr(4);
	const ::std::size_t nc(6);

	matrix_type A(nr,nc, value_type(1));

	matrix_type X;
	matrix_type E;

	E = ublasx::scalar_matrix<value_type>(nr, nc, value_type(1));

	for (::std::ptrdiff_t k = nr-1; k >= 0; --k)
	{
		X = ublasx::triu(A, -k);

		for (::std::size_t i = k+1; i <= ::std::min(nc-k,nr-1); ++i)
		{
			E(i,i-k-1) = value_type(0);
		}

		//BOOST_UBLASX_DEBUG_TRACE( "E=" << E );
		BOOST_UBLASX_DEBUG_TRACE( "triu(A," << -k << ")=" << X );
		BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, nr, nc, tol );
	}
}

BOOST_UBLASX_TEST_DEF( real_horizontal_matrix_col_major_klt0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real - Horizontal - Column Major - k < 0" );

	typedef double value_type;
	typedef ublas::matrix<value_type,ublas::column_major> matrix_type;

	const ::std::size_t nr(4);
	const ::std::size_t nc(6);

	matrix_type A(nr,nc, value_type(1));

	matrix_type X;
	matrix_type E;

	E = ublasx::scalar_matrix<value_type>(nr, nc, value_type(1));

	for (::std::ptrdiff_t k = nr-1; k >= 0; --k)
	{
		X = ublasx::triu(A, -k);

		for (::std::size_t i = k+1; i <= ::std::min(nc-k,nr-1); ++i)
		{
			E(i,i-k-1) = value_type(0);
		}

		//BOOST_UBLASX_DEBUG_TRACE( "E=" << E );
		BOOST_UBLASX_DEBUG_TRACE( "triu(A," << -k << ")=" << X );
		BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, nr, nc, tol );
	}
}

BOOST_UBLASX_TEST_DEF( real_vertical_matrix_row_major_keq0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real - Vertical - Row Major - k == 0" );

	typedef double value_type;
	typedef ublas::matrix<value_type,ublas::row_major> matrix_type;

	const ::std::size_t nr(6);
	const ::std::size_t nc(4);

	matrix_type A(nr,nc, value_type(1));

	matrix_type X;
	matrix_type E;

	X = ublasx::triu(A);
	E = ublasx::triangular_matrix<value_type,ublas::upper>(nr,nc);
	E(0,0) = E(0,1) = E(0,2) = E(0,3)
		   = E(1,1) = E(1,2) = E(1,3)
					= E(2,2) = E(2,3)
							 = E(3,3)
							 = value_type(1);

	BOOST_UBLASX_DEBUG_TRACE( "Input Matrix A=" << A );
	BOOST_UBLASX_DEBUG_TRACE( "triu(A)=" << X );
	BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, nr, nc, tol );
}

BOOST_UBLASX_TEST_DEF( real_vertical_matrix_col_major_keq0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real - Vertical - Column Major - k == 0" );

	typedef double value_type;
	typedef ublas::matrix<value_type,ublas::column_major> matrix_type;

	const ::std::size_t nr(6);
	const ::std::size_t nc(4);

	matrix_type A(nr,nc, value_type(1));

	matrix_type X;
	matrix_type E;

	X = ublasx::triu(A);
	E = ublasx::triangular_matrix<value_type,ublas::upper>(nr,nc);
	E(0,0) = E(0,1) = E(0,2) = E(0,3)
		   = E(1,1) = E(1,2) = E(1,3)
					= E(2,2) = E(2,3)
							 = E(3,3)
							 = value_type(1);

	BOOST_UBLASX_DEBUG_TRACE( "Input Matrix A=" << A );
	BOOST_UBLASX_DEBUG_TRACE( "triu(A)=" << X );
	BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, nr, nc, tol );
}

BOOST_UBLASX_TEST_DEF( real_vertical_matrix_row_major_kgt0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real - Vertical - Row Major - k > 0" );

	typedef double value_type;
	typedef ublas::matrix<value_type,ublas::row_major> matrix_type;

	const ::std::size_t nr(6);
	const ::std::size_t nc(4);

	matrix_type A(nr,nc, value_type(1));

	matrix_type X;
	matrix_type E;

	X = ublasx::triu(A);
	E = ublasx::triangular_matrix<value_type,ublas::upper>(nr,nc);
	E(0,0) = E(0,1) = E(0,2) = E(0,3)
		   = E(1,1) = E(1,2) = E(1,3)
					= E(2,2) = E(2,3)
							 = E(3,3)
							 = value_type(1);

	for (::std::size_t k = 1; k < nc; ++k)
	{
		X = ublasx::triu(A, k);

		for (::std::size_t i = 0; i <= ::std::min(nc-k,nr-1); ++i)
		{
			E(i,i+k-1) = value_type(0);
		}

		//BOOST_UBLASX_DEBUG_TRACE( "E=" << E );
		BOOST_UBLASX_DEBUG_TRACE( "triu(A," << k << ")=" << X );
		BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, nr, nc, tol );
	}
}

BOOST_UBLASX_TEST_DEF( real_vertical_matrix_col_major_kgt0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real - Vertical - Column Major - k > 0" );

	typedef double value_type;
	typedef ublas::matrix<value_type,ublas::column_major> matrix_type;

	const ::std::size_t nr(6);
	const ::std::size_t nc(4);

	matrix_type A(nr,nc, value_type(1));

	matrix_type X;
	matrix_type E;

	E = ublasx::triangular_matrix<value_type,ublas::upper>(nr,nc);
	E(0,0) = E(0,1) = E(0,2) = E(0,3)
		   = E(1,1) = E(1,2) = E(1,3)
					= E(2,2) = E(2,3)
							 = E(3,3)
							 = value_type(1);

	for (::std::size_t k = 1; k < nc; ++k)
	{
		X = ublasx::triu(A, k);

		for (::std::size_t i = 0; i <= ::std::min(nc-k,nr-1); ++i)
		{
			E(i,i+k-1) = value_type(0);
		}

		//BOOST_UBLASX_DEBUG_TRACE( "E=" << E );
		BOOST_UBLASX_DEBUG_TRACE( "triu(A," << k << ")=" << X );
		BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, nr, nc, tol );
	}
}

BOOST_UBLASX_TEST_DEF( real_vertical_matrix_row_major_klt0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real - Vertical - Row Major - k < 0" );

	typedef double value_type;
	typedef ublas::matrix<value_type,ublas::row_major> matrix_type;

	const ::std::size_t nr(6);
	const ::std::size_t nc(4);

	matrix_type A(nr,nc, value_type(1));

	matrix_type X;
	matrix_type E;

	E = ublasx::scalar_matrix<value_type>(nr, nc, value_type(1));

	for (::std::ptrdiff_t k = nr-1; k >= 0; --k)
	{
		X = ublasx::triu(A, -k);

		for (::std::size_t i = k+1; i < nr; ++i)
		{
			E(i,::std::min(i-k-1,nc-1)) = value_type(0);
		}

		//BOOST_UBLASX_DEBUG_TRACE( "E=" << E );
		BOOST_UBLASX_DEBUG_TRACE( "triu(A," << k << ")=" << X );
		BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, nr, nc, tol );
	}
}

BOOST_UBLASX_TEST_DEF( real_vertical_matrix_col_major_klt0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real - Vertical - Column Major - k < 0" );

	typedef double value_type;
	typedef ublas::matrix<value_type,ublas::column_major> matrix_type;

	const ::std::size_t nr(6);
	const ::std::size_t nc(4);

	matrix_type A(nr,nc, value_type(1));

	matrix_type X;
	matrix_type E;

	E = ublasx::scalar_matrix<value_type>(nr, nc, value_type(1));

	for (::std::ptrdiff_t k = nr-1; k >= 0; --k)
	{
		X = ublasx::triu(A, -k);

		for (::std::size_t i = k+1; i < nr; ++i)
		{
			E(i,::std::min(i-k-1,nc-1)) = value_type(0);
		}

		//BOOST_UBLASX_DEBUG_TRACE( "E=" << E );
		BOOST_UBLASX_DEBUG_TRACE( "triu(A," << k << ")=" << X );
		BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, nr, nc, tol );
	}
}

BOOST_UBLASX_TEST_DEF( complex_square_matrix_row_major_keq0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Complex - Square - Row Major - k == 0" );

	typedef double real_type;
	typedef ::std::complex<real_type> value_type;
	typedef ublas::matrix<value_type,ublas::row_major> matrix_type;

	const ::std::size_t n(4);

	matrix_type A(n,n, value_type(1));

	matrix_type X;
	matrix_type E;

	X = ublasx::triu(A);
	E = ublasx::triangular_matrix<value_type,ublas::upper>(n,n);
	E(0,0) = E(0,1) = E(0,2) = E(0,3)
		   = E(1,1) = E(1,2) = E(1,3)
					= E(2,2) = E(2,3)
							 = E(3,3)
							 = value_type(1);

	BOOST_UBLASX_DEBUG_TRACE( "Input Matrix A=" << A );
	BOOST_UBLASX_DEBUG_TRACE( "triu(A)=" << X );
	BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, n, n, tol );
}

BOOST_UBLASX_TEST_DEF( complex_square_matrix_col_major_keq0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Complex - Square - Column Major - k == 0" );

	typedef double real_type;
	typedef ::std::complex<real_type> value_type;
	typedef ublas::matrix<value_type,ublas::column_major> matrix_type;

	const ::std::size_t n(4);

	matrix_type A(n,n, value_type(1));

	matrix_type X;
	matrix_type E;

	X = ublasx::triu(A);
	E = ublasx::triangular_matrix<value_type,ublas::upper>(n,n);
	E(0,0) = E(0,1) = E(0,2) = E(0,3)
		   = E(1,1) = E(1,2) = E(1,3)
					= E(2,2) = E(2,3)
							 = E(3,3)
							 = value_type(1);

	BOOST_UBLASX_DEBUG_TRACE( "Input Matrix A=" << A );
	BOOST_UBLASX_DEBUG_TRACE( "triu(A)=" << X );
	BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, n, n, tol );
}

BOOST_UBLASX_TEST_DEF( complex_square_matrix_row_major_kgt0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Complex - Square - Row Major - k > 0" );

	typedef double real_type;
	typedef ::std::complex<real_type> value_type;
	typedef ublas::matrix<value_type,ublas::row_major> matrix_type;

	const ::std::size_t n(4);

	matrix_type A(n,n, value_type(1));

	matrix_type X;
	matrix_type E;

	E = ublasx::triangular_matrix<value_type,ublas::upper>(n,n);
	E(0,0) = E(0,1) = E(0,2) = E(0,3)
		   = E(1,1) = E(1,2) = E(1,3)
					= E(2,2) = E(2,3)
							 = E(3,3)
							 = value_type(1);

	for (::std::size_t k = 1; k < n; ++k)
	{
		X = ublasx::triu(A, k);

		for (::std::size_t i = 0; i <= (n-k); ++i)
		{
			E(i,i+k-1) = value_type(0);
		}

		//BOOST_UBLASX_DEBUG_TRACE( "E=" << E );
		BOOST_UBLASX_DEBUG_TRACE( "triu(A," << k << ")=" << X );
		BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, n, n, tol );
	}
}

BOOST_UBLASX_TEST_DEF( complex_square_matrix_col_major_kgt0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Complex - Square - Column Major - k > 0" );

	typedef double real_type;
	typedef ::std::complex<real_type> value_type;
	typedef ublas::matrix<value_type,ublas::column_major> matrix_type;

	const ::std::size_t n(4);

	matrix_type A(n,n, value_type(1));

	matrix_type X;
	matrix_type E;

	E = ublasx::triangular_matrix<value_type,ublas::upper>(n,n);
	E(0,0) = E(0,1) = E(0,2) = E(0,3)
		   = E(1,1) = E(1,2) = E(1,3)
					= E(2,2) = E(2,3)
							 = E(3,3)
							 = value_type(1);

	for (::std::size_t k = 1; k < n; ++k)
	{
		X = ublasx::triu(A, k);

		for (::std::size_t i = 0; i <= (n-k); ++i)
		{
			E(i,i+k-1) = value_type(0);
		}

		//BOOST_UBLASX_DEBUG_TRACE( "E=" << E );
		BOOST_UBLASX_DEBUG_TRACE( "triu(A," << k << ")=" << X );
		BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, n, n, tol );
	}
}

BOOST_UBLASX_TEST_DEF( complex_square_matrix_row_major_klt0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Complex - Square - Row Major - k < 0" );

	typedef double real_type;
	typedef ::std::complex<real_type> value_type;
	typedef ublas::matrix<value_type,ublas::row_major> matrix_type;

	const ::std::size_t n(4);

	matrix_type A(n,n, value_type(1));

	matrix_type X;
	matrix_type E;

	E = ublasx::scalar_matrix<value_type>(n, n, value_type(1));

	for (::std::ptrdiff_t k = n-1; k >= 0; --k)
	{
		X = ublasx::triu(A, -k);

		for (::std::size_t i = k+1; i < n; ++i)
		{
			E(i,i-k-1) = value_type(0);
		}

		//BOOST_UBLASX_DEBUG_TRACE( "E=" << E );
		BOOST_UBLASX_DEBUG_TRACE( "triu(A," << -k << ")=" << X );
		BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, n, n, tol );
	}
}

BOOST_UBLASX_TEST_DEF( complex_square_matrix_col_major_klt0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Complex - Square - Column Major - k < 0" );

	typedef double real_type;
	typedef ::std::complex<real_type> value_type;
	typedef ublas::matrix<value_type,ublas::column_major> matrix_type;

	const ::std::size_t n(4);

	matrix_type A(n,n, value_type(1));

	matrix_type X;
	matrix_type E;

	E = ublasx::scalar_matrix<value_type>(n, n, value_type(1));

	for (::std::ptrdiff_t k = n-1; k >= 0; --k)
	{
		X = ublasx::triu(A, -k);

		for (::std::size_t i = k+1; i < n; ++i)
		{
			E(i,i-k-1) = value_type(0);
		}

		//BOOST_UBLASX_DEBUG_TRACE( "E=" << E );
		BOOST_UBLASX_DEBUG_TRACE( "triu(A," << -k << ")=" << X );
		BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, n, n, tol );
	}
}

BOOST_UBLASX_TEST_DEF( complex_horizontal_matrix_row_major_keq0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Complex - Horizontal - Row Major - k == 0" );

	typedef double real_type;
	typedef ::std::complex<real_type> value_type;
	typedef ublas::matrix<value_type,ublas::row_major> matrix_type;

	const ::std::size_t nr(4);
	const ::std::size_t nc(6);

	matrix_type A(nr,nc, value_type(1));

	matrix_type X;
	matrix_type E;

	X = ublasx::triu(A);
	E = ublasx::triangular_matrix<value_type,ublas::upper>(nr,nc);
	E(0,0) = E(0,1) = E(0,2) = E(0,3) = E(0,4) = E(0,5)
		   = E(1,1) = E(1,2) = E(1,3) = E(1,4) = E(1,5)
					= E(2,2) = E(2,3) = E(2,4) = E(2,5)
							 = E(3,3) = E(3,4) = E(3,5)
							 = value_type(1);

	BOOST_UBLASX_DEBUG_TRACE( "Input Matrix A=" << A );
	BOOST_UBLASX_DEBUG_TRACE( "triu(A)=" << X );
	BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, nr, nc, tol );
}

BOOST_UBLASX_TEST_DEF( complex_horizontal_matrix_col_major_keq0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Complex - Horizontal - Column Major - k == 0" );

	typedef double real_type;
	typedef ::std::complex<real_type> value_type;
	typedef ublas::matrix<value_type,ublas::column_major> matrix_type;

	const ::std::size_t nr(4);
	const ::std::size_t nc(6);

	matrix_type A(nr,nc, value_type(1));

	matrix_type X;
	matrix_type E;

	X = ublasx::triu(A);
	E = ublasx::triangular_matrix<value_type,ublas::upper>(nr,nc);
	E(0,0) = E(0,1) = E(0,2) = E(0,3) = E(0,4) = E(0,5)
		   = E(1,1) = E(1,2) = E(1,3) = E(1,4) = E(1,5)
					= E(2,2) = E(2,3) = E(2,4) = E(2,5)
							 = E(3,3) = E(3,4) = E(3,5)
							 = value_type(1);

	BOOST_UBLASX_DEBUG_TRACE( "Input Matrix A=" << A );
	BOOST_UBLASX_DEBUG_TRACE( "triu(A)=" << X );
	BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, nr, nc, tol );
}

BOOST_UBLASX_TEST_DEF( complex_horizontal_matrix_row_major_kgt0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Complex - Horizontal - Row Major - k > 0" );

	typedef double real_type;
	typedef ::std::complex<real_type> value_type;
	typedef ublas::matrix<value_type,ublas::row_major> matrix_type;

	const ::std::size_t nr(4);
	const ::std::size_t nc(6);

	matrix_type A(nr,nc, value_type(1));

	matrix_type X;
	matrix_type E;

	E = ublasx::triangular_matrix<value_type,ublas::upper>(nr,nc);
	E(0,0) = E(0,1) = E(0,2) = E(0,3) = E(0,4) = E(0,5)
		   = E(1,1) = E(1,2) = E(1,3) = E(1,4) = E(1,5)
					= E(2,2) = E(2,3) = E(2,4) = E(2,5)
							 = E(3,3) = E(3,4) = E(3,5)
							 = value_type(1);

	for (::std::size_t k = 1; k < nc; ++k)
	{
		X = ublasx::triu(A, k);

		for (::std::size_t i = 0; i <= ::std::min(nc-k,nr-1); ++i)
		{
			E(i,i+k-1) = value_type(0);
		}

		//BOOST_UBLASX_DEBUG_TRACE( "E=" << E );
		BOOST_UBLASX_DEBUG_TRACE( "triu(A," << k << ")=" << X );
		BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, nr, nc, tol );
	}
}

BOOST_UBLASX_TEST_DEF( complex_horizontal_matrix_col_major_kgt0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Complex - Horizontal - Column Major - k > 0" );

	typedef double real_type;
	typedef ::std::complex<real_type> value_type;
	typedef ublas::matrix<value_type,ublas::column_major> matrix_type;

	const ::std::size_t nr(4);
	const ::std::size_t nc(6);

	matrix_type A(nr,nc, value_type(1));

	matrix_type X;
	matrix_type E;

	E = ublasx::triangular_matrix<value_type,ublas::upper>(nr,nc);
	E(0,0) = E(0,1) = E(0,2) = E(0,3) = E(0,4) = E(0,5)
		   = E(1,1) = E(1,2) = E(1,3) = E(1,4) = E(1,5)
					= E(2,2) = E(2,3) = E(2,4) = E(2,5)
							 = E(3,3) = E(3,4) = E(3,5)
							 = value_type(1);

	for (::std::size_t k = 1; k < nc; ++k)
	{
		X = ublasx::triu(A, k);

		for (::std::size_t i = 0; i <= ::std::min(nc-k,nr-1); ++i)
		{
			E(i,i+k-1) = value_type(0);
		}

		//BOOST_UBLASX_DEBUG_TRACE( "E=" << E );
		BOOST_UBLASX_DEBUG_TRACE( "triu(A," << k << ")=" << X );
		BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, nr, nc, tol );
	}
}

BOOST_UBLASX_TEST_DEF( complex_horizontal_matrix_row_major_klt0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Complex - Horizontal - Row Major - k < 0" );

	typedef double real_type;
	typedef ::std::complex<real_type> value_type;
	typedef ublas::matrix<value_type,ublas::row_major> matrix_type;

	const ::std::size_t nr(4);
	const ::std::size_t nc(6);

	matrix_type A(nr,nc, value_type(1));

	matrix_type X;
	matrix_type E;

	E = ublasx::scalar_matrix<value_type>(nr, nc, value_type(1));

	for (::std::ptrdiff_t k = nr-1; k >= 0; --k)
	{
		X = ublasx::triu(A, -k);

		for (::std::size_t i = k+1; i <= ::std::min(nc-k,nr-1); ++i)
		{
			E(i,i-k-1) = value_type(0);
		}

		//BOOST_UBLASX_DEBUG_TRACE( "E=" << E );
		BOOST_UBLASX_DEBUG_TRACE( "triu(A," << -k << ")=" << X );
		BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, nr, nc, tol );
	}
}

BOOST_UBLASX_TEST_DEF( complex_horizontal_matrix_col_major_klt0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Complex - Horizontal - Column Major - k < 0" );

	typedef double real_type;
	typedef ::std::complex<real_type> value_type;
	typedef ublas::matrix<value_type,ublas::column_major> matrix_type;

	const ::std::size_t nr(4);
	const ::std::size_t nc(6);

	matrix_type A(nr,nc, value_type(1));

	matrix_type X;
	matrix_type E;

	E = ublasx::scalar_matrix<value_type>(nr, nc, value_type(1));

	for (::std::ptrdiff_t k = nr-1; k >= 0; --k)
	{
		X = ublasx::triu(A, -k);

		for (::std::size_t i = k+1; i <= ::std::min(nc-k,nr-1); ++i)
		{
			E(i,i-k-1) = value_type(0);
		}

		//BOOST_UBLASX_DEBUG_TRACE( "E=" << E );
		BOOST_UBLASX_DEBUG_TRACE( "triu(A," << -k << ")=" << X );
		BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, nr, nc, tol );
	}
}

BOOST_UBLASX_TEST_DEF( complex_vertical_matrix_row_major_keq0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Complex - Vertical - Row Major - k == 0" );

	typedef double real_type;
	typedef ::std::complex<real_type> value_type;
	typedef ublas::matrix<value_type,ublas::row_major> matrix_type;

	const ::std::size_t nr(6);
	const ::std::size_t nc(4);

	matrix_type A(nr,nc, value_type(1));

	matrix_type X;
	matrix_type E;

	X = ublasx::triu(A);
	E = ublasx::triangular_matrix<value_type,ublas::upper>(nr,nc);
	E(0,0) = E(0,1) = E(0,2) = E(0,3)
		   = E(1,1) = E(1,2) = E(1,3)
					= E(2,2) = E(2,3)
							 = E(3,3)
							 = value_type(1);

	BOOST_UBLASX_DEBUG_TRACE( "Input Matrix A=" << A );
	BOOST_UBLASX_DEBUG_TRACE( "triu(A)=" << X );
	BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, nr, nc, tol );
}

BOOST_UBLASX_TEST_DEF( complex_vertical_matrix_col_major_keq0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Complex - Vertical - Column Major - k == 0" );

	typedef double real_type;
	typedef ::std::complex<real_type> value_type;
	typedef ublas::matrix<value_type,ublas::column_major> matrix_type;

	const ::std::size_t nr(6);
	const ::std::size_t nc(4);

	matrix_type A(nr,nc, value_type(1));

	matrix_type X;
	matrix_type E;

	X = ublasx::triu(A);
	E = ublasx::triangular_matrix<value_type,ublas::upper>(nr,nc);
	E(0,0) = E(0,1) = E(0,2) = E(0,3)
		   = E(1,1) = E(1,2) = E(1,3)
					= E(2,2) = E(2,3)
							 = E(3,3)
							 = value_type(1);

	BOOST_UBLASX_DEBUG_TRACE( "Input Matrix A=" << A );
	BOOST_UBLASX_DEBUG_TRACE( "triu(A)=" << X );
	BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, nr, nc, tol );
}

BOOST_UBLASX_TEST_DEF( complex_vertical_matrix_row_major_kgt0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Complex - Vertical - Row Major - k > 0" );

	typedef double real_type;
	typedef ::std::complex<real_type> value_type;
	typedef ublas::matrix<value_type,ublas::row_major> matrix_type;

	const ::std::size_t nr(6);
	const ::std::size_t nc(4);

	matrix_type A(nr,nc, value_type(1));

	matrix_type X;
	matrix_type E;

	X = ublasx::triu(A);
	E = ublasx::triangular_matrix<value_type,ublas::upper>(nr,nc);
	E(0,0) = E(0,1) = E(0,2) = E(0,3)
		   = E(1,1) = E(1,2) = E(1,3)
					= E(2,2) = E(2,3)
							 = E(3,3)
							 = value_type(1);

	for (::std::size_t k = 1; k < nc; ++k)
	{
		X = ublasx::triu(A, k);

		for (::std::size_t i = 0; i <= ::std::min(nc-k,nr-1); ++i)
		{
			E(i,i+k-1) = value_type(0);
		}

		//BOOST_UBLASX_DEBUG_TRACE( "E=" << E );
		BOOST_UBLASX_DEBUG_TRACE( "triu(A," << k << ")=" << X );
		BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, nr, nc, tol );
	}
}

BOOST_UBLASX_TEST_DEF( complex_vertical_matrix_col_major_kgt0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Complex - Vertical - Column Major - k > 0" );

	typedef double real_type;
	typedef ::std::complex<real_type> value_type;
	typedef ublas::matrix<value_type,ublas::column_major> matrix_type;

	const ::std::size_t nr(6);
	const ::std::size_t nc(4);

	matrix_type A(nr,nc, value_type(1));

	matrix_type X;
	matrix_type E;

	E = ublasx::triangular_matrix<value_type,ublas::upper>(nr,nc);
	E(0,0) = E(0,1) = E(0,2) = E(0,3)
		   = E(1,1) = E(1,2) = E(1,3)
					= E(2,2) = E(2,3)
							 = E(3,3)
							 = value_type(1);

	for (::std::size_t k = 1; k < nc; ++k)
	{
		X = ublasx::triu(A, k);

		for (::std::size_t i = 0; i <= ::std::min(nc-k,nr-1); ++i)
		{
			E(i,i+k-1) = value_type(0);
		}

		//BOOST_UBLASX_DEBUG_TRACE( "E=" << E );
		BOOST_UBLASX_DEBUG_TRACE( "triu(A," << k << ")=" << X );
		BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, nr, nc, tol );
	}
}

BOOST_UBLASX_TEST_DEF( complex_vertical_matrix_row_major_klt0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Complex - Vertical - Row Major - k < 0" );

	typedef double real_type;
	typedef ::std::complex<real_type> value_type;
	typedef ublas::matrix<value_type,ublas::row_major> matrix_type;

	const ::std::size_t nr(6);
	const ::std::size_t nc(4);

	matrix_type A(nr,nc, value_type(1));

	matrix_type X;
	matrix_type E;

	E = ublasx::scalar_matrix<value_type>(nr, nc, value_type(1));

	for (::std::ptrdiff_t k = nr-1; k >= 0; --k)
	{
		X = ublasx::triu(A, -k);

		for (::std::size_t i = k+1; i < nr; ++i)
		{
			E(i,::std::min(i-k-1,nc-1)) = value_type(0);
		}

		//BOOST_UBLASX_DEBUG_TRACE( "E=" << E );
		BOOST_UBLASX_DEBUG_TRACE( "triu(A," << k << ")=" << X );
		BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, nr, nc, tol );
	}
}

BOOST_UBLASX_TEST_DEF( complex_vertical_matrix_col_major_klt0 )
{
	BOOST_UBLASX_DEBUG_TRACE( "Test Case: Complex - Vertical - Column Major - k < 0" );

	typedef double real_type;
	typedef ::std::complex<real_type> value_type;
	typedef ublas::matrix<value_type,ublas::column_major> matrix_type;

	const ::std::size_t nr(6);
	const ::std::size_t nc(4);

	matrix_type A(nr,nc, value_type(1));

	matrix_type X;
	matrix_type E;

	E = ublasx::scalar_matrix<value_type>(nr, nc, value_type(1));

	for (::std::ptrdiff_t k = nr-1; k >= 0; --k)
	{
		X = ublasx::triu(A, -k);

		for (::std::size_t i = k+1; i < nr; ++i)
		{
			E(i,::std::min(i-k-1,nc-1)) = value_type(0);
		}

		//BOOST_UBLASX_DEBUG_TRACE( "E=" << E );
		BOOST_UBLASX_DEBUG_TRACE( "triu(A," << k << ")=" << X );
		BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( X, E, nr, nc, tol );
	}
}

int main()
{
	BOOST_UBLASX_TEST_BEGIN();

	BOOST_UBLASX_TEST_DO( real_square_matrix_col_major_keq0 );
	BOOST_UBLASX_TEST_DO( real_square_matrix_row_major_keq0 );
	BOOST_UBLASX_TEST_DO( real_square_matrix_col_major_kgt0 );
	BOOST_UBLASX_TEST_DO( real_square_matrix_row_major_kgt0 );
	BOOST_UBLASX_TEST_DO( real_square_matrix_row_major_klt0 );
	BOOST_UBLASX_TEST_DO( real_square_matrix_col_major_klt0 );

	BOOST_UBLASX_TEST_DO( real_horizontal_matrix_row_major_keq0 );
	BOOST_UBLASX_TEST_DO( real_horizontal_matrix_col_major_keq0 );
	BOOST_UBLASX_TEST_DO( real_horizontal_matrix_row_major_kgt0 );
	BOOST_UBLASX_TEST_DO( real_horizontal_matrix_col_major_kgt0 );
	BOOST_UBLASX_TEST_DO( real_horizontal_matrix_row_major_klt0 );
	BOOST_UBLASX_TEST_DO( real_horizontal_matrix_col_major_klt0 );

	BOOST_UBLASX_TEST_DO( real_vertical_matrix_row_major_keq0 );
	BOOST_UBLASX_TEST_DO( real_vertical_matrix_col_major_keq0 );
	BOOST_UBLASX_TEST_DO( real_vertical_matrix_row_major_kgt0 );
	BOOST_UBLASX_TEST_DO( real_vertical_matrix_col_major_kgt0 );
	BOOST_UBLASX_TEST_DO( real_vertical_matrix_row_major_klt0 );
	BOOST_UBLASX_TEST_DO( real_vertical_matrix_col_major_klt0 );

	BOOST_UBLASX_TEST_DO( complex_square_matrix_col_major_keq0 );
	BOOST_UBLASX_TEST_DO( complex_square_matrix_row_major_keq0 );
	BOOST_UBLASX_TEST_DO( complex_square_matrix_col_major_kgt0 );
	BOOST_UBLASX_TEST_DO( complex_square_matrix_row_major_kgt0 );
	BOOST_UBLASX_TEST_DO( complex_square_matrix_row_major_klt0 );
	BOOST_UBLASX_TEST_DO( complex_square_matrix_col_major_klt0 );

	BOOST_UBLASX_TEST_DO( complex_horizontal_matrix_row_major_keq0 );
	BOOST_UBLASX_TEST_DO( complex_horizontal_matrix_col_major_keq0 );
	BOOST_UBLASX_TEST_DO( complex_horizontal_matrix_row_major_kgt0 );
	BOOST_UBLASX_TEST_DO( complex_horizontal_matrix_col_major_kgt0 );
	BOOST_UBLASX_TEST_DO( complex_horizontal_matrix_row_major_klt0 );
	BOOST_UBLASX_TEST_DO( complex_horizontal_matrix_col_major_klt0 );

	BOOST_UBLASX_TEST_DO( complex_vertical_matrix_row_major_keq0 );
	BOOST_UBLASX_TEST_DO( complex_vertical_matrix_col_major_keq0 );
	BOOST_UBLASX_TEST_DO( complex_vertical_matrix_row_major_kgt0 );
	BOOST_UBLASX_TEST_DO( complex_vertical_matrix_col_major_kgt0 );
	BOOST_UBLASX_TEST_DO( complex_vertical_matrix_row_major_klt0 );
	BOOST_UBLASX_TEST_DO( complex_vertical_matrix_col_major_klt0 );

	BOOST_UBLASX_TEST_END();
}


//  (C) Copyright Eric Niebler 2005.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

// Test case for p_square_quantile.hpp

#include <boost/random.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/accumulators/numeric/functional/vector.hpp>
#include <boost/accumulators/numeric/functional/complex.hpp>
#include <boost/accumulators/numeric/functional/valarray.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/p_square_quantile.hpp>
#include <sstream>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>

using namespace boost;
using namespace unit_test;
using namespace boost::accumulators;

typedef accumulator_set<double, stats<tag::p_square_quantile> > accumulator_t;

///////////////////////////////////////////////////////////////////////////////
// test_stat
//
void test_stat()
{
    // tolerance in %
    double epsilon = 1;

    // a random number generator
    boost::lagged_fibonacci607 rng;

    accumulator_t acc0(quantile_probability = 0.001);
    accumulator_t acc1(quantile_probability = 0.01 );
    accumulator_t acc2(quantile_probability = 0.1  );
    accumulator_t acc3(quantile_probability = 0.25 );
    accumulator_t acc4(quantile_probability = 0.5  );
    accumulator_t acc5(quantile_probability = 0.75 );
    accumulator_t acc6(quantile_probability = 0.9  );
    accumulator_t acc7(quantile_probability = 0.99 );
    accumulator_t acc8(quantile_probability = 0.999);

    for (int i=0; i<100000; ++i)
    {
        double sample = rng();
        acc0(sample);
        acc1(sample);
        acc2(sample);
        acc3(sample);
        acc4(sample);
        acc5(sample);
        acc6(sample);
        acc7(sample);
        acc8(sample);
    }

    BOOST_CHECK_CLOSE( p_square_quantile(acc0), 0.001, 18*epsilon );
    BOOST_CHECK_CLOSE( p_square_quantile(acc1), 0.01 , 7*epsilon );
    BOOST_CHECK_CLOSE( p_square_quantile(acc2), 0.1  , 3*epsilon );
    BOOST_CHECK_CLOSE( p_square_quantile(acc3), 0.25 , 2*epsilon );
    BOOST_CHECK_CLOSE( p_square_quantile(acc4), 0.5  , epsilon );
    BOOST_CHECK_CLOSE( p_square_quantile(acc5), 0.75 , epsilon );
    BOOST_CHECK_CLOSE( p_square_quantile(acc6), 0.9  , epsilon );
    BOOST_CHECK_CLOSE( p_square_quantile(acc7), 0.99 , epsilon );
    BOOST_CHECK_CLOSE( p_square_quantile(acc8), 0.999, epsilon );
}

///////////////////////////////////////////////////////////////////////////////
// test_persistency
//
void test_persistency()
{
    // "persistent" storage
    std::stringstream ss;
    // tolerance in %
    double epsilon = 1;
    // a random number generator
    boost::lagged_fibonacci607 rng;
    {
        accumulator_t acc1(quantile_probability = 0.75 );
        accumulator_t acc2(quantile_probability = 0.999);

        for (int i=0; i<100000; ++i)
        {
            double sample = rng();
            acc1(sample);
            acc2(sample);
        }

        BOOST_CHECK_CLOSE(p_square_quantile(acc1), 0.75 , epsilon);
        BOOST_CHECK_CLOSE(p_square_quantile(acc2), 0.999, epsilon);
        boost::archive::text_oarchive oa(ss);
        acc1.serialize(oa, 0);
        acc2.serialize(oa, 0);
    }
    accumulator_t acc1(quantile_probability = 0.75);
    accumulator_t acc2(quantile_probability = 0.999);
    boost::archive::text_iarchive ia(ss);
    acc1.serialize(ia, 0);
    acc2.serialize(ia, 0);
    BOOST_CHECK_CLOSE(p_square_quantile(acc1), 0.75 , epsilon);
    BOOST_CHECK_CLOSE(p_square_quantile(acc2), 0.999, epsilon);
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("p_square_quantile test");

    test->add(BOOST_TEST_CASE(&test_stat));
    test->add(BOOST_TEST_CASE(&test_persistency));

    return test;
}


#pragma once

#include <tf/tf.h>
#include <Eigen/Dense>

namespace mil_tools
{
template <class T>
inline T make_rgba(double r, double g, double b, double a)
{
  T c;
  c.r = r;
  c.g = g;
  c.b = b;
  c.a = a;
  return c;
}

template <class T>
inline T make_xyz(double x, double y, double z)
{
  T p;
  p.x = x;
  p.y = y;
  p.z = z;
  return p;
}

template <class T>
inline T vec2xyz(Eigen::Vector3d v)
{
  return make_xyz<T>(v(0), v(1), v(2));
}
template <class T>
inline T vec2xyz(tf::Vector3 v)
{
  return make_xyz<T>(v.x(), v.y(), v.z());
}
template <class T>
inline Eigen::Vector3d xyz2vec(T m)
{
  return Eigen::Vector3d(m.x, m.y, m.z);
}

inline Eigen::Vector3d vec2vec(tf::Vector3 v)
{
  return Eigen::Vector3d(v[0], v[1], v[2]);
}
inline tf::Vector3 vec2vec(Eigen::Vector3d v)
{
  return tf::Vector3(v[0], v[1], v[2]);
}
inline Eigen::Quaterniond quat2quat(const tf::Quaternion &q)
{
  return Eigen::Quaterniond(q[3], q[0], q[1], q[2]);
}
inline tf::Quaternion quat2quat(const Eigen::Quaterniond &q)
{
  return tf::Quaternion(q.x(), q.y(), q.z(), q.w());
}

template <class T>
inline T make_xyzw(double x, double y, double z, double w)
{
  T q;
  q.x = x;
  q.y = y;
  q.z = z;
  q.w = w;
  return q;
}

template <class T>
inline T vec2xyzw(Eigen::Vector4d v)
{
  return vec2xyzw<T>(v(0), v(1), v(2), v(3));
}
template <class T>
inline Eigen::Quaterniond xyzw2quat(T m)
{
  return Eigen::Quaterniond(m.w, m.x, m.y, m.z);
}
template <class T>
inline T vec2wxyz(Eigen::Vector4d v)
{
  return make_xyzw<T>(v(1), v(2), v(3), v(0));
}
inline tf::Quaternion vec2quat(Eigen::Vector4d v)
{
  return tf::Quaternion(v[0], v[1], v[2], v[3]);
}
template <class T>
inline T quat2xyzw(Eigen::Quaterniond q)
{
  return make_xyzw<T>(q.x(), q.y(), q.z(), q.w());
}
}


#include <GL/freeglut.h>
#include <vector>
#include <Eigen/Eigen>
#include <iostream>
#include <iomanip>

#include "../include/structures.h"
#include "../include/transformations.h"
#include "../../point_to_point_with_scale_tait_bryan_wc_jacobian.h"
#include "../../point_to_point_with_scale_rodrigues_wc_jacobian.h"
#include "../../point_to_point_with_scale_quaternion_wc_jacobian.h"
#include "../../quaternion_constraint_jacobian.h"
#include "../include/cauchy.h"

const unsigned int window_width = 1920;
const unsigned int window_height = 1080;
int mouse_old_x, mouse_old_y;
int mouse_buttons = 0;
float rotate_x = 0.0, rotate_y = 0.0;
float translate_z = -10.0;
float translate_x, translate_y = 0.0;

bool initGL(int *argc, char **argv);
void display();
void keyboard(unsigned char key, int x, int y);
void mouse(int button, int state, int x, int y);
void motion(int x, int y);
void reshape(int w, int h);
void printHelp();

std::vector<Eigen::Affine3d> trajectory;
std::vector<Eigen::Vector3d> points_global;
std::vector<std::vector<Eigen::Vector3d>> points_local;
std::vector<double> scales;

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

	for(size_t i = 0 ; i < 10000; i++){
		Eigen::Vector3d p;
		p.x() = ((float(rand()%1000000))/1000000.0f - 0.5) * 100;
		p.y() = ((float(rand()%1000000))/1000000.0f - 0.5) * 100;
		p.z() = ((float(rand()%1000000))/1000000.0f - 0.5) * 100;
		points_global.push_back(p);
	}

	TaitBryanPose pose;
	pose.px = -4;
	pose.py = 0;
	pose.pz = 0;
	pose.om = 0;
	pose.fi = 0;
	pose.ka = 0;
	trajectory.push_back(affine_matrix_from_pose_tait_bryan(pose));
	scales.push_back(1.0);

	pose.px = -2;
	pose.py = 0;
	pose.pz = 0;
	pose.om = 0;
	pose.fi = 0;
	pose.ka = 0;
	trajectory.push_back(affine_matrix_from_pose_tait_bryan(pose));
	scales.push_back(1.0);

	pose.px = -0;
	pose.py = 0;
	pose.pz = 0;
	pose.om = 0;
	pose.fi = 0;
	pose.ka = 0;
	trajectory.push_back(affine_matrix_from_pose_tait_bryan(pose));
	scales.push_back(1.0);

	pose.px = 2;
	pose.py = 0;
	pose.pz = 0;
	pose.om = 0;
	pose.fi = 0;
	pose.ka = 0;
	trajectory.push_back(affine_matrix_from_pose_tait_bryan(pose));
	scales.push_back(1.0);

	pose.px = 4;
	pose.py = 0;
	pose.pz = 0;
	pose.om = 0;
	pose.fi = 0;
	pose.ka = 0;
	trajectory.push_back(affine_matrix_from_pose_tait_bryan(pose));
	scales.push_back(1.0);

	for(size_t i = 0; i < trajectory.size(); i++){
		Eigen::Affine3d m_inv = trajectory[i].inverse();
		std::vector<Eigen::Vector3d> pp;

		for(size_t j = 0 ; j < points_global.size(); j++){
			Eigen::Vector3d vt = m_inv * points_global[j];
			pp.push_back(vt);
		}
		points_local.push_back(pp);
	}

	if (false == initGL(&argc, argv)) {
		return 4;
	}

	printHelp();
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMouseFunc(mouse);
	glutMotionFunc(motion);
	glutMainLoop();

	return 0;
}

bool initGL(int *argc, char **argv) {
	glutInit(argc, argv);
	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);
	glutInitWindowSize(window_width, window_height);
	glutCreateWindow("point_to_point_with_scale");
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMotionFunc(motion);

	// default initialization
	glClearColor(1.0, 1.0, 1.0, 1.0);
	glEnable(GL_DEPTH_TEST);

	// viewport
	glViewport(0, 0, window_width, window_height);

	// projection
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) window_width / (GLfloat) window_height, 0.01,
			10000.0);
	glutReshapeFunc(reshape);

	return true;
}

void display() {
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glTranslatef(translate_x, translate_y, translate_z);
	glRotatef(rotate_x, 1.0, 0.0, 0.0);
	glRotatef(rotate_y, 0.0, 0.0, 1.0);

	for(size_t i = 0 ; i < trajectory.size(); i++){
		Eigen::Affine3d &m = trajectory[i];

		glBegin(GL_LINES);
			glColor3f(1.0f, 0.0f, 0.0f);
			glVertex3f(m(0,3), m(1,3), m(2,3));
			glVertex3f(m(0,3) + m(0,0), m(1,3) + m(1,0), m(2,3) + m(2,0));

			glColor3f(0.0f, 1.0f, 0.0f);
			glVertex3f(m(0,3), m(1,3), m(2,3));
			glVertex3f(m(0,3) + m(0,1), m(1,3) + m(1,1), m(2,3) + m(2,1));

			glColor3f(0.0f, 0.0f, 1.0f);
			glVertex3f(m(0,3), m(1,3), m(2,3));
			glVertex3f(m(0,3) + m(0,2), m(1,3) + m(1,2), m(2,3) + m(2,2));
		glEnd();
	}

	glColor3f(1,0,0);
	glPointSize(3);
	glBegin(GL_POINTS);

	for(size_t i = 0 ; i < trajectory.size(); i++){
		Eigen::Affine3d &m = trajectory[i];

		if(i == 0)glColor3f(1,0,0);
		if(i == 1)glColor3f(0,1,0);
		if(i == 2)glColor3f(0,0,1);
		if(i == 3)glColor3f(1,0,1);
		if(i == 4)glColor3f(1,1,0);

		Eigen::Affine3d m_scale = Eigen::Affine3d::Identity();
		m_scale(0,0) = scales[i];
		m_scale(1,1) = scales[i];
		m_scale(2,2) = scales[i];

		for(size_t j = 0 ; j < points_local[i].size(); j++){
			Eigen::Vector3d vt;
			vt = (m * m_scale) * points_local[i][j];
			glVertex3f(vt.x(), vt.y(), vt.z());
		}
	}
	glEnd();
	glPointSize(1);

	glColor3f(0.8,0.8,0.8);
	glBegin(GL_LINE_STRIP);
	for(size_t i = 0 ; i < trajectory.size(); i++){
		Eigen::Affine3d &m = trajectory[i];
		glVertex3f(m(0,3), m(1,3), m(2,3));
	}
	glEnd();

	glutSwapBuffers();
}

void keyboard(unsigned char key, int /*x*/, int /*y*/) {
	switch (key) {
		case (27): {
			glutDestroyWindow(glutGetWindow());
			return;
		}
		case 'n':{
			for(size_t i = 0 ; i < trajectory.size(); i++){
				TaitBryanPose pose;
				pose.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.0;
				pose.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.0;
				pose.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 1.0;
				pose.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.1;
				pose.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.1;
				pose.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.1;

				trajectory[i] = trajectory[i] * affine_matrix_from_pose_tait_bryan(pose);
			}
			break;
		}
		case 's':{
			for(size_t i = 1; i < scales.size(); i++){
				scales[i] += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.5;
			}
			break;
		}

		case 't':{

			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			for(size_t i = 0 ; i < points_local.size(); i++){
				for(size_t j = 0 ; j < points_local.size(); j++){
					if(i != j){

						TaitBryanPose pose_1 = pose_tait_bryan_from_affine_matrix(trajectory[i]);
						TaitBryanPose pose_2 = pose_tait_bryan_from_affine_matrix(trajectory[j]);
						double s_1 = scales[i];
						double s_2 = scales[j];

						for(size_t k = 0 ; k < points_local[i].size(); k++){
							Eigen::Vector3d &p_1 = points_local[i][k];
							Eigen::Vector3d &p_2 = points_local[j][k];
							double delta_x;
							double delta_y;
							double delta_z;
							point_to_point_with_scale_tait_bryan_wc(delta_x, delta_y, delta_z, pose_1.px, pose_1.py, pose_1.pz, pose_1.om, pose_1.fi, pose_1.ka, pose_2.px, pose_2.py, pose_2.pz, pose_2.om, pose_2.fi, pose_2.ka, p_1.x(), p_1.y(), p_1.z(), p_2.x(), p_2.y(), p_2.z(), s_1, s_2);

							Eigen::Matrix<double, 3, 14, Eigen::RowMajor> jacobian;
							point_to_point_with_scale_tait_bryan_wc_jacobian(jacobian, pose_1.px, pose_1.py, pose_1.pz, pose_1.om, pose_1.fi, pose_1.ka, pose_2.px, pose_2.py, pose_2.pz, pose_2.om, pose_2.fi, pose_2.ka, p_1.x(), p_1.y(), p_1.z(), p_2.x(), p_2.y(), p_2.z(), s_1, s_2);

							int ir = tripletListB.size();
							int ic_1 = i * 7;
							int ic_2 = j * 7;

							tripletListA.emplace_back(ir     , ic_1    , -jacobian(0,0));
							tripletListA.emplace_back(ir     , ic_1 + 1, -jacobian(0,1));
							tripletListA.emplace_back(ir     , ic_1 + 2, -jacobian(0,2));
							tripletListA.emplace_back(ir     , ic_1 + 3, -jacobian(0,3));
							tripletListA.emplace_back(ir     , ic_1 + 4, -jacobian(0,4));
							tripletListA.emplace_back(ir     , ic_1 + 5, -jacobian(0,5));
							tripletListA.emplace_back(ir     , ic_1 + 6, -jacobian(0,6));

							tripletListA.emplace_back(ir     , ic_2    , -jacobian(0,7));
							tripletListA.emplace_back(ir     , ic_2 + 1, -jacobian(0,8));
							tripletListA.emplace_back(ir     , ic_2 + 2, -jacobian(0,9));
							tripletListA.emplace_back(ir     , ic_2 + 3, -jacobian(0,10));
							tripletListA.emplace_back(ir     , ic_2 + 4, -jacobian(0,11));
							tripletListA.emplace_back(ir     , ic_2 + 5, -jacobian(0,12));
							tripletListA.emplace_back(ir     , ic_2 + 6, -jacobian(0,13));

							tripletListA.emplace_back(ir + 1 , ic_1    , -jacobian(1,0));
							tripletListA.emplace_back(ir + 1 , ic_1 + 1, -jacobian(1,1));
							tripletListA.emplace_back(ir + 1 , ic_1 + 2, -jacobian(1,2));
							tripletListA.emplace_back(ir + 1 , ic_1 + 3, -jacobian(1,3));
							tripletListA.emplace_back(ir + 1 , ic_1 + 4, -jacobian(1,4));
							tripletListA.emplace_back(ir + 1 , ic_1 + 5, -jacobian(1,5));
							tripletListA.emplace_back(ir + 1 , ic_1 + 6, -jacobian(1,6));

							tripletListA.emplace_back(ir + 1 , ic_2    , -jacobian(1,7));
							tripletListA.emplace_back(ir + 1 , ic_2 + 1, -jacobian(1,8));
							tripletListA.emplace_back(ir + 1 , ic_2 + 2, -jacobian(1,9));
							tripletListA.emplace_back(ir + 1 , ic_2 + 3, -jacobian(1,10));
							tripletListA.emplace_back(ir + 1 , ic_2 + 4, -jacobian(1,11));
							tripletListA.emplace_back(ir + 1 , ic_2 + 5, -jacobian(1,12));
							tripletListA.emplace_back(ir + 1 , ic_2 + 6, -jacobian(1,13));

							tripletListA.emplace_back(ir + 2 , ic_1    , -jacobian(2,0));
							tripletListA.emplace_back(ir + 2 , ic_1 + 1, -jacobian(2,1));
							tripletListA.emplace_back(ir + 2 , ic_1 + 2, -jacobian(2,2));
							tripletListA.emplace_back(ir + 2 , ic_1 + 3, -jacobian(2,3));
							tripletListA.emplace_back(ir + 2 , ic_1 + 4, -jacobian(2,4));
							tripletListA.emplace_back(ir + 2 , ic_1 + 5, -jacobian(2,5));
							tripletListA.emplace_back(ir + 2 , ic_1 + 6, -jacobian(2,6));

							tripletListA.emplace_back(ir + 2 , ic_2    , -jacobian(2,7));
							tripletListA.emplace_back(ir + 2 , ic_2 + 1, -jacobian(2,8));
							tripletListA.emplace_back(ir + 2 , ic_2 + 2, -jacobian(2,9));
							tripletListA.emplace_back(ir + 2 , ic_2 + 3, -jacobian(2,10));
							tripletListA.emplace_back(ir + 2 , ic_2 + 4, -jacobian(2,11));
							tripletListA.emplace_back(ir + 2 , ic_2 + 5, -jacobian(2,12));
							tripletListA.emplace_back(ir + 2 , ic_2 + 6, -jacobian(2,13));

							tripletListP.emplace_back(ir    , ir    ,  cauchy(delta_x, 1));
							tripletListP.emplace_back(ir + 1, ir + 1,  cauchy(delta_y, 1));
							tripletListP.emplace_back(ir + 2, ir + 2,  cauchy(delta_z, 1));

							tripletListB.emplace_back(ir    , 0,  delta_x);
							tripletListB.emplace_back(ir + 1, 0,  delta_y);
							tripletListB.emplace_back(ir + 2, 0,  delta_z);
						}
					}
				}
			}

			int ir = tripletListB.size();
			tripletListA.emplace_back(ir     , 0, 1);
			tripletListA.emplace_back(ir + 1 , 1, 1);
			tripletListA.emplace_back(ir + 2 , 2, 1);
			tripletListA.emplace_back(ir + 3 , 3, 1);
			tripletListA.emplace_back(ir + 4 , 4, 1);
			tripletListA.emplace_back(ir + 5 , 5, 1);
			tripletListA.emplace_back(ir + 6 , 6, 1);

			tripletListP.emplace_back(ir     , ir,     1000000);
			tripletListP.emplace_back(ir + 1 , ir + 1, 1000000);
			tripletListP.emplace_back(ir + 2 , ir + 2, 1000000);
			tripletListP.emplace_back(ir + 3 , ir + 3, 1000000);
			tripletListP.emplace_back(ir + 4 , ir + 4, 1000000);
			tripletListP.emplace_back(ir + 5 , ir + 5, 1000000);
			tripletListP.emplace_back(ir + 6 , ir + 6, 1000000);

			tripletListB.emplace_back(ir     , 0, 0);
			tripletListB.emplace_back(ir + 1 , 0, 0);
			tripletListB.emplace_back(ir + 2 , 0, 0);
			tripletListB.emplace_back(ir + 3 , 0, 0);
			tripletListB.emplace_back(ir + 4 , 0, 0);
			tripletListB.emplace_back(ir + 5 , 0, 0);
			tripletListB.emplace_back(ir + 6 , 0, 0);

			Eigen::SparseMatrix<double> matA(tripletListB.size(), trajectory.size() * 7);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(trajectory.size() * 7, trajectory.size() * 7);
			Eigen::SparseMatrix<double> AtPB(trajectory.size() * 7, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == trajectory.size() * 7){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				int counter = 0;

				for(size_t i = 0; i < trajectory.size(); i++){
					TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(trajectory[i]);
					pose.px += h_x[counter++];
					pose.py += h_x[counter++];
					pose.pz += h_x[counter++];
					pose.om += h_x[counter++];
					pose.fi += h_x[counter++];
					pose.ka += h_x[counter++];
					scales[i] += h_x[counter++];
					trajectory[i] = affine_matrix_from_pose_tait_bryan(pose);
				}
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}

			break;
		}
		case 'r':{
			for(size_t i = 0; i < trajectory.size(); i++){
				TaitBryanPose posetb = pose_tait_bryan_from_affine_matrix(trajectory[i]);
				posetb.om += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				posetb.fi += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				posetb.ka += (float(rand()%1000000)/1000000.0 - 0.5) * 2.0 * 0.00001;
				trajectory[i] = affine_matrix_from_pose_tait_bryan(posetb);
			}

			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			for(size_t i = 0 ; i < points_local.size(); i++){
				for(size_t j = 0 ; j < points_local.size(); j++){
					if(i != j){
						RodriguesPose pose_1 = pose_rodrigues_from_affine_matrix(trajectory[i]);
						RodriguesPose pose_2 = pose_rodrigues_from_affine_matrix(trajectory[j]);
						double s_1 = scales[i];
						double s_2 = scales[j];

						for(size_t k = 0 ; k < points_local[i].size(); k++){
							Eigen::Vector3d &p_1 = points_local[i][k];
							Eigen::Vector3d &p_2 = points_local[j][k];
							double delta_x;
							double delta_y;
							double delta_z;
							point_to_point_with_scale_rodrigues_wc(delta_x, delta_y, delta_z, pose_1.px, pose_1.py, pose_1.pz, pose_1.sx, pose_1.sy, pose_1.sz, pose_2.px, pose_2.py, pose_2.pz, pose_2.sx, pose_2.sy, pose_2.sz, p_1.x(), p_1.y(), p_1.z(), p_2.x(), p_2.y(), p_2.z(), s_1, s_2);

							Eigen::Matrix<double, 3, 14, Eigen::RowMajor> jacobian;
							point_to_point_with_scale_rodrigues_wc_jacobian(jacobian, pose_1.px, pose_1.py, pose_1.pz, pose_1.sx, pose_1.sy, pose_1.sz, pose_2.px, pose_2.py, pose_2.pz, pose_2.sx, pose_2.sy, pose_2.sz, p_1.x(), p_1.y(), p_1.z(), p_2.x(), p_2.y(), p_2.z(), s_1, s_2);

							int ir = tripletListB.size();
							int ic_1 = i * 7;
							int ic_2 = j * 7;

							tripletListA.emplace_back(ir     , ic_1    , -jacobian(0,0));
							tripletListA.emplace_back(ir     , ic_1 + 1, -jacobian(0,1));
							tripletListA.emplace_back(ir     , ic_1 + 2, -jacobian(0,2));
							tripletListA.emplace_back(ir     , ic_1 + 3, -jacobian(0,3));
							tripletListA.emplace_back(ir     , ic_1 + 4, -jacobian(0,4));
							tripletListA.emplace_back(ir     , ic_1 + 5, -jacobian(0,5));
							tripletListA.emplace_back(ir     , ic_1 + 6, -jacobian(0,6));

							tripletListA.emplace_back(ir     , ic_2    , -jacobian(0,7));
							tripletListA.emplace_back(ir     , ic_2 + 1, -jacobian(0,8));
							tripletListA.emplace_back(ir     , ic_2 + 2, -jacobian(0,9));
							tripletListA.emplace_back(ir     , ic_2 + 3, -jacobian(0,10));
							tripletListA.emplace_back(ir     , ic_2 + 4, -jacobian(0,11));
							tripletListA.emplace_back(ir     , ic_2 + 5, -jacobian(0,12));
							tripletListA.emplace_back(ir     , ic_2 + 6, -jacobian(0,13));

							tripletListA.emplace_back(ir + 1 , ic_1    , -jacobian(1,0));
							tripletListA.emplace_back(ir + 1 , ic_1 + 1, -jacobian(1,1));
							tripletListA.emplace_back(ir + 1 , ic_1 + 2, -jacobian(1,2));
							tripletListA.emplace_back(ir + 1 , ic_1 + 3, -jacobian(1,3));
							tripletListA.emplace_back(ir + 1 , ic_1 + 4, -jacobian(1,4));
							tripletListA.emplace_back(ir + 1 , ic_1 + 5, -jacobian(1,5));
							tripletListA.emplace_back(ir + 1 , ic_1 + 6, -jacobian(1,6));

							tripletListA.emplace_back(ir + 1 , ic_2    , -jacobian(1,7));
							tripletListA.emplace_back(ir + 1 , ic_2 + 1, -jacobian(1,8));
							tripletListA.emplace_back(ir + 1 , ic_2 + 2, -jacobian(1,9));
							tripletListA.emplace_back(ir + 1 , ic_2 + 3, -jacobian(1,10));
							tripletListA.emplace_back(ir + 1 , ic_2 + 4, -jacobian(1,11));
							tripletListA.emplace_back(ir + 1 , ic_2 + 5, -jacobian(1,12));
							tripletListA.emplace_back(ir + 1 , ic_2 + 6, -jacobian(1,13));

							tripletListA.emplace_back(ir + 2 , ic_1    , -jacobian(2,0));
							tripletListA.emplace_back(ir + 2 , ic_1 + 1, -jacobian(2,1));
							tripletListA.emplace_back(ir + 2 , ic_1 + 2, -jacobian(2,2));
							tripletListA.emplace_back(ir + 2 , ic_1 + 3, -jacobian(2,3));
							tripletListA.emplace_back(ir + 2 , ic_1 + 4, -jacobian(2,4));
							tripletListA.emplace_back(ir + 2 , ic_1 + 5, -jacobian(2,5));
							tripletListA.emplace_back(ir + 2 , ic_1 + 6, -jacobian(2,6));

							tripletListA.emplace_back(ir + 2 , ic_2    , -jacobian(2,7));
							tripletListA.emplace_back(ir + 2 , ic_2 + 1, -jacobian(2,8));
							tripletListA.emplace_back(ir + 2 , ic_2 + 2, -jacobian(2,9));
							tripletListA.emplace_back(ir + 2 , ic_2 + 3, -jacobian(2,10));
							tripletListA.emplace_back(ir + 2 , ic_2 + 4, -jacobian(2,11));
							tripletListA.emplace_back(ir + 2 , ic_2 + 5, -jacobian(2,12));
							tripletListA.emplace_back(ir + 2 , ic_2 + 6, -jacobian(2,13));

							tripletListP.emplace_back(ir    , ir    ,  cauchy(delta_x, 1));
							tripletListP.emplace_back(ir + 1, ir + 1,  cauchy(delta_y, 1));
							tripletListP.emplace_back(ir + 2, ir + 2,  cauchy(delta_z, 1));

							tripletListB.emplace_back(ir    , 0,  delta_x);
							tripletListB.emplace_back(ir + 1, 0,  delta_y);
							tripletListB.emplace_back(ir + 2, 0,  delta_z);
						}
					}
				}
			}

			int ir = tripletListB.size();
			tripletListA.emplace_back(ir     , 0, 1);
			tripletListA.emplace_back(ir + 1 , 1, 1);
			tripletListA.emplace_back(ir + 2 , 2, 1);
			tripletListA.emplace_back(ir + 3 , 3, 1);
			tripletListA.emplace_back(ir + 4 , 4, 1);
			tripletListA.emplace_back(ir + 5 , 5, 1);
			tripletListA.emplace_back(ir + 6 , 6, 1);

			tripletListP.emplace_back(ir     , ir,     1000000);
			tripletListP.emplace_back(ir + 1 , ir + 1, 1000000);
			tripletListP.emplace_back(ir + 2 , ir + 2, 1000000);
			tripletListP.emplace_back(ir + 3 , ir + 3, 1000000);
			tripletListP.emplace_back(ir + 4 , ir + 4, 1000000);
			tripletListP.emplace_back(ir + 5 , ir + 5, 1000000);
			tripletListP.emplace_back(ir + 6 , ir + 6, 1000000);

			tripletListB.emplace_back(ir     , 0, 0);
			tripletListB.emplace_back(ir + 1 , 0, 0);
			tripletListB.emplace_back(ir + 2 , 0, 0);
			tripletListB.emplace_back(ir + 3 , 0, 0);
			tripletListB.emplace_back(ir + 4 , 0, 0);
			tripletListB.emplace_back(ir + 5 , 0, 0);
			tripletListB.emplace_back(ir + 6 , 0, 0);

			Eigen::SparseMatrix<double> matA(tripletListB.size(), trajectory.size() * 7);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(trajectory.size() * 7, trajectory.size() * 7);
			Eigen::SparseMatrix<double> AtPB(trajectory.size() * 7, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == trajectory.size() * 7){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				int counter = 0;

				for(size_t i = 0; i < trajectory.size(); i++){
					RodriguesPose pose = pose_rodrigues_from_affine_matrix(trajectory[i]);
					pose.px += h_x[counter++];
					pose.py += h_x[counter++];
					pose.pz += h_x[counter++];
					pose.sx += h_x[counter++];
					pose.sy += h_x[counter++];
					pose.sz += h_x[counter++];
					scales[i] += h_x[counter++];
					trajectory[i] = affine_matrix_from_pose_rodrigues(pose);
				}
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
		case 'q':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			for(size_t i = 0 ; i < points_local.size(); i++){
				for(size_t j = 0 ; j < points_local.size(); j++){
					if(i != j){
						QuaternionPose pose_1 = pose_quaternion_from_affine_matrix(trajectory[i]);
						QuaternionPose pose_2 = pose_quaternion_from_affine_matrix(trajectory[j]);
						double s_1 = scales[i];
						double s_2 = scales[j];

						for(size_t k = 0 ; k < points_local[i].size(); k++){
							Eigen::Vector3d &p_1 = points_local[i][k];
							Eigen::Vector3d &p_2 = points_local[j][k];
							double delta_x;
							double delta_y;
							double delta_z;
							point_to_point_with_scale_quaternion_wc(delta_x, delta_y, delta_z, pose_1.px, pose_1.py, pose_1.pz, pose_1.q0, pose_1.q1, pose_1.q2, pose_1.q3, pose_2.px, pose_2.py, pose_2.pz, pose_2.q0, pose_2.q1, pose_2.q2, pose_2.q3, p_1.x(), p_1.y(), p_1.z(), p_2.x(), p_2.y(), p_2.z(), s_1, s_2);

							Eigen::Matrix<double, 3, 16, Eigen::RowMajor> jacobian;
							point_to_point_with_scale_quaternion_wc_jacobian(jacobian, pose_1.px, pose_1.py, pose_1.pz, pose_1.q0, pose_1.q1, pose_1.q2, pose_1.q3, pose_2.px, pose_2.py, pose_2.pz, pose_2.q0, pose_2.q1, pose_2.q2, pose_2.q3, p_1.x(), p_1.y(), p_1.z(), p_2.x(), p_2.y(), p_2.z(), s_1, s_2);

							int ir = tripletListB.size();
							int ic_1 = i * 8;
							int ic_2 = j * 8;

							tripletListA.emplace_back(ir     , ic_1    , -jacobian(0,0));
							tripletListA.emplace_back(ir     , ic_1 + 1, -jacobian(0,1));
							tripletListA.emplace_back(ir     , ic_1 + 2, -jacobian(0,2));
							tripletListA.emplace_back(ir     , ic_1 + 3, -jacobian(0,3));
							tripletListA.emplace_back(ir     , ic_1 + 4, -jacobian(0,4));
							tripletListA.emplace_back(ir     , ic_1 + 5, -jacobian(0,5));
							tripletListA.emplace_back(ir     , ic_1 + 6, -jacobian(0,6));
							tripletListA.emplace_back(ir     , ic_1 + 7, -jacobian(0,7));

							tripletListA.emplace_back(ir     , ic_2    , -jacobian(0,8));
							tripletListA.emplace_back(ir     , ic_2 + 1, -jacobian(0,9));
							tripletListA.emplace_back(ir     , ic_2 + 2, -jacobian(0,10));
							tripletListA.emplace_back(ir     , ic_2 + 3, -jacobian(0,11));
							tripletListA.emplace_back(ir     , ic_2 + 4, -jacobian(0,12));
							tripletListA.emplace_back(ir     , ic_2 + 5, -jacobian(0,13));
							tripletListA.emplace_back(ir     , ic_2 + 6, -jacobian(0,14));
							tripletListA.emplace_back(ir     , ic_2 + 7, -jacobian(0,15));

							tripletListA.emplace_back(ir + 1 , ic_1    , -jacobian(1,0));
							tripletListA.emplace_back(ir + 1 , ic_1 + 1, -jacobian(1,1));
							tripletListA.emplace_back(ir + 1 , ic_1 + 2, -jacobian(1,2));
							tripletListA.emplace_back(ir + 1 , ic_1 + 3, -jacobian(1,3));
							tripletListA.emplace_back(ir + 1 , ic_1 + 4, -jacobian(1,4));
							tripletListA.emplace_back(ir + 1 , ic_1 + 5, -jacobian(1,5));
							tripletListA.emplace_back(ir + 1 , ic_1 + 6, -jacobian(1,6));
							tripletListA.emplace_back(ir + 1 , ic_1 + 7, -jacobian(1,7));

							tripletListA.emplace_back(ir + 1 , ic_2    , -jacobian(1,8));
							tripletListA.emplace_back(ir + 1 , ic_2 + 1, -jacobian(1,9));
							tripletListA.emplace_back(ir + 1 , ic_2 + 2, -jacobian(1,10));
							tripletListA.emplace_back(ir + 1 , ic_2 + 3, -jacobian(1,11));
							tripletListA.emplace_back(ir + 1 , ic_2 + 4, -jacobian(1,12));
							tripletListA.emplace_back(ir + 1 , ic_2 + 5, -jacobian(1,13));
							tripletListA.emplace_back(ir + 1 , ic_2 + 6, -jacobian(1,14));
							tripletListA.emplace_back(ir + 1 , ic_2 + 7, -jacobian(1,15));

							tripletListA.emplace_back(ir + 2 , ic_1    , -jacobian(2,0));
							tripletListA.emplace_back(ir + 2 , ic_1 + 1, -jacobian(2,1));
							tripletListA.emplace_back(ir + 2 , ic_1 + 2, -jacobian(2,2));
							tripletListA.emplace_back(ir + 2 , ic_1 + 3, -jacobian(2,3));
							tripletListA.emplace_back(ir + 2 , ic_1 + 4, -jacobian(2,4));
							tripletListA.emplace_back(ir + 2 , ic_1 + 5, -jacobian(2,5));
							tripletListA.emplace_back(ir + 2 , ic_1 + 6, -jacobian(2,6));
							tripletListA.emplace_back(ir + 2 , ic_1 + 7, -jacobian(2,7));

							tripletListA.emplace_back(ir + 2 , ic_2    , -jacobian(2,8));
							tripletListA.emplace_back(ir + 2 , ic_2 + 1, -jacobian(2,9));
							tripletListA.emplace_back(ir + 2 , ic_2 + 2, -jacobian(2,10));
							tripletListA.emplace_back(ir + 2 , ic_2 + 3, -jacobian(2,11));
							tripletListA.emplace_back(ir + 2 , ic_2 + 4, -jacobian(2,12));
							tripletListA.emplace_back(ir + 2 , ic_2 + 5, -jacobian(2,13));
							tripletListA.emplace_back(ir + 2 , ic_2 + 6, -jacobian(2,14));
							tripletListA.emplace_back(ir + 2 , ic_2 + 7, -jacobian(2,15));

							tripletListP.emplace_back(ir    , ir    ,  cauchy(delta_x, 1));
							tripletListP.emplace_back(ir + 1, ir + 1,  cauchy(delta_y, 1));
							tripletListP.emplace_back(ir + 2, ir + 2,  cauchy(delta_z, 1));

							tripletListB.emplace_back(ir    , 0,  delta_x);
							tripletListB.emplace_back(ir + 1, 0,  delta_y);
							tripletListB.emplace_back(ir + 2, 0,  delta_z);
						}
					}
				}
			}

			int ir = tripletListB.size();
			tripletListA.emplace_back(ir     , 0, 1);
			tripletListA.emplace_back(ir + 1 , 1, 1);
			tripletListA.emplace_back(ir + 2 , 2, 1);
			tripletListA.emplace_back(ir + 3 , 3, 1);
			tripletListA.emplace_back(ir + 4 , 4, 1);
			tripletListA.emplace_back(ir + 5 , 5, 1);
			tripletListA.emplace_back(ir + 6 , 6, 1);
			tripletListA.emplace_back(ir + 7 , 7, 1);

			tripletListP.emplace_back(ir     , ir,     1000000);
			tripletListP.emplace_back(ir + 1 , ir + 1, 1000000);
			tripletListP.emplace_back(ir + 2 , ir + 2, 1000000);
			tripletListP.emplace_back(ir + 3 , ir + 3, 1000000);
			tripletListP.emplace_back(ir + 4 , ir + 4, 1000000);
			tripletListP.emplace_back(ir + 5 , ir + 5, 1000000);
			tripletListP.emplace_back(ir + 6 , ir + 6, 1000000);
			tripletListP.emplace_back(ir + 7 , ir + 7, 1000000);

			tripletListB.emplace_back(ir     , 0, 0);
			tripletListB.emplace_back(ir + 1 , 0, 0);
			tripletListB.emplace_back(ir + 2 , 0, 0);
			tripletListB.emplace_back(ir + 3 , 0, 0);
			tripletListB.emplace_back(ir + 4 , 0, 0);
			tripletListB.emplace_back(ir + 5 , 0, 0);
			tripletListB.emplace_back(ir + 6 , 0, 0);
			tripletListB.emplace_back(ir + 7 , 0, 0);

			for(size_t i = 0 ; i < trajectory.size(); i++){
				int ic = i * 8;
				ir = tripletListB.size();
				QuaternionPose pose = pose_quaternion_from_affine_matrix(trajectory[i]);

				double delta;
				quaternion_constraint(delta, pose.q0, pose.q1, pose.q2, pose.q3);

				Eigen::Matrix<double, 1, 4> jacobian;
				quaternion_constraint_jacobian(jacobian, pose.q0, pose.q1, pose.q2, pose.q3);

				tripletListA.emplace_back(ir, ic + 3 , -jacobian(0,0));
				tripletListA.emplace_back(ir, ic + 4 , -jacobian(0,1));
				tripletListA.emplace_back(ir, ic + 5 , -jacobian(0,2));
				tripletListA.emplace_back(ir, ic + 6 , -jacobian(0,3));

				tripletListP.emplace_back(ir, ir, 1000000.0);

				tripletListB.emplace_back(ir, 0, delta);
			}

			Eigen::SparseMatrix<double> matA(tripletListB.size(), trajectory.size() * 8);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(trajectory.size() * 8, trajectory.size() * 8);
			Eigen::SparseMatrix<double> AtPB(trajectory.size() * 8, 1);

			{
			Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
			AtPA = (AtP) * matA;
			AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}

			if(h_x.size() == trajectory.size() * 8){
				for(size_t i = 0 ; i < h_x.size(); i++){
					std::cout << h_x[i] << std::endl;
				}
				std::cout << "AtPA=AtPB SOLVED" << std::endl;
				std::cout << "update" << std::endl;

				int counter = 0;

				for(size_t i = 0; i < trajectory.size(); i++){
					QuaternionPose pose = pose_quaternion_from_affine_matrix(trajectory[i]);
					pose.px += h_x[counter++];
					pose.py += h_x[counter++];
					pose.pz += h_x[counter++];
					pose.q0 += h_x[counter++];
					pose.q1 += h_x[counter++];
					pose.q2 += h_x[counter++];
					pose.q3 += h_x[counter++];
					scales[i] += h_x[counter++];
					trajectory[i] = affine_matrix_from_pose_quaternion(pose);
				}
			}else{
				std::cout << "AtPA=AtPB FAILED" << std::endl;
			}
			break;
		}
	}
	printHelp();
	glutPostRedisplay();
}

void mouse(int button, int state, int x, int y) {
	if (state == GLUT_DOWN) {
		mouse_buttons |= 1 << button;
	} else if (state == GLUT_UP) {
		mouse_buttons = 0;
	}

	mouse_old_x = x;
	mouse_old_y = y;
}

void motion(int x, int y) {
	float dx, dy;
	dx = (float) (x - mouse_old_x);
	dy = (float) (y - mouse_old_y);

	if (mouse_buttons & 1) {
		rotate_x += dy * 0.2f;
		rotate_y += dx * 0.2f;

	} else if (mouse_buttons & 4) {
		translate_z += dy * 0.05f;
	} else if (mouse_buttons & 3) {
		translate_x += dx * 0.05f;
		translate_y -= dy * 0.05f;
	}

	mouse_old_x = x;
	mouse_old_y = y;

	glutPostRedisplay();
}

void reshape(int w, int h) {
	glViewport(0, 0, (GLsizei) w, (GLsizei) h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) w / (GLfloat) h, 0.01, 10000.0);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}

void printHelp() {
	std::cout << "-------help-------" << std::endl;
	std::cout << "n: add noise to trajectory" << std::endl;
	std::cout << "s: add noise to scales" << std::endl;
	std::cout << "t: optimize (Tait-Bryan)" << std::endl;
	std::cout << "r: optimize (Rodrigues)" << std::endl;
	std::cout << "q: optimize (Quaternion)" << std::endl;
}


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// 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 <boost/config.hpp>
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_utility.hpp>

/*
  Sample output:

  original graph:
  0 --> 3 2 3
  1 --> 3
  2 --> 0
  3 --> 2

  removing edges (0,3)
  0 --> 2
  1 --> 3
  2 --> 0
  3 --> 2
  removing edge (0,2) and (3, 2)
  0 -->
  1 --> 3
  2 --> 0
  3 -->

 */

using namespace boost;

typedef adjacency_list< vecS, vecS, directedS > Graph;

int main()
{
    typedef std::pair< std::size_t, std::size_t > Edge;
    Edge edges[6] = { Edge(0, 3), Edge(0, 2), Edge(0, 3), Edge(1, 3),
        Edge(2, 0), Edge(3, 2) };

#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
    // VC++ can't handle iterator constructor
    Graph g(4);
    for (std::size_t j = 0; j < 6; ++j)
        add_edge(edges[j].first, edges[j].second, g);
#else
    Graph g(edges, edges + 6, 4);
#endif

    std::cout << "original graph:" << std::endl;
    print_graph(g, get(vertex_index, g));
    std::cout << std::endl;

    std::cout << "removing edges (0,3)" << std::endl;
    remove_out_edge_if(vertex(0, g), incident_to(vertex(3, g), g), g);
    print_graph(g, get(vertex_index, g));

    std::cout << "removing edge (0,2) and (3, 2)" << std::endl;
    remove_edge_if(incident_to(vertex(2, g), g), g);
    print_graph(g, get(vertex_index, g));

    return 0;
}


#ifndef MANIFOLDS_TESTS_POINTWISE_COMPARISON_HH
#define MANIFOLDS_TESTS_POINTWISE_COMPARISON_HH

#include <boost/test/unit_test.hpp>
#include <random>
#include "data/matrix.hh"

namespace manifolds {
template <class Eng, class Rand, int... Ns>
auto MakeInput(Rand &r, Eng &e, std::integer_sequence<int, Ns...>) {
  return make_my_tuple(((void)Ns, r(e))...);
}

template <int N, class Eng, class Rand> auto MakeInput(Rand &r, Eng &e) {
  return MakeInput(r, e, std::make_integer_sequence<int, N>());
}

template <class M>
void ComparePoint(M m1, M m2, double tolerance, std::true_type) {
  for (std::size_t r = 0; r < M::num_rows; r++)
    for (std::size_t c = 0; c < M::num_cols; c++)
      BOOST_CHECK_CLOSE(m1.Coeff(r, c), m2.Coeff(r, c), tolerance);
}

template <class X>
void ComparePoint(X x1, X x2, double tolerance, std::false_type) {
  BOOST_CHECK_CLOSE(x1, x2, tolerance);
}

template <class F> void ComparePoint(F f1, F f2, double tolerance) {
  ComparePoint(f1, f2, tolerance, IsMatrix<F>());
}

inline void PrintTupleIter(const tuple<> &) {}

template <class Tuple> void PrintTupleIter(const Tuple &t) {
  std::cout << std::get<0>(t);
  if (std::tuple_size<Tuple>::value > 1)
    std::cout << ", ";
  PrintTupleIter(remove_element<0>(t));
}

template <class Tuple> void PrintTuple(const Tuple &t) {
  std::cout << "(";
  PrintTupleIter(t);
  std::cout << ")\n";
}

static const int pointwise_default_num_points = 100;
static const double pointwise_default_tolerance = 0;
static const double pointwise_default_min = -100;
static const double pointwise_default_max = 100;

template <class F, class FRef>
void PointwiseEqual(F f, FRef fr, int num_points = pointwise_default_num_points,
                    double tolerance = pointwise_default_tolerance,
                    double min = pointwise_default_min,
                    double max = pointwise_default_max) {
  std::minstd_rand e;
  std::uniform_real_distribution<> r(min, max);
  for (int i = 0; i < num_points; i++) {
    auto x = MakeInput<F::input_dim>(r, e);
    // PrintTuple(x);
    ComparePoint(f(x), fr(x), tolerance);
  }
}
}

#endif


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

#include <numeric>
#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE ndimvector_test

// Standard includes
#include <iostream>

// Third party includes
#include <boost/test/unit_test.hpp>

// Local VOTCA includes
#include "votca/tools/NDimVector.h"

using namespace votca::tools;

BOOST_AUTO_TEST_SUITE(ndimvector_test)

BOOST_AUTO_TEST_CASE(constructor) {

  NDimVector<int, 3> a(1, 3, 4);
  BOOST_CHECK_EQUAL(a.dimension(0), 1);
  BOOST_CHECK_EQUAL(a.dimension(1), 3);
  BOOST_CHECK_EQUAL(a.dimension(2), 4);
  BOOST_CHECK_EQUAL(a.size(), 12);

  a(0, 1, 2) = 2;
  a(0, 1, 3) = 4;
  int sum = std::accumulate(a.begin(), a.end(), 0);
  BOOST_CHECK_EQUAL(sum, 6);

  BOOST_CHECK_EQUAL(a(0, 1, 3), 4);

  NDimVector<std::vector<int>, 3> b(2, 3, 4);
  b(0, 2, 2).push_back(1);
}

BOOST_AUTO_TEST_SUITE_END()