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

//  See http://www.boost.org/libs/config/test for most recent version.

//
// This test prints out informative information about <math.h>, <float.h>
// and <limits>.  Note that this file does require a correctly configured
// Boost setup, and so can't be folded into config_info which is designed
// to function without Boost.Confg support.  Each test is documented in
// more detail below.
//

#include <boost/limits.hpp>
#include <limits.h>
#include <math.h>
#include <cmath>
#include <float.h>
#include <iostream>
#include <iomanip>
#include <cstring>
#include <boost/type_traits/alignment_of.hpp>

#ifdef BOOST_NO_STDC_NAMESPACE
namespace std{ using ::strcmp; using ::pow; using ::fabs; using ::sqrt; using ::sin; using ::atan2; }
#endif

static unsigned int indent = 4;
static unsigned int width = 40;

void print_macro(const char* name, const char* value)
{
   // if name == value+1 then then macro is not defined,
   // in which case we don't print anything:
   if(0 != std::strcmp(name, value+1))
   {
      for(unsigned i = 0; i < indent; ++i) std::cout.put(' ');
      std::cout << std::setw(width);
      std::cout.setf(std::istream::left, std::istream::adjustfield);
      std::cout << name;
      if(value[1])
      {
         // macro has a value:
         std::cout << value << "\n";
      }
      else
      {
         // macro is defined but has no value:
         std::cout << " [no value]\n";
      }
   }
}

#define PRINT_MACRO(X) print_macro(#X, BOOST_STRINGIZE(=X))

template <class T>
void print_expression(const char* expression, T val)
{
   for(unsigned i = 0; i < indent; ++i) std::cout.put(' ');
   std::cout << std::setw(width);
   std::cout.setf(std::istream::left, std::istream::adjustfield);
   std::cout << std::setprecision(std::numeric_limits<T>::digits10+2);
   std::cout << expression << "=" << val << std::endl;
}

#define PRINT_EXPRESSION(E) print_expression(#E, E);

template <class T>
void print_limits(T, const char* name)
{
   //
   // Output general information on numeric_limits, as well as
   // probing known and supected problems.
   //
   std::cout <<
      "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n"
      "std::numeric_limits information for type " << name << std::endl;
   std::cout <<
      "    is_specialized       = " << std::numeric_limits<T>::is_specialized << std::endl;
   std::cout <<
      "    min" "()                = " << std::setprecision(std::numeric_limits<T>::digits10 + 2) << (std::numeric_limits<T>::min)() << std::endl;
   std::cout <<
      "    max" "()                = " << std::setprecision(std::numeric_limits<T>::digits10 + 2) << (std::numeric_limits<T>::max)() << std::endl;
   std::cout <<
      "    digits               = " << std::numeric_limits<T>::digits << std::endl;
   std::cout <<
      "    digits10             = " << std::numeric_limits<T>::digits10 << std::endl;
   std::cout <<
      "    is_signed            = " << std::numeric_limits<T>::is_signed << std::endl;
   std::cout <<
      "    is_integer           = " << std::numeric_limits<T>::is_integer << std::endl;
   std::cout <<
      "    is_exact             = " << std::numeric_limits<T>::is_exact << std::endl;
   std::cout <<
      "    radix                = " << std::numeric_limits<T>::radix << std::endl;

   std::cout <<
      "    epsilon()            = " << std::setprecision(std::numeric_limits<T>::digits10 + 2) << (std::numeric_limits<T>::epsilon)() << std::endl;
   std::cout <<
      "    round_error()        = " << std::setprecision(std::numeric_limits<T>::digits10 + 2) << (std::numeric_limits<T>::round_error)() << std::endl;

   std::cout <<
      "    min_exponent         = " << std::numeric_limits<T>::min_exponent << std::endl;
   std::cout <<
      "    min_exponent10       = " << std::numeric_limits<T>::min_exponent10 << std::endl;
   std::cout <<
      "    max_exponent         = " << std::numeric_limits<T>::max_exponent << std::endl;
   std::cout <<
      "    max_exponent10       = " << std::numeric_limits<T>::max_exponent10 << std::endl;
   std::cout <<
      "    has_infinity         = " << std::numeric_limits<T>::has_infinity << std::endl;
   std::cout <<
      "    has_quiet_NaN        = " << std::numeric_limits<T>::has_quiet_NaN << std::endl;
   std::cout <<
      "    has_signaling_NaN    = " << std::numeric_limits<T>::has_signaling_NaN << std::endl;
   std::cout <<
      "    has_denorm           = " << std::numeric_limits<T>::has_denorm << std::endl;
   std::cout <<
      "    has_denorm_loss      = " << std::numeric_limits<T>::has_denorm_loss << std::endl;

   std::cout <<
      "    infinity()           = " << std::setprecision(std::numeric_limits<T>::digits10 + 2) << (std::numeric_limits<T>::infinity)() << std::endl;
   std::cout <<
      "    quiet_NaN()          = " << std::setprecision(std::numeric_limits<T>::digits10 + 2) << (std::numeric_limits<T>::quiet_NaN)() << std::endl;
   std::cout <<
      "    signaling_NaN()      = " << std::setprecision(std::numeric_limits<T>::digits10 + 2) << (std::numeric_limits<T>::signaling_NaN)() << std::endl;
   std::cout <<
      "    denorm_min()         = " << std::setprecision(std::numeric_limits<T>::digits10 + 2) << (std::numeric_limits<T>::denorm_min)() << std::endl;

   std::cout <<
      "    is_iec559            = " << std::numeric_limits<T>::is_iec559 << std::endl;
   std::cout <<
      "    is_bounded           = " << std::numeric_limits<T>::is_bounded << std::endl;
   std::cout <<
      "    is_modulo            = " << std::numeric_limits<T>::is_modulo << std::endl;
   std::cout <<
      "    traps                = " << std::numeric_limits<T>::traps << std::endl;
   std::cout <<
      "    tinyness_before      = " << std::numeric_limits<T>::tinyness_before << std::endl;
   std::cout <<
      "    round_style          = " << std::numeric_limits<T>::round_style << std::endl << std::endl;

   if(std::numeric_limits<T>::is_exact == 0)
   {
      bool r = std::numeric_limits<T>::epsilon() == std::pow(static_cast<T>(std::numeric_limits<T>::radix), 1-std::numeric_limits<T>::digits);
      if(r)
         std::cout << "Epsilon has sane value of std::pow(std::numeric_limits<T>::radix, 1-std::numeric_limits<T>::digits)." << std::endl;
      else
         std::cout << "CAUTION: epsilon does not have a sane value." << std::endl;
      std::cout << std::endl;
   }
   std::cout <<
      "    sizeof(" << name << ") = " << sizeof(T) << std::endl;
   std::cout <<
      "    alignment_of<" << name << "> = " << boost::alignment_of<T>::value << std::endl << std::endl;
}
/*
template <class T>
bool is_same_type(T, T)
{
   return true;
}*/
bool is_same_type(float, float)
{ return true; }
bool is_same_type(double, double)
{ return true; }
bool is_same_type(long double, long double)
{ return true; }
template <class T, class U>
bool is_same_type(T, U)
{
   return false;
}

//
// We need this to test whether abs has been overloaded for
// the floating point types or not:
//
namespace std{
#if !BOOST_WORKAROUND(BOOST_MSVC, == 1300) && \
    !defined(_LIBCPP_VERSION)
template <class T>
char abs(T)
{
   return ' ';
}
#endif
}

template <class T>
void test_overloads(T, const char* name)
{
   //
   // Probe known and suspected problems with the std lib Math functions.
   //
   std::cout <<
      "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n"
      "Math function overload information for type " << name << std::endl;

   //
   // Are the math functions overloaded for type T,
   // or do we just get double versions?
   //
   bool r = is_same_type(std::fabs(T(0)), T(0));
   r &= is_same_type(std::sqrt(T(0)), T(0));
   r &= is_same_type(std::sin(T(0)), T(0));
   if(r)
      std::cout << "The Math functions are overloaded for type " << name << std::endl;
   else
      std::cout << "CAUTION: The Math functions are NOT overloaded for type " << name << std::endl;

   //
   // Check that a few of the functions work OK, we do this because if these
   // are implemented as double precision internally then we can get
   // overflow or underflow when passing arguments of other types.
   //
   r = (std::fabs((std::numeric_limits<T>::max)()) == (std::numeric_limits<T>::max)());
   r &= (std::fabs(-(std::numeric_limits<T>::max)()) == (std::numeric_limits<T>::max)());
   r &= (std::fabs((std::numeric_limits<T>::min)()) == (std::numeric_limits<T>::min)());
   r &= (std::fabs(-(std::numeric_limits<T>::min)()) == (std::numeric_limits<T>::min)());
   if(r)
      std::cout << "std::fabs looks OK for type " << name << std::endl;
   else
      std::cout << "CAUTION: std::fabs is broken for type " << name << std::endl;

   //
   // abs not overloaded for real arguments with VC6 (and others?)
   //
   r = (std::abs((std::numeric_limits<T>::max)()) == (std::numeric_limits<T>::max)());
   r &= (std::abs(-(std::numeric_limits<T>::max)()) == (std::numeric_limits<T>::max)());
   r &= (std::abs((std::numeric_limits<T>::min)()) == (std::numeric_limits<T>::min)());
   r &= (std::abs(-(std::numeric_limits<T>::min)()) == (std::numeric_limits<T>::min)());
   if(r)
      std::cout << "std::abs looks OK for type " << name << std::endl;
   else
      std::cout << "CAUTION: std::abs is broken for type " << name << std::endl;

   //
   // std::sqrt on FreeBSD converts long double arguments to double leading to
   // overflow/underflow:
   //
   r = (std::sqrt((std::numeric_limits<T>::max)()) < (std::numeric_limits<T>::max)());
   if(r)
      std::cout << "std::sqrt looks OK for type " << name << std::endl;
   else
      std::cout << "CAUTION: std::sqrt is broken for type " << name << std::endl;

   //
   // Sanity check for atan2: verify that it returns arguments in the correct
   // range and not just atan(x/y).
   //
   static const T half_pi = static_cast<T>(1.57079632679489661923132169163975144L);

   T val = std::atan2(T(-1), T(-1));
   r = -half_pi > val;
   val = std::atan2(T(1), T(-1));
   r &= half_pi < val;
   val = std::atan2(T(1), T(1));
   r &= (val > 0) && (val < half_pi);
   val = std::atan2(T(-1), T(1));
   r &= (val < 0) && (val > -half_pi);
   if(r)
      std::cout << "std::atan2 looks OK for type " << name << std::endl;
   else
      std::cout << "CAUTION: std::atan2 is broken for type " << name << std::endl;
}

int main()
{
   //
   // Start by printing the values of the macros from float.h
   //
   std::cout <<
      "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n"
      "Macros from <math.h>" << std::endl;

#ifdef __BORLANDC__
   // Turn off hardware exceptions so we don't just abort
   // when calling numeric_limits members.
   _control87(MCW_EM,MCW_EM);
#endif

   PRINT_EXPRESSION(HUGE_VAL);
#ifdef HUGE_VALF
   PRINT_EXPRESSION(HUGE_VALF);
#endif
#ifdef HUGE_VALL
   PRINT_EXPRESSION(HUGE_VALL);
#endif
#ifdef INFINITY
   PRINT_EXPRESSION(INFINITY);
#endif

   PRINT_MACRO(NAN);
   PRINT_MACRO(FP_INFINITE);
   PRINT_MACRO(FP_NAN);
   PRINT_MACRO(FP_NORMAL);
   PRINT_MACRO(FP_SUBNORMAL);
   PRINT_MACRO(FP_ZERO);
   PRINT_MACRO(FP_FAST_FMA);
   PRINT_MACRO(FP_FAST_FMAF);
   PRINT_MACRO(FP_FAST_FMAL);
   PRINT_MACRO(FP_ILOGB0);
   PRINT_MACRO(FP_ILOGBNAN);
   PRINT_MACRO(MATH_ERRNO);
   PRINT_MACRO(MATH_ERREXCEPT);

   PRINT_EXPRESSION(FLT_MIN_10_EXP);
   PRINT_EXPRESSION(FLT_DIG);
   PRINT_EXPRESSION(FLT_MIN_EXP);
   PRINT_EXPRESSION(FLT_EPSILON);
   PRINT_EXPRESSION(FLT_RADIX);
   PRINT_EXPRESSION(FLT_MANT_DIG);
   PRINT_EXPRESSION(FLT_ROUNDS);
   PRINT_EXPRESSION(FLT_MAX);
   PRINT_EXPRESSION(FLT_MAX_10_EXP);
   PRINT_EXPRESSION(FLT_MAX_EXP);
   PRINT_EXPRESSION(FLT_MIN);
   PRINT_EXPRESSION(DBL_DIG);
   PRINT_EXPRESSION(DBL_MIN_EXP);
   PRINT_EXPRESSION(DBL_EPSILON);
   PRINT_EXPRESSION(DBL_MANT_DIG);
   PRINT_EXPRESSION(DBL_MAX);
   PRINT_EXPRESSION(DBL_MIN);
   PRINT_EXPRESSION(DBL_MAX_10_EXP);
   PRINT_EXPRESSION(DBL_MAX_EXP);
   PRINT_EXPRESSION(DBL_MIN_10_EXP);
   PRINT_EXPRESSION(LDBL_MAX_10_EXP);
   PRINT_EXPRESSION(LDBL_MAX_EXP);
   PRINT_EXPRESSION(LDBL_MIN);
   PRINT_EXPRESSION(LDBL_MIN_10_EXP);
   PRINT_EXPRESSION(LDBL_DIG);
   PRINT_EXPRESSION(LDBL_MIN_EXP);
   PRINT_EXPRESSION(LDBL_EPSILON);
   PRINT_EXPRESSION(LDBL_MANT_DIG);
   PRINT_EXPRESSION(LDBL_MAX);

   std::cout << std::endl;

   //
   // print out numeric_limits info:
   //
   print_limits(float(0), "float");
   print_limits(double(0), "double");
   print_limits((long double)(0), "long double");

   //
   // print out function overload information:
   //
   test_overloads(float(0), "float");
   test_overloads(double(0), "double");
   test_overloads((long double)(0), "long double");
   return 0;
}


#pragma once

#include <boost/serialization/vector.hpp>
#include "vector.hpp"

namespace ncv
{
        namespace tensor
        {
                ///
                /// \brief serialize a tensor
                ///
                template
                <
                        typename ttensor
                >
                typename ttensor::Scalar* save(const ttensor& t, typename ttensor::Scalar* data)
                {
                        tensor::map_vector(data, t.size()) = tensor::map_vector(t.data(), t.size());
                        return data + t.size();
                }

                ///
                /// \brief serialize a tensor
                ///
                template
                <
                        typename ttensor
                >
                const typename ttensor::Scalar* load(ttensor& t, const typename ttensor::Scalar* data)
                {
                        tensor::map_vector(t.data(), t.size()) = tensor::map_vector(data, t.size());
                        return data + t.size();
                }
        }
}

namespace boost
{
        namespace serialization
        {
                ///
                /// \brief serialize matrices and vectors
                ///
                template
                <
                        class tarchive,
                        class tvalue,
                        int Rows,
                        int Cols,
                        int Options
                >
                void serialize(tarchive& ar, Eigen::Matrix<tvalue, Rows, Cols, Options>& mat, const unsigned int)
                {
                        if (tarchive::is_saving::value)
                        {
                                int rows = mat.rows(), cols = mat.cols();
                                ar & rows; ar & cols;

                                for (int i = 0; i < mat.size(); i ++)
                                {
                                        ar & mat(i);
                                }
                        }

                        else
                        {
                                int rows = 0, cols = 0;
                                ar & rows; ar & cols;

                                mat.resize(rows, cols);
                                for (int i = 0; i < mat.size(); i ++)
                                {
                                        ar & mat(i);
                                }
                        }
                }
        }
}


#include "knapsacksolver/generator.hpp"

#include <boost/program_options.hpp>

using namespace knapsacksolver;

int main(int argc, char *argv[])
{
    namespace po = boost::program_options;

    // Parse program options
    Generator data;
    std::string output_file = "";
    std::string plot_file = "";
    po::options_description desc("Allowed options");
    desc.add_options()
        ("help,h", "produce help message")
        (",t", po::value<std::string>(&data.t)->required(), "set instancetance type (u, wc, sc, isc, asc, ss, sw, mstr, pceil, circle)")
        (",n", po::value<ItemIdx>(&data.n)->required(), "set item number")
        (",r", po::value<Profit>(&data.r), "set R")
        ("ka", po::value<Profit>(&data.k1), "set k1 (for mstr instancetances)")
        ("kb", po::value<Profit>(&data.k2), "set k2 (for mstr instancetances)")
        (",d", po::value<double>(&data.d), "set d (for mstr (6), pceil (3) and circle (3/2) instancetances)")
        (",H", po::value<int>(&data.h), "set h")
        ("hmax", po::value<int>(&data.hmax), "set hmax")
        (",x", po::value<double>(&data.x), "set x")
        (",s", po::value<Seed>(&data.s), "set seed")
        ("spanner", "set spanner")
        ("normal", "set normal")
        ("dw", po::value<double>(&data.dw), "set dw")
        (",m", po::value<Profit>(&data.m), "set m (for spanner instancetances)")
        (",v", po::value<Profit>(&data.v), "set v (for spanner instancetances)")
        (",o", po::value<std::string>(&output_file), "set output file")
        (",p", po::value<std::string>(&plot_file), "set plot file")
        ;
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help")) {
        std::cout << desc << std::endl;;
        return 1;
    }
    try {
        po::notify(vm);
    } catch (const po::required_option& e) {
        std::cout << desc << std::endl;;
        return 1;
    }
    data.spanner = vm.count("spanner");
    data.normal = vm.count("normal");

    std::cout << data << std::endl;
    Instance instance = data.generate();

    if (plot_file != "")
        instance.plot(plot_file);
    if (output_file != "")
        instance.write(output_file);

    return 0;
}


#include <boost/simd/include/functions/bitwise_cast.hpp>
#include <iostream>

int main()
{
  using boost::simd::bitwise_cast;

  std::cout << std::hex
            << bitwise_cast<int>( 1.f )           << " "
            << bitwise_cast<float>( 0x3F800000 )  << "\n";
}


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

#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/weighted_mean.hpp>
#include <boost/accumulators/statistics/variates/covariate.hpp>
#include <sstream>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>

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

template<typename T>
void assert_is_double(T const &)
{
    BOOST_MPL_ASSERT((is_same<T, double>));
}

typedef accumulator_set<int, stats<tag::mean,
        tag::mean_of_variates<int, tag::covariate1> > > mean_t;

typedef accumulator_set<int, stats<tag::mean(immediate),
        tag::mean_of_variates<int, tag::covariate1>(immediate) > > immediate_mean_t;

///////////////////////////////////////////////////////////////////////////////
// test_stat
//
void test_stat()
{
    mean_t acc, test_acc(sample = 0);

    acc(1, covariate1 = 3);
    BOOST_CHECK_CLOSE(1., mean(acc), 1e-5);
    BOOST_CHECK_EQUAL(1u, count(acc));
    BOOST_CHECK_EQUAL(1, sum(acc));
    BOOST_CHECK_CLOSE(3., (accumulators::mean_of_variates<int, tag::covariate1>(acc)), 1e-5);

    acc(0, covariate1 = 4);
    BOOST_CHECK_CLOSE(0.5, mean(acc), 1e-5);
    BOOST_CHECK_EQUAL(2u, count(acc));
    BOOST_CHECK_EQUAL(1, sum(acc));
    BOOST_CHECK_CLOSE(3.5, (accumulators::mean_of_variates<int, tag::covariate1>(acc)), 1e-5);

    acc(2, covariate1 = 8);
    BOOST_CHECK_CLOSE(1., mean(acc), 1e-5);
    BOOST_CHECK_EQUAL(3u, count(acc));
    BOOST_CHECK_EQUAL(3, sum(acc));
    BOOST_CHECK_CLOSE(5., (accumulators::mean_of_variates<int, tag::covariate1>(acc)), 1e-5);

    assert_is_double(mean(acc));

    immediate_mean_t acc2, test_acc2(sample = 0);

    acc2(1, covariate1 = 3);
    BOOST_CHECK_CLOSE(1., mean(acc2), 1e-5);
    BOOST_CHECK_EQUAL(1u, count(acc2));
    BOOST_CHECK_CLOSE(3., (accumulators::mean_of_variates<int, tag::covariate1>(acc2)), 1e-5);

    acc2(0, covariate1 = 4);
    BOOST_CHECK_CLOSE(0.5, mean(acc2), 1e-5);
    BOOST_CHECK_EQUAL(2u, count(acc2));
    BOOST_CHECK_CLOSE(3.5, (accumulators::mean_of_variates<int, tag::covariate1>(acc2)), 1e-5);

    acc2(2, covariate1 = 8);
    BOOST_CHECK_CLOSE(1., mean(acc2), 1e-5);
    BOOST_CHECK_EQUAL(3u, count(acc2));
    BOOST_CHECK_CLOSE(5., (accumulators::mean_of_variates<int, tag::covariate1>(acc2)), 1e-5);

    assert_is_double(mean(acc2));
}

///////////////////////////////////////////////////////////////////////////////
// test_persistency
//
void test_persistency()
{
    std::stringstream ss;
    {
        mean_t acc1;
        immediate_mean_t acc2;

        acc1(1, covariate1 = 3);
        acc1(0, covariate1 = 4);
        acc1(2, covariate1 = 8);
        acc2(1, covariate1 = 3);
        acc2(0, covariate1 = 4);
        acc2(2, covariate1 = 8);
        BOOST_CHECK_CLOSE(1., mean(acc1), 1e-5);
        BOOST_CHECK_CLOSE(5., (accumulators::mean_of_variates<int, tag::covariate1>(acc1)), 1e-5);
        BOOST_CHECK_CLOSE(1., mean(acc2), 1e-5);
        BOOST_CHECK_CLOSE(5., (accumulators::mean_of_variates<int, tag::covariate1>(acc2)), 1e-5);
        boost::archive::text_oarchive oa(ss);
        acc1.serialize(oa, 0);
        acc2.serialize(oa, 0);
    }
    mean_t acc1;
    immediate_mean_t acc2;
    boost::archive::text_iarchive ia(ss);
    acc1.serialize(ia, 0);
    acc2.serialize(ia, 0);
    BOOST_CHECK_CLOSE(1., mean(acc1), 1e-5);
    BOOST_CHECK_CLOSE(5., (accumulators::mean_of_variates<int, tag::covariate1>(acc2)), 1e-5);
}

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

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

    return test;
}


#include "crop_images.hpp"

#include "for_each.hpp"

#include <boost/filesystem.hpp>

#include <opencv2/highgui.hpp>

#include <iomanip>
#include <sstream>

namespace ocv{

/**
 * @brief crop the image into non-overlap small block
 * @param directory directory of the image
 * @param img_name name of the image
 * @param size block size of the image
 * @return the images after cropped
 */
std::vector<cv::Mat> crop_image(std::string const &directory,
                                std::string const &img_name,
                                cv::Size2i const &size)
{
    cv::Mat const Img = cv::imread(directory + "/" + img_name);
    std::vector<cv::Mat> result;
    if(!Img.empty()){
        auto func = [&](int, int, cv::Mat const &input)
        {
            result.emplace_back(input);
        };
        ocv::for_each_block(Img, size, func, size);
    }

    return result;
}

/**
 * @brief crop all of the images of the directory into non-overlap small block
 * @param directory directory of the image
 * @param size block size of the image
 * @return the images after cropped
 */
std::vector<cv::Mat> crop_directory_images(std::string const &directory,
                                           cv::Size2i const &size)
{
    using namespace boost::filesystem;

    path info(directory);
    std::vector<cv::Mat> result;
    if(is_directory(info)){
        directory_iterator it{info};
        while(it != directory_iterator{}){
            if(is_regular_file(*it)){
                auto const File = it->path().filename().generic_string();
                auto files = crop_image(directory, File, size);
                result.insert(std::end(result), std::begin(files), std::end(files));
            }
            ++it;
        }
    }

    return result;
}

cv::Mat crop_image(const cv::Mat &input,
                   int x_center, int y_center,
                   int x_extend, int y_extend)
{
    int const start_x = std::max(0, x_center-x_extend);
    int const end_x = std::min(input.cols-1, x_center+x_extend);
    int const start_y = std::max(0, y_center-y_extend);
    int const end_y = std::min(input.rows-1, y_center+y_extend);

    return input(cv::Rect(cv::Point(start_x, start_y),
                          cv::Point(end_x, end_y)));
}

cv::Mat crop_image(const cv::Mat &input,
                   float x_center, float y_center,
                   int x_extend, int y_extend)
{
    return crop_image(input, static_cast<int>(x_center),
                      static_cast<int>(y_center),
                      x_extend, y_extend);
}

}


/**
 *   @file gtest_eigenhdf.cpp
 *   @brief write to and read from hdf5 files via eigen2hdf wrapper
 *
 *  Detailed description
 *
 *  Note: eigen2hdf always uses 2 dimensional arrays, also for
 *        vectors, this is not the case for h5py.
 */

#include <gtest/gtest.h>
#include <Eigen/Dense>
#include <Eigen/Sparse>

#include "aux/eigen2hdf.hpp"

// write and read from hdf5 files using wrapper functions
// testing for type double (std::complex<double> is missing)

TEST(eigenhdf, row_col_vector)
{
  typedef Eigen::Array<double, 1, Eigen::Dynamic> col_array_t;
  typedef Eigen::Array<double, Eigen::Dynamic, 1> row_array_t;
  const char* fname = "gtest_eigenhdf_row_vector.h5";

  hid_t h5_file = H5Fcreate(fname, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);

  ASSERT_TRUE(h5_file > 0);

  col_array_t X(100);
  X.setRandom();
  eigen2hdf::save(h5_file, "X", X);

  row_array_t Y(100);
  Y.setRandom();
  eigen2hdf::save(h5_file, "Y", Y);

  herr_t info = H5Fclose(h5_file);
  ASSERT_TRUE(info == 0);

  // -- re-open -- //
  h5_file = H5Fopen(fname, H5F_ACC_RDONLY, H5P_DEFAULT);
  ASSERT_TRUE(h5_file > 0);

  col_array_t Xn;
  eigen2hdf::load(h5_file, "X", Xn);
  EXPECT_EQ(Xn.rows(), X.rows());
  EXPECT_EQ(Xn.cols(), X.cols());

  row_array_t Yn;
  eigen2hdf::load(h5_file, "Y", Yn);
  EXPECT_EQ(Yn.rows(), Y.rows());
  EXPECT_EQ(Yn.cols(), Y.cols());

  EXPECT_TRUE(((Yn - Y).abs() == 0).any());
  EXPECT_TRUE(((Xn - X).abs() == 0).any());

  info = H5Fclose(h5_file);
  ASSERT_TRUE(info == 0);
}

TEST(eigenhdf, c_array)
{
  typedef Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> c_array_t;
  const char* fname = "gtest_eigenhdf_col_vector.h5";

  hid_t h5_file = H5Fcreate(fname, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);

  ASSERT_TRUE(h5_file > 0);

  c_array_t X(100, 10);
  X.setRandom();
  eigen2hdf::save(h5_file, "X", X);

  c_array_t Y(20, 100);
  Y.setRandom();
  eigen2hdf::save(h5_file, "Y", Y);

  herr_t info = H5Fclose(h5_file);
  ASSERT_TRUE(info == 0);

  // -- re-open -- //
  h5_file = H5Fopen(fname, H5F_ACC_RDONLY, H5P_DEFAULT);
  ASSERT_TRUE(h5_file > 0);

  c_array_t Xn;
  eigen2hdf::load(h5_file, "X", Xn);
  EXPECT_EQ(Xn.rows(), X.rows());
  EXPECT_EQ(Xn.cols(), X.cols());

  c_array_t Yn;
  eigen2hdf::load(h5_file, "Y", Yn);
  EXPECT_EQ(Yn.rows(), Y.rows());
  EXPECT_EQ(Yn.cols(), Y.cols());

  EXPECT_TRUE(((Yn - Y).abs() == 0).any());
  EXPECT_TRUE(((Xn - X).abs() == 0).any());

  info = H5Fclose(h5_file);
  ASSERT_TRUE(info == 0);
}

TEST(eigenhdf, sparse_matrix)
{
  typedef Eigen::SparseMatrix<double, Eigen::RowMajor> sp_cmat_t;

  int n = 10;
  int m = 144;

  sp_cmat_t K(n, m);

  for (int i = 0; i < n; ++i) {
    int j = i + 1;
    if (j < m) K.insert(i, j) = i + j;
    j = i - 1;
    if (j >= 0 && j < m) K.insert(i, j) = i - j;
  }
  K.makeCompressed();

  const char* fname = "gtest_eigenhdf_sparse.h5";
  hid_t h5_file = H5Fcreate(fname, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
  eigen2hdf::save_sparse(h5_file, "S", K);
  hid_t info = H5Fclose(h5_file);
  ASSERT_TRUE(info == 0);

  // -- open --
  h5_file = H5Fopen(fname, H5F_ACC_RDONLY, H5P_DEFAULT);
  sp_cmat_t Kl;

  eigen2hdf::load_sparse(h5_file, "S", Kl);

  ASSERT_TRUE(Kl.isCompressed());

  ASSERT_EQ(Kl.rows(), K.rows());
  ASSERT_EQ(Kl.cols(), K.cols());

  EXPECT_TRUE((K - Kl).sum() == 0);

  info = H5Fclose(h5_file);
  ASSERT_TRUE(info == 0);

  // -- open as col major matrix
  typedef Eigen::SparseMatrix<double, Eigen::ColMajor> sp_fmat_t;
  typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> array_t;

  h5_file = H5Fopen(fname, H5F_ACC_RDONLY, H5P_DEFAULT);
  sp_fmat_t Kf;

  eigen2hdf::load_sparse(h5_file, "S", Kf);

  ASSERT_TRUE(Kf.isCompressed());

  ASSERT_EQ(Kf.rows(), K.rows());
  ASSERT_EQ(Kf.cols(), K.cols());

  array_t K2 = K;
  array_t Kf2 = Kf;

  EXPECT_TRUE((K2 - Kf2).cwiseAbs().sum() == 0);

  info = H5Fclose(h5_file);
  ASSERT_TRUE(info == 0);
}


/*
 * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
 * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <fstream>
#include <iostream>

#include <Eigen/Geometry>
#include <mav_msgs/conversions.h>
#include <mav_msgs/default_topics.h>
#include <mav_msgs/eigen_mav_msgs.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <trajectory_msgs/MultiDOFJointTrajectory.h>

bool sim_running = false;

static const int64_t kNanoSecondsInSecond = 1000000000;

void callback(const sensor_msgs::ImuPtr& /*msg*/) {
  sim_running = true;
}

class WaypointWithTime {
 public:
  WaypointWithTime()
      : waiting_time(0) {
  }

  WaypointWithTime(double t, float x, float y, float z, float _yaw)
      : position(x, y, z), yaw(_yaw), waiting_time(t) {
  }

  Eigen::Vector3d position;
  double yaw;
  double waiting_time;
};

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

  ros::init(argc, argv, "waypoint_publisher");
  ros::NodeHandle nh;

  ROS_INFO("Started waypoint_publisher.");

  ros::V_string args;
  ros::removeROSArgs(argc, argv, args);

  if (args.size() != 2 && args.size() != 3) {
    ROS_ERROR("Usage: waypoint_publisher <waypoint_file>"
        "\nThe waypoint file should be structured as: space separated: wait_time [s] x[m] y[m] z[m] yaw[deg])");
    return -1;
  }

  std::vector<WaypointWithTime> waypoints;
  const float DEG_2_RAD = M_PI / 180.0;

  std::ifstream wp_file(args.at(1).c_str());

  if (wp_file.is_open()) {
    double t, x, y, z, yaw;
    // Only read complete waypoints.
    while (wp_file >> t >> x >> y >> z >> yaw) {
      waypoints.push_back(WaypointWithTime(t, x, y, z, yaw * DEG_2_RAD));
    }
    wp_file.close();
    ROS_INFO("Read %d waypoints.", (int )waypoints.size());
  }

  else {
    ROS_ERROR_STREAM("Unable to open poses file: " << args.at(1));
    return -1;
  }

  // The IMU is used, to determine if the simulator is running or not.
  ros::Subscriber sub = nh.subscribe("imu", 10, &callback);

  ros::Publisher wp_pub =
      nh.advertise<trajectory_msgs::MultiDOFJointTrajectory>(
      mav_msgs::default_topics::COMMAND_TRAJECTORY, 10);

  ROS_INFO("Wait for simulation to become ready...");

  while (!sim_running && ros::ok()) {
    ros::spinOnce();
    ros::Duration(0.1).sleep();
  }

  ROS_INFO("...ok");

  // Wait for 30s such that everything can settle and the mav flies to the initial position.
  ros::Duration(30).sleep();

  ROS_INFO("Start publishing waypoints.");

  trajectory_msgs::MultiDOFJointTrajectoryPtr msg(new trajectory_msgs::MultiDOFJointTrajectory);
  msg->header.stamp = ros::Time::now();
  msg->points.resize(waypoints.size());
  msg->joint_names.push_back("base_link");
  int64_t time_from_start_ns = 0;
  for (size_t i = 0; i < waypoints.size(); ++i) {
    WaypointWithTime& wp = waypoints[i];

    mav_msgs::EigenTrajectoryPoint trajectory_point;
    trajectory_point.position_W = wp.position;
    trajectory_point.setFromYaw(wp.yaw);
    trajectory_point.time_from_start_ns = time_from_start_ns;

    time_from_start_ns += static_cast<int64_t>(wp.waiting_time * kNanoSecondsInSecond);

    mav_msgs::msgMultiDofJointTrajectoryPointFromEigen(trajectory_point, &msg->points[i]);
  }
  wp_pub.publish(msg);

  return 0;
}


/*
 * #%L
 * %%
 * Copyright (C) 2011 - 2017 BMW Car IT GmbH
 * %%
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 * #L%
 */
#include "GeocastBroadcastFilter.h"

#include <string>

#include <boost/geometry.hpp>

#include "joynr/serializer/Serializer.h"

GeocastBroadcastFilter::GeocastBroadcastFilter()
{
}

bool GeocastBroadcastFilter::filter(
        const joynr::vehicle::RadioStation& discoveredStation,
        const joynr::vehicle::GeoPosition& geoPosition,
        const vehicle::RadioNewStationDiscoveredBroadcastFilterParameters& filterParameters)
{
    if (filterParameters.getPositionOfInterest().empty() ||
        filterParameters.getRadiusOfInterestArea().empty()) {
        // filter parameter not set, so we do no filtering
        return true;
    }

    joynr::vehicle::GeoPosition positionOfInterest;
    try {
        joynr::serializer::deserializeFromJson(
                positionOfInterest, filterParameters.getPositionOfInterest());
    } catch (const std::invalid_argument& e) {
        JOYNR_LOG_ERROR(logger(),
                        "Unable to deserialize geo position object from: {} - error: {}",
                        filterParameters.getPositionOfInterest(),
                        e.what());
        return true;
    }
    int radiusOfInterestArea = std::stoi(filterParameters.getRadiusOfInterestArea());

    // calculate distance between two geo positions using the haversine formula
    // (cf. http://en.wikipedia.org/wiki/Haversine_formula)
    double earthRadius = 6371000.0; // in meters

    typedef boost::geometry::model::point<
            double,
            2,
            boost::geometry::cs::spherical_equatorial<boost::geometry::degree>> BoostGeoPosition;

    BoostGeoPosition boostGeoPosition(geoPosition.getLatitude(), geoPosition.getLongitude());
    BoostGeoPosition boostPositionOfInterest(
            positionOfInterest.getLatitude(), positionOfInterest.getLongitude());

    double distance = boost::geometry::distance(
            boostGeoPosition,
            boostPositionOfInterest,
            boost::geometry::strategy::distance::haversine<double>(earthRadius));

    return distance < radiusOfInterestArea;
}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  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_IS_NLEZ_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IS_NLEZ_HPP_INCLUDED

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

 /*!

    @ingroup group-predicates
    Function object implementing is_nlez capabilities

    Returns @ref True if x is not less or equal to @ref Zero else returns @ref False.

    @par Semantic:

    @code
    as_logical_t<T> r = is_nlez(x);
    @endcode

    is similar to:

    @code
    as_logical_t<T> r = !(x <= 0);
    @endcode

    @par Note:

    Due to existence of nan, this is not equivalent to a call to @ref is_gt
z    for floating types

  **/
  const boost::dispatch::functor<tag::is_nlez_> is_nlez = {};
} }
#endif

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

#endif


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://svn.win.tue.nl/trac/MCRL2/browser/trunk/COPYING
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
/// \file enumerator_test.cpp
/// \brief Add your file description here.

#include <boost/test/minimal.hpp>
#include "mcrl2/core/detail/print_utility.h"
#include "mcrl2/data/substitutions/mutable_map_substitution.h"
#include "mcrl2/pbes/enumerator.h"
#include "mcrl2/pbes/parse.h"
#include "mcrl2/pbes/pbes.h"
#include "mcrl2/pbes/rewriters/simplify_rewriter.h"

using namespace mcrl2;
using namespace mcrl2::pbes_system;

  const std::string VARSPEC =
    "datavar         \n"
    "  m: Nat;       \n"
    "  n: Nat;       \n"
    "  b: Bool;      \n"
    "  c: Bool;      \n"
    "                \n"
    "predvar         \n"
    "  X: Bool, Pos; \n"
    "  Y: Nat;       \n"
    ;

void test_enumerator()
{
  typedef pbes_system::simplify_data_rewriter<data::rewriter> pbes_rewriter;
  typedef data::enumerator_list_element<pbes_expression> enumerator_element;

  data::data_specification data_spec;
  data_spec.add_context_sort(data::sort_nat::nat());
  data::rewriter datar(data_spec);
  pbes_rewriter R(datar);

  data::variable_list v;
  v.push_front(data::variable("n", data::sort_nat::nat()));
  pbes_expression phi = parse_pbes_expression("val(n < 2)", VARSPEC);
  data::mutable_indexed_substitution<> sigma;
  data::enumerator_identifier_generator id_generator("x");
  data::enumerator_algorithm<pbes_rewriter> E(R, data_spec, datar, id_generator);
  std::vector<pbes_system::pbes_expression> solutions;
  std::deque<enumerator_element> P;
  P.push_back(enumerator_element(v, phi));
  E.next(P, sigma, is_not_true());
  while (!P.empty())
  {
    solutions.push_back(P.front().expression());
    P.pop_front();
    E.next(P, sigma, is_not_true());
  }
  std::clog << "solutions = " << core::detail::print_list(solutions) << std::endl;
  BOOST_CHECK(solutions.size() >= 1);
}

void test_enumerator_with_iterator()
{
  typedef pbes_system::simplify_data_rewriter<data::rewriter> pbes_rewriter;
  typedef data::enumerator_list_element<pbes_expression> enumerator_element;

  data::data_specification data_spec;
  data_spec.add_context_sort(data::sort_nat::nat());
  data::rewriter datar(data_spec);
  pbes_rewriter R(datar);

  data::variable_list v;
  v.push_front(data::variable("n", data::sort_nat::nat()));
  pbes_expression phi = parse_pbes_expression("val(n < 2)", VARSPEC);
  data::mutable_indexed_substitution<> sigma;
  data::enumerator_algorithm_with_iterator<pbes_rewriter, enumerator_element, pbes_system::is_not_true> E(R, data_spec, datar, 20);
  std::vector<pbes_system::pbes_expression> solutions;

  std::deque<enumerator_element> P;
  P.push_back(enumerator_element(v, phi));
  for (auto i = E.begin(sigma, P); i != E.end(); ++i)
  {
    solutions.push_back(i->expression());
  }
  std::clog << "solutions = " << core::detail::print_list(solutions) << std::endl;
  BOOST_CHECK(solutions.size() >= 1);
}

void test_enumerator_with_substitutions()
{
  typedef pbes_system::simplify_data_rewriter<data::rewriter> pbes_rewriter;
  typedef data::enumerator_list_element_with_substitution<pbes_expression> enumerator_element;

  data::data_specification data_spec;
  data_spec.add_context_sort(data::sort_nat::nat());
  data::rewriter datar(data_spec);
  pbes_rewriter R(datar);

  data::variable_list v;
  v.push_front(data::variable("n", data::sort_nat::nat()));
  pbes_expression phi = parse_pbes_expression("val(n < 2)", VARSPEC);
  data::mutable_indexed_substitution<> sigma;
  data::enumerator_algorithm_with_iterator<pbes_rewriter, enumerator_element, pbes_system::is_not_false> E(R, data_spec, datar);
  std::vector<pbes_system::pbes_expression> solutions;

  std::deque<enumerator_element> P;
  P.push_back(enumerator_element(v, phi));
  for (auto i = E.begin(sigma, P); i != E.end(); ++i)
  {
    solutions.push_back(i->expression());
    data::mutable_map_substitution<> sigma;
    i->add_assignments(v, sigma, datar);
    std::clog << "  solutions " << i->expression() << " substitution = " << sigma << std::endl;
    BOOST_CHECK(R(phi, sigma) == i->expression());
  }
  std::clog << "solutions = " << core::detail::print_list(solutions) << std::endl;
  BOOST_CHECK(solutions.size() >= 1);
}

int test_main(int argc, char** argv)
{
  log::mcrl2_logger::set_reporting_level(log::debug);
  test_enumerator();
  test_enumerator_with_iterator();
  test_enumerator_with_substitutions();

  return 0;
}


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

#include <mpllibs/metamonad/make_monadic.hpp>
#include <mpllibs/metamonad/already_monadic.hpp>
#include <mpllibs/metamonad/maybe.hpp>
#include <mpllibs/metamonad/metafunction.hpp>
#include <mpllibs/metamonad/syntax.hpp>

#include <boost/test/unit_test.hpp>

#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/assert.hpp>

#include "common.hpp"

using boost::mpl::plus;

using mpllibs::metamonad::just;

namespace
{
  MPLLIBS_METAFUNCTION(add, (A)(B))
  ((plus<typename A::type, typename B::type>));

  MPLLIBS_METAFUNCTION(make_just, (X)) ((just<X>));
}

BOOST_AUTO_TEST_CASE(test_make_monadic)
{
  using mpllibs::metamonad::make_monadic;
  using mpllibs::metamonad::already_monadic;
  using mpllibs::metamonad::maybe_tag;
  using mpllibs::metamonad::syntax;

  using boost::mpl::equal_to;

  typedef int11::tag int_tag;
  typedef maybe_tag<int_tag> mt;

  // test_monadic_values
  BOOST_MPL_ASSERT((
    equal_to<
      just<int13>,
      make_monadic<mt, syntax<already_monadic<just<int13> > > >::type
    >
  ));

  // test_metafunction_call
  BOOST_MPL_ASSERT((
    equal_to<
      int13,
      make_monadic<
        mt,
        syntax<
          add<already_monadic<just<int11> >, already_monadic<just<int2> > >
        >
      >::type
    >
  ));

  // test_nested_function_calls
  BOOST_MPL_ASSERT((
    equal_to<
      int13,
      make_monadic<
        mt,
        syntax<
          add<
            already_monadic<make_just<int11> >,
            already_monadic<make_just<int2> >
          >
        >
      >::type
    >
  ));

  // test_variadic_metafunction_call
  BOOST_MPL_ASSERT((
    equal_to<
      int13,
      make_monadic<
        mt,
        syntax<
          plus<already_monadic<just<int11> >, already_monadic<just<int2> > >
        >
      >::type
    >
  ));
}


#include "lineslam.h"
#include "utils.h"
#include <Eigen/StdVector>
#include <stdint.h>

#ifdef _MSC_VER
#include <unordered_set>
#else
#include <tr1/unordered_set>
#endif

#include "g2o/config.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/solver.h"
#include "g2o/core/robust_kernel_impl.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/optimization_algorithm_gauss_newton.h"
#include "g2o/solvers/dense/linear_solver_dense.h"
#include "g2o/types/icp/types_icp.h"
#include "g2o/solvers/structure_only/structure_only_solver.h"

#if defined G2O_HAVE_CHOLMOD
#include "g2o/solvers/cholmod/linear_solver_cholmod.h"
#elif defined G2O_HAVE_CSPARSE
#include "g2o/solvers/csparse/linear_solver_csparse.h"
#endif

#include "g2o/types/slam3d/vertex_se3.h"
#include <fstream>
#include "../external/levmar-2.6/levmar.h"

using namespace Eigen;

extern cv::Mat K, distCoeffs;
extern SystemParameters sysPara;

#ifdef SLAM_LBA
void Map3d::correctPose(vector<PoseConstraint> pcs)
{
	// form a pose graph
	int numLocalConstraint = 10;
	// ----- G2O parameter setting -----
	int maxIters = 15;
	// some handy typedefs
	typedef g2o::BlockSolver< g2o::BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic> >  MyBlockSolver;
	typedef g2o::LinearSolverCSparse<MyBlockSolver::PoseMatrixType> MyLinearSolver;

	// setup the solver
	g2o::SparseOptimizer optimizer;
	optimizer.setVerbose(false);
	MyLinearSolver* linearSolver = new MyLinearSolver();

	MyBlockSolver* solver_ptr = new MyBlockSolver(linearSolver);
	g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
//	g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr);
	optimizer.setAlgorithm(solver);

	// -- add the parameter representing the sensor offset  !!!!
	g2o::ParameterSE3Offset* sensorOffset = new g2o::ParameterSE3Offset;
	sensorOffset->setOffset(Eigen::Isometry3d::Identity());
	sensorOffset->setId(0);
	optimizer.addParameter(sensorOffset);

	// ----- set g2o vertices ------
	int vertex_id = 0;
	tr1::unordered_map<int,int> camvid2fid, camfid2vid;

	// ----- optimization parameters -----
	vector<g2o::VertexSE3*> vertices;
	vector<g2o::EdgeSE3*> edges;
	// ---- camera pose parameters ----
	for(int i = 0; i < keyframeIdx.size(); ++i) {
		cv::Mat qi = r2q(frames[keyframeIdx[i]].R);
		Eigen::Isometry3d pose;
		Eigen:: Quaterniond q(qi.at<double>(0),qi.at<double>(1),qi.at<double>(2),qi.at<double>(3));
		pose = q;
		pose.translation() = Eigen::Vector3d(frames[keyframeIdx[i]].t.at<double>(0),
			frames[keyframeIdx[i]].t.at<double>(1),frames[keyframeIdx[i]].t.at<double>(2));
		g2o::VertexSE3 * v_se3 = new g2o::VertexSE3();
		v_se3->setId(vertex_id);
		v_se3->setEstimate(pose);
		if (i==0)
			v_se3->setFixed(true);
		else
			v_se3->setFixed(false);
		optimizer.addVertex(v_se3);
		camvid2fid[vertex_id] = keyframeIdx[i];
		camfid2vid[keyframeIdx[i]] = vertex_id;
		vertices.push_back(v_se3);
		++vertex_id;
  	}
	// g2o edges
	// visual odometry edges
	for(int i=1; i<vertices.size(); ++i) {
		g2o::VertexSE3* cur  = vertices[i];
		for(int j=1; j <= numLocalConstraint; ++j) {
			if(i-j <0 ) continue;
			g2o::VertexSE3* prev = vertices[i-j];
			Eigen::Isometry3d t = prev->estimate().inverse() * cur->estimate();
			g2o::EdgeSE3* e = new g2o::EdgeSE3;
			e->setVertex(0, prev);
			e->setVertex(1, cur);
			e->setMeasurement(t);
			// approximate information by number of common lanmarks
			int comLmk = 0;
			for(int k=0; k<frames[keyframeIdx[i]].lines.size(); ++k) {
				int gid = frames[keyframeIdx[i]].lines[k].gid;
				if (gid < 0) continue;
				for(int h = 0; h < lmklines[gid].frmId_lnLid.size(); ++h) {
					if (lmklines[gid].frmId_lnLid[h][0] == keyframeIdx[i-j]) {
						++comLmk;
						break;
					}
				}
			}
			e->information() = e->information() * comLmk;
	//		cout<<"keyframe "<<i<<" & "<<i-j<<": "<<comLmk <<endl;
			optimizer.addEdge(e);
			edges.push_back(e);
		}
	}
	// loop closure edges
	for(int i=0; i<pcs.size(); ++i) {
		g2o::VertexSE3* from = vertices[pcs[i].from];
        g2o::VertexSE3* to   = vertices[pcs[i].to];
        Eigen::Isometry3d relpose;
		cv::Mat qi = r2q(pcs[i].R);
		Eigen:: Quaterniond q(qi.at<double>(0),qi.at<double>(1),qi.at<double>(2),qi.at<double>(3));
		relpose = q;
		relpose.translation() = Eigen::Vector3d(pcs[i].t.at<double>(0),
								pcs[i].t.at<double>(1),pcs[i].t.at<double>(2));
        g2o::EdgeSE3* e = new g2o::EdgeSE3;
        e->setVertex(0, from);
        e->setVertex(1, to);
        e->setMeasurement(relpose);
		e->information() = e->information() * pcs[i].numMatches;
		optimizer.addEdge(e);
        edges.push_back(e);
	}
	// optimize
	// ----- start g2o -----
	MyTimer timer;
	timer.start();
	optimizer.initializeOptimization();
	optimizer.computeActiveErrors();
	cout<<"loopclosing cost "<<optimizer.activeChi2();
//	optimizer.setVerbose(1);
	optimizer.optimize(maxIters);
	timer.end();

	optimizer.computeActiveErrors();
	cout<<" ==> " <<optimizer.activeChi2()<<endl;
	cout<<", used time: "<<timer.time_s<<" sec. \n ";

	// ====== update poses =======
	for(int i = 0; i < keyframeIdx.size(); ++i) {
		if(vertices[i]->fixed()) continue;
		Vector3d t = vertices[i]->estimate().translation();
		Quaterniond q(vertices[i]->estimate().rotation());
		double qd[] = {q.w(), q.x(), q.y(), q.z()};
		frames[keyframeIdx[i]].R = q2r(qd);
		frames[keyframeIdx[i]].t = (cv::Mat_<double>(3,1)<<t(0),t(1),t(2));
	}

	optimizer.clear();

	// correct landmarks
	lba_g2o(sysPara.num_pos_lba+5,sysPara.num_frm_lba+5, 1);
	write2file (*this, "_lpcls");
	//cin>>maxIters;
}
#endif


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2011 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#include <cstdlib>
#include <iostream>
#include <boost/timer.hpp>
#include "dune/config.h"
#include "dune/common/stdstreams.hh"
#include "dune/grid/uggrid.hh"
#include "lagrangespace.hh"
#include "fetransfer.hh"

bool check(int order)
{
  int const dim = 2;
  int const level = 3;

  typedef Dune::UGGrid<dim> Grid;
  typedef Grid::Traits::LeafIndexSet IS;

  std::auto_ptr<Grid> grid(new Grid);
  grid->createBegin();
  Dune::FieldVector<double,dim> v;
  // vertices
  v[0]=0; v[1]=0;
  grid->insertVertex(v);
  v[0]=1; v[1]=0;
  grid->insertVertex(v);
  v[0]=1; v[1]=1;
  grid->insertVertex(v);
  v[0]=0; v[1]=1;
  grid->insertVertex(v);
  // elements
  std::vector<unsigned int> vid(3);
  vid[0]=0; vid[1]=1; vid[2]=2;
  grid->insertElement(Dune::GeometryType(Dune::GeometryType::simplex,2),vid);
  vid[0]=0; vid[1]=2; vid[2]=3;
  grid->insertElement(Dune::GeometryType(Dune::GeometryType::simplex,2),vid);
  grid->createEnd();
  grid->globalRefine(level);

  GridManager<Grid> gridManager(grid);

  bool pass = true;

  typedef FEFunctionSpace<ContinuousLagrangeMapper<double,Grid> > H1Space;
  H1Space spaceH1(gridManager,gridManager.grid().leafIndexSet(),order);

  typedef FEFunctionSpace<DiscontinuousLagrangeMapper<double,Grid> > L2Space;
  L2Space spaceL2(gridManager,gridManager.grid().leafIndexSet(),order-1);

  H1Space::Element<2>::type f(spaceH1);
  for (int i=0; i<spaceH1.dimension(); ++i)
    (*f)[i] = (rand()%1000)/1000.0;

  L2Space::Element<3>::type g(spaceL2);
  for (int i=0; i<spaceH1.dimension(); ++i)
    (*g)[i] = (rand()%1000)/1000.0;

  std::vector<Dune::FieldVector<double,dim> > x(1000);
  std::vector<H1Space::Element<2>::type::ValueType> fVal(x.size());
  std::vector<L2Space::Element<3>::type::ValueType> gVal(x.size());
  for (int i=0; i<x.size(); ++i) {
    for (int j=0; j<dim; ++j)
      x[i][j] = (rand()%100002)/100001.0; // crude numbers to prevent test points to lie on cell boundaries.
    fVal[i] = f.value(x[i]);
    gVal[i] = g.value(x[i]);
  }

  for (int l=0; l<7; ++l) {
    for (Grid::Codim<0>::Partition<Dune::All_Partition>::LeafIterator ci=gridManager.grid().leafbegin<0>();
         ci!=gridManager.grid().leafend<0>(); ++ci) {
      int r = rand()%100;
      if (r < 20)
        gridManager.mark((l>3 && ci->level()>level)? -1: 1,ci);
    }
    gridManager.adaptAtOnce();

    double ferr = 0, gerr = 0, favg = 0, gavg = 0;
    for (int i=0; i<x.size(); ++i) {
      double fd = (fVal[i]-f.value(x[i])).two_norm();
      double gd = (gVal[i]-g.value(x[i])).two_norm();
      ferr = std::max(ferr,fd);
      gerr = std::max(gerr,gd);
      favg += fd/x.size();
      gavg += gd/x.size();
    }

    if (ferr > 1e-13) {
      std::cerr << "Continuous mapper of order " << order << " failed in step " << l
                << ": maximal error " << ferr << " average error " << favg << '\n';
      pass = false;
    }

    if (gerr > 1e-13) {
      std::cerr << "Disontinuous mapper of order " << order << " failed in step " << l
                << ": maximal error " << gerr << " average error " << gavg << '\n';
      pass = false;
    }
  }
  return pass;
}

int main(void)
{
  bool pass = true;
  for (int i=1; i<5; ++i)
    pass &= check(i);

  if (pass)
    std::cout << "passed.\n";

  return 0;
}


//
// Copyright (c) 2018-2020 CNRS INRIA
//

#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
#include "pinocchio/algorithm/kinematics-derivatives.hpp"
#include "pinocchio/algorithm/center-of-mass-derivatives.hpp"

#include <boost/python/tuple.hpp>

namespace pinocchio
{
  namespace python
  {
    namespace bp = boost::python;

    bp::tuple getJointVelocityDerivatives_proxy(const Model & model,
                                                Data & data,
                                                const Model::JointIndex jointId,
                                                ReferenceFrame rf)
    {
      typedef Data::Matrix6x Matrix6x;

      Matrix6x partial_dq(Matrix6x::Zero(6,model.nv));
      Matrix6x partial_dv(Matrix6x::Zero(6,model.nv));

      getJointVelocityDerivatives(model,data,jointId,rf,
                                  partial_dq,partial_dv);

      return bp::make_tuple(partial_dq,partial_dv);
    }

    bp::tuple getJointAccelerationDerivatives_proxy(const Model & model,
                                                    Data & data,
                                                    const Model::JointIndex jointId,
                                                    ReferenceFrame rf)
    {
      typedef Data::Matrix6x Matrix6x;

      Matrix6x v_partial_dq(Matrix6x::Zero(6,model.nv));
      Matrix6x a_partial_dq(Matrix6x::Zero(6,model.nv));
      Matrix6x a_partial_dv(Matrix6x::Zero(6,model.nv));
      Matrix6x a_partial_da(Matrix6x::Zero(6,model.nv));

      getJointAccelerationDerivatives(model,data,jointId,rf,
                                      v_partial_dq,a_partial_dq,
                                      a_partial_dv,a_partial_da);

      return bp::make_tuple(v_partial_dq,a_partial_dq,a_partial_dv,a_partial_da);
    }

    Data::Matrix3x getCoMVelocityDerivatives_proxy(const Model & model,
                                                   Data & data)
    {
      typedef Data::Matrix3x Matrix3x;
      Matrix3x partial_dq(Matrix3x::Zero(3,model.nv));
      getCenterOfMassVelocityDerivatives(model,data,partial_dq);
      return partial_dq;
    }

    void exposeKinematicsDerivatives()
    {
      using namespace Eigen;

      bp::def("computeForwardKinematicsDerivatives",
              &computeForwardKinematicsDerivatives<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd,VectorXd>,
              bp::args("model","data","q","v","a"),
              "Computes all the terms required to compute the derivatives of the placement, spatial velocity and acceleration\n"
              "for any joint of the model.\n"
              "The results are stored in data.\n\n"
              "Parameters:\n"
              "\tmodel: model of the kinematic tree\n"
              "\tdata: data related to the model\n"
              "\tq: the joint configuration vector (size model.nq)\n"
              "\tv: the joint velocity vector (size model.nv)\n"
              "\ta: the joint acceleration vector (size model.nv)\n");

      bp::def("getJointVelocityDerivatives",
              getJointVelocityDerivatives_proxy,
              bp::args("model","data","joint_id","reference_frame"),
              "Computes the partial derivatives of the spatial velocity of a given joint with respect to\n"
              "the joint configuration and velocity and returns them as a tuple.\n"
              "The Jacobians can be either expressed in the LOCAL frame of the joint, in the LOCAL_WORLD_ALIGNED frame or in the WORLD coordinate frame depending on the value of reference_frame.\n"
              "You must first call computeForwardKinematicsDerivatives before calling this function.\n\n"
              "Parameters:\n"
              "\tmodel: model of the kinematic tree\n"
              "\tdata: data related to the model\n"
              "\tjoint_id: index of the joint\n"
              "\treference_frame: reference frame in which the resulting derivatives are expressed\n");

      bp::def("getJointAccelerationDerivatives",
              getJointAccelerationDerivatives_proxy,
              bp::args("model","data","joint_id","reference_frame"),
              "Computes the partial derivatives of the spatial acceleration of a given joint with respect to\n"
              "the joint configuration, velocity and acceleration and returns them as a tuple.\n"
              "The Jacobians can be either expressed in the LOCAL frame of the joint, in the LOCAL_WORLD_ALIGNED frame or in the WORLD coordinate frame depending on the value of reference_frame.\n"
              "You must first call computeForwardKinematicsDerivatives before calling this function.\n\n"
              "Parameters:\n"
              "\tmodel: model of the kinematic tree\n"
              "\tdata: data related to the model\n"
              "\tjoint_id: index of the joint\n"
              "\treference_frame: reference frame in which the resulting derivatives are expressed\n");

      bp::def("getCenterOfMassVelocityDerivatives",
              getCoMVelocityDerivatives_proxy,
              bp::args("model","data"),
              "Computes the partial derivaties of the center of mass velocity with respect to\n"
              "the joint configuration.\n"
              "You must first call computeForwardKinematicsDerivatives and centerOfMass(q,v) "
              "before calling this function.\n\n"
              "Parameters:\n"
              "\tmodel: model of the kinematic tree\n"
              "\tdata: data related to the model\n");

    }

  } // namespace python
} // namespace pinocchio


/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2020 Scientific Computing and Imaging Institute,
   University of Utah.

   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.
*/

///@author
///   David Weinstein
///   Department of Computer Science
///   University of Utah
///@date  March 2001

#include <Dataflow/Network/Module.h>
#include <Dataflow/Network/Ports/FieldPort.h>
#include <Dataflow/Network/Ports/MatrixPort.h>
#include <Core/Algorithms/Geometry/CoregPts.h>
#include <Core/Datatypes/DenseMatrix.h>
#include <Core/Datatypes/Field.h>
#include <Core/Datatypes/FieldInformation.h>
#include <Dataflow/GuiInterface/GuiVar.h>
#include <iostream>
#include <boost/scoped_ptr.hpp>

namespace SCIRun {

/// @class CoregisterPointClouds
/// @brief CoregisterPointClouds the first three points of two PointCloudFields

class CoregisterPointClouds : public Module
{
  public:
    CoregisterPointClouds(GuiContext* ctx);
    virtual ~CoregisterPointClouds();
    virtual void execute();
    void tcl_command( GuiArgs&, void * );
  private:
    GuiInt allowScale_;
    GuiInt allowRotate_;
    GuiInt allowTranslate_;
    GuiInt seed_;
    GuiInt iters_;
    GuiDouble misfitTol_;
    GuiString method_;
    int abort_;
    boost::shared_ptr<MusilRNG> mr_;
};

DECLARE_MAKER(CoregisterPointClouds)

CoregisterPointClouds::CoregisterPointClouds(GuiContext* ctx)
  : Module("CoregisterPointClouds", ctx, Filter, "MiscField", "SCIRun"),
    allowScale_(get_ctx()->subVar("allowScale"), 1),
    allowRotate_(get_ctx()->subVar("allowRotate"), 1),
    allowTranslate_(get_ctx()->subVar("allowTranslate"), 1),
    seed_(get_ctx()->subVar("seed"), 1),
    iters_(get_ctx()->subVar("iters"), 1000),
    misfitTol_(get_ctx()->subVar("misfitTol"), 0.001),
    method_(get_ctx()->subVar("method"))
{
}

CoregisterPointClouds::~CoregisterPointClouds()
{
}

void
CoregisterPointClouds::execute()
{
  FieldHandle fixedH, mobileH;
  VMesh::Node::size_type nnodes;

  get_input_handle("Fixed PointCloudField", fixedH, true);

  FieldInformation ffi(fixedH);
  if (!(ffi.is_pointcloudmesh()))
  {
    error("input is not a pointcloud");
    return;
  }

  VField* fixedPC = fixedH->vfield();
  VMesh* fixedM = fixedH->vmesh();

  get_input_handle("Mobile PointCloudField", mobileH, true);

  FieldInformation mfi(mobileH);
  if (!(mfi.is_pointcloudmesh()))
  {
    error("input is not a pointcloud");
    return;
  }

  VField* mobilePC = mobileH->vfield();
  VMesh* mobileM = mobileH->vmesh();

  update_state(Executing);

  fixedM->size(nnodes);
  if (nnodes < 3)
  {
    error("Fixed PointCloudField needs at least 3 input points.");
    return;
  }

  mobileM->size(nnodes);
  if (nnodes < 3)
  {
    error("Mobile PointCloudField needs at least 3 input points.");
    return;
  }

  std::vector<Point> fixedPts, mobilePts;
  Transform trans;

  VMesh::Node::iterator fni, fne, mni, mne;
  fixedM->begin(fni); fixedM->end(fne);
  mobileM->begin(mni); mobileM->end(mne);
  Point p;
  while (fni != fne)
  {
    fixedM->get_center(p, *fni);
    fixedPts.push_back(p);
    ++fni;
  }
  while (mni != mne)
  {
    mobileM->get_center(p, *mni);
    mobilePts.push_back(p);
    ++mni;
  }

  int allowScale = allowScale_.get();
  int allowRotate = allowRotate_.get();
  int allowTranslate = allowTranslate_.get();
  std::string method = method_.get();

  boost::scoped_ptr<CoregPts> coreg;
  if (method == "Analytic")
  {
    coreg.reset(new CoregPtsAnalytic(allowScale, allowRotate, allowTranslate));
  }
  else if (method == "Procrustes")
  {
    coreg.reset(new CoregPtsProcrustes(allowScale, allowRotate, allowTranslate));
  }
  else
  { // method == "Simplex"
    FieldHandle dfieldH;
    if (!get_input_handle("DistanceField From Fixed", dfieldH))
    {
      error("Simplex needs a distance field.");
      return;
    }

    VField* dfieldP = dfieldH->vfield();

    if (!dfieldP->is_scalar())
    {
      error("Simplex needs a distance field.");
      return;
    }

    int seed = seed_.get();
    seed_.set(seed+1);
    mr_.reset(new MusilRNG(seed));
    (*mr_)();
    abort_=0;
    coreg.reset(new CoregPtsSimplexSearch(iters_.get(), misfitTol_.get(), abort_, dfieldP,
                                      *mr_, allowScale, allowRotate, allowTranslate));
  }
  coreg->setOrigPtsA(mobilePts);
  coreg->setOrigPtsP(fixedPts);
  coreg->getTrans(trans);
  double misfit;
  coreg->getMisfit(misfit);

  MatrixHandle dm(new DenseMatrix(trans));
  send_output_handle("Transform", dm);
}

/// Commands invoked from the Gui.  Pause/unpause/stop the search.

void
CoregisterPointClouds::tcl_command(GuiArgs& args, void* userdata)
{
  if (args[1] == "stop")
  {
    abort_ = 1;
  }
  else
  {
    Module::tcl_command(args, userdata);
  }
}
} // End namespace SCIRun


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_PRINT_SIZE_INCLUDE
#define MTL_PRINT_SIZE_INCLUDE

#include <sstream>

#include <boost/numeric/mtl/operation/look_at_each_nonzero.hpp>

namespace mtl {

    struct print_size_max
    {
	print_size_max() : max(0) {}

	template <typename T>
	void operator()(const T& x)
	{
	    std::ostringstream st;
	    st << x;
	    std::size_t s= st.str().size();
	    // std::cout << "Size is " << s << ": " << st.str() << '\n';
	    if (s > max) max= s;
	}

	std::size_t max;
    };

    namespace mat {

	template <typename Matrix>
	std::size_t inline print_size(const Matrix& A)
	{
	    print_size_max p;
	    // look_at_each_nonzero(A, p); // Doesn't work on all expressions yet
	    for (size_t r = 0, nr = num_rows(A); r < nr; ++r)
		for (size_t c = 0, nc= num_cols(A); c < nc; ++c)
		    p(A(r, c));
	    return p.max;
	}
    }

} // namespace mtl

#endif // MTL_PRINT_SIZE_INCLUDE


#ifndef INCLUDE_SWIFT_VIO_EXTRINSIC_MODELS_HPP_
#define INCLUDE_SWIFT_VIO_EXTRINSIC_MODELS_HPP_

#include <Eigen/Core>

#include <ceres/local_parameterization.h>

#include <swift_vio/ceres/JacobianHelpers.hpp>
#include <okvis/ModelSwitch.hpp>

#include <okvis/ceres/PoseLocalParameterization.hpp>
#include <okvis/kinematics/Transformation.hpp>
#include <okvis/kinematics/sophus_operators.hpp>

namespace swift_vio {
class Extrinsic_p_CB final : public ::ceres::LocalParameterization {
  // of T_BC only p_CB is variable.
 public:
  static const int kModelId = 1;
  static const size_t kNumParams = 3;
  static const size_t kGlobalDim = 7;
  static inline int getMinimalDim() { return kNumParams; }
  static inline Eigen::MatrixXd initCov(double sigma_translation,
                                        double /*sigma_orientation*/) {
    return Eigen::MatrixXd::Identity(3, 3) *
           (sigma_translation * sigma_translation);
  }

  static void dT_BC_dExtrinsic(const Eigen::Matrix3d& R_BC,
                               Eigen::Matrix<double, 6, kNumParams>* j) {
    j->topRows<3>() = -R_BC;
    j->bottomRows<3>().setZero();
  }

  static void dpC_dExtrinsic_AIDP(const Eigen::Vector3d& /*pC*/,
                             const Eigen::Matrix3d& /*R_CB*/,
                             Eigen::MatrixXd* dpC_dT,
                             const Eigen::Matrix3d* R_CfCa,
                             const Eigen::Vector4d* ab1rho) {
    *dpC_dT = (*ab1rho)[3] * (Eigen::Matrix3d::Identity() - (*R_CfCa));
  }

  static void dpC_dExtrinsic_HPP(const Eigen::Vector4d& hpC,
                             const Eigen::Matrix3d& /*R_CB*/,
                             Eigen::MatrixXd* dpC_dT) {
      *dpC_dT = Eigen::Matrix3d::Identity() * hpC[3];
  }

  static void dhC_dExtrinsic_HPP(const Eigen::Matrix<double, 4, 1>& hpC,
                 const Eigen::Matrix<double, 3, 3>& /*R_CB*/,
                 Eigen::Matrix<double, 4, kNumParams>* dhC_deltaTBC) {
    dhC_deltaTBC->topLeftCorner<3, 3>() = Eigen::Matrix3d::Identity() * hpC[3];
    dhC_deltaTBC->row(3).setZero();
  }

  // the error state is $\delta p_B^C$ or $\delta p_S^C$
  static void updateState(const Eigen::Vector3d& r, const Eigen::Quaterniond& q,
                          const Eigen::VectorXd& delta,
                          Eigen::Vector3d* r_delta,
                          Eigen::Quaterniond* q_delta) {
    *r_delta = r - q * delta;
    *q_delta = q;
  }

  template <typename Scalar>
  static void oplus(
      const Scalar* const deltaT_BC,
      std::pair<Eigen::Matrix<Scalar, 3, 1>, Eigen::Quaternion<Scalar>>* T_BC) {
    Eigen::Map<const Eigen::Matrix<Scalar, 3, 1>> delta_t(deltaT_BC);
    T_BC->first -= T_BC->second * delta_t;
  }

  template <typename Scalar>
  static void ominus(const Scalar* T_BC,
                     const Scalar* T_BC_plus,
                     Scalar* delta) {
    Eigen::Map<const Eigen::Quaternion<Scalar>> q(T_BC + 3);
    Eigen::Map<Eigen::Vector3d> deltaVec(delta);
    deltaVec = q.conjugate() * (Eigen::Map<const Eigen::Vector3d>(T_BC) -
                                Eigen::Map<const Eigen::Vector3d>(T_BC_plus));
  }

  static void toDimensionLabels(std::vector<std::string>* extrinsicLabels) {
    *extrinsicLabels = {"p_CS_C_x[m]", "p_CS_C_y", "p_CS_C_z"};
  }

  static void toMinDimensionLabels(std::vector<std::string>* extrinsicLabels) {
    *extrinsicLabels = {"p_CS_C_x[m]", "p_CS_C_y", "p_CS_C_z"};
  }

  static void toDesiredStdevs(Eigen::VectorXd* desiredStdevs) {
    desiredStdevs->resize(3);
    desiredStdevs->setConstant(0.04);
  }

  static void toParamValues(
      const okvis::kinematics::Transformation& T_BC,
      Eigen::VectorXd* extrinsic_opt_coeffs) {
    *extrinsic_opt_coeffs = T_BC.q().conjugate() * (-T_BC.r());
  }

  template <class Scalar>
  static std::pair<Eigen::Matrix<Scalar, 3, 1>, Eigen::Quaternion<Scalar>> get_T_BC(
      const okvis::kinematics::Transformation& T_BC_base,
      const Scalar* parameters) {
    Eigen::Matrix<Scalar, 3, 1> t_CB_C(parameters[0], parameters[1], parameters[2]);
    Eigen::Quaternion<Scalar> q_BC = T_BC_base.q().cast<Scalar>();
    return std::make_pair(q_BC * (-t_CB_C), q_BC);
  }

  /// \brief Computes the Jacobian from minimal space to naively overparameterised space as used by ceres.
  /// @param[in] x Variable.
  /// @param[out] jacobian the Jacobian (dimension minDim x dim).
  /// \return True on success.
  static bool liftJacobian(const double* /*x*/, double* jacobian) {
    Eigen::Map<Eigen::Matrix<double, kNumParams, kGlobalDim, Eigen::RowMajor> > J_lift(jacobian);
    J_lift.setIdentity();
    return true;
  }

  bool Plus(const double *x, const double *delta, double *x_plus_delta) const final {
    // transform to okvis::kinematics framework
    std::pair<Eigen::Matrix<double, 3, 1>, Eigen::Quaternion<double>> T(
          Eigen::Vector3d(x[0], x[1], x[2]),
          Eigen::Quaterniond(x[6], x[3], x[4], x[5]));
    // call oplus operator in okvis::kinematis
    oplus(delta, &T);

    // copy back
    const Eigen::Vector3d& r = T.first;
    x_plus_delta[0] = r[0];
    x_plus_delta[1] = r[1];
    x_plus_delta[2] = r[2];
    return true;
  }

  bool ComputeJacobian(const double */*x*/, double *jacobian) const final {
    Eigen::Map<Eigen::Matrix<double, kGlobalDim, kNumParams, Eigen::RowMajor>> j(jacobian);
    j.setIdentity();
    return true;
  }

  int GlobalSize() const final { return kGlobalDim; }
  int LocalSize() const final { return kNumParams; }
};

// This is different from PoseLocalParameterization in liftJacobian() and
// ComputeJacobian() in that both are set to identity here.
class Extrinsic_p_BC_q_BC_base : public ::ceres::LocalParameterization {
  // T_BC is represented by p_BC and R_BC in the states.
 public:
  static const size_t kNumParams = 6;
  static const size_t kGlobalDim = 7;

  static inline int getMinimalDim() { return kNumParams; }

  static inline Eigen::MatrixXd initCov(double sigma_translation,
                                        double sigma_orientation) {
    Eigen::Matrix<double, 6, 6> cov = Eigen::Matrix<double, 6, 6>::Identity();
    cov.topLeftCorner<3, 3>() *= (sigma_translation * sigma_translation);
    cov.bottomRightCorner<3, 3>() *= (sigma_orientation * sigma_orientation);
    return cov;
  }

  static void updateState(const Eigen::Vector3d& r, const Eigen::Quaterniond& q,
                          const Eigen::VectorXd& delta,
                          Eigen::Vector3d* r_delta,
                          Eigen::Quaterniond* q_delta) {
    Eigen::Vector3d deltaAlpha = delta.segment<3>(3);
    *r_delta = r + delta.head<3>();
    *q_delta = okvis::kinematics::expAndTheta(deltaAlpha) * q;
    q_delta->normalize();
  }

  /**
   * X can be B or C0.
   */
  template <typename Scalar>
  static void oplus(
      const Scalar* const deltaT_XC,
      std::pair<Eigen::Matrix<Scalar, 3, 1>, Eigen::Quaternion<Scalar>>* T_XC) {
    Eigen::Map<const Eigen::Matrix<Scalar, 6, 1>> delta(deltaT_XC);
    T_XC->first += delta.template head<3>();
    Eigen::Matrix<Scalar, 3, 1> omega = delta.template tail<3>();
    Eigen::Quaternion<Scalar> dq = okvis::kinematics::expAndTheta(omega);
    T_XC->second = dq * T_XC->second;
  }

  /**
   * This implementation is preferred over okvis::ceres::
   * PoseLocalParameterization::minus(x, x_plus_delta, delta);
   */
  template <typename Scalar>
  static void ominus(const Scalar* T_XC,
                     const Scalar* T_XC_plus,
                     Scalar* delta) {
    delta[0] = T_XC_plus[0] - T_XC[0];
    delta[1] = T_XC_plus[1] - T_XC[1];
    delta[2] = T_XC_plus[2] - T_XC[2];
    const Eigen::Quaterniond q_plus(T_XC_plus[6], T_XC_plus[3],
                                           T_XC_plus[4], T_XC_plus[5]);
    const Eigen::Quaterniond q(T_XC[6], T_XC[3], T_XC[4], T_XC[5]);
    Eigen::Map<Eigen::Vector3d> delta_q(&delta[3]);
    double theta;
    delta_q = okvis::kinematics::logAndTheta(q_plus * q.inverse(), &theta);
  }

  /**
   * @brief toParamValues
   * @param T_XC
   * @param extrinsic_opt_coeffs
   */
  static void toParamValues(
      const okvis::kinematics::Transformation& T_XC,
      Eigen::VectorXd* extrinsic_opt_coeffs) {
    *extrinsic_opt_coeffs = T_XC.coeffs();
  }

  /// \brief Computes the Jacobian from minimal space to naively overparameterised space as used by ceres.
  ///     It must be the pseudo inverse of the local parametrization Jacobian as obtained by COmputeJacobian.
  /// @param[in] x Variable.
  /// @param[out] jacobian the Jacobian (dimension minDim x dim).
  /// \return True on success.
  static bool liftJacobian(const double* /*x*/, double* jacobian) {
    Eigen::Map<Eigen::Matrix<double, kNumParams, kGlobalDim, Eigen::RowMajor> > J_lift(jacobian);
    J_lift.setIdentity();
    return true;
  }

  bool Plus(const double *x, const double *delta, double *x_plus_delta) const final {
    // transform to okvis::kinematics framework
    std::pair<Eigen::Matrix<double, 3, 1>, Eigen::Quaternion<double>> T(
          Eigen::Vector3d(x[0], x[1], x[2]),
          Eigen::Quaterniond(x[6], x[3], x[4], x[5]));
    // call oplus operator in okvis::kinematis
    oplus(delta, &T);

    // copy back
    const Eigen::Vector3d& r = T.first;
    x_plus_delta[0] = r[0];
    x_plus_delta[1] = r[1];
    x_plus_delta[2] = r[2];
    const Eigen::Vector4d& q = T.second.coeffs();
    x_plus_delta[3] = q[0];
    x_plus_delta[4] = q[1];
    x_plus_delta[5] = q[2];
    x_plus_delta[6] = q[3];
    return true;
  }

  bool ComputeJacobian(const double */*x*/, double *jacobian) const final {
    Eigen::Map<Eigen::Matrix<double, kGlobalDim, kNumParams, Eigen::RowMajor>> j(jacobian);
    j.setIdentity();
    return true;
  }

  int GlobalSize() const final { return kGlobalDim; }
  int LocalSize() const final { return kNumParams; }
};

class Extrinsic_p_BC_q_BC final : public Extrinsic_p_BC_q_BC_base {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  static const int kModelId = 2;

  static void toDimensionLabels(std::vector<std::string>* extrinsicLabels) {
    *extrinsicLabels = {"p_SC_S_x[m]", "p_SC_S_y", "p_SC_S_z", "q_SC_x", "q_SC_y", "q_SC_z", "q_SC_w"};
  }

  static void toMinDimensionLabels(std::vector<std::string>* extrinsicLabels) {
    *extrinsicLabels = {"p_SC_S_x[m]", "p_SC_S_y", "p_SC_S_z", "theta_SC_x", "theta_SC_y", "theta_SC_z"};
  }

  static void toDesiredStdevs(Eigen::VectorXd* desiredStdevs) {
    desiredStdevs->resize(6);
    desiredStdevs->head<3>().setConstant(0.04);
    desiredStdevs->tail<3>().setConstant(0.01);
  }

  template <class Scalar>
  static std::pair<Eigen::Matrix<Scalar, 3, 1>, Eigen::Quaternion<Scalar>> get_T_BC(
      const okvis::kinematics::Transformation& /*T_BC_base*/,
      const Scalar* parameters) {
    Eigen::Matrix<Scalar, 3, 1> t_BC_B(parameters[0], parameters[1], parameters[2]);
    Eigen::Quaternion<Scalar> q_BC(parameters[6], parameters[3], parameters[4],
                                   parameters[5]);
    return std::make_pair(t_BC_B, q_BC);
  }

  static void dT_BC_dExtrinsic(
      Eigen::Matrix<double, 6, Extrinsic_p_BC_q_BC::kNumParams>* j) {
    j->setIdentity();
  }

  /**
   * @brief dpC_dExtrinsic_AIDP anchored inverse depth
   * @param pC pC = (T_BC^{-1} * T_WB_{f_i}^{-1} * T_WB_a * T_BC * [a, b, 1, \rho]^T)_{1:3}
   *     R_BC = exp(\delta\theta) \hat{R}_BC
   *     t_BC = \delta t + \hat{t}_BC
   * @param R_CB
   * @param dpC_dT
   * @param R_CfCa
   * @param ab1rho
   */
  static void dpC_dExtrinsic_AIDP(const Eigen::Vector3d& pC,
                                  const Eigen::Matrix3d& R_CB,
                                  Eigen::MatrixXd* dpC_dT,
                                  const Eigen::Matrix3d* R_CfCa,
                                  const Eigen::Vector4d* ab1rho) {
    dpC_dT->resize(3, 6);
    dpC_dT->block<3, 3>(0, 0) =
        ((*R_CfCa) - Eigen::Matrix3d::Identity()) * R_CB * (*ab1rho)[3];
    dpC_dT->block<3, 3>(0, 3) =
        (okvis::kinematics::crossMx(pC) -
         (*R_CfCa) * okvis::kinematics::crossMx(ab1rho->head<3>())) *
        R_CB;
  }

  /**
   * @brief dpC_dExtrinsic_HPP homogeneous point
   * @param hpC hpC = T_BC^{-1} * T_WB_{f_i}^{-1} * [x,y,z,w]_W^T
   *     hpC = [pC, w]
   *     R_BC = exp(\delta\theta) \hat{R}_BC
   *     t_BC = \delta t + \hat{t}_BC
   * @param R_CB
   * @param dpC_dT 3x6
   */
  static void dpC_dExtrinsic_HPP(const Eigen::Vector4d& hpC,
                                 const Eigen::Matrix3d& R_CB,
                                 Eigen::MatrixXd* dpC_dT) {
    dpC_dT->resize(3, 6);
    dpC_dT->block<3, 3>(0, 0) = -R_CB * hpC[3];
    dpC_dT->block<3, 3>(0, 3) = okvis::kinematics::crossMx(hpC.head<3>()) * R_CB;
  }

  // see dpC_dExtrinsic_HPP
  static void dhC_dExtrinsic_HPP(const Eigen::Matrix<double, 4, 1>& hpC,
                 const Eigen::Matrix<double, 3, 3>& R_CB,
                 Eigen::Matrix<double, 4, kNumParams>* dhC_deltaTBC) {
    dhC_deltaTBC->block<3, 3>(0, 0) = -R_CB * hpC[3];
    dhC_deltaTBC->block<3, 3>(0, 3) = okvis::kinematics::crossMx(hpC.head<3>()) * R_CB;
    dhC_deltaTBC->row(3).setZero();
  }
};

class Extrinsic_p_C0C_q_C0C final : public Extrinsic_p_BC_q_BC_base {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  static const int kModelId = 3;

  static void toDimensionLabels(std::vector<std::string>* extrinsicLabels) {
    *extrinsicLabels = {"p_C0C_C0_x[m]", "p_C0C_C0_y", "p_C0C_C0_z", "q_C0C_x", "q_C0C_y", "q_C0C_z", "q_C0C_w"};
  }

  static void toMinDimensionLabels(std::vector<std::string>* extrinsicLabels) {
    *extrinsicLabels = {"p_C0C_C0_x[m]", "p_C0C_C0_y", "p_C0C_C0_z", "theta_C0C_x", "theta_C0C_y", "theta_C0C_z"};
  }

  static void toDesiredStdevs(Eigen::VectorXd* desiredStdevs) {
    desiredStdevs->resize(6);
    desiredStdevs->head<3>().setConstant(0.04);
    desiredStdevs->tail<3>().setConstant(0.01);
  }

  static void dT_BC_dExtrinsic(const okvis::kinematics::Transformation& T_BCi,
                               const okvis::kinematics::Transformation& T_BC0,
                               Eigen::Matrix<double, 6, kNumParams>* j_C0Ci,
                               Eigen::Matrix<double, 6, kNumParams>* j_BC0);

  static void dpC_dExtrinsic_AIDP(const Eigen::Vector3d& /*pC*/,
                                  const Eigen::Matrix3d& /*R_CB*/,
                                  Eigen::MatrixXd* /*dpC_dT*/,
                                  const Eigen::Matrix3d* /*R_CfCa*/,
                                  const Eigen::Vector4d* /*ab1rho*/) {
    throw std::runtime_error("Method not implemented!");
  }

  static void dpC_dExtrinsic_HPP(const Eigen::Vector4d& /*hpC*/,
                                 const Eigen::Matrix3d& /*R_CB*/,
                                 Eigen::MatrixXd* /*dpC_dT*/) {
    throw std::runtime_error("Method not implemented!");
  }
};

#ifndef EXTRINSIC_MODEL_CASES
#define EXTRINSIC_MODEL_CASES               \
  EXTRINSIC_MODEL_CASE(Extrinsic_p_CB)      \
  EXTRINSIC_MODEL_CASE(Extrinsic_p_BC_q_BC) \
  EXTRINSIC_MODEL_CASE(Extrinsic_p_C0C_q_C0C)
#endif

inline int ExtrinsicModelGetMinimalDim(int model_id) {
  switch (model_id) {
#define MODEL_CASES EXTRINSIC_MODEL_CASES
#define EXTRINSIC_MODEL_CASE(ExtrinsicModel) \
  case ExtrinsicModel::kModelId:             \
    return ExtrinsicModel::getMinimalDim();

    MODEL_SWITCH_CASES

#undef EXTRINSIC_MODEL_CASE
#undef MODEL_CASES
  }
  return 0;
}

inline Eigen::MatrixXd ExtrinsicModelInitCov(int model_id,
                                             double sigma_translation,
                                             double sigma_orientation) {
  switch (model_id) {
#define MODEL_CASES EXTRINSIC_MODEL_CASES
#define EXTRINSIC_MODEL_CASE(ExtrinsicModel) \
  case ExtrinsicModel::kModelId:             \
    return ExtrinsicModel::initCov(sigma_translation, sigma_orientation);

    MODEL_SWITCH_CASES

#undef EXTRINSIC_MODEL_CASE
#undef MODEL_CASES
  }
}

inline int ExtrinsicModelNameToId(std::string extrinsic_opt_rep, bool* isFixed = nullptr) {
  std::transform(extrinsic_opt_rep.begin(), extrinsic_opt_rep.end(),
                 extrinsic_opt_rep.begin(),
                 [](unsigned char c) { return std::toupper(c); });
  if (extrinsic_opt_rep.compare("P_BC_Q_BC") == 0) {
    if (isFixed) {
      *isFixed = false;
    }
    return Extrinsic_p_BC_q_BC::kModelId;
  } else if (extrinsic_opt_rep.compare("P_C0C_Q_C0C") == 0) {
    if (isFixed) {
      *isFixed = false;
    }
    return Extrinsic_p_C0C_q_C0C::kModelId;
  } else if (extrinsic_opt_rep.compare("P_CB") == 0) {
    if (isFixed) {
      *isFixed = false;
    }
    return Extrinsic_p_CB::kModelId;
  } else {
    if (isFixed) {
      *isFixed = true;
    }
    return Extrinsic_p_BC_q_BC::kModelId;
  }
}

inline void ExtrinsicModelUpdateState(int model_id, const Eigen::Vector3d& r,
                                      const Eigen::Quaterniond& q,
                                      const Eigen::VectorXd& delta,
                                      Eigen::Vector3d* r_delta,
                                      Eigen::Quaterniond* q_delta) {
  switch (model_id) {
#define MODEL_CASES EXTRINSIC_MODEL_CASES
#define EXTRINSIC_MODEL_CASE(ExtrinsicModel) \
  case ExtrinsicModel::kModelId:             \
    return ExtrinsicModel::updateState(r, q, delta, r_delta, q_delta);

    MODEL_SWITCH_CASES

#undef EXTRINSIC_MODEL_CASE
#undef MODEL_CASES
  }
}

inline void ExtrinsicModelOplus(int model_id, const double* const deltaT_XC,
                                okvis::kinematics::Transformation* T_XC) {
  std::pair<Eigen::Matrix<double, 3, 1>, Eigen::Quaternion<double>>
      pair_T_XC = std::make_pair(T_XC->r(), T_XC->q());
  switch (model_id) {
#define MODEL_CASES EXTRINSIC_MODEL_CASES
#define EXTRINSIC_MODEL_CASE(ExtrinsicModel)                          \
  case ExtrinsicModel::kModelId:                                      \
    ExtrinsicModel::oplus(deltaT_XC, &pair_T_XC);                     \
    T_XC->set(pair_T_XC.first, pair_T_XC.second);                     \
    break;

    MODEL_SWITCH_CASES

#undef EXTRINSIC_MODEL_CASE
#undef MODEL_CASES
  }
}

inline void ExtrinsicModelOminus(int model_id, const double* rq,
                                   const double* rq_delta,
                                   double* delta) {
  switch (model_id) {
#define MODEL_CASES EXTRINSIC_MODEL_CASES
#define EXTRINSIC_MODEL_CASE(ExtrinsicModel) \
  case ExtrinsicModel::kModelId:             \
    return ExtrinsicModel::ominus(rq, rq_delta, delta);

    MODEL_SWITCH_CASES

#undef EXTRINSIC_MODEL_CASE
#undef MODEL_CASES
  }
}

inline void ExtrinsicModel_dpC_dExtrinsic_AIDP(int model_id, const Eigen::Vector3d& pC,
                                               const Eigen::Matrix3d& R_CB,
                                               Eigen::MatrixXd* dpC_dT,
                                               const Eigen::Matrix3d* R_CfCa,
                                               const Eigen::Vector4d* ab1rho) {
  switch (model_id) {
#define MODEL_CASES EXTRINSIC_MODEL_CASES
#define EXTRINSIC_MODEL_CASE(ExtrinsicModel) \
  case ExtrinsicModel::kModelId:             \
    return ExtrinsicModel::dpC_dExtrinsic_AIDP(pC, R_CB, dpC_dT, R_CfCa, ab1rho);

    MODEL_SWITCH_CASES

#undef EXTRINSIC_MODEL_CASE
#undef MODEL_CASES
  }
}

inline void ExtrinsicModel_dpC_dExtrinsic_HPP(int model_id, const Eigen::Vector4d& hpC,
                                              const Eigen::Matrix3d& R_CB,
                                              Eigen::MatrixXd* dpC_dT) {
  switch (model_id) {
#define MODEL_CASES EXTRINSIC_MODEL_CASES
#define EXTRINSIC_MODEL_CASE(ExtrinsicModel) \
  case ExtrinsicModel::kModelId:             \
    return ExtrinsicModel::dpC_dExtrinsic_HPP(hpC, R_CB, dpC_dT);

    MODEL_SWITCH_CASES

#undef EXTRINSIC_MODEL_CASE
#undef MODEL_CASES
  }
}

inline void ExtrinsicModelToDimensionLabels(
    int model_id, std::vector<std::string>* dimensionLabels) {
    switch (model_id) {
  #define MODEL_CASES EXTRINSIC_MODEL_CASES
  #define EXTRINSIC_MODEL_CASE(ExtrinsicModel) \
    case ExtrinsicModel::kModelId:             \
      return ExtrinsicModel::toDimensionLabels(dimensionLabels);

      MODEL_SWITCH_CASES

  #undef EXTRINSIC_MODEL_CASE
  #undef MODEL_CASES
    }
}

inline void ExtrinsicModelToMinDimensionLabels(
    int model_id, std::vector<std::string>* dimensionLabels) {
    switch (model_id) {
  #define MODEL_CASES EXTRINSIC_MODEL_CASES
  #define EXTRINSIC_MODEL_CASE(ExtrinsicModel) \
    case ExtrinsicModel::kModelId:             \
      return ExtrinsicModel::toMinDimensionLabels(dimensionLabels);

      MODEL_SWITCH_CASES

  #undef EXTRINSIC_MODEL_CASE
  #undef MODEL_CASES
    }
}

inline void ExtrinsicModelToParamValues(
    int model_id, const okvis::kinematics::Transformation& T_XC,
    Eigen::VectorXd* extrinsic_opt_coeffs) {
  switch (model_id) {
#define MODEL_CASES EXTRINSIC_MODEL_CASES
#define EXTRINSIC_MODEL_CASE(ExtrinsicModel)                    \
  case ExtrinsicModel::kModelId:                                \
    return ExtrinsicModel::toParamValues(                       \
        T_XC, extrinsic_opt_coeffs);

    MODEL_SWITCH_CASES

#undef EXTRINSIC_MODEL_CASE
#undef MODEL_CASES
  }
}

inline void ExtrinsicModelToDesiredStdevs(
    int model_id, Eigen::VectorXd* desiredStdevs) {
  switch (model_id) {
#define MODEL_CASES EXTRINSIC_MODEL_CASES
#define EXTRINSIC_MODEL_CASE(ExtrinsicModel)                    \
  case ExtrinsicModel::kModelId:                                \
    return ExtrinsicModel::toDesiredStdevs(desiredStdevs);

    MODEL_SWITCH_CASES

#undef EXTRINSIC_MODEL_CASE
#undef MODEL_CASES
  }
}
}  // namespace swift_vio
#endif  // INCLUDE_SWIFT_VIO_EXTRINSIC_MODELS_HPP_


#include "../agg.hpp"
#include <boost/accumulators/statistics/kurtosis.hpp>

ARRAY_AGGREGATE_FNC(akurtosis, tag::kurtosis);
//SQL_AGGREGATE_DBL_ONLY_FNC(v_kurtosis, tag::kurtosis);