#include <iostream>
#include <random>
#include <chrono>
#include <array>

#include <boost/program_options.hpp>

#include "EnumerateBitIndex.h"

using namespace std;
using namespace boost;
using namespace boost::program_options;
using namespace realcore;

int main(int argc, char* argv[])
{
  // オプション設定
  options_description option;

  option.add_options()
    ("count,c", value<uint64_t>()->default_value(10000), "反復回数(default: 1万回)")
    ("help,h", "ヘルプを表示");

  variables_map arg_map;
  store(parse_command_line(argc, argv, option), arg_map);

  if(arg_map.count("help")){
    cout << "Usage: " << argv[0] << " [options]" << endl;
    cout << option;
    cout << endl;
    cout << "Repeat the following procedure on specified times * OnBitPattern(0-64):" << endl;
    cout << " 1. Scanning the bit sequence method" << endl;
    cout << " 2. Table lookup method" << endl;
    cout << " 3. Enumerate rightmost bit index" << endl;
    cout << endl;

    return 0;
  }

  // 乱数初期化
  random_device seed_generator;
  size_t seed = seed_generator();
  mt19937_64 random_generator(seed);

  cout << "random seed:\t" << seed << endl;

  // OnBit数ごとに統計を取るためOnBit数別に乱数を記録する
  const uint64_t iteration_count = arg_map["count"].as<uint64_t>();
  constexpr size_t kBitPattern = 65;    // OnBit数: 0-64の65通り

  // 格納リストの初期化
  std::array<vector<uint64_t>, kBitPattern> on_bit_value_list;

  for(auto &value : on_bit_value_list){
    value.reserve(iteration_count);
  }

  // OnBit数ごとに乱数を生成する
  for(size_t on_bit_count=0; on_bit_count<kBitPattern; ++on_bit_count){
    for(size_t i=0; i<iteration_count; i++){
      std::array<size_t, 64> shift_value_map{{0}};   // 生成済みのシフト値
      size_t shift_count = 0;
      uint64_t value = 0;

      while(shift_count != on_bit_count){
        size_t shift = random_generator() % 64;

        if(shift_value_map[shift] == 0){
          shift_value_map[shift] = 1;
          ++shift_count;

          value |= 1ULL << shift;
        }
      }

      on_bit_value_list[on_bit_count].push_back(value);
    }
  }

  // ScanBitSequence
  vector<uint64_t> time_scan;
  time_scan.reserve(kBitPattern);

  {
    uint64_t index_sum = 0;
    vector<size_t> index_list;
    index_list.reserve(64);

    for(const auto &value_list : on_bit_value_list){
      auto start_time = chrono::system_clock::now();

      for(auto value : value_list){
        index_list.clear();
        ScanBitSequence(value, &index_list);
        index_sum = accumulate(index_list.begin(), index_list.end(), index_sum);
      }

      auto elapsed_time = chrono::system_clock::now() - start_time;
      time_scan.emplace_back(chrono::duration_cast<chrono::milliseconds>(elapsed_time).count());
    }

    cout << "ScanBitSequence index sum: " << index_sum << endl;
  }

  // TableLookup
  vector<uint64_t> time_shift_map;
  time_shift_map.reserve(kBitPattern);

  {
    uint64_t index_sum = 0;
    vector<size_t> index_list;
    index_list.reserve(64);

    for(const auto &value_list : on_bit_value_list){
      auto start_time = chrono::system_clock::now();

      for(auto value : value_list){
        index_list.clear();
        ShiftMap(value, &index_list);
        index_sum = accumulate(index_list.begin(), index_list.end(), index_sum);
      }

      auto elapsed_time = chrono::system_clock::now() - start_time;
      time_shift_map.emplace_back(chrono::duration_cast<chrono::milliseconds>(elapsed_time).count());
    }

    cout << "TableLookup index sum: " << index_sum << endl;
  }

  // EnumerateRightmostBit
  vector<uint64_t> time_rightmost;
  time_rightmost.reserve(kBitPattern);

  {
    uint64_t index_sum = 0;
    vector<size_t> index_list;
    index_list.reserve(64);

    for(const auto &value_list : on_bit_value_list){
      auto start_time = chrono::system_clock::now();

      for(auto value : value_list){
        index_list.clear();
        EnumerateRightmostBit(value, &index_list);
        index_sum = accumulate(index_list.begin(), index_list.end(), index_sum);
      }

      auto elapsed_time = chrono::system_clock::now() - start_time;
      time_rightmost.emplace_back(chrono::duration_cast<chrono::milliseconds>(elapsed_time).count());
    }

    cout << "EnumerateRightmostBit index sum: " << index_sum << endl;
  }

  // 経過時間を出力
  cout << "PopCount,ScanBitSequence(ms),TableLookup(ms),EnumerateRightmostBit(ms)" << endl;

  for(size_t i=0; i<kBitPattern; i++){
    cout << i << ",";
    cout << time_scan[i] << ",";
    cout << time_shift_map[i] << ",";
    cout << time_rightmost[i] << endl;
  }

  return 0;
}


#include "powerlawCommon.h"
//#include <boost/program_options.hpp>
#include "tkdCmdParser.h"

/**
 * @author: W.M. Otte (wim@invivonmr.uu.nl); Image Sciences Institute, UMC Utrecht, NL.
 * @date: 19-11-2009
 *
 * Estimate powerlaw scaling parameter from input distribution.
 *
 * ***************************************************************************
 * Method: "Power-law distributions in empirical data", Clauset et al, 2009
 * http://www.santafe.edu/~aaronc/powerlaws/
 * ***************************************************************************
 */
class PowerLawFit
{

public:

	typedef double ValueType;
	typedef std::vector< ValueType > VectorType;

	/**
	 * Power law fit.
	 */
	void run( const std::string& inputFileName, bool nosmall, bool finite,
					double startXmin, double incrementXmin, double endXmin,
						bool bootstrap, unsigned int bootstrapIterations, bool verbose )
	{
		// [ 1 ] read input from text file ...
		VectorType values = getInput( inputFileName );

		// [ 2 ] bootstrap or single fit ...
		VectorType results;

		if ( bootstrap )
		{
			graph::Powerlaw< ValueType >::BootstrapFit( values, results, nosmall, finite, startXmin, incrementXmin, endXmin, bootstrapIterations, verbose );

			if ( ! results.empty() )
			{
				std::cout << "Alpha," << results.at( 0 ) <<  std::endl;
				std::cout << "Xmin," << results.at( 1 ) << std::endl;
				std::cout << "Log-likelihood," << results.at( 2 ) << std::endl;
				std::cout << "Alpha_sd," << results.at( 3 ) <<  std::endl;
				std::cout << "Xmin_sd," << results.at( 4 ) << std::endl;
				std::cout << "Log-likelihood_sd," << results.at( 5 ) << std::endl;

			}
			else
			{
				std::cerr << "*** ERROR ***: maximum likelihood "
					"bootstrap estimation failed! -> check input ..." << std::endl;
			}

		}
		else
		{
			graph::Powerlaw< ValueType >::SingleFit( values, results, nosmall, finite,
					startXmin, incrementXmin, endXmin );

			if ( ! results.empty() )
			{
				std::cout << "Alpha," << results.at( 0 ) << std::endl;
				std::cout << "Xmin," << results.at( 1 ) << std::endl;
				std::cout << "Log-likelihood," << results.at( 2 ) << std::endl;
			}
			else
			{
				std::cerr << "*** ERROR ***: maximum likelihood "
					"single estimation failed! -> check input ..." << std::endl;
			}
		}
	}

protected:

	/**
	 * Return input from given text file as vector.
	 */
	VectorType getInput( const std::string& input )
	{
		std::ifstream inFile;

		inFile.open( input.c_str() );
		if ( !inFile )
		{
			std::cout << "*** ERROR ***: Unable to open: " << input << "." << std::endl;
			exit( EXIT_FAILURE );
		}

		double x;
		VectorType output;

		while ( inFile >> x )
		{
			output.push_back( x );
		}
		inFile.close();

		/**
		 * Negative values will be converted to complex numbers in matlab,
		 * but not with the stl ...
		 *
		 * No support is given (yet) for complex number mle.
		 */
		if ( *( std::min_element( output.begin(), output.end() ) ) < 0 )
		{
			std::cerr << "*** ERROR ***: Negative input not supported!" << std::endl;
			exit (EXIT_FAILURE );
		}

		return output;
	}
};

// ************************************************************************************

/**
 * Option list.
 */
struct parameters
{
	std::string input;
	bool nosmall;
	bool finite;
	bool bootstrap;
	bool verbose;
	double startXmin;
	double incrementXmin;
	double endXmin;
	int bootstrapIterations;

};

/**
 * Fit powerlaw to list of numbers.
 */
int main(int argc, char* argv[])
{
	tkd::CmdParser p( argv[0], "Fits a power-law distributional model to data." );

		parameters list;

		list.nosmall = false;
		list.finite = false;
		list.bootstrap = false;
		list.verbose = false;
		list.startXmin = 1.5;
		list.incrementXmin = 0.01;
		list.endXmin = 3.5;
		list.bootstrapIterations = 1000;

		p.AddArgument( list.input, "input" )
				->AddAlias( "i" )
				->SetDescription( "Input file with distribution values in column format" )
				->SetRequired( true );

		p.AddArgument( list.nosmall, "nosmall" )
				->SetDescription( "Truncate the search over xmin values before the finite-size bias becomes significant (default: false)" );

		p.AddArgument( list.finite, "finite" )
				->SetDescription( "Use an experimental finite-size correction (default: false)" );

		p.AddArgument( list.bootstrap, "bootstrap" )
				->SetDescription( "Run non-parametric bootstrap instead of single estimation (default: false)" );

		p.AddArgument( list.verbose, "verbose" )
				->SetDescription( "Print boostrap status (default: false)" );

		p.AddArgument( list.startXmin, "start-x-min" )
				->SetDescription( "Start value for discrete xmin estimation (default: 1.5)" );

		p.AddArgument( list.incrementXmin, "increment-x-min" )
				->SetDescription( "Increment value for discrete xmin estimation (default: 0.01)" );

		p.AddArgument( list.endXmin, "end-x-min" )
				->SetDescription( "End value for discrete xmin estimation (default: 3.5)" );

		p.AddArgument( list.bootstrapIterations, "iterations" )
				->SetDescription( "Bootstrap iterations (default: 1000)" );

		if ( !p.Parse( argc, argv ) )
		{
			p.PrintUsage( std::cout );
			return EXIT_FAILURE;
		}

        // run application ...
        PowerLawFit powerlawFit;

    	powerlawFit.run( list.input, list.nosmall, list.finite,
							list.startXmin, list.incrementXmin, list.endXmin,
											list.bootstrap, list.bootstrapIterations, list.verbose );

    return EXIT_SUCCESS;
}


#ifndef VI_PID_CONFIG_HPP
#define VI_PID_CONFIG_HPP

// Libraries
#include <boost/cstdfloat.hpp>

// Ros
#include <ros/ros.h>

// Standard
#include <cstdint>
#include <string>

namespace vi
{

using float64_t = boost::float64_t; ///< Alias for 64 bit float

/// @brief Class to store config values for vehicle interface
class PIDConfig
{
public:
    /// @brief Default constructor
    /// @param pnh Private nodehandle used to snipe params
    PIDConfig(ros::NodeHandle& pnh, const std::string& prefix)
    {
        pnh.getParam(prefix + "_p",     m_p);
        pnh.getParam(prefix + "_i",     m_i);
        pnh.getParam(prefix + "_d",     m_d);
        pnh.getParam("max_dt_s",        m_max_dt_s);
        pnh.getParam("ctl_buffer_size", m_ctl_buffer_size);
    }

    /// @brief Default destructor for forward declares
    ~PIDConfig() = default;

    /// @brief Accessor
    /// @return Val
    /// @{
    float64_t    getPGain()             const noexcept {return m_p;}
    float64_t    getIGain()             const noexcept {return m_i;}
    float64_t    getDGain()             const noexcept {return m_d;}
    float64_t    getMaxDtS()            const noexcept {return m_max_dt_s;}
    std::int32_t getControlBufferSize() const noexcept {return m_ctl_buffer_size;}
    /// @}

private:
    /// @brief Gains
    /// @{
    float64_t m_p{0.0}; ///< Proportional gain
    float64_t m_i{0.0}; ///< Integral gain
    float64_t m_d{0.0}; ///< Derivative gain
    /// @}

    /// @brief Sanity checks
    /// @{
    float64_t m_max_dt_s; ///< Max allowable DT in seconds
    /// @}

    /// @brief Filter configs
    /// @{
    std::int32_t m_ctl_buffer_size{100};
    /// @}
};

} // namespace vi

#endif // VI_PID_CONFIG_HPP


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/arithmetic/include/functions/idivround.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/constant/constant.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/inf.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/minf.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/nan.hpp>
#include <boost/simd/include/constants/valmin.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/simd/include/constants/five.hpp>
#include <boost/simd/include/constants/two.hpp>
#include <boost/simd/include/constants/three.hpp>
#include <boost/simd/include/constants/mfour.hpp>

NT2_TEST_CASE_TPL ( idivround_real__2_0,  BOOST_SIMD_SIMD_REAL_TYPES)
{
  using boost::simd::idivround;
  using boost::simd::tag::idivround_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef typename boost::dispatch::meta::call<idivround_(vT,vT)>::type r_t;

  // specific values tests
  NT2_TEST_ULP_EQUAL(idivround(boost::simd::Inf<vT>(), boost::simd::Inf<vT>()), boost::simd::Zero<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(idivround(boost::simd::Minf<vT>(), boost::simd::Minf<vT>()), boost::simd::Zero<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(idivround(boost::simd::Mone<vT>(), boost::simd::Mone<vT>()), boost::simd::One<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(idivround(boost::simd::Nan<vT>(), boost::simd::Nan<vT>()), boost::simd::Zero<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(idivround(boost::simd::One<vT>(), boost::simd::One<vT>()), boost::simd::One<r_t>(), 0);
} // end of test for floating_

NT2_TEST_CASE_TPL ( idivround_unsigned_int__2_0,  BOOST_SIMD_SIMD_UNSIGNED_TYPES)
{

  using boost::simd::idivround;
  using boost::simd::tag::idivround_;
  using boost::simd::native;
  typedef typename boost::dispatch::meta::call<idivround_(T,T)>::type r_t;

  NT2_TEST_ULP_EQUAL(idivround(boost::simd::Five<T>(),boost::simd::Three<T>()), boost::simd::Two<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(idivround(boost::simd::Four<T>(),boost::simd::Three<T>()), boost::simd::One<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(idivround(boost::simd::Four<T>(),boost::simd::Zero<T>()), boost::simd::Valmax<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(idivround(boost::simd::One<T>(), boost::simd::One<T>()), boost::simd::One<r_t>(), 0);
} // end of test for unsigned_int_

NT2_TEST_CASE_TPL ( idivround_signed_int__2_0,  BOOST_SIMD_SIMD_INTEGRAL_SIGNED_TYPES)
{

  using boost::simd::idivround;
  using boost::simd::tag::idivround_;
  using boost::simd::native;
  typedef typename boost::dispatch::meta::call<idivround_(T,T)>::type r_t;

  // specific values tests
  NT2_TEST_ULP_EQUAL(idivround(boost::simd::Five<T>(),boost::simd::Three<T>()), boost::simd::Two<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(idivround(boost::simd::Four<T>(),boost::simd::Three<T>()), boost::simd::One<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(idivround(boost::simd::Four<T>(),boost::simd::Zero<T>()), boost::simd::Valmax<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(idivround(boost::simd::Mfour<T>(),boost::simd::Zero<T>()), boost::simd::Valmin<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(idivround(boost::simd::Mone<T>(), boost::simd::Mone<T>()), boost::simd::One<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(idivround(boost::simd::One<T>(), boost::simd::One<T>()), boost::simd::One<r_t>(), 0);
} // end of test for signed_int_


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/hyperbolic/include/functions/atanh.hpp>

#include <nt2/sdk/unit/tests/cover.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/module.hpp>

#include <nt2/sdk/meta/cardinal_of.hpp>
#include <nt2/include/functions/aligned_load.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <iostream>

#include <nt2/include/functions/aligned_load.hpp>

NT2_TEST_CASE_TPL ( atanh,  NT2_SIMD_REAL_TYPES)
{
  using nt2::atanh;
  using nt2::tag::atanh_;
  using boost::simd::native;
  using nt2::meta::cardinal_of;
  using nt2::aligned_load;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;

  static const nt2::uint32_t NR = NT2_NB_RANDOM_TEST;
  {
    NT2_CREATE_BUF(tab_a0,T, NR, T(-1), T(1));
    for(nt2::uint32_t j = 0; j < NR;j+=cardinal_of<vT>::value)
      {
        vT a0 = aligned_load<vT>(&tab_a0[0],j);
        vT v = atanh(a0);
        for(nt2::uint32_t i = 0; i< cardinal_of<vT>::value; i++)
        {

          NT2_TEST_ULP_EQUAL( v[i],T(nt2::atanh (a0[i])), 1.0);
        }
      }
  }
} // end of test for floating_


#include <gtest/gtest.h>
#include <boost/math/distributions.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <stan/math/prim.hpp>
#include <test/unit/math/prim/prob/util.hpp>
#include <limits>
#include <vector>

TEST(ProbDistributionsMultiNormalPrec, vectorized) {
  // Test scalar/vector combinations.
  boost::random::mt19937 rng;

  Eigen::VectorXd mu(3);
  Eigen::RowVectorXd mu_t(3);
  std::vector<Eigen::VectorXd> vec_mu(2);
  std::vector<Eigen::RowVectorXd> vec_mu_t(2);
  vec_mu[0] = mu;
  vec_mu_t[0] = mu;
  mu << 2.0, -1.0, 4.0;
  vec_mu[1] = mu;
  vec_mu_t[1] = mu;
  mu << 1.0, -1.0, 3.0;
  mu_t = mu;

  Eigen::MatrixXd Sigma(3, 3);
  Sigma << 9.0, -3.0, 0.0, -3.0, 4.0, 0.0, 0.0, 0.0, 5.0;

  EXPECT_NO_THROW(stan::math::multi_normal_prec_rng(mu, Sigma, rng));
  EXPECT_NO_THROW(stan::math::multi_normal_prec_rng(mu_t, Sigma, rng));
  EXPECT_NO_THROW(stan::math::multi_normal_prec_rng(vec_mu, Sigma, rng));
  EXPECT_NO_THROW(stan::math::multi_normal_prec_rng(vec_mu_t, Sigma, rng));
}

TEST(ProbDistributionsMultiNormalPrec, policiesSigma) {
  boost::random::mt19937 rng;

  double nan = std::numeric_limits<double>::quiet_NaN();
  double inf = std::numeric_limits<double>::infinity();
  double ninf = -std::numeric_limits<double>::infinity();

  Eigen::VectorXd mu(2);
  mu << 1.0, -1.0;
  Eigen::MatrixXd Sigma(2, 2);
  Sigma << 9.0, -3.0, -3.0, 4.0;
  EXPECT_NO_THROW(stan::math::multi_normal_prec_rng(mu, Sigma, rng));

  // non-symmetric
  Sigma(0, 1) = -2.5;
  EXPECT_THROW(stan::math::multi_normal_prec_rng(mu, Sigma, rng),
               std::domain_error);
  Sigma(0, 1) = -3;

  // not positive-definite
  Sigma(0, 1) = -10;
  EXPECT_THROW(stan::math::multi_normal_prec_rng(mu, Sigma, rng),
               std::domain_error);
  Sigma(0, 1) = -3;

  // not square
  Eigen::MatrixXd rect_Sigma(2, 3);
  rect_Sigma << 1, 0, 0, 0, 1, 0;
  EXPECT_THROW(stan::math::multi_normal_prec_rng(mu, rect_Sigma, rng),
               std::invalid_argument);

  // NaN
  Sigma(0, 0) = nan;
  EXPECT_THROW(stan::math::multi_normal_prec_rng(mu, Sigma, rng),
               std::domain_error);
  Sigma(0, 0) = 9.0;

  // inf
  Sigma(0, 0) = inf;
  EXPECT_THROW(stan::math::multi_normal_prec_rng(mu, Sigma, rng),
               std::domain_error);
  Sigma(0, 0) = ninf;
  EXPECT_THROW(stan::math::multi_normal_prec_rng(mu, Sigma, rng),
               std::domain_error);
  Sigma(0, 0) = 9.0;
}

TEST(ProbDistributionsMultiNormalPrec, policiesMu) {
  boost::random::mt19937 rng;

  double nan = std::numeric_limits<double>::quiet_NaN();
  double inf = std::numeric_limits<double>::infinity();
  double ninf = -std::numeric_limits<double>::infinity();

  Eigen::VectorXd mu(3, 1);
  mu << 1.0, -1.0, 3.0;
  Eigen::MatrixXd Sigma(3, 3);
  Sigma << 9.0, -3.0, 0.0, -3.0, 4.0, 0.0, 0.0, 0.0, 5.0;

  EXPECT_NO_THROW(stan::math::multi_normal_prec_rng(mu, Sigma, rng));

  mu(0) = inf;
  EXPECT_THROW(stan::math::multi_normal_prec_rng(mu, Sigma, rng),
               std::domain_error);

  mu(0) = ninf;
  EXPECT_THROW(stan::math::multi_normal_prec_rng(mu, Sigma, rng),
               std::domain_error);

  mu(0) = nan;
  EXPECT_THROW(stan::math::multi_normal_prec_rng(mu, Sigma, rng),
               std::domain_error);

  mu(0) = 1.0;
}

TEST(ProbDistributionsMultiNormalPrec, SizeMismatch) {
  boost::random::mt19937 rng;
  Eigen::VectorXd mu(2);
  mu << 1.0, -1.0;
  Eigen::MatrixXd Sigma(3, 3);
  Sigma << 9.0, -3.0, 0.0, -3.0, 4.0, 0.0, 0.0, 0.0, 5.0;
  EXPECT_THROW(stan::math::multi_normal_prec_rng(mu, Sigma, rng),
               std::invalid_argument);
}

TEST(ProbDistributionsMultiNormalPrec, marginalOneChiSquareGoodnessFitTest) {
  boost::random::mt19937 rng;

  Eigen::MatrixXd sigma(3, 3);
  sigma << 9.0, -3.0, 0.0, -3.0, 4.0, 1.0, 0.0, 1.0, 3.0;
  Eigen::MatrixXd L = sigma.inverse();

  std::vector<Eigen::VectorXd> mu(3);
  mu[0].resize(3);
  mu[1].resize(3);
  mu[2].resize(3);
  mu[0] << 2.0, -2.0, 11.0;
  mu[1] << 7.0, -3.0, 5.0;
  mu[2] << 5.0, -6.0, 1.0;

  int N = 10000;
  int K = boost::math::round(2 * std::pow(N, 0.4));
  boost::math::normal_distribution<> dist(mu[0](0), sqrt(sigma(0, 0)));

  std::vector<double> quantiles;
  for (int i = 1; i < K; i++)
    quantiles.push_back(quantile(dist, i * std::pow(K, -1.0)));
  quantiles.push_back(std::numeric_limits<double>::max());

  Eigen::VectorXd a(mu[0].rows());
  std::vector<double> samples;
  for (int count = 0; count < N; ++count) {
    a = stan::math::multi_normal_prec_rng(mu, L, rng)[0];
    samples.push_back(a(0));
  }

  assert_matches_quantiles(samples, quantiles, 1e-6);
}

TEST(ProbDistributionsMultiNormalPrec, marginalTwoChiSquareGoodnessFitTest) {
  boost::random::mt19937 rng;

  Eigen::MatrixXd sigma(3, 3);
  sigma << 9.0, -3.0, 0.0, -3.0, 4.0, 1.0, 0.0, 1.0, 3.0;
  Eigen::MatrixXd L = sigma.inverse();

  std::vector<Eigen::RowVectorXd> mu(3);
  mu[0].resize(3);
  mu[1].resize(3);
  mu[2].resize(3);
  mu[0] << 1.0, 5.0, -1.0;
  mu[1] << 2.0, -2.0, 11.0;
  mu[2] << 7.0, 0.0, 3.0;

  int N = 10000;
  int K = boost::math::round(2 * std::pow(N, 0.4));
  boost::math::normal_distribution<> dist(mu[1](1), sqrt(sigma(1, 1)));

  std::vector<double> quantiles;
  for (int i = 1; i < K; i++)
    quantiles.push_back(quantile(dist, i * std::pow(K, -1.0)));
  quantiles.push_back(std::numeric_limits<double>::max());

  Eigen::VectorXd a(mu[0].rows());
  std::vector<double> samples;
  for (int count = 0; count < N; ++count) {
    a = stan::math::multi_normal_prec_rng(mu, L, rng)[1];
    samples.push_back(a(1));
  }

  assert_matches_quantiles(samples, quantiles, 1e-6);
}

TEST(ProbDistributionsMultiNormalPrec, marginalThreeChiSquareGoodnessFitTest) {
  boost::random::mt19937 rng;

  Eigen::MatrixXd sigma(3, 3);
  sigma << 9.0, -3.0, 0.0, -3.0, 4.0, 1.0, 0.0, 1.0, 16.0;
  Eigen::MatrixXd L = sigma.inverse();

  Eigen::VectorXd mu(3);
  mu << 2.0, -2.0, 11.0;

  int N = 10000;
  int K = boost::math::round(2 * std::pow(N, 0.4));
  boost::math::normal_distribution<> dist(mu(2), sqrt(sigma(2, 2)));

  std::vector<double> quantiles;
  for (int i = 1; i < K; i++)
    quantiles.push_back(quantile(dist, i * std::pow(K, -1.0)));
  quantiles.push_back(std::numeric_limits<double>::max());

  Eigen::VectorXd a(mu.rows());
  std::vector<double> samples;
  for (int count = 0; count < N; ++count) {
    a = stan::math::multi_normal_prec_rng(mu, L, rng);
    samples.push_back(a(2));
  }

  assert_matches_quantiles(samples, quantiles, 1e-6);
}


// -*- coding: utf-8 -*-
#include <doctest/doctest.h>  // for ResultBuilder, CHECK

#include <boost/graph/adjacency_list.hpp>         // for vecS (ptr only)
#include <boost/graph/detail/adjacency_list.hpp>  // for vertices, get, out_...
#include <boost/graph/graph_selectors.hpp>        // for directedS
#include <boost/graph/graph_traits.hpp>           // for graph_traits, graph...
#include <boost/graph/properties.hpp>             // for edge_property_tag
#include <boost/iterator/iterator_facade.hpp>     // for operator!=, operator==
#include <boost/move/utility_core.hpp>            // for move
#include <boost/pending/property.hpp>             // for no_property, property
#include <boost/property_map/property_map.hpp>    // for get, iterator_prope...
#include <cstddef>                                // for size_t
#include <netoptim/min_cycle_ratio.hpp>           // for min_cycle_ratio
#include <ostream>                                // for operator<<
#include <py2cpp/fractions.hpp>                   // for Fraction, operator-
#include <py2cpp/nx2bgl.hpp>                      // for grAdaptor, EdgeView
#include <type_traits>                            // for move
#include <utility>                                // for pair
#include <vector>                                 // for vector
// from fractions import Fraction

namespace boost {

    enum edge_id_tag_t { id_tag };  // a unique #
    BOOST_INSTALL_PROPERTY(edge, id_tag);

}  // namespace boost

using graph_t
    = boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
                            boost::property<boost::edge_id_tag_t, size_t>>;
using Vertex = typename boost::graph_traits<graph_t>::vertex_descriptor;
// using Edge_it = typename boost::graph_traits<graph_t>::edge_iterator;

static auto create_test_case1() {
    using Edge = std::pair<int, int>;

    const auto num_nodes = 5;
    enum nodes { A, B, C, D, E };
    static Edge edge_array[] = {Edge{A, B}, Edge{B, C}, Edge{C, D}, Edge{D, E}, Edge{E, A}};
    size_t indices[] = {0, 1, 2, 3, 4};
    int num_arcs = sizeof(edge_array) / sizeof(Edge);
    auto g = graph_t(edge_array, edge_array + num_arcs, indices, num_nodes);
    return py::grAdaptor<graph_t>{std::move(g)};
}

static auto create_test_case_timing() {
    using Edge = std::pair<int, int>;

    const auto num_nodes = 3;
    enum nodes { A, B, C };
    Edge edge_array[] = {Edge{A, B}, Edge{B, A}, Edge{B, C}, Edge{C, B},
                         Edge{B, C}, Edge{C, B}, Edge{C, A}, Edge{A, C}};
    size_t indices[] = {0, 1, 2, 3, 4, 5, 6, 7};
    int num_arcs = sizeof(edge_array) / sizeof(Edge);

    auto g = graph_t(edge_array, edge_array + num_arcs, indices, num_nodes);
    return py::grAdaptor<graph_t>{std::move(g)};
}

TEST_CASE("Test Parametric (boost)") {
    using EdgeIndexMap = typename boost::property_map<graph_t, boost::edge_id_tag_t>::type;
    using IterMap = boost::iterator_property_map<int*, EdgeIndexMap, int, int&>;

    auto G = create_test_case1();
    int cost[] = {5, 1, 1, 1, 1};
    EdgeIndexMap edge_id = boost::get(boost::id_tag, G);
    IterMap cost_pa(cost, edge_id);

    auto get_cost = [&](const auto& e) -> int { return boost::get(cost_pa, e); };
    auto get_time = [&](const auto&) -> int { return 1; };

    auto dist = std::vector<fun::Fraction<int>>(G.number_of_nodes(), fun::Fraction<int>(0));
    auto r = fun::Fraction<int>(5);
    const auto c = min_cycle_ratio(G, r, get_cost, get_time, dist);
    CHECK(!c.empty());
    CHECK(c.size() == 5);
    CHECK(r == fun::Fraction<int>(9, 5));
    // print(r);
    // print(c);
    // print(dist.items());
}

TEST_CASE("Test Parametric of Timing Graph (boost)") {
    using EdgeIndexMap = typename boost::property_map<graph_t, boost::edge_id_tag_t>::type;
    using IterMap = boost::iterator_property_map<int*, EdgeIndexMap, int, int&>;

    auto G = create_test_case_timing();
    int cost[] = {7, -1, 5, 4, 3, 0, 2, 4};
    EdgeIndexMap edge_id = boost::get(boost::id_tag, G);
    IterMap cost_pa(cost, edge_id);

    const auto get_cost = [&](const auto& e) -> int { return boost::get(cost_pa, e); };
    const auto get_time = [&](const auto& /*e*/) -> int { return 1; };

    auto dist = std::vector<fun::Fraction<int>>(G.number_of_nodes(), fun::Fraction<int>(0));
    auto r = fun::Fraction<int>(7);
    const auto c = min_cycle_ratio(G, r, get_cost, get_time, dist);
    CHECK(!c.empty());
    CHECK(r == fun::Fraction<int>(1, 1));
    CHECK(c.size() == 3);
    // print(r);
    // print(c);
    // print(dist.items());
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2010 Jitse Niesen <jitse@maths.leeds.ac.uk>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"
#include <limits>
#include <Eigen/Eigenvalues>

template<typename MatrixType> void eigensolver(const MatrixType& m)
{
  typedef typename MatrixType::Index Index;
  /* this test covers the following files:
     EigenSolver.h
  */
  Index rows = m.rows();
  Index cols = m.cols();

  typedef typename MatrixType::Scalar Scalar;
  typedef typename NumTraits<Scalar>::Real RealScalar;
  typedef Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> VectorType;
  typedef Matrix<RealScalar, MatrixType::RowsAtCompileTime, 1> RealVectorType;
  typedef typename std::complex<typename NumTraits<typename MatrixType::Scalar>::Real> Complex;

  MatrixType a = MatrixType::Random(rows,cols);
  MatrixType a1 = MatrixType::Random(rows,cols);
  MatrixType symmA =  a.adjoint() * a + a1.adjoint() * a1;

  EigenSolver<MatrixType> ei0(symmA);
  VERIFY_IS_EQUAL(ei0.info(), Success);
  VERIFY_IS_APPROX(symmA * ei0.pseudoEigenvectors(), ei0.pseudoEigenvectors() * ei0.pseudoEigenvalueMatrix());
  VERIFY_IS_APPROX((symmA.template cast<Complex>()) * (ei0.pseudoEigenvectors().template cast<Complex>()),
    (ei0.pseudoEigenvectors().template cast<Complex>()) * (ei0.eigenvalues().asDiagonal()));

  EigenSolver<MatrixType> ei1(a);
  VERIFY_IS_EQUAL(ei1.info(), Success);
  VERIFY_IS_APPROX(a * ei1.pseudoEigenvectors(), ei1.pseudoEigenvectors() * ei1.pseudoEigenvalueMatrix());
  VERIFY_IS_APPROX(a.template cast<Complex>() * ei1.eigenvectors(),
                   ei1.eigenvectors() * ei1.eigenvalues().asDiagonal());
  VERIFY_IS_APPROX(ei1.eigenvectors().colwise().norm(), RealVectorType::Ones(rows).transpose());
  VERIFY_IS_APPROX(a.eigenvalues(), ei1.eigenvalues());

  EigenSolver<MatrixType> eiNoEivecs(a, false);
  VERIFY_IS_EQUAL(eiNoEivecs.info(), Success);
  VERIFY_IS_APPROX(ei1.eigenvalues(), eiNoEivecs.eigenvalues());
  VERIFY_IS_APPROX(ei1.pseudoEigenvalueMatrix(), eiNoEivecs.pseudoEigenvalueMatrix());

  MatrixType id = MatrixType::Identity(rows, cols);
  VERIFY_IS_APPROX(id.operatorNorm(), RealScalar(1));

  if (rows > 2)
  {
    // Test matrix with NaN
    a(0,0) = std::numeric_limits<typename MatrixType::RealScalar>::quiet_NaN();
    EigenSolver<MatrixType> eiNaN(a);
    VERIFY_IS_EQUAL(eiNaN.info(), NoConvergence);
  }
}

template<typename MatrixType> void eigensolver_verify_assert(const MatrixType& m)
{
  EigenSolver<MatrixType> eig;
  VERIFY_RAISES_ASSERT(eig.eigenvectors());
  VERIFY_RAISES_ASSERT(eig.pseudoEigenvectors());
  VERIFY_RAISES_ASSERT(eig.pseudoEigenvalueMatrix());
  VERIFY_RAISES_ASSERT(eig.eigenvalues());

  MatrixType a = MatrixType::Random(m.rows(),m.cols());
  eig.compute(a, false);
  VERIFY_RAISES_ASSERT(eig.eigenvectors());
  VERIFY_RAISES_ASSERT(eig.pseudoEigenvectors());
}

void test_eigensolver_generic()
{
  int s;
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1( eigensolver(Matrix4f()) );
    s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4);
    CALL_SUBTEST_2( eigensolver(MatrixXd(s,s)) );

    // some trivial but implementation-wise tricky cases
    CALL_SUBTEST_2( eigensolver(MatrixXd(1,1)) );
    CALL_SUBTEST_2( eigensolver(MatrixXd(2,2)) );
    CALL_SUBTEST_3( eigensolver(Matrix<double,1,1>()) );
    CALL_SUBTEST_4( eigensolver(Matrix2d()) );
  }

  CALL_SUBTEST_1( eigensolver_verify_assert(Matrix4f()) );
  s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4);
  CALL_SUBTEST_2( eigensolver_verify_assert(MatrixXd(s,s)) );
  CALL_SUBTEST_3( eigensolver_verify_assert(Matrix<double,1,1>()) );
  CALL_SUBTEST_4( eigensolver_verify_assert(Matrix2d()) );

  // Test problem size constructors
  CALL_SUBTEST_5(EigenSolver<MatrixXf>(s));

  // regression test for bug 410
  CALL_SUBTEST_2(
  {
     MatrixXd A(1,1);
     A(0,0) = std::sqrt(-1.);
     Eigen::EigenSolver<MatrixXd> solver(A);
     MatrixXd V(1, 1);
     V(0,0) = solver.eigenvectors()(0,0).real();
  }
  );

  EIGEN_UNUSED_VARIABLE(s)
}


#include <boost/test/unit_test.hpp>
#include "functions/integral_polynomial.hh"
#include "functions/integral_polynomial_simplifications.hh"
#include "functions/streaming.hh"
#include "functions/operators.hh"
#include "functions/polynomial_simplifications.hh"
#include "pointwise_equal.hh"

BOOST_AUTO_TEST_CASE(integral_polynomial_test) {
  using namespace manifolds;
  auto ip1 = IP<1, -2>();
  auto ip2 = IP<3, 4>();
  auto p1 = GetPolynomial(1, -2);
  auto p2 = GetPolynomial(3, 4);
  PointwiseEqual(ip1, p1, 2);
  PointwiseEqual(ip2, p2, 2);
  auto ip3 = ip1 + ip1 * ip2;
  auto p3 = p1 + p1 * p2;
  PointwiseEqual(ip3, p3, 4);
  static_assert(is_polynomial<IntegralPolynomial<0, 1> >::value, "");
  BOOST_CHECK_EQUAL(-IP<0>(), zero);
}


// Copyright (c) 2012-2017 VideoStitch SAS
// Copyright (c) 2018 stitchEm

#ifndef __SO3_PARAMETERZATION_HPP__
#define __SO3_PARAMETERZATION_HPP__

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

namespace VideoStitch {
namespace Calibration {

class SO3Parameterization : public ceres::LocalParameterization {
 public:
  virtual ~SO3Parameterization() {}

  virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const {
    double* ptrBase = (double*)x;
    double* ptrResult = (double*)x_plus_delta;
    Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > rotation(ptrBase);
    Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > rotationResult(ptrResult);

    Eigen::Vector3d axis;
    axis(0) = delta[0];
    axis(1) = delta[1];
    axis(2) = delta[2];
    double angle = axis.norm();
    axis.normalize();

    Eigen::AngleAxisd aa(angle, axis);
    Eigen::Matrix3d Rupdate;
    Rupdate = aa.toRotationMatrix();

    rotationResult = Rupdate * rotation;

    return true;
  }

  virtual bool ComputeJacobian(const double* /*x*/, double* jacobian) const {
    double* row[9];
    for (int i = 0; i < 9; i++) {
      row[i] = &jacobian[i * 3];
      for (int j = 0; j < 3; j++) {
        row[i][j] = 0;
      }
    }

    row[1][2] = 1;
    row[2][1] = -1;
    row[3][2] = -1;
    row[5][0] = 1;
    row[6][1] = 1;
    row[7][0] = -1;

    return true;
  }

  virtual int GlobalSize() const { return 9; }

  virtual int LocalSize() const { return 3; }
};

}  // namespace Calibration
}  // namespace VideoStitch

#endif


/*-----------------------------------------------------------------------------+
Copyright (c) 2008-2009: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#define BOOST_TEST_MODULE icl::misc unit test

#define BOOST_ICL_TEST_CHRONO

#include <libs/icl/test/disable_test_warnings.hpp>
#include <string>
#include <vector>
#include <boost/mpl/list.hpp>
#include "../unit_test_unwarned.hpp"
#include <boost/test/test_case_template.hpp>

// interval instance types
#include "../test_type_lists.hpp"
#include "../test_value_maker.hpp"

#include <boost/type_traits/is_same.hpp>
#include <boost/icl/rational.hpp>

#include <boost/icl/detail/interval_morphism.hpp>
#include <boost/icl/interval_map.hpp>
#include <boost/icl/split_interval_set.hpp>
#include "../test_laws.hpp"

using namespace std;
using namespace boost;
using namespace unit_test;
using namespace boost::icl;

BOOST_AUTO_TEST_CASE(test_law_complementarity)
{
    //LAW Inner complementarity: x + between(x) == hull(x)
    //LAW: length(x) + length(between(x)) = length(hull(x))
    typedef interval_map<rational<int>, int> RatioMapT;
    typedef interval_set<rational<int> > RatioSetT;
    typedef RatioSetT::interval_type     IntervalT;
    typedef RatioSetT::element_type      RatT;
    typedef RatioSetT::difference_type   DiffT;

    RatioSetT set_a;
    (((set_a += IntervalT(RatT(0),   RatT(1)  ) )
             -= IntervalT(RatT(1,9), RatT(2,9))  )
             -= IntervalT(RatT(3,9), RatT(4,9))   )
             -= IntervalT(RatT(5,9), RatT(6,9));

    RatioSetT between_a  = RatioSetT(hull(set_a)) - set_a;
    RatioSetT between_a2;
    between(between_a2, set_a);
    BOOST_CHECK_EQUAL( between_a, between_a2 );

    DiffT len_set_a     = length(set_a);
    DiffT len_between_a = length(between_a);

    //cout << set_a     << " length= " << len_set_a     << endl;
    //cout << between_a << " length= " << len_between_a << endl;

    RatioSetT span_a = set_a + between_a;
    RatioSetT hull_a = RatioSetT(hull(set_a));
    //cout << span_a << endl;

    BOOST_CHECK_EQUAL( span_a, hull_a );
    BOOST_CHECK_EQUAL( len_set_a + len_between_a, length(hull_a) );

    BOOST_CHECK((has_inner_complementarity<RatioSetT,RatioSetT>(set_a)));
    BOOST_CHECK((has_length_complementarity(set_a)));
    BOOST_CHECK((has_length_as_distance<RatioSetT,RatioSetT>(set_a)));
}

BOOST_AUTO_TEST_CASE(test_between)
{
    //LAW: between(a,b) == between(b,a);
    typedef int T;
    typedef interval<T>::type IntervalT;
    typedef interval_set<T>   IntervalSetT;

    IntervalT itv_a = I_D(1,3);
    IntervalT itv_b = I_D(5,7);

    IntervalT beween_a_b = between(itv_a, itv_b);
    IntervalT beween_b_a = between(itv_b, itv_a);

    //cout << beween_a_b << endl;
    //cout << beween_b_a << endl;
    BOOST_CHECK_EQUAL( beween_a_b, beween_b_a );
}

BOOST_AUTO_TEST_CASE(element_iteration)
{
    interval_map<int,int> map_a;
    map_a += make_pair(interval<int>::right_open(0,3),1);
    //cout << map_a << endl;

    //for(interval_map<int,int>::element_iterator elem = elements_begin(map_a);
    //    elem != elements_end(map_a); elem++)
    //    cout << "(" << elem->first << "," << elem->second << ")";
    //cout << "\n-------------------------------------\n";

    std::pair<const int, int> search_pair(2,1);

    //interval_map<int,int>::element_const_iterator found
    interval_map<int,int>::element_iterator found
        = std::find(elements_begin(map_a), elements_end(map_a), search_pair);
    // cout << "(" << found->first << "," << found->second << ")\n";
    BOOST_CHECK_EQUAL( found->first,  2 );
    BOOST_CHECK_EQUAL( found->second, 1 );

    // Assignment of an associated value via element_iterator
    const_cast<int&>(found->second) = 2;
    // cout << map_a << endl;
    BOOST_CHECK_EQUAL( map_a.begin()->second, 2 );
}

BOOST_AUTO_TEST_CASE(test_interval_bounds_1)
{
    BOOST_CHECK_EQUAL(left_bracket(interval_bounds::closed()),      "[");
    BOOST_CHECK_EQUAL(left_bracket(interval_bounds::right_open()),  "[");
    BOOST_CHECK_EQUAL(left_bracket(interval_bounds::left_open()),   "(");
    BOOST_CHECK_EQUAL(left_bracket(interval_bounds::open()),        "(");
    BOOST_CHECK_EQUAL(right_bracket(interval_bounds::closed()),     "]");
    BOOST_CHECK_EQUAL(right_bracket(interval_bounds::right_open()), ")");
    BOOST_CHECK_EQUAL(right_bracket(interval_bounds::left_open()),  "]");
    BOOST_CHECK_EQUAL(right_bracket(interval_bounds::open()),       ")");

    continuous_interval<double> a_1 = continuous_interval<double>(-5.0, -2.3, interval_bounds::closed());
    continuous_interval<double> b_1 = continuous_interval<double>(-2.6, 4.0, interval_bounds::closed());

    split_interval_set<double> a, b, a_o_b, b_o_a;
    a_o_b += a_1;
    a_o_b += b_1;

    b_o_a += b_1;
    b_o_a += a_1;

    BOOST_CHECK_EQUAL(a_o_b, b_o_a);

    continuous_interval<double> c_1 = continuous_interval<double>(1.0, 3.0, interval_bounds::closed());
    continuous_interval<double> b_2 = right_subtract(b_1, c_1);

    BOOST_CHECK_EQUAL(b_2.bounds(), interval_bounds::right_open());
    BOOST_CHECK_EQUAL(icl::bounds(b_2), interval_bounds::right_open());

    continuous_interval<double> L0T = continuous_interval<double>(0.0, 0.0, interval_bounds::closed());
    continuous_interval<double> C0T = continuous_interval<double>(0.0, 0.0, interval_bounds::left_open());
    continuous_interval<double> L0D = continuous_interval<double>(0.0, 0.0, interval_bounds::right_open());
    continuous_interval<double> C0D = continuous_interval<double>(0.0, 0.0, interval_bounds::open());

    BOOST_CHECK_EQUAL(icl::is_empty(L0T), false);
    BOOST_CHECK_EQUAL(icl::is_empty(C0T), true);
    BOOST_CHECK_EQUAL(icl::is_empty(L0D), true);
    BOOST_CHECK_EQUAL(icl::is_empty(C0D), true);

    continuous_interval<double> L0_1T = continuous_interval<double>(0.0, 1.0, interval_bounds::closed());
    continuous_interval<double> L1_2T = continuous_interval<double>(1.0, 2.0, interval_bounds::closed());
    BOOST_CHECK_EQUAL(icl::exclusive_less(L0_1T, L1_2T), false);
    BOOST_CHECK_EQUAL(icl::inner_bounds(L0_1T, L1_2T) == interval_bounds::open(), true);

    continuous_interval<double> L0_1D = continuous_interval<double>(0.0, 1.0, interval_bounds::right_open());
    BOOST_CHECK_EQUAL(icl::exclusive_less(L0_1D, L1_2T), true);
    BOOST_CHECK_EQUAL(icl::inner_bounds(L0_1D, L1_2T) == interval_bounds::right_open(), true);

    continuous_interval<double> C1_2T = continuous_interval<double>(1.0, 2.0, interval_bounds::left_open());
    BOOST_CHECK_EQUAL(icl::exclusive_less(L0_1T, C1_2T), true);
    BOOST_CHECK_EQUAL(icl::inner_bounds(L0_1T, C1_2T) == interval_bounds::left_open(), true);

    BOOST_CHECK_EQUAL(icl::exclusive_less(L0_1D, C1_2T), true);
    BOOST_CHECK_EQUAL(icl::inner_bounds(L0_1D, C1_2T) == interval_bounds::closed(), true);

    BOOST_CHECK_EQUAL(static_cast<int>(icl::right(L0_1T.bounds()).bits()), 1);
    BOOST_CHECK_EQUAL(static_cast<int>(icl::right(L0_1D.bounds()).bits()), 0);

    BOOST_CHECK_EQUAL(icl::right_bounds(L0_1D, L0_1T), interval_bounds::left_open());
}

BOOST_AUTO_TEST_CASE(test_infinities)
{
    BOOST_CHECK(( has_std_infinity<double>::value));
    BOOST_CHECK((!has_std_infinity<int>::value));

    BOOST_CHECK(( has_max_infinity<int>::value ));
    BOOST_CHECK((!has_max_infinity<double>::value ));

    //--------------------------------------------------------------------------
    BOOST_CHECK_EQUAL( numeric_infinity<double>::value(),  (std::numeric_limits<double>::infinity)() );
    BOOST_CHECK_EQUAL( numeric_infinity<int>::value(),     (std::numeric_limits<int>::max)() );
    BOOST_CHECK_EQUAL( numeric_infinity<std::string>::value(), std::string() );

    //--------------------------------------------------------------------------
    BOOST_CHECK_EQUAL( infinity<double>::value(),  (std::numeric_limits<double>::infinity)() );
    BOOST_CHECK_EQUAL( infinity<int>::value(),     (std::numeric_limits<int>::max)() );
    BOOST_CHECK_EQUAL( infinity<std::string>::value(), identity_element<std::string>::value() );

    //--------------------------------------------------------------------------
    BOOST_CHECK_EQUAL( infinity<chrono::duration<double> >::value()
                     , chrono::duration<double>((std::numeric_limits<double>::infinity)()) );
    BOOST_CHECK_EQUAL( infinity<chrono::duration<int> >::value()
                     , chrono::duration<int>((std::numeric_limits<int>::max)()) );
}

BOOST_AUTO_TEST_CASE(test_difference_types)
{
    BOOST_CHECK(( boost::is_same< int,            difference_type_of<int>::type >::value ));
    BOOST_CHECK(( boost::is_same< double,         difference_type_of<double>::type >::value ));
    BOOST_CHECK(( boost::is_same< std::ptrdiff_t, difference_type_of<int*>::type >::value ));

    BOOST_CHECK(( has_difference_type<std::string>::value ));
    BOOST_CHECK(( boost::is_same< std::string::difference_type, difference_type_of<std::string>::type >::value ));
    BOOST_CHECK(( boost::is_same< std::ptrdiff_t, difference_type_of<std::string>::type >::value ));

    BOOST_CHECK((  boost::is_same<                    chrono::duration<int>
                                 , difference_type_of<chrono::duration<int> >::type >::value ));
    BOOST_CHECK((  boost::is_same<                    chrono::duration<double>
                                 , difference_type_of<chrono::duration<double> >::type >::value ));

    BOOST_CHECK((  boost::is_same<                    Now::time_point::duration
                                 , difference_type_of<Now::time_point>::type >::value ));

    typedef boost::chrono::time_point<Now, boost::chrono::duration<double> > contin_timeT;
    BOOST_CHECK((  boost::is_same<                    contin_timeT::duration
                                 , difference_type_of<contin_timeT>::type >::value ));

    typedef boost::chrono::time_point<Now, boost::chrono::duration<int> > discr_timeT;
    BOOST_CHECK((  boost::is_same<                    chrono::duration<int>
                                 , difference_type_of<discr_timeT>::type >::value ));
}

BOOST_AUTO_TEST_CASE(test_size_types)
{
    BOOST_CHECK(( boost::is_same< std::size_t,    size_type_of<int>::type >::value ));
    BOOST_CHECK(( boost::is_same< std::size_t,    size_type_of<double>::type >::value ));
    BOOST_CHECK(( boost::is_same< std::size_t,    size_type_of<int*>::type >::value ));
    BOOST_CHECK(( boost::is_same< std::size_t,    size_type_of<std::string>::type >::value ));
    BOOST_CHECK(( boost::is_same<              chrono::duration<int>
                                , size_type_of<chrono::duration<int> >::type >::value ));
    BOOST_CHECK(( boost::is_same<              chrono::duration<double>
                                , size_type_of<chrono::duration<double> >::type >::value ));

    typedef boost::chrono::time_point<Now, boost::chrono::duration<int> > discr_timeT;
    BOOST_CHECK((  boost::is_same< chrono::duration<int>
                                 , size_type_of<discr_timeT>::type >::value ));

    typedef boost::chrono::time_point<Now, boost::chrono::duration<double> > contin_timeT;
    BOOST_CHECK((  boost::is_same< contin_timeT::duration
                                 , size_type_of<contin_timeT>::type >::value ));
}

BOOST_AUTO_TEST_CASE(test_chrono_identity_elements)
{
    chrono::duration<int> idel_i = icl::identity_element<chrono::duration<int> >::value();
    //cout << "dur<int>0 = " << idel_i << endl;
    chrono::duration<double> idel_d = icl::identity_element<chrono::duration<int> >::value();
    //cout << "dur<dbl>0 = " << idel_d << endl;

    BOOST_CHECK(( boost::is_same<              chrono::duration<int>
                                , size_type_of<chrono::duration<int> >::type >::value ));
}


#include <big_random.h>
#include <big_types.h>
#include <big_return_codes.h>
#include <min_map/big_compute_index_reordering.h>
#include <min_map/big_compute_partitioned_jacobian.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(big_compute_partitioned_jacobian);

BOOST_AUTO_TEST_CASE(test_case1)
{
  typedef ublas::vector<size_t>            idx_vector_type;
  typedef ublas::compressed_matrix<double> matrix_type;

  typedef size_t size_type;

  matrix_type A;
  A.resize(10,10,false);

  big::Random<double> value(0.0,1.0);
  for(size_t i=0;i<A.size1();++i)
  {
    for(size_t j=0;j<A.size2();++j)
      A(i,j) = value();
  }

  idx_vector_type bitmask;

  bitmask.resize(10,false);

  bitmask(0) = big::IN_NON_ACTIVE;
  bitmask(1) = big::IN_NON_ACTIVE;
  bitmask(2) = big::IN_ACTIVE;
  bitmask(3) = big::IN_NON_ACTIVE;
  bitmask(4) = big::IN_ACTIVE;
  bitmask(5) = big::IN_NON_ACTIVE;
  bitmask(6) = big::IN_ACTIVE;
  bitmask(7) = big::IN_NON_ACTIVE;
  bitmask(8) = big::IN_NON_ACTIVE;
  bitmask(9) = big::IN_ACTIVE;

  idx_vector_type old2new;
  idx_vector_type new2old;

  big::compute_index_reordering( bitmask, old2new, new2old );

  matrix_type A_aa;
  matrix_type A_ab;

  big::compute_partitioned_jacobian(
    A
    ,  bitmask
    , old2new
    ,  4
    ,  6
    ,  A_aa
    ,  A_ab
    );

  double tol = 0.01;

  BOOST_CHECK( A_aa.size1()==4 );
  BOOST_CHECK( A_aa.size2()==4 );
  for(size_type i = 0;i<4;++i)
  {
    for(size_type j = 0;j<4;++j)
    {
      BOOST_CHECK_CLOSE( double( A_aa(i,j) ), double( A(new2old(i),new2old(j)) ), tol );
    }
  }

  BOOST_CHECK( A_ab.size1()==4 );
  BOOST_CHECK( A_ab.size2()==6 );
  for(size_type i = 0;i<4;++i)
  {
    for(size_type j = 0;j<6;++j)
    {
      BOOST_CHECK_CLOSE( double( A_ab(i,j) ), double( A(new2old(i),new2old(j+4)) ), tol );
    }
  }

}

BOOST_AUTO_TEST_CASE(test_case2)
{
  typedef ublas::vector<size_t>            idx_vector_type;
  typedef ublas::compressed_matrix<double> matrix_type;

  typedef size_t size_type;

  matrix_type A;
  A.resize(10,10,false);

  big::Random<double> value(0.0,1.0);
  for(size_t i=0;i<A.size1();++i)
  {
    for(size_t j=0;j<A.size2();++j)
      A(i,j) = value();
  }

  idx_vector_type bitmask;

  bitmask.resize(10,false);

  bitmask(0) = big::IN_ACTIVE;
  bitmask(1) = big::IN_ACTIVE;
  bitmask(2) = big::IN_ACTIVE;
  bitmask(3) = big::IN_ACTIVE;
  bitmask(4) = big::IN_ACTIVE;
  bitmask(5) = big::IN_ACTIVE;
  bitmask(6) = big::IN_ACTIVE;
  bitmask(7) = big::IN_ACTIVE;
  bitmask(8) = big::IN_ACTIVE;
  bitmask(9) = big::IN_ACTIVE;

  idx_vector_type old2new;
  idx_vector_type new2old;

  big::compute_index_reordering( bitmask, old2new, new2old );

  matrix_type A_aa;
  matrix_type A_ab;

  big::compute_partitioned_jacobian(
                                    A
                                    , bitmask
                                    , old2new
                                    , 10
                                    , 0
                                    , A_aa
                                    , A_ab
                                    );

  double tol = 0.01;

  BOOST_CHECK( A_aa.size1()==10 );
  BOOST_CHECK( A_aa.size2()==10 );
  for(size_type i = 0;i<10;++i)
  {
    for(size_type j = 0;j<10;++j)
    {
      BOOST_CHECK_CLOSE( double( A_aa(i,j) ), double( A(new2old(i),new2old(j)) ), tol );
    }
  }

  BOOST_CHECK( A_ab.size1()==10 );
  BOOST_CHECK( A_ab.size2()==0 );

}

BOOST_AUTO_TEST_CASE(test_case3)
{
  typedef ublas::vector<size_t>            idx_vector_type;
  typedef ublas::compressed_matrix<double> matrix_type;

  matrix_type A;
  A.resize(10,10,false);

  big::Random<double> value(0.0,1.0);
  for(size_t i=0;i<A.size1();++i)
  {
    for(size_t j=0;j<A.size2();++j)
      A(i,j) = value();
  }

  idx_vector_type bitmask;

  bitmask.resize(10,false);

  bitmask(0) = big::IN_NON_ACTIVE;
  bitmask(1) = big::IN_NON_ACTIVE;
  bitmask(2) = big::IN_NON_ACTIVE;
  bitmask(3) = big::IN_NON_ACTIVE;
  bitmask(4) = big::IN_NON_ACTIVE;
  bitmask(5) = big::IN_NON_ACTIVE;
  bitmask(6) = big::IN_NON_ACTIVE;
  bitmask(7) = big::IN_NON_ACTIVE;
  bitmask(8) = big::IN_NON_ACTIVE;
  bitmask(9) = big::IN_NON_ACTIVE;

  idx_vector_type old2new;
  idx_vector_type new2old;

  big::compute_index_reordering( bitmask, old2new, new2old );

  matrix_type A_aa;
  matrix_type A_ab;

  big::compute_partitioned_jacobian(
                                    A
                                    , bitmask
                                    , old2new
                                    , 0
                                    , 10
                                    , A_aa
                                    , A_ab
                                    );

  BOOST_CHECK( A_aa.size1()==0 );
  BOOST_CHECK( A_aa.size2()==0 );

  BOOST_CHECK( A_ab.size1()==0 );
  BOOST_CHECK( A_ab.size2()==10 );

}

BOOST_AUTO_TEST_SUITE_END();


#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <rr_platform/calibrate_image.h>
#include <rr_platform/camera_pose.h>
#include <cmath>
#include <sensor_msgs/JointState.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/CameraInfo.h>
#include <boost/algorithm/string.hpp>

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

double px_per_meter;  // resolution of overhead view
double camera_dist_max;  //distance to look ahead of the car
double camera_dist_min;  //avoid the front bumper
double camera_fov_horizontal;  // radians
double camera_fov_vertical;
double cam_mount_angle;  //angle of camera from horizontal
double cam_mount_height;  //camera height from ground in meters
double cam_mount_x;  // distance from camera to base_footprint

bool fov_callback_called;

Size mapSize;  // pixels = cm
Size imageSize;
Mat transform_matrix;

map<string, Publisher> transform_pubs;

void loadCameraPoseFromTf() {
    ROS_INFO("image_transform is loading camera pose from tf...");
    tf::TransformListener listener;

    geometry_msgs::PoseStamped ps_src_cam;  // source pose, origin
    geometry_msgs::PoseStamped ps_dst_base;  // destination pose, camera frame origin from base_footprint

    tf::Quaternion tf_no_rotation;
    tf_no_rotation.setRPY(0, 0, 0);
    tf::quaternionTFToMsg(tf_no_rotation, ps_src_cam.pose.orientation);

    ps_src_cam.header.frame_id = "camera";

    bool success = false;
    while(!success) {
        try {
            listener.waitForTransform("base_footprint", "camera", ros::Time(0), ros::Duration(5.0));
            listener.transformPose("base_footprint", ps_src_cam, ps_dst_base);
            success = true;
        } catch(tf2::LookupException& e) {
            ROS_ERROR("tf LookupException: %s", e.what());
        }
    }

    tf::Quaternion tf_camera_rotation;
    tf::quaternionMsgToTF(ps_dst_base.pose.orientation, tf_camera_rotation);

    double roll, pitch, yaw;
    tf::Matrix3x3(tf_camera_rotation).getRPY(roll, pitch, yaw);

    cam_mount_angle = pitch;
    cam_mount_height = ps_dst_base.pose.position.z;
    cam_mount_x = ps_dst_base.pose.position.x;
}

void fovCallback(const sensor_msgs::CameraInfoConstPtr& msg) {
    double fx = msg->P[0]; //horizontal focal length of rectified image, in px
    double fy = msg->P[5]; //vertical focal length
    imageSize = Size(msg->width, msg->height);
    camera_fov_horizontal = 2 * atan2(imageSize.width, 2*fx);
    camera_fov_vertical = 2 * atan2(imageSize.height, 2*fy);
    fov_callback_called = true;
}

void loadCameraFOV(NodeHandle& nh) {
    auto infoSub = nh.subscribe("/camera/camera_info", 1, fovCallback);

    Time t_start = Time::now();
    fov_callback_called = false;
    while (!fov_callback_called) {
        if ((Time::now() - t_start).toSec() > 10) {
            ROS_WARN("[Image_Transform] setting camera FOV from camera_info timed out");
            break;
        }

        spinOnce();
        Duration(0.05).sleep();
    }
    ROS_INFO("Using horizontal FOV %f and vertical FOV %f", camera_fov_horizontal, camera_fov_vertical);
}

/*
 * Start with a horizontal line on the groud at dmin meters horizonally in front of the
 * camera. It fills half the camera's FOV, from the center to the right edge. Then back
 * up the car so that the line is dmax meters away horizontally from the camera. The
 * apparent length of this hypothetical line (in pixels) is the output of this function.
 * See https://www.desmos.com/calculator/jsofcq1bi5
 * See https://drive.google.com/file/d/0Bw7-7Y3CUDw1Z0ZqdmdRZ3dUTE0/view?usp=sharing
 */
double pxFromDist_X(double dmin, double dmax) {
    double min_hyp = sqrt(dmin*dmin + cam_mount_height*cam_mount_height);
    double max_hyp = sqrt(dmax*dmax + cam_mount_height*cam_mount_height);
    double theta1 = atan((min_hyp / max_hyp) * tan(camera_fov_horizontal / 2));
    return imageSize.width * (theta1 / camera_fov_horizontal);
}

//calculate the y coord of the input image from the specified distance
// see https://www.desmos.com/calculator/pwjwlnnx77
double pxFromDist_Y(double dist) {
    double tmp = atan(cam_mount_height/dist) - cam_mount_angle + camera_fov_vertical/2;
    return imageSize.height * tmp / (camera_fov_vertical);
}

void setTransformFromGeometry() {
    //set width and height of the rectangle in front of the robot
    // the actual output image will show more than this rectangle
    float close_corner_dist = sqrt(pow(camera_dist_min, 2) + pow(cam_mount_height, 2));
    float rectangle_w = close_corner_dist * tan(camera_fov_horizontal/2) * px_per_meter * 2;
    float rectangle_h = (camera_dist_max - camera_dist_min) * px_per_meter;

    //find coordinates for corners above rectangle in input image
    float x_top_spread = pxFromDist_X(camera_dist_min, camera_dist_max);
    float y_bottom = pxFromDist_Y(camera_dist_min);
    float y_top    = pxFromDist_Y(camera_dist_max);

    //set the ouput image size to include the whole transformed image,
    // not just the target rectangle
    mapSize.width = static_cast<int>(rectangle_w * (imageSize.width / x_top_spread) / 2.0);
    mapSize.height = static_cast<int>(rectangle_h);

    Point2f src[4] = {
        Point2f(imageSize.width/2.f - x_top_spread, y_top), //top left
        Point2f(imageSize.width/2.f + x_top_spread, y_top), //top right
        Point2f(0, y_bottom),                             //bottom left
        Point2f(imageSize.width, y_bottom)                //bottom right
    };

    Point2f dst[4] = {
        Point2f(mapSize.width/2.f - rectangle_w/2.f, 0),              //top left
        Point2f(mapSize.width/2.f + rectangle_w/2.f, 0),              //top right
        Point2f(mapSize.width/2.f - rectangle_w/2.f, mapSize.height), //bottom left
        Point2f(mapSize.width/2.f + rectangle_w/2.f, mapSize.height)  //bottom right
    };

    transform_matrix = getPerspectiveTransform(src, dst);
}

void TransformImage(const sensor_msgs::ImageConstPtr& msg, string& topic) {
    //if no one is listening or the transform is undefined, give up
    if(transform_pubs[topic].getNumSubscribers() == 0 || transform_matrix.empty()) {
        return;
    }

    cv_bridge::CvImagePtr cv_ptr;
    cv_ptr = cv_bridge::toCvCopy(msg, "mono8");
    const Mat &inimage = cv_ptr->image;

    Mat warp_img;
    warpPerspective(inimage, warp_img, transform_matrix, mapSize);

    double map_length = camera_dist_max + cam_mount_x;
    Mat outimage(static_cast<int>(map_length * px_per_meter), warp_img.cols, CV_8UC1, Scalar(0));
    Rect out_warp_roi(0, 0, warp_img.cols, warp_img.rows);

    warp_img.copyTo(outimage(out_warp_roi));

    sensor_msgs::Image outmsg;
    cv_ptr->image = outimage;
    cv_ptr->toImageMsg(outmsg);
    transform_pubs[topic].publish(outmsg);
}

int main(int argc, char **argv) {
    init(argc, argv, "image_transform");
    NodeHandle nh;
    NodeHandle pnh("~");

    bool all_defined = true;
    all_defined &= pnh.getParam("px_per_meter", px_per_meter);
    all_defined &= pnh.getParam("map_dist_max", camera_dist_max);
    all_defined &= pnh.getParam("map_dist_min", camera_dist_min);

    // the launch file can provide camera information in case camera_info is not published
    all_defined &= pnh.getParam("fallback_fov_horizontal", camera_fov_horizontal);
    all_defined &= pnh.getParam("fallback_fov_vertical", camera_fov_vertical);
    all_defined &= pnh.getParam("fallback_image_width", imageSize.width);
    all_defined &= pnh.getParam("fallback_image_height", imageSize.height);

    if (!all_defined) {
        ROS_WARN("[Image Transform] Not all launch params defined");
    }

    loadCameraFOV(nh); //spins ROS event loop for a bit
    loadCameraPoseFromTf();
    setTransformFromGeometry();
    ROS_INFO("Calculated perspective transform. Used height %f and angle %f", cam_mount_height, cam_mount_angle);

    string topicsConcat;
    pnh.getParam("transform_topics", topicsConcat);
    vector<string> topics;
    boost::split(topics, topicsConcat, boost::is_any_of(" ,"));
    vector<Subscriber> transform_subs;
    ROS_INFO_STREAM("Found " << topics.size() << " topics in param.");
    for(const string& topic : topics) {
        if (topic.size() == 0) {
          continue;
        }

        transform_subs.push_back(nh.subscribe<sensor_msgs::Image>(topic, 1,
                                 boost::bind(TransformImage, _1, topic)));
        ROS_INFO_STREAM("Image_transform subscribed to " << topic);
        string newTopic(topic + "_transformed");
        ROS_INFO_STREAM("Creating new topic " << newTopic);
        transform_pubs[topic] = nh.advertise<sensor_msgs::Image>(newTopic, 1);
    }

    spin();

    return 0;
}


//---------------------------------------------------------------------------//
//!
//! \file   Utility_PhysicalConstants.hpp
//! \author Alex Robinson
//! \brief  Decl. of class that stores a variety of physical constants w/units
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_PHYSICAL_CONSTANTS_HPP
#define UTILITY_PHYSICAL_CONSTANTS_HPP

// Boost Includes
#include <boost/units/quantity.hpp>
#include <boost/units/systems/cgs/velocity.hpp>
#include <boost/units/systems/cgs/time.hpp>
#include <boost/units/systems/cgs/length.hpp>
#include <boost/units/systems/si/temperature.hpp>

// FRENSIE Includes
#include "Utility_RawPhysicalConstants.hpp"
#include "Utility_ElectronVoltUnit.hpp"
#include "Utility_AtomicMassUnit.hpp"
#include "Utility_UnitTraits.hpp"

namespace Utility{

//! The physical constants class
class PhysicalConstants : public RawPhysicalConstants
{
private:

  // The MeV-s unit
  typedef UnitTraits<Units::MegaElectronVolt>::template GetMultipliedUnitType<boost::units::cgs::time>::type MeVSecond;

  // The MeV/K unit
  typedef UnitTraits<Units::MegaElectronVolt>::template GetMultipliedUnitType<UnitTraits<boost::units::si::temperature>::InverseUnit>::type MeVPerK;

public:

  //! The speed of light (cm/s)
  static const boost::units::quantity<boost::units::cgs::velocity> speed_of_light_q;

  //! Planck's constant (MeV-s)
  static const boost::units::quantity<MeVSecond> planck_constant_q;

  //! Plank's constant by pi (MeV-s)
  static const boost::units::quantity<MeVSecond> h_bar_q;

  //! Rest mass energy of electron (MeV)
  static const boost::units::quantity<Units::MegaElectronVolt> electron_rest_mass_energy_q;

  //! Rest mass energy of neutron (MeV)
  static const boost::units::quantity<Units::MegaElectronVolt> neutron_rest_mass_energy_q;

  //! The rest mass of neutron (amu)
  static const boost::units::quantity<Units::AtomicMass> neutron_rest_mass_amu_q;

  //! The classical electron radius (cm)
  static const boost::units::quantity<boost::units::cgs::length> classical_electron_radius_q;

  //! The boltzmann constant (MeV/K)
  static const boost::units::quantity<MeVPerK> boltzmann_constant_q;
};

} // end Utility namespace

#endif // end UTILITY_PHYSICAL_CONSTANTS_HPP

//---------------------------------------------------------------------------//
// end Utility_PhysicalConstants.hpp
//---------------------------------------------------------------------------//


#include <boost/graph/directed_graph.hpp>

int main(int, char*[])
{
  typedef boost::directed_graph<> Graph;
  Graph g;
  boost::graph_traits< Graph >::vertex_descriptor v0 = g.add_vertex();
  boost::graph_traits< Graph >::vertex_descriptor v1 = g.add_vertex();

  g.add_edge(v0, v1);

  return 0;
}


#include "z_module.hpp"

#include <gmpxx.h>
#include <boost/multiprecision/cpp_int.hpp>

#include <iostream>

auto main() -> int {    // NOLINT
    boost::multiprecision::cpp_int n1 = -101;   // NOLINT
    mpz_class n2 = -101;

    fgs::Z<5> a1{n1.str()};     // NOLINT
    fgs::Z<5> a2{n2.get_str()};

    std::clog
        << static_cast<boost::multiprecision::cpp_int>(a1)  << '\n' // NOLINT
        << static_cast<mpz_class>(a2)                       << '\n';
}


/************************************************************************/
/*                                                                      */
/*               Copyright 2007-2019 by Hans Meine                      */
/*                                                                      */
/*    Permission is hereby granted, free of charge, to any person       */
/*    obtaining a copy of this software and associated documentation    */
/*    files (the "Software"), to deal in the Software without           */
/*    restriction, including without limitation the rights to use,      */
/*    copy, modify, merge, publish, distribute, sublicense, and/or      */
/*    sell copies of the Software, and to permit persons to whom the    */
/*    Software is furnished to do so, subject to the following          */
/*    conditions:                                                       */
/*                                                                      */
/*    The above copyright notice and this permission notice shall be    */
/*    included in all copies or substantial portions of the             */
/*    Software.                                                         */
/*                                                                      */
/*    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND    */
/*    EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES   */
/*    OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND          */
/*    NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT       */
/*    HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,      */
/*    WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING      */
/*    FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR     */
/*    OTHER DEALINGS IN THE SOFTWARE.                                   */
/*                                                                      */
/************************************************************************/

#include "vigra/subpixel_watershed.hxx"

#include "python_types.hxx"
#include <boost/python.hpp>
namespace python = boost::python;

using namespace vigra;

template <class SPWSType>
class SPWSWrapper : public SPWSType
{
  public:
    typedef Vector2 Coordinate;

    SPWSWrapper(NumpyFImage const & img)
    : SPWSType(srcImageRange(img))
    {}

    void
    findCriticalPointsIf(NumpyFImage const & mask)
    {
        this->findCriticalPoints(srcImage(mask));
    }

    python::list
    saddles()
    {
        if(this->saddles_.size() == 0)
            this->findCriticalPoints();
        python::list plist;
        plist.append(python::object());
        for(unsigned int i=1; i<this->saddles_.size(); ++i)
        {
            plist.append(Coordinate(this->saddles_[i][0], this->saddles_[i][1]));
        }
        return plist;
    }

    python::list
    maxima()
    {
        if(this->maxima_.size() == 0)
            this->findCriticalPoints();
        python::list plist;
        plist.append(python::object());
        for(unsigned int i=1; i<this->maxima_.size(); ++i)
        {
            plist.append(Coordinate(this->maxima_[i][0], this->maxima_[i][1]));
        }
        return plist;
    }

    python::list
    minima()
    {
        if(this->minima_.size() == 0)
            this->findCriticalPoints();
        python::list plist;
        plist.append(python::object());
        for(unsigned int i=1; i<this->minima_.size(); ++i)
        {
            plist.append(Coordinate(this->minima_[i][0], this->minima_[i][1]));
        }
        return plist;
    }

    python::object
    edge(int index)
    {
        double x = this->saddles_[index][0];
        double y = this->saddles_[index][1];

        typename SPWSType::PointArray
            forwardCurve, backwardCurve;

        int findex = this->flowLine(x, y, true, 1e-4, forwardCurve);
        int bindex = this->flowLine(x, y, false, 1e-4, backwardCurve);

        python::list ppoints;
        for(int i = forwardCurve.size() - 1; i >= 0; --i)
            ppoints.append(forwardCurve[i]);
        for(int i = 1; i < (int)backwardCurve.size(); ++i)
            ppoints.append(backwardCurve[i]);

        return python::make_tuple(findex, bindex, ppoints, forwardCurve.size() - 1);
    }

    python::list
    edges(double threshold)
    {
        if(this->saddles_.size() == 0)
            this->findCriticalPoints();
        python::list plist;
        for(unsigned int i=1; i<this->saddles_.size(); ++i)
        {
            if(this->image_(this->saddles_[i][0], this->saddles_[i][1]) >= threshold)
                plist.append(edge(i));
            else
                plist.append(python::object());
        }
        return plist;
    }

    python::tuple
    debugCP()
    {
        python::list minima, saddles, maxima;
        //typename SPWSType::PointArray minima, sources;
        //std::vector<unsigned int> counts;

        int w = this->image_.width();
        int h = this->image_.height();

        typedef typename SPWSType::SplineImageView::value_type Value;

        double d = 1.0 / this->cpOversampling_;

        // search for critical points
        int percent = -1, lastPercent = -1;
        for(int y=1; y <= h-2; ++y)
        {
            percent = 100 * y / h;
            if(percent != lastPercent)
            {
                std::cerr << "debugCP(): " << percent << "%\r";
                lastPercent = percent;
            }
            for(int x=1; x <= w-2; ++x)
            {
                for(double dy = 0.0; dy < 1.0; dy += d)
                {
                    for(double dx = 0.0; dx < 1.0; dx += d)
                    {
                        double xx, yy;

                        CriticalPoint type = findCriticalPointNewtonMethod(
                            this->image_, x + dx, y + dy,
                            &xx, &yy, this->stepEpsilon_);
                        if(type == Failed)
                            continue;

                        (type == Saddle ? saddles
                         : (type == Minimum ? minima : maxima))
                               .append(python::make_tuple(
                                           Coordinate(xx, yy),
                                           Coordinate(x + dx, y + dy)));
                    }
                }
            }
        }
        std::cerr << "debugCP(): done.\n";

        return python::make_tuple(minima, saddles, maxima);
    }

    python::tuple
    findCriticalPointsInFacet(int x, int y)
    {
        typename SPWSType::PointArray minima, saddles, maxima;
        findCriticalPointsInFacet(x, y, minima, saddles, maxima);
        python::list pminima, pmaxima, psaddles;
        for(unsigned int i=0; i<minima.size(); ++i)
        {
            pminima.append(minima[i]);
        }
        for(unsigned int i=0; i<saddles.size(); ++i)
        {
            psaddles.append(saddles[i]);
        }
        for(unsigned int i=0; i<maxima.size(); ++i)
        {
            pmaxima.append(maxima[i]);
        }
        return python::make_tuple(pminima, psaddles, pmaxima);
    }
};

template <class SPWSType>
python::class_<SPWSWrapper<SPWSType> > &
defSubPixelWS(char const * name)
{
    typedef SPWSWrapper<SPWSType> SPWS;

    static python::class_<SPWS> theclass(
        name, python::init<NumpyFImage const &>());
    theclass
        .def("width", &SPWS::width)
        .def("height", &SPWS::height)
        .def("saddles", &SPWS::saddles)
        .def("maxima", &SPWS::maxima)
        .def("minima", &SPWS::minima)
        .def("debugCP", &SPWS::debugCP)
        .def("edge", &SPWS::edge)
        .def("edges", &SPWS::edges)
        .def("findCriticalPoints", (void(SPWS::*)())&SPWS::findCriticalPoints)
        .def("findCriticalPoints", &SPWS::findCriticalPointsIf)
        .def_readwrite("initialStep", &SPWS::initialStep_)
        .def_readwrite("minCPDist", &SPWS::minCPDist_)
        .def_readwrite("cpOversampling", &SPWS::cpOversampling_)
        //.def("findCriticalPointsInFacet", &SPWS::findCriticalPointsInFacet)
    ;

    return theclass;
}

void defSPWS()
{
    defSubPixelWS<SubPixelWatersheds<SplineImageView<2, GrayValue> > >(
        "SubPixelWatersheds2");
    defSubPixelWS<SubPixelWatersheds<SplineImageView<3, GrayValue> > >(
        "SubPixelWatersheds3");
    defSubPixelWS<SubPixelWatersheds<SplineImageView<5, GrayValue> > >(
        "SubPixelWatersheds5");
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2014, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#include <iostream>

#ifndef BOOST_TEST_MODULE
#define BOOST_TEST_MODULE test_distance_spherical_equatorial_pl_l
#endif

#include <boost/test/included/unit_test.hpp>

#include "test_distance_se_common.hpp"

typedef bg::cs::spherical_equatorial<bg::degree> cs_type;
typedef bg::model::point<double, 2, cs_type> point_type;
typedef bg::model::segment<point_type> segment_type;
typedef bg::model::multi_point<point_type> multi_point_type;
typedef bg::model::segment<point_type> segment_type;
typedef bg::model::linestring<point_type> linestring_type;
typedef bg::model::multi_linestring<linestring_type> multi_linestring_type;

namespace services = bg::strategy::distance::services;
typedef bg::default_distance_result<point_type>::type return_type;

typedef bg::strategy::distance::haversine<double> point_point_strategy;
typedef bg::strategy::distance::cross_track<> point_segment_strategy;

//===========================================================================

template <typename Strategy>
inline bg::default_distance_result<point_type>::type
pp_distance(std::string const& wkt1,
            std::string const& wkt2,
            Strategy const& strategy)
{
    point_type p1, p2;
    bg::read_wkt(wkt1, p1);
    bg::read_wkt(wkt2, p2);
    return bg::distance(p1, p2) * strategy.radius();
}

template <typename Strategy>
inline bg::default_comparable_distance_result<point_type>::type
pp_comparable_distance(std::string const& wkt1,
                       std::string const& wkt2,
                       Strategy const&)
{
    point_type p1, p2;
    bg::read_wkt(wkt1, p1);
    bg::read_wkt(wkt2, p2);
    return bg::comparable_distance(p1, p2);
}

template <typename Strategy>
inline bg::default_distance_result<point_type>::type
ps_distance(std::string const& wkt1,
            std::string const& wkt2,
            Strategy const& strategy)
{
    point_type p;
    segment_type s;
    bg::read_wkt(wkt1, p);
    bg::read_wkt(wkt2, s);
    return bg::distance(p, s, strategy);
}

template <typename Strategy>
inline bg::default_comparable_distance_result<point_type>::type
ps_comparable_distance(std::string const& wkt1,
                       std::string const& wkt2,
                       Strategy const& strategy)
{
    point_type p;
    segment_type s;
    bg::read_wkt(wkt1, p);
    bg::read_wkt(wkt2, s);
    return bg::comparable_distance(p, s, strategy);
}

template <typename Strategy, typename T>
T to_comparable(Strategy const& strategy, T const& distance)
{
    namespace services = bg::strategy::distance::services;

    typedef typename services::comparable_type
        <
            Strategy
        >::type comparable_strategy;

    typedef typename services::result_from_distance
        <
            comparable_strategy,
            point_type,
            bg::point_type<segment_type>::type
        > get_comparable_distance;

    comparable_strategy cstrategy = services::get_comparable
        <
            Strategy
        >::apply(strategy);

    return get_comparable_distance::apply(cstrategy, distance);
}

//===========================================================================

template <typename Strategy>
void test_distance_point_segment(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "point/segment distance tests" << std::endl;
#endif
    typedef test_distance_of_geometries<point_type, segment_type> tester;

    double const d2r = bg::math::d2r<double>();

    tester::apply("p-s-01",
                  "POINT(0 0)",
                  "SEGMENT(2 0,3 0)",
                  2.0 * d2r * strategy.radius(),
                  to_comparable(strategy, 2.0 * d2r * strategy.radius()),
                  strategy);
    tester::apply("p-s-02",
                  "POINT(2.5 3)",
                  "SEGMENT(2 0,3 0)",
                  3.0 * d2r * strategy.radius(),
                  to_comparable(strategy, 3.0 * d2r * strategy.radius()),
                  strategy);
    tester::apply("p-s-03",
                  "POINT(2 0)",
                  "SEGMENT(2 0,3 0)",
                  0,
                  strategy);
    tester::apply("p-s-04",
                  "POINT(3 0)",
                  "SEGMENT(2 0,3 0)",
                  0,
                  strategy);
    tester::apply("p-s-05",
                  "POINT(2.5 0)",
                  "SEGMENT(2 0,3 0)",
                  0,
                  strategy);
    tester::apply("p-s-06",
                  "POINT(3.5 3)",
                  "SEGMENT(2 0,3 0)",
                  pp_distance("POINT(3 0)", "POINT(3.5 3)", strategy),
                  pp_comparable_distance("POINT(3 0)",
                                         "POINT(3.5 3)",
                                         strategy),
                  strategy);
    // very small distances to segment
    tester::apply("p-s-07",
                  "POINT(90 1e-3)",
                  "SEGMENT(0.5 0,175.5 0)",
                  1e-3 * d2r * strategy.radius(),
                  to_comparable(strategy, 1e-3 * d2r * strategy.radius()),
                  strategy);
    tester::apply("p-s-08",
                  "POINT(90 1e-4)",
                  "SEGMENT(0.5 0,175.5 0)",
                  1e-4 * d2r * strategy.radius(),
                  to_comparable(strategy, 1e-4 * d2r * strategy.radius()),
                  strategy);
    tester::apply("p-s-09",
                  "POINT(90 1e-5)",
                  "SEGMENT(0.5 0,175.5 0)",
                  1e-5 * d2r * strategy.radius(),
                  to_comparable(strategy, 1e-5 * d2r * strategy.radius()),
                  strategy);
    tester::apply("p-s-10",
                  "POINT(90 1e-6)",
                  "SEGMENT(0.5 0,175.5 0)",
                  1e-6 * d2r * strategy.radius(),
                  to_comparable(strategy, 1e-6 * d2r * strategy.radius()),
                  strategy);
    tester::apply("p-s-11",
                  "POINT(90 1e-7)",
                  "SEGMENT(0.5 0,175.5 0)",
                  1e-7 * d2r * strategy.radius(),
                  to_comparable(strategy, 1e-7 * d2r * strategy.radius()),
                  strategy);
    tester::apply("p-s-12",
                  "POINT(90 1e-8)",
                  "SEGMENT(0.5 0,175.5 0)",
                  1e-8 * d2r * strategy.radius(),
                  to_comparable(strategy, 1e-8 * d2r * strategy.radius()),
                  strategy);
}

//===========================================================================

template <typename Strategy>
void test_distance_point_linestring(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "point/linestring distance tests" << std::endl;
#endif
    typedef test_distance_of_geometries<point_type, linestring_type> tester;

    double const d2r = bg::math::d2r<double>();

    tester::apply("p-l-01",
                  "POINT(0 0)",
                  "LINESTRING(2 0,2 0)",
                  2.0 * d2r * strategy.radius(),
                  to_comparable(strategy, 2.0 * d2r * strategy.radius()),
                  strategy);
    tester::apply("p-l-02",
                  "POINT(0 0)",
                  "LINESTRING(2 0,3 0)",
                  2.0 * d2r * strategy.radius(),
                  to_comparable(strategy, 2.0 * d2r * strategy.radius()),
                  strategy);
    tester::apply("p-l-03",
                  "POINT(2.5 3)",
                  "LINESTRING(2 0,3 0)",
                  3.0 * d2r * strategy.radius(),
                  to_comparable(strategy, 3.0 * d2r * strategy.radius()),
                  strategy);
    tester::apply("p-l-04",
                  "POINT(2 0)",
                  "LINESTRING(2 0,3 0)",
                  0,
                  strategy);
    tester::apply("p-l-05",
                  "POINT(3 0)",
                  "LINESTRING(2 0,3 0)",
                  0,
                  strategy);
    tester::apply("p-l-06",
                  "POINT(2.5 0)",
                  "LINESTRING(2 0,3 0)",
                  0,
                  strategy);
    tester::apply("p-l-07",
                  "POINT(7.5 10)",
                  "LINESTRING(1 0,2 0,3 0,4 0,5 0,6 0,7 0,8 0,9 0)",
                  10.0 * d2r * strategy.radius(),
                  to_comparable(strategy, 10.0 * d2r * strategy.radius()),
                  strategy);
    tester::apply("p-l-08",
                  "POINT(7.5 10)",
                  "LINESTRING(1 1,2 1,3 1,4 1,5 1,6 1,7 1,20 2,21 2)",
                  ps_distance("POINT(7.5 10)", "SEGMENT(7 1,20 2)", strategy),
                  ps_comparable_distance("POINT(7.5 10)",
                                         "SEGMENT(7 1,20 2)",
                                         strategy),
                  strategy);
}

//===========================================================================

template <typename Strategy>
void test_distance_point_multilinestring(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "point/multilinestring distance tests" << std::endl;
#endif
    typedef test_distance_of_geometries
        <
            point_type, multi_linestring_type
        > tester;

    double const d2r = bg::math::d2r<double>();

    tester::apply("p-ml-01",
                  "POINT(0 0)",
                  "MULTILINESTRING((-5 0,-3 0),(2 0,3 0))",
                  2.0 * d2r * strategy.radius(),
                  to_comparable(strategy, 2.0 * d2r * strategy.radius()),
                  strategy);
    tester::apply("p-ml-02",
                  "POINT(2.5 3)",
                  "MULTILINESTRING((-5 0,-3 0),(2 0,3 0))",
                  3.0 * d2r * strategy.radius(),
                  to_comparable(strategy, 3.0 * d2r * strategy.radius()),
                  strategy);
    tester::apply("p-ml-03",
                  "POINT(2 0)",
                  "MULTILINESTRING((-5 0,-3 0),(2 0,3 0))",
                  0,
                  strategy);
    tester::apply("p-ml-04",
                  "POINT(3 0)",
                  "MULTILINESTRING((-5 0,-3 0),(2 0,3 0))",
                  0,
                  strategy);
    tester::apply("p-ml-05",
                  "POINT(2.5 0)",
                  "MULTILINESTRING((-5 0,-3 0),(2 0,3 0))",
                  0,
                  strategy);
    tester::apply("p-ml-06",
                  "POINT(7.5 10)",
                  "MULTILINESTRING((-5 0,-3 0),(2 0,3 0,4 0,5 0,6 0,20 1,21 1))",
                  ps_distance("POINT(7.5 10)", "SEGMENT(6 0,20 1)", strategy),
                  ps_comparable_distance("POINT(7.5 10)",
                                         "SEGMENT(6 0,20 1)",
                                         strategy),
                  strategy);
    tester::apply("p-ml-07",
                  "POINT(-8 10)",
                  "MULTILINESTRING((-20 10,-19 11,-18 10,-6 0,-5 0,-3 0),(2 0,6 0,20 1,21 1))",
                  ps_distance("POINT(-8 10)", "SEGMENT(-6 0,-18 10)", strategy),
                  ps_comparable_distance("POINT(-8 10)",
                                         "SEGMENT(-6 0,-18 10)",
                                         strategy),
                  strategy);
}

//===========================================================================

template <typename Strategy>
void test_distance_linestring_multipoint(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "linestring/multipoint distance tests" << std::endl;
#endif
    typedef test_distance_of_geometries
        <
            linestring_type, multi_point_type
        > tester;

    tester::apply("l-mp-01",
                  "LINESTRING(2 0,0 2,100 80)",
                  "MULTIPOINT(0 0,1 0,0 1,1 1)",
                  ps_distance("POINT(1 1)", "SEGMENT(2 0,0 2)", strategy),
                  ps_comparable_distance("POINT(1 1)",
                                         "SEGMENT(2 0,0 2)",
                                         strategy),
                  strategy);
    tester::apply("l-mp-02",
                  "LINESTRING(4 0,0 4,100 80)",
                  "MULTIPOINT(0 0,1 0,0 1,1 1)",
                  ps_distance("POINT(1 1)", "SEGMENT(0 4,4 0)", strategy),
                  ps_comparable_distance("POINT(1 1)",
                                         "SEGMENT(0 4,4 0)",
                                         strategy),
                  strategy);
    tester::apply("l-mp-03",
                  "LINESTRING(1 1,2 2,100 80)",
                  "MULTIPOINT(0 0,1 0,0 1,1 1)",
                  0,
                  strategy);
    tester::apply("l-mp-04",
                  "LINESTRING(3 3,4 4,100 80)",
                  "MULTIPOINT(0 0,1 0,0 1,1 1)",
                  pp_distance("POINT(1 1)", "POINT(3 3)", strategy),
                  pp_comparable_distance("POINT(1 1)", "POINT(3 3)", strategy),
                  strategy);
    tester::apply("l-mp-05",
                  "LINESTRING(0 0,10 0,10 10,0 10,0 0)",
                  "MULTIPOINT(1 -1,80 80,5 0,150 90)",
                  0,
                  strategy);
}

//===========================================================================

template <typename Strategy>
void test_distance_multipoint_multilinestring(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "multipoint/multilinestring distance tests" << std::endl;
#endif
    typedef test_distance_of_geometries
        <
            multi_point_type, multi_linestring_type
        > tester;

    tester::apply("mp-ml-01",
                  "MULTIPOINT(0 0,1 0,0 1,1 1)",
                  "MULTILINESTRING((2 0,0 2),(2 2,3 3))",
                  ps_distance("POINT(1 1)", "SEGMENT(2 0,0 2)", strategy),
                  ps_comparable_distance("POINT(1 1)",
                                         "SEGMENT(2 0,0 2)",
                                         strategy),
                  strategy);
    tester::apply("mp-ml-02",
                  "MULTIPOINT(0 0,1 0,0 1,1 1)",
                  "MULTILINESTRING((3 0,0 3),(4 4,5 5))",
                  ps_distance("POINT(1 1)", "SEGMENT(3 0,0 3)", strategy),
                  ps_comparable_distance("POINT(1 1)",
                                         "SEGMENT(3 0,0 3)",
                                         strategy),
                  strategy);
    tester::apply("mp-ml-03",
                  "MULTIPOINT(0 0,1 0,0 1,1 1)",
                  "MULTILINESTRING((4 4,5 5),(1 1,2 2))",
                  0,
                  strategy);
    tester::apply("mp-ml-04",
                  "MULTIPOINT(0 0,1 0,0 1,1 1)",
                  "MULTILINESTRING((4 4,3 3),(4 4,5 5))",
                  pp_distance("POINT(1 1)", "POINT(3 3)", strategy),
                  pp_comparable_distance("POINT(1 1)", "POINT(3 3)", strategy),
                  strategy);
}

//===========================================================================

template <typename Strategy>
void test_distance_multipoint_segment(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "multipoint/segment distance tests" << std::endl;
#endif
    typedef test_distance_of_geometries<multi_point_type, segment_type> tester;

    double d2r = bg::math::d2r<double>();

    tester::apply("mp-s-01",
                  "MULTIPOINT(0 0,1 0,0 1,1 1)",
                  "SEGMENT(2 0,0 2)",
                  ps_distance("POINT(1 1)", "SEGMENT(2 0,0 2)", strategy),
                  ps_comparable_distance("POINT(1 1)",
                                         "SEGMENT(2 0,0 2)",
                                         strategy),
                  strategy);
    tester::apply("mp-s-02",
                  "MULTIPOINT(0 0,1 0,0 1,1 1)",
                  "SEGMENT(0 -3,1 -10)",
                  3.0 * d2r * strategy.radius(),
                  to_comparable(strategy, 3.0 * d2r * strategy.radius()),
                  strategy);
    tester::apply("mp-s-03",
                  "MULTIPOINT(0 0,1 0,0 1,1 1)",
                  "SEGMENT(1 1,2 2)",
                  0,
                  strategy);
    tester::apply("mp-s-04",
                  "MULTIPOINT(0 0,1 0,0 1,1 1)",
                  "SEGMENT(3 3,4 4)",
                  pp_distance("POINT(1 1)", "POINT(3 3)", strategy),
                  pp_comparable_distance("POINT(1 1)", "POINT(3 3)", strategy),
                  strategy);
    tester::apply("mp-s-05",
                  "MULTIPOINT(0 0,1 0,0 1,1 1)",
                  "SEGMENT(0.5 -3,1 -10)",
                  pp_distance("POINT(1 0)", "POINT(0.5 -3)", strategy),
                  pp_comparable_distance("POINT(1 0)",
                                         "POINT(0.5 -3)",
                                         strategy),
                  strategy);
}

//===========================================================================

template <typename Point, typename Strategy>
void test_more_empty_input_pointlike_linear(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "testing on empty inputs... " << std::flush;
#endif
    bg::model::linestring<Point> line_empty;
    bg::model::multi_point<Point> multipoint_empty;
    bg::model::multi_linestring<bg::model::linestring<Point> > multiline_empty;

    Point point = from_wkt<Point>("POINT(0 0)");
    bg::model::linestring<Point> line =
        from_wkt<bg::model::linestring<Point> >("LINESTRING(0 0,1 1)");

    // 1st geometry is empty
    test_empty_input(multipoint_empty, line, strategy);

    // 2nd geometry is empty
    test_empty_input(point, line_empty, strategy);
    test_empty_input(point, multiline_empty, strategy);

    // both geometries are empty
    test_empty_input(multipoint_empty, line_empty, strategy);
    test_empty_input(multipoint_empty, multiline_empty, strategy);

#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << "done!" << std::endl;
#endif
}

//===========================================================================
//===========================================================================
//===========================================================================

BOOST_AUTO_TEST_CASE( test_all_point_segment )
{
    test_distance_point_segment(point_segment_strategy());
    test_distance_point_segment(point_segment_strategy(earth_radius_km));
    test_distance_point_segment(point_segment_strategy(earth_radius_miles));
}

BOOST_AUTO_TEST_CASE( test_all_point_linestring )
{
    test_distance_point_linestring(point_segment_strategy());
    test_distance_point_linestring(point_segment_strategy(earth_radius_km));
    test_distance_point_linestring(point_segment_strategy(earth_radius_miles));
}

BOOST_AUTO_TEST_CASE( test_all_point_multilinestring )
{
    test_distance_point_multilinestring(point_segment_strategy());
    test_distance_point_multilinestring(point_segment_strategy(earth_radius_km));
    test_distance_point_multilinestring(point_segment_strategy(earth_radius_miles));
}

BOOST_AUTO_TEST_CASE( test_all_linestring_multipoint )
{
    test_distance_linestring_multipoint(point_segment_strategy());
    test_distance_linestring_multipoint(point_segment_strategy(earth_radius_km));
    test_distance_linestring_multipoint(point_segment_strategy(earth_radius_miles));
}

BOOST_AUTO_TEST_CASE( test_all_multipoint_multilinestring )
{
    test_distance_multipoint_multilinestring(point_segment_strategy());
    test_distance_multipoint_multilinestring(point_segment_strategy(earth_radius_km));
    test_distance_multipoint_multilinestring(point_segment_strategy(earth_radius_miles));
}

BOOST_AUTO_TEST_CASE( test_all_multipoint_segment )
{
    test_distance_multipoint_segment(point_segment_strategy());
    test_distance_multipoint_segment(point_segment_strategy(earth_radius_km));
    test_distance_multipoint_segment(point_segment_strategy(earth_radius_miles));
}

BOOST_AUTO_TEST_CASE( test_all_empty_input_pointlike_linear )
{
    test_more_empty_input_pointlike_linear
        <
            point_type
        >(point_segment_strategy());

    test_more_empty_input_pointlike_linear
        <
            point_type
        >(point_segment_strategy(earth_radius_km));

    test_more_empty_input_pointlike_linear
        <
            point_type
        >(point_segment_strategy(earth_radius_miles));
}


#include <boost/compute/random/uniform_real_distribution.hpp>


#include <Eigen/Core>
#include <Eigen/Geometry>
#include <opencv2/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/core/utility.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/video/tracking.hpp>
#include <opencv2/plot.hpp>

#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include "g2o/solvers/csparse/linear_solver_csparse.h"
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/types/slam3d/se3quat.h>
#include <g2o/types/sba/types_six_dof_expmap.h>

#include "data.hpp"

class Reconstructor {
public:
  void reconstructAndBundleAdjust(KFData& kf_data,  const cv::Mat& K, int max_opt_step, double max_reproj_dist);

  // void refinePixel(const cv::Mat& src_img, const cv::Mat& dest_img, const std::vector<cv::Point2f>& src_fts, std::vector<cv::Point2f>& dest_fts);

};


#ifndef HOPS_AUTOCORRELATION_HPP
#define HOPS_AUTOCORRELATION_HPP

#include <Eigen/Core>
#include <unsupported/Eigen/FFT>
#include <memory>

namespace hops {
    inline size_t nextGoodSizeFFT(size_t N) {
        if (N <= 2) {
            return 2;
        }
        while (true) {
            size_t m = N;
            while ((m % 2) == 0) {
            m /= 2;
            }
            while ((m % 3) == 0) {
            m /= 3;
            }
            while ((m % 5) == 0) {
            m /= 5;
            }
            if (m <= 1) {
            return N;
            }
            N++;
        }
    }

    template <typename StateType>
    void computeAutocorrelations (const std::vector<StateType>& draws,
                                  Eigen::VectorXd& autocorrelations,
                                  unsigned long dimension) {
        computeAutocorrelations(&draws, autocorrelations, dimension);
    }

    template <typename StateType>
    void computeAutocorrelations (const std::vector<StateType>* draws,
                                  Eigen::VectorXd& autocorrelations,
                                  unsigned long dimension) {
        size_t N = draws->size();
        Eigen::VectorXd X = Eigen::VectorXd::Zero(N);
        for (size_t n = 0; n < N; ++n) {
            X(n) = (*draws)[n](dimension);
        }

        Eigen::FFT<typename StateType::Scalar> fft;
        size_t M = nextGoodSizeFFT(N);
        size_t Mt2 = 2 * M;

        // center and pad X
        Eigen::VectorXd centeredX(Mt2);
        centeredX.setZero();
        centeredX.head(N) = X.array() - X.mean();

        // See https://en.wikipedia.org/wiki/Autocorrelation#Efficient_computation for a quick
        // explanation on what follows
        Eigen::VectorXcd frequency(Mt2);
        fft.fwd(frequency, centeredX);

        frequency = frequency.cwiseAbs2();

        Eigen::VectorXcd autocorrelationsTmp(Mt2);
        fft.inv(autocorrelationsTmp, frequency);

        // use "biased" estimate as recommended by Geyer (1992)
        autocorrelations = autocorrelationsTmp.head(N).real().array() / (N * N * 2);
        autocorrelations /= autocorrelations(0);
    }
}

#endif // HOPS_AUTOCORRELATION_HPP


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  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_SIMD_FUNCTION_FLOOR_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_FLOOR_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing floor capabilities

    Computes the floor of its parameter.

    @par semantic:
    For any given value @c x of type @c T:

    @code
    T r = floor(x);
    @endcode

    is the greatest integral value of type @c T less or equal to @c x.

    @see  ceil, round, nearbyint, trunc, ifloor

  **/
  Value floor(Value const & v0);
} }
#endif

#include <boost/simd/function/scalar/floor.hpp>
#include <boost/simd/function/simd/floor.hpp>

#endif