/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 *  @file   testGaussianFactorGraphUnordered.cpp
 *  @brief  Unit tests for Linear Factor Graph
 *  @author Christian Potthast
 *  @author Frank Dellaert
 *  @author Luca Carlone
 *  @author Richard Roberts
 **/

#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;

#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>

#include <gtsam/base/debug.h>
#include <gtsam/base/VerticalBlockMatrix.h>
#include <gtsam/inference/VariableSlots.h>
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianBayesNet.h>

using namespace std;
using namespace gtsam;

//static SharedDiagonal
//  sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1), sigma_02 = noiseModel::Isotropic::Sigma(2,0.2),
//  constraintModel = noiseModel::Constrained::All(2);

/* ************************************************************************* */
TEST(GaussianFactorGraph, initialization) {
  // Create empty graph
  GaussianFactorGraph fg;
  SharedDiagonal unit2 = noiseModel::Unit::Create(2);

  fg +=
    JacobianFactor(0, 10*eye(2), -1.0*ones(2), unit2),
    JacobianFactor(0, -10*eye(2),1, 10*eye(2), (Vector(2) << 2.0, -1.0), unit2),
    JacobianFactor(0, -5*eye(2), 2, 5*eye(2), (Vector(2) << 0.0, 1.0), unit2),
    JacobianFactor(1, -5*eye(2), 2, 5*eye(2), (Vector(2) << -1.0, 1.5), unit2);

  EXPECT_LONGS_EQUAL(4, (long)fg.size());

  // Test sparse, which takes a vector and returns a matrix, used in MATLAB
  // Note that this the augmented vector and the RHS is in column 7
  Matrix expectedIJS = (Matrix(3, 22) <<
          1.,   2.,  1.,  2.,     3.,   4.,   3.,   4.,  3.,  4.,    5.,  6., 5., 6., 5., 6.,    7.,  8., 7., 8.,  7., 8.,
          1.,   2.,  7.,  7.,     1.,   2.,   3.,   4.,  7.,  7.,    1.,  2., 5., 6., 7., 7.,    3.,  4., 5., 6.,  7., 7.,
          10., 10., -1., -1.,   -10., -10.,  10.,  10.,  2., -1.,   -5., -5., 5., 5., 0., 1.,   -5., -5., 5., 5., -1., 1.5
  );
  Matrix actualIJS = fg.sparseJacobian_();
  EQUALITY(expectedIJS, actualIJS);
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, sparseJacobian) {
  // Create factor graph:
  // x1 x2 x3 x4 x5  b
  //  1  2  3  0  0  4
  //  5  6  7  0  0  8
  //  9 10  0 11 12 13
  //  0  0  0 14 15 16

  // Expected - NOTE that we transpose this!
  Matrix expectedT = (Matrix(16, 3) <<
      1., 1., 2.,
      1., 2., 4.,
      1., 3., 6.,
      2., 1.,10.,
      2., 2.,12.,
      2., 3.,14.,
      1., 6., 8.,
      2., 6.,16.,
      3., 1.,18.,
      3., 2.,20.,
      3., 4.,22.,
      3., 5.,24.,
      4., 4.,28.,
      4., 5.,30.,
      3., 6.,26.,
      4., 6.,32.);

  Matrix expected = expectedT.transpose();

  GaussianFactorGraph gfg;
  SharedDiagonal model = noiseModel::Isotropic::Sigma(2, 0.5);
  gfg.add(0, (Matrix(2, 3) << 1., 2., 3., 5., 6., 7.), (Vector(2) << 4., 8.), model);
  gfg.add(0, (Matrix(2, 3) << 9.,10., 0., 0., 0., 0.), 1, (Matrix(2, 2) << 11., 12., 14., 15.), (Vector(2) << 13.,16.), model);

  Matrix actual = gfg.sparseJacobian_();

  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, matrices) {
  // Create factor graph:
  // x1 x2 x3 x4 x5  b
  //  1  2  3  0  0  4
  //  5  6  7  0  0  8
  //  9 10  0 11 12 13
  //  0  0  0 14 15 16

  Matrix A00 = (Matrix(2, 3) << 1, 2, 3, 5, 6, 7);
  Matrix A10 = (Matrix(2, 3) << 9, 10, 0, 0, 0, 0);
  Matrix A11 = (Matrix(2, 2) << 11, 12, 14, 15);

  GaussianFactorGraph gfg;
  SharedDiagonal model = noiseModel::Unit::Create(2);
  gfg.add(0, A00, (Vector(2) << 4., 8.), model);
  gfg.add(0, A10, 1, A11, (Vector(2) << 13.,16.), model);

  Matrix Ab(4,6);
  Ab <<
      1, 2, 3, 0, 0, 4,
      5, 6, 7, 0, 0, 8,
      9,10, 0,11,12,13,
      0, 0, 0,14,15,16;

  // augmented versions
  EXPECT(assert_equal(Ab, gfg.augmentedJacobian()));
  EXPECT(assert_equal(Ab.transpose() * Ab, gfg.augmentedHessian()));

  // jacobian
  Matrix A = Ab.leftCols(Ab.cols()-1);
  Vector b = Ab.col(Ab.cols()-1);
  Matrix actualA; Vector actualb; boost::tie(actualA,actualb) = gfg.jacobian();
  EXPECT(assert_equal(A, actualA));
  EXPECT(assert_equal(b, actualb));

  // hessian
  Matrix L = A.transpose() * A;
  Vector eta = A.transpose() * b;
  Matrix actualL; Vector actualeta; boost::tie(actualL,actualeta) = gfg.hessian();
  EXPECT(assert_equal(L, actualL));
  EXPECT(assert_equal(eta, actualeta));

  // hessianBlockDiagonal
  VectorValues expectLdiagonal; // Make explicit that diagonal is sum-squares of columns
  expectLdiagonal.insert(0, (Vector(3) << 1+25+81, 4+36+100, 9+49));
  expectLdiagonal.insert(1, (Vector(2) << 121+196, 144+225));
  EXPECT(assert_equal(expectLdiagonal, gfg.hessianDiagonal()));

  // hessianBlockDiagonal
  map<Key,Matrix> actualBD = gfg.hessianBlockDiagonal();
  LONGS_EQUAL(2,actualBD.size());
  EXPECT(assert_equal(A00.transpose()*A00 + A10.transpose()*A10,actualBD[0]));
  EXPECT(assert_equal(A11.transpose()*A11,actualBD[1]));
}

/* ************************************************************************* */
static GaussianFactorGraph createSimpleGaussianFactorGraph() {
  GaussianFactorGraph fg;
  SharedDiagonal unit2 = noiseModel::Unit::Create(2);
  // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_]
  fg += JacobianFactor(2, 10*eye(2), -1.0*ones(2), unit2);
  // odometry between x1 and x2: x2-x1=[0.2;-0.1]
  fg += JacobianFactor(0, 10*eye(2), 2, -10*eye(2), (Vector(2) << 2.0, -1.0), unit2);
  // measurement between x1 and l1: l1-x1=[0.0;0.2]
  fg += JacobianFactor(1, 5*eye(2), 2, -5*eye(2), (Vector(2) << 0.0, 1.0), unit2);
  // measurement between x2 and l1: l1-x2=[-0.2;0.3]
  fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), (Vector(2) << -1.0, 1.5), unit2);
  return fg;
}

/* ************************************************************************* */
TEST( GaussianFactorGraph, gradient )
{
  GaussianFactorGraph fg = createSimpleGaussianFactorGraph();

  // Construct expected gradient
  // 2*f(x) = 100*(x1+c[X(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + 25*(l1-x2-[-0.2;0.3])^2
  // worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5]
  VectorValues expected = map_list_of<Key, Vector>
    (1, (Vector(2) << 5.0, -12.5))
    (2, (Vector(2) << 30.0, 5.0))
    (0, (Vector(2) << -25.0, 17.5));

  // Check the gradient at delta=0
  VectorValues zero = VectorValues::Zero(expected);
  VectorValues actual = fg.gradient(zero);
  EXPECT(assert_equal(expected, actual));
  EXPECT(assert_equal(expected, fg.gradientAtZero()));

  // Check the gradient at the solution (should be zero)
  VectorValues solution = fg.optimize();
  VectorValues actual2 = fg.gradient(solution);
  EXPECT(assert_equal(VectorValues::Zero(solution), actual2));
}

/* ************************************************************************* */
TEST( GaussianFactorGraph, transposeMultiplication )
{
  GaussianFactorGraph A = createSimpleGaussianFactorGraph();

  Errors e; e +=
    (Vector(2) <<  0.0, 0.0),
    (Vector(2) << 15.0, 0.0),
    (Vector(2) <<  0.0,-5.0),
    (Vector(2) << -7.5,-5.0);

  VectorValues expected;
  expected.insert(1, (Vector(2) <<  -37.5,-50.0));
  expected.insert(2, (Vector(2) << -150.0, 25.0));
  expected.insert(0, (Vector(2) <<  187.5, 25.0));

  VectorValues actual = A.transposeMultiply(e);
  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST(GaussianFactorGraph, eliminate_empty )
{
  // eliminate an empty factor
  GaussianFactorGraph gfg;
  gfg.add(JacobianFactor());
  GaussianBayesNet::shared_ptr actualBN;
  GaussianFactorGraph::shared_ptr remainingGFG;
  boost::tie(actualBN, remainingGFG) = gfg.eliminatePartialSequential(Ordering());

  // expected Bayes net is empty
  GaussianBayesNet expectedBN;

  // expected remaining graph should be the same as the original, still containing the empty factor
  GaussianFactorGraph expectedLF = gfg;

  // check if the result matches
  EXPECT(assert_equal(*actualBN, expectedBN));
  EXPECT(assert_equal(*remainingGFG, expectedLF));
}

/* ************************************************************************* */
TEST( GaussianFactorGraph, matrices2 )
{
  GaussianFactorGraph gfg = createSimpleGaussianFactorGraph();
  Matrix A; Vector b; boost::tie(A,b) = gfg.jacobian();
  Matrix AtA; Vector eta; boost::tie(AtA,eta) = gfg.hessian();
  EXPECT(assert_equal(A.transpose()*A, AtA));
  EXPECT(assert_equal(A.transpose()*b, eta));
}

/* ************************************************************************* */
TEST( GaussianFactorGraph, multiplyHessianAdd )
{
  GaussianFactorGraph gfg = createSimpleGaussianFactorGraph();

  VectorValues x = map_list_of<Key, Vector>
    (0, (Vector(2) << 1,2))
    (1, (Vector(2) << 3,4))
    (2, (Vector(2) << 5,6));

  VectorValues expected;
  expected.insert(0, (Vector(2) <<  -450, -450));
  expected.insert(1, (Vector(2) << 0, 0));
  expected.insert(2, (Vector(2) <<  950, 1050));

  VectorValues actual;
  gfg.multiplyHessianAdd(1.0, x, actual);
  EXPECT(assert_equal(expected, actual));

  // now, do it with non-zero y
  gfg.multiplyHessianAdd(1.0, x, actual);
  EXPECT(assert_equal(2*expected, actual));
}

/* ************************************************************************* */
static GaussianFactorGraph createGaussianFactorGraphWithHessianFactor() {
  GaussianFactorGraph gfg = createSimpleGaussianFactorGraph();
  gfg += HessianFactor(1, 2, 100*eye(2,2), zeros(2,2),   (Vector(2) << 0.0, 1.0),
                                           400*eye(2,2), (Vector(2) << 1.0, 1.0), 3.0);
  return gfg;
}

/* ************************************************************************* */
TEST( GaussianFactorGraph, multiplyHessianAdd2 )
{
  GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor();

  // brute force
  Matrix AtA; Vector eta; boost::tie(AtA,eta) = gfg.hessian();
  Vector X(6); X<<1,2,3,4,5,6;
  Vector Y(6); Y<<-450, -450, 300, 400, 2950, 3450;
  EXPECT(assert_equal(Y,AtA*X));

  VectorValues x = map_list_of<Key, Vector>
    (0, (Vector(2) << 1,2))
    (1, (Vector(2) << 3,4))
    (2, (Vector(2) << 5,6));

  VectorValues expected;
  expected.insert(0, (Vector(2) <<  -450, -450));
  expected.insert(1, (Vector(2) <<  300, 400));
  expected.insert(2, (Vector(2) << 2950, 3450));

  VectorValues actual;
  gfg.multiplyHessianAdd(1.0, x, actual);
  EXPECT(assert_equal(expected, actual));

  // now, do it with non-zero y
  gfg.multiplyHessianAdd(1.0, x, actual);
  EXPECT(assert_equal(2*expected, actual));
}

/* ************************************************************************* */
TEST( GaussianFactorGraph, multiplyHessianAdd3 )
{
  GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor();

  // brute force
  Matrix AtA; Vector eta; boost::tie(AtA,eta) = gfg.hessian();
  Vector X(6); X<<1,2,3,4,5,6;
  Vector Y(6); Y<<-450, -450, 300, 400, 2950, 3450;
  EXPECT(assert_equal(Y,AtA*X));

    double* x = &X[0];

    Vector fast_y = gtsam::zero(6);
    gfg.multiplyHessianAdd(1.0, x, fast_y.data());
    EXPECT(assert_equal(Y, fast_y));

    // now, do it with non-zero y
    gfg.multiplyHessianAdd(1.0, x, fast_y.data());
    EXPECT(assert_equal(2*Y, fast_y));

}

/* ************************************************************************* */
TEST( GaussianFactorGraph, matricesMixed )
{
  GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor();
  Matrix A; Vector b; boost::tie(A,b) = gfg.jacobian(); // incorrect !
  Matrix AtA; Vector eta; boost::tie(AtA,eta) = gfg.hessian(); // correct
  EXPECT(assert_equal(A.transpose()*A, AtA));
  Vector expected = - (Vector(6) << -25, 17.5, 5, -13.5, 29, 4);
  EXPECT(assert_equal(expected, eta));
  EXPECT(assert_equal(A.transpose()*b, eta));
}

/* ************************************************************************* */
TEST( GaussianFactorGraph, gradientAtZero )
{
  GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor();
  VectorValues expected;
  VectorValues actual = gfg.gradientAtZero();
  expected.insert(0, (Vector(2) << -25, 17.5));
  expected.insert(1, (Vector(2) << 5, -13.5));
  expected.insert(2, (Vector(2) << 29, 4));
  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST( GaussianFactorGraph, clone ) {
  // 2 variables, frontal has dim=4
  VerticalBlockMatrix blockMatrix(list_of(4)(2)(1), 4);
  blockMatrix.matrix() <<
      1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1,
      0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2,
      0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.3,
      0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4;
  GaussianConditional cg(list_of(1)(2), 1, blockMatrix);

  GaussianFactorGraph init_graph = createGaussianFactorGraphWithHessianFactor();
  init_graph.push_back(GaussianFactor::shared_ptr()); /// Add null factor
  init_graph.push_back(GaussianConditional(cg));

  GaussianFactorGraph exp_graph = createGaussianFactorGraphWithHessianFactor(); // Created separately
  exp_graph.push_back(GaussianFactor::shared_ptr()); /// Add null factor
  exp_graph.push_back(GaussianConditional(cg));

  GaussianFactorGraph actCloned = init_graph.clone();
  EXPECT(assert_equal(init_graph, actCloned)); // Same as the original version

  // Apply an in-place change to init_graph and compare
  JacobianFactor::shared_ptr jacFactor0 = boost::dynamic_pointer_cast<JacobianFactor>(init_graph.at(0));
  CHECK(jacFactor0);
  jacFactor0->getA(jacFactor0->begin()) *= 7.;
  EXPECT(assert_inequal(init_graph, exp_graph));
  EXPECT(assert_equal(exp_graph, actCloned));
}

/* ************************************************************************* */
TEST( GaussianFactorGraph, negate ) {
  GaussianFactorGraph init_graph = createGaussianFactorGraphWithHessianFactor();
  init_graph.push_back(GaussianFactor::shared_ptr()); /// Add null factor
  GaussianFactorGraph actNegation = init_graph.negate();
  GaussianFactorGraph expNegation;
  expNegation.push_back(init_graph.at(0)->negate());
  expNegation.push_back(init_graph.at(1)->negate());
  expNegation.push_back(init_graph.at(2)->negate());
  expNegation.push_back(init_graph.at(3)->negate());
  expNegation.push_back(init_graph.at(4)->negate());
  expNegation.push_back(GaussianFactor::shared_ptr());
  EXPECT(assert_equal(expNegation, actNegation));
}

/* ************************************************************************* */
TEST( GaussianFactorGraph, hessianDiagonal )
{
  GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor();
  VectorValues expected;
  Matrix infoMatrix = gfg.hessian().first;
  Vector d = infoMatrix.diagonal();

  VectorValues actual = gfg.hessianDiagonal();
  expected.insert(0, d.segment<2>(0));
  expected.insert(1, d.segment<2>(2));
  expected.insert(2, d.segment<2>(4));
  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */


/*
 * Copyright 2017-2020 CNRS INRIA
 */

#include <Eigen/Core>

#if EIGEN_VERSION_AT_LEAST(3,2,0)

#include "eigenpy/solvers/solvers.hpp"
#include "eigenpy/solvers/ConjugateGradient.hpp"

#if EIGEN_VERSION_AT_LEAST(3,3,5)
  #include "eigenpy/solvers/LeastSquaresConjugateGradient.hpp"
#endif

namespace eigenpy
{
  void exposeSolvers()
  {
    using namespace Eigen;
    ConjugateGradientVisitor< ConjugateGradient<MatrixXd,Lower|Upper> >::expose();
#if EIGEN_VERSION_AT_LEAST(3,3,5)
    LeastSquaresConjugateGradientVisitor< LeastSquaresConjugateGradient<MatrixXd, LeastSquareDiagonalPreconditioner<MatrixXd::Scalar> > >::expose();
#endif

    // Conjugate gradient with limited BFGS preconditioner
    ConjugateGradientVisitor< ConjugateGradient<MatrixXd,Lower|Upper,IdentityPreconditioner > >::expose("IdentityConjugateGradient");
//    ConjugateGradientVisitor< ConjugateGradient<MatrixXd,Lower|Upper,LimitedBFGSPreconditioner<double,Dynamic,Lower|Upper> > >::expose("LimitedBFGSConjugateGradient");
  }
} // namespace eigenpy

#endif


#include <elle/bytes.hh>

#include <cmath>

#include <boost/algorithm/string/case_conv.hpp>

#include <elle/err.hh>
#include <elle/printf.hh>
#include <elle/unreachable.hh>

namespace elle
{
  std::string
  human_data_size(double bytes, bool si)
  {
    // FIXME: use Boost instead.
    // http://www.boost.org/doc/libs/1_62_0/doc/html/boost_units/Examples.html#boost_units.Examples.autoscale
    constexpr const char* engineer[] =
      {"B", "kB", "MB", "GB", "TB", "EB", "ZB"};
    constexpr const char* computer[] =
      {"B", "KiB", "MiB", "GiB", "TiB", "EiB", "ZiB"};
    static_assert(sizeof engineer == sizeof computer,
                  "different scales");
    constexpr auto size = sizeof engineer / sizeof(*engineer);

    auto scale = si ? 1000. : 1024.;
    auto const& unit = si ? engineer : computer;
    for (uint64_t i = 0; i < size; ++i)
      if (i == size - 1 || bytes / std::pow(scale, i+1) < 1)
        return elle::sprintf("%.1f %s", bytes / pow(scale, i), unit[i]);
    elle::unreachable();
  }

  long double
  convert_capacity(long double value, std::string unit)
  {
    boost::to_lower(unit);
    if (unit == "b" || unit == "")
      return value;
    else if (unit == "kb")
      return value * 1000;
    else if (unit == "kib")
      return value * 1024;
    else if (unit == "mb")
      return value * 1000 * 1000;
    else if (unit == "mib")
      return value * 1024 * 1024;
    else if (unit == "gb")
      return value * 1000 * 1000 * 1000;
    else if (unit == "gib")
      return value * 1024 * 1024 * 1024;
    else if (unit == "tb")
      return value * 1000 * 1000 * 1000 * 1000;
    else if (unit == "tib")
      return value * 1024 * 1024 * 1024 * 1024;
    else
      elle::err("invalid capacity unit: %s", unit);
  }

  int64_t
  convert_capacity(std::string value)
  {
    boost::to_lower(value);
    std::string const unit = [&]
      {
        auto const to_find = std::vector<std::string>
        {
          // "b" MUST be the last element.
          "kb", "mb", "gb", "tb", "kib", "mib", "gib", "tib", "b"
        };
        const char* res = nullptr;
        for (auto const& t: to_find)
          if (res = std::strstr(value.c_str(), t.c_str()))
            break;
        return res ? res : "";
      }();
    auto const double_value =
      std::stold(value.substr(0, value.size() - unit.size()));
    return std::llround(convert_capacity(double_value, unit));
  }

  boost::optional<int64_t>
  convert_capacity(boost::optional<std::string> capacity)
  {
    return capacity
      ? convert_capacity(*capacity)
      : boost::optional<int64_t>{};
  }
}


#define TINYPLY_IMPLEMENTATION

#include <iostream>
#include <fstream>
#include <string>
#include <vector>

#include <Eigen/Dense>

#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>

#include "Filesystem.hpp"
#include "NumPyIO.hpp"

#include "stereo_utils.hpp"

using namespace stereo_utils;

void stereo_utils::convert_eigen_matrix_2_mat(
	const Eigen::MatrixXf &e,
	cv::Mat &m ) {
	cv::eigen2cv( e, m );
}

void stereo_utils::load_true_disparity(const std::string &fn, cv::Mat &trueDisp) {
	Eigen::MatrixXf eTrueDisp;
	read_npy_2_eigen_matrix(fn, eTrueDisp);
	convert_eigen_matrix_2_mat( eTrueDisp, trueDisp );
}

DiffStat stereo_utils::compute_diff_statistics(
	const cv::Mat &diff, const cv::Mat &mask ) {

	if ( 1 != diff.channels() ) {
		throw std::runtime_error("diff must be 1 channel. ");
	}

	float acc      = 0.f;
	int   n        = 0;

	DiffStat ds;
	ds.mean   = 0.f;
	ds.minVal = diff.cols * 2;
	ds.maxVal = 0.f;

	if ( !mask.empty() ) {
		assert( diff.rows == mask.rows );
		assert( diff.cols == mask.cols );

		for ( int i = 0; i < diff.rows; ++i ) {
			const float *pd   = diff.ptr<float>(i);
			const uint8_t *pm = mask.ptr<uint8_t>(i);
			for ( int j = 0; j < diff.cols; ++j ) {
				const uint8_t m = pm[j];
				if ( m > 0 ) {
					const float d = pd[j];

					if ( d > ds.maxVal ) {
						ds.maxVal = d;
					}

					if ( d < ds.minVal ) {
						ds.minVal = d;
					}

					acc += d;
					n++;
				}
			}
		}

		if ( n == 0 ) {
			n = 1;
		}

		ds.mean = acc / n;
	} else {
		for ( int i = 0; i < diff.rows; ++i ) {
			const float *pd   = diff.ptr<float>(i);
			for ( int j = 0; j < diff.cols; ++j ) {
				const float d = pd[j];

				if ( d > ds.maxVal ) {
					ds.maxVal = d;
				}

				if ( d < ds.minVal ) {
					ds.minVal = d;
				}

				acc += d;
				n++;
			}
		}

		if ( n == 0 ) {
			n = 1;
		}

		ds.mean = acc / n;
	}

	return ds;
}

void stereo_utils::compare_with_true_disparity(
	const cv::Mat &trueDisp,
	const cv::Mat &predDisp,
	cv::Mat &diff ) {

	cv::Mat maskFloat;
	cv::threshold( trueDisp, maskFloat, 0, 255, cv::THRESH_BINARY);

	cv::Mat mask;
	maskFloat.convertTo(mask, CV_8UC1);

	// std::cout << "mask.type() = " << mask.type() << std::endl;

	// It is assumed that the data type of trueDisp and
	// predDisp is floating point.
	diff = cv::abs( trueDisp - predDisp );

	DiffStat ds = compute_diff_statistics( diff, mask );

	std::cout << "diff mean = " << ds.mean << ", "
	          << "diff min = "  << ds.minVal << ", "
			  << "diff max = "  << ds.maxVal << ". \n";
}

void stereo_utils::save_float_image_self_normalize(
	const std::string fn,
	const cv::Mat &img ) {

	const int type = img.type() % 8;
	assert( type == 5 || type == 6 );
	assert( 1 == img.channels() );

	test_directory_by_filename(fn);

	// Normalize the image.
	double minVal, maxVal;
	cv::minMaxLoc(img, &minVal, &maxVal);

	cv::Mat normalized = (img - minVal) / ( maxVal - minVal ) * 255;

	cv::imwrite(fn, normalized);
}


#include <gtest/gtest.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "UKF/Types.h"
#include "UKF/Integrator.h"
#include "UKF/StateVector.h"
#include "comparisons.h"

enum MyFields {
    LatLon,
    Altitude,
    Velocity,
    Acceleration,
    Attitude,
    AngularVelocity,
    Position
};

using MyStateVector = UKF::StateVector<
    UKF::Field<LatLon, UKF::Vector<2>>,
    UKF::Field<Velocity, UKF::Vector<3>>,
    UKF::Field<Attitude, UKF::Quaternion>,
    UKF::Field<Altitude, real_t>
>;

TEST(StateVectorTest, Instantiation) {
    MyStateVector test_state;

    EXPECT_EQ(10, MyStateVector::MaxRowsAtCompileTime);
    EXPECT_EQ(10, test_state.size());
    EXPECT_EQ(9, MyStateVector::CovarianceMatrix::MaxRowsAtCompileTime);
    EXPECT_EQ(9, MyStateVector::CovarianceMatrix::MaxColsAtCompileTime);
    EXPECT_EQ(10, MyStateVector::SigmaPointDistribution::MaxRowsAtCompileTime);
    EXPECT_EQ(19, MyStateVector::SigmaPointDistribution::MaxColsAtCompileTime);
}

TEST(StateVectorTest, Assignment) {
    MyStateVector test_state;

    test_state.set_field<LatLon>(UKF::Vector<2>(-37.8136, 144.9631));
    test_state.set_field<Velocity>(UKF::Vector<3>(1, 2, 3));
    test_state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
    test_state.set_field<Altitude>(10);

    EXPECT_VECTOR_EQ(UKF::Vector<2>(-37.8136, 144.9631), test_state.get_field<LatLon>());
    EXPECT_VECTOR_EQ(UKF::Vector<3>(1, 2, 3), test_state.get_field<Velocity>());
    EXPECT_QUATERNION_EQ(UKF::Quaternion(1, 0, 0, 0), test_state.get_field<Attitude>());
    EXPECT_EQ(10, test_state.get_field<Altitude>());
}

TEST(StateVectorTest, Arithmetic) {

}

TEST(StateVectorTest, DefaultParameters) {
    MyStateVector test_state;

    EXPECT_FLOAT_EQ(1.0, UKF::Parameters::AlphaSquared<MyStateVector>);
    EXPECT_FLOAT_EQ(0.0, UKF::Parameters::Beta<MyStateVector>);
    EXPECT_FLOAT_EQ(3.0, UKF::Parameters::Kappa<MyStateVector>);
    EXPECT_FLOAT_EQ(3.0, UKF::Parameters::Lambda<MyStateVector>);
    EXPECT_FLOAT_EQ(0.0, UKF::Parameters::MRP_A<MyStateVector>);
    EXPECT_FLOAT_EQ(2.0, UKF::Parameters::MRP_F<MyStateVector>);
    EXPECT_FLOAT_EQ(1.0 / 4.0, UKF::Parameters::Sigma_WM0<MyStateVector>);
    EXPECT_FLOAT_EQ(1.0 / 4.0, UKF::Parameters::Sigma_WC0<MyStateVector>);
    EXPECT_FLOAT_EQ(1.0 / 24.0, UKF::Parameters::Sigma_WMI<MyStateVector>);
    EXPECT_FLOAT_EQ(1.0 / 24.0, UKF::Parameters::Sigma_WCI<MyStateVector>);
}

using AlternateStateVector = UKF::StateVector<
    UKF::Field<Attitude, UKF::Quaternion>,
    UKF::Field<AngularVelocity, UKF::Vector<3>>,
    UKF::Field<Acceleration, UKF::Vector<3>>
>;

namespace UKF {
namespace Parameters {
template <> constexpr real_t AlphaSquared<AlternateStateVector> = 1e-6;
template <> constexpr real_t Beta<AlternateStateVector> = 2.0;
template <> constexpr real_t Kappa<AlternateStateVector> = 3.0;
}}

TEST(StateVectorTest, CustomParameters) {
    AlternateStateVector test_state;

    EXPECT_FLOAT_EQ(1e-6, UKF::Parameters::AlphaSquared<AlternateStateVector>);
    EXPECT_FLOAT_EQ(2.0, UKF::Parameters::Beta<AlternateStateVector>);
    EXPECT_FLOAT_EQ(3.0, UKF::Parameters::Kappa<AlternateStateVector>);
    EXPECT_FLOAT_EQ(12e-6 - 9, UKF::Parameters::Lambda<AlternateStateVector>);
    EXPECT_FLOAT_EQ(0.0, UKF::Parameters::MRP_A<AlternateStateVector>);
    EXPECT_FLOAT_EQ(2.0, UKF::Parameters::MRP_F<AlternateStateVector>);
    EXPECT_FLOAT_EQ(-749999, UKF::Parameters::Sigma_WM0<AlternateStateVector>);
    EXPECT_FLOAT_EQ(-749996, UKF::Parameters::Sigma_WC0<AlternateStateVector>);
    EXPECT_FLOAT_EQ(41666.667, UKF::Parameters::Sigma_WMI<AlternateStateVector>);
    EXPECT_FLOAT_EQ(41666.667, UKF::Parameters::Sigma_WCI<AlternateStateVector>);

    EXPECT_FLOAT_EQ(1.0, UKF::Parameters::AlphaSquared<MyStateVector>);
    EXPECT_FLOAT_EQ(0.0, UKF::Parameters::Beta<MyStateVector>);
    EXPECT_FLOAT_EQ(3.0, UKF::Parameters::Kappa<MyStateVector>);
    EXPECT_FLOAT_EQ(3.0, UKF::Parameters::Lambda<MyStateVector>);
    EXPECT_FLOAT_EQ(0.0, UKF::Parameters::MRP_A<MyStateVector>);
    EXPECT_FLOAT_EQ(2.0, UKF::Parameters::MRP_F<MyStateVector>);
    EXPECT_FLOAT_EQ(1.0 / 4.0, UKF::Parameters::Sigma_WM0<MyStateVector>);
    EXPECT_FLOAT_EQ(1.0 / 4.0, UKF::Parameters::Sigma_WC0<MyStateVector>);
    EXPECT_FLOAT_EQ(1.0 / 24.0, UKF::Parameters::Sigma_WMI<MyStateVector>);
    EXPECT_FLOAT_EQ(1.0 / 24.0, UKF::Parameters::Sigma_WCI<MyStateVector>);
}

TEST(StateVectorTest, SigmaPointGeneration) {
    MyStateVector test_state;

    test_state.set_field<LatLon>(UKF::Vector<2>(45, 135));
    test_state.set_field<Velocity>(UKF::Vector<3>(1, 2, 3));
    test_state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
    test_state.set_field<Altitude>(10);

    MyStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero();
    covariance.diagonal() << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0;

    MyStateVector::SigmaPointDistribution sigma_points, target_sigma_points;

    target_sigma_points << 45, 48.464,     45,     45,     45,     45,     45,     45,     45,     45, 41.536,     45,     45,     45,     45,     45,     45,     45,     45,
                          135,    135, 138.46,    135,    135,    135,    135,    135,    135,    135,    135, 131.54,    135,    135,    135,    135,    135,    135,    135,
                            1,      1,      1,  4.464,      1,      1,      1,      1,      1,      1,      1,      1, -2.464,      1,      1,      1,      1,      1,      1,
                            2,      2,      2,      2,  5.464,      2,      2,      2,      2,      2,      2,      2,      2, -1.464,      2,      2,      2,      2,      2,
                            3,      3,      3,      3,      3,  6.464,      3,      3,      3,      3,      3,      3,      3,      3, -0.464,      3,      3,      3,      3,
                            0,      0,      0,      0,      0,      0,  0.866,      0,      0,      0,      0,      0,      0,      0,      0, -0.866,      0,      0,      0,
                            0,      0,      0,      0,      0,      0,      0,  0.866,      0,      0,      0,      0,      0,      0,      0,      0, -0.866,      0,      0,
                            0,      0,      0,      0,      0,      0,      0,      0,  0.866,      0,      0,      0,      0,      0,      0,      0,      0, -0.866,      0,
                            1,      1,      1,      1,      1,      1,    0.5,    0.5,    0.5,      1,      1,      1,      1,      1,      1,    0.5,    0.5,    0.5,      1,
                           10,     10,     10,     10,     10,     10,     10,     10,     10, 13.464,     10,     10,     10,     10,     10,     10,     10,     10,  6.536;
    sigma_points = test_state.calculate_sigma_point_distribution((covariance *
        (MyStateVector::covariance_size() + UKF::Parameters::Lambda<MyStateVector>)).llt().matrixL());

    EXPECT_VECTOR_EQ(target_sigma_points.col(0),  sigma_points.col(0));
    EXPECT_VECTOR_EQ(target_sigma_points.col(1),  sigma_points.col(1));
    EXPECT_VECTOR_EQ(target_sigma_points.col(2),  sigma_points.col(2));
    EXPECT_VECTOR_EQ(target_sigma_points.col(3),  sigma_points.col(3));
    EXPECT_VECTOR_EQ(target_sigma_points.col(4),  sigma_points.col(4));
    EXPECT_VECTOR_EQ(target_sigma_points.col(5),  sigma_points.col(5));
    EXPECT_VECTOR_EQ(target_sigma_points.col(6),  sigma_points.col(6));
    EXPECT_VECTOR_EQ(target_sigma_points.col(7),  sigma_points.col(7));
    EXPECT_VECTOR_EQ(target_sigma_points.col(8),  sigma_points.col(8));
    EXPECT_VECTOR_EQ(target_sigma_points.col(9),  sigma_points.col(9));
    EXPECT_VECTOR_EQ(target_sigma_points.col(10), sigma_points.col(10));
    EXPECT_VECTOR_EQ(target_sigma_points.col(11), sigma_points.col(11));
    EXPECT_VECTOR_EQ(target_sigma_points.col(12), sigma_points.col(12));
    EXPECT_VECTOR_EQ(target_sigma_points.col(13), sigma_points.col(13));
    EXPECT_VECTOR_EQ(target_sigma_points.col(14), sigma_points.col(14));
    EXPECT_VECTOR_EQ(target_sigma_points.col(15), sigma_points.col(15));
    EXPECT_VECTOR_EQ(target_sigma_points.col(16), sigma_points.col(16));
    EXPECT_VECTOR_EQ(target_sigma_points.col(17), sigma_points.col(17));
    EXPECT_VECTOR_EQ(target_sigma_points.col(18), sigma_points.col(18));
}

TEST(StateVectorTest, SigmaPointMean) {
    MyStateVector test_state;

    test_state.set_field<LatLon>(UKF::Vector<2>(-37.8136, 144.9631));
    test_state.set_field<Velocity>(UKF::Vector<3>(1, 2, 3));
    test_state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
    test_state.set_field<Altitude>(10);

    MyStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero();
    covariance.diagonal() << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0;

    MyStateVector::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution((covariance *
        (MyStateVector::covariance_size() + UKF::Parameters::Lambda<MyStateVector>)).llt().matrixL());

    EXPECT_VECTOR_EQ(test_state, MyStateVector::calculate_sigma_point_mean(sigma_points));
}

TEST(StateVectorTest, SigmaPointDeltas) {
    MyStateVector test_state;

    test_state.set_field<LatLon>(UKF::Vector<2>(-37.8136, 144.9631));
    test_state.set_field<Velocity>(UKF::Vector<3>(1, 2, 3));
    test_state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
    test_state.set_field<Altitude>(10);

    MyStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero();
    covariance.diagonal() << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0;

    MyStateVector::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution((covariance *
        (MyStateVector::covariance_size() + UKF::Parameters::Lambda<MyStateVector>)).llt().matrixL());
    MyStateVector test_mean = MyStateVector::calculate_sigma_point_mean(sigma_points);
    MyStateVector::SigmaPointDeltas sigma_point_deltas, target_sigma_point_deltas;

    target_sigma_point_deltas <<  0,  3.464,      0,      0,      0,      0,      0,      0,      0,      0, -3.464,      0,      0,      0,      0,      0,      0,      0,      0,
                                  0,      0,  3.464,      0,      0,      0,      0,      0,      0,      0,      0, -3.464,      0,      0,      0,      0,      0,      0,      0,
                                  0,      0,      0,  3.464,      0,      0,      0,      0,      0,      0,      0,      0, -3.464,      0,      0,      0,      0,      0,      0,
                                  0,      0,      0,      0,  3.464,      0,      0,      0,      0,      0,      0,      0,      0, -3.464,      0,      0,      0,      0,      0,
                                  0,      0,      0,      0,      0,  3.464,      0,      0,      0,      0,      0,      0,      0,      0, -3.464,      0,      0,      0,      0,
                                  0,      0,      0,      0,      0,      0,  3.464,      0,      0,      0,      0,      0,      0,      0,      0, -3.464,      0,      0,      0,
                                  0,      0,      0,      0,      0,      0,      0,  3.464,      0,      0,      0,      0,      0,      0,      0,      0, -3.464,      0,      0,
                                  0,      0,      0,      0,      0,      0,      0,      0,  3.464,      0,      0,      0,      0,      0,      0,      0,      0, -3.464,      0,
                                  0,      0,      0,      0,      0,      0,      0,      0,      0,  3.464,      0,      0,      0,      0,      0,      0,      0,      0, -3.464;
    sigma_point_deltas = test_mean.calculate_sigma_point_deltas(sigma_points);

    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(0),  sigma_point_deltas.col(0));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(1),  sigma_point_deltas.col(1));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(2),  sigma_point_deltas.col(2));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(3),  sigma_point_deltas.col(3));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(4),  sigma_point_deltas.col(4));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(5),  sigma_point_deltas.col(5));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(6),  sigma_point_deltas.col(6));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(7),  sigma_point_deltas.col(7));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(8),  sigma_point_deltas.col(8));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(9),  sigma_point_deltas.col(9));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(10), sigma_point_deltas.col(10));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(11), sigma_point_deltas.col(11));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(12), sigma_point_deltas.col(12));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(13), sigma_point_deltas.col(13));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(14), sigma_point_deltas.col(14));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(15), sigma_point_deltas.col(15));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(16), sigma_point_deltas.col(16));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(17), sigma_point_deltas.col(17));
    EXPECT_VECTOR_EQ(target_sigma_point_deltas.col(18), sigma_point_deltas.col(18));
}

TEST(StateVectorTest, SigmaPointCovariance) {
    MyStateVector test_state;

    test_state.set_field<LatLon>(UKF::Vector<2>(-37.8136, 144.9631));
    test_state.set_field<Velocity>(UKF::Vector<3>(1, 2, 3));
    test_state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
    test_state.set_field<Altitude>(10);

    MyStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero();
    covariance.diagonal() << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0;

    MyStateVector::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution((covariance *
        (MyStateVector::covariance_size() + UKF::Parameters::Lambda<MyStateVector>)).llt().matrixL());
    MyStateVector test_mean = MyStateVector::calculate_sigma_point_mean(sigma_points);
    MyStateVector::SigmaPointDeltas sigma_point_deltas = test_mean.calculate_sigma_point_deltas(sigma_points);
    MyStateVector::CovarianceMatrix calculated_covariance = MyStateVector::calculate_sigma_point_covariance(sigma_point_deltas);

    EXPECT_VECTOR_EQ(covariance.col(0),  calculated_covariance.col(0));
    EXPECT_VECTOR_EQ(covariance.col(1),  calculated_covariance.col(1));
    EXPECT_VECTOR_EQ(covariance.col(2),  calculated_covariance.col(2));
    EXPECT_VECTOR_EQ(covariance.col(3),  calculated_covariance.col(3));
    EXPECT_VECTOR_EQ(covariance.col(4),  calculated_covariance.col(4));
    EXPECT_VECTOR_EQ(covariance.col(5),  calculated_covariance.col(5));
    EXPECT_VECTOR_EQ(covariance.col(6),  calculated_covariance.col(6));
    EXPECT_VECTOR_EQ(covariance.col(7),  calculated_covariance.col(7));
    EXPECT_VECTOR_EQ(covariance.col(8),  calculated_covariance.col(8));
}

TEST(StateVectorTest, SmallAlphaSigmaPoints) {
    AlternateStateVector test_state;

    test_state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
    test_state.set_field<AngularVelocity>(UKF::Vector<3>(1, 2, 3));
    test_state.set_field<Acceleration>(UKF::Vector<3>(1, 2, 3));

    AlternateStateVector::CovarianceMatrix covariance = MyStateVector::CovarianceMatrix::Zero();
    covariance.diagonal() << 1000.0, 1000.0, 1000.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0;

    AlternateStateVector::SigmaPointDistribution sigma_points = test_state.calculate_sigma_point_distribution((covariance *
        (AlternateStateVector::covariance_size() + UKF::Parameters::Lambda<AlternateStateVector>)).llt().matrixL());
    AlternateStateVector test_mean = AlternateStateVector::calculate_sigma_point_mean(sigma_points);
    AlternateStateVector::SigmaPointDeltas sigma_point_deltas = test_mean.calculate_sigma_point_deltas(sigma_points);
    AlternateStateVector::CovarianceMatrix calculated_covariance = AlternateStateVector::calculate_sigma_point_covariance(sigma_point_deltas);

    EXPECT_VECTOR_EQ(covariance.col(0),  calculated_covariance.col(0));
    EXPECT_VECTOR_EQ(covariance.col(1),  calculated_covariance.col(1));
    EXPECT_VECTOR_EQ(covariance.col(2),  calculated_covariance.col(2));
    EXPECT_VECTOR_EQ(covariance.col(3),  calculated_covariance.col(3));
    EXPECT_VECTOR_EQ(covariance.col(4),  calculated_covariance.col(4));
    EXPECT_VECTOR_EQ(covariance.col(5),  calculated_covariance.col(5));
    EXPECT_VECTOR_EQ(covariance.col(6),  calculated_covariance.col(6));
    EXPECT_VECTOR_EQ(covariance.col(7),  calculated_covariance.col(7));
    EXPECT_VECTOR_EQ(covariance.col(8),  calculated_covariance.col(8));

    EXPECT_QUATERNION_EQ(test_state.get_field<Attitude>(), test_mean.get_field<Attitude>());
    EXPECT_VECTOR_EQ(test_state.get_field<AngularVelocity>(), test_mean.get_field<AngularVelocity>());
    EXPECT_VECTOR_EQ(test_state.get_field<Acceleration>(), test_mean.get_field<Acceleration>());
}

/*
These are linear, but they don't have to be – just makes it easier to
calculate the predicted output.
*/

using ProcessModelTestStateVector = UKF::StateVector<
    UKF::Field<Position, UKF::Vector<3>>,
    UKF::Field<Velocity, UKF::Vector<3>>
>;

namespace UKF {
template <> template <>
ProcessModelTestStateVector ProcessModelTestStateVector::derivative<>() const {
    ProcessModelTestStateVector temp;
    /* Position derivative. */
    temp.set_field<Position>(get_field<Velocity>());

    /* Velocity derivative. */
    temp.set_field<Velocity>(UKF::Vector<3>(0, 0, 0));

    return temp;
}

template <> template <>
ProcessModelTestStateVector ProcessModelTestStateVector::derivative<UKF::Vector<3>>(
        const UKF::Vector<3>& acceleration) const {
    ProcessModelTestStateVector temp;
    /* Position derivative. */
    temp.set_field<Position>(get_field<Velocity>());

    /* Velocity derivative. */
    temp.set_field<Velocity>(acceleration);

    return temp;
}

}

TEST(StateVectorTest, ProcessModel) {
    ProcessModelTestStateVector test_state;
    UKF::Vector<6> expected_state;

    test_state.set_field<Position>(UKF::Vector<3>(0, 0, 0));
    test_state.set_field<Velocity>(UKF::Vector<3>(1, 2, 3));

    expected_state << 0.1, 0.2, 0.3, 1, 2, 3;
    EXPECT_VECTOR_EQ(expected_state, test_state.process_model(0.1));
    EXPECT_VECTOR_EQ(expected_state, test_state.process_model<UKF::IntegratorRK4>(0.1));
    EXPECT_VECTOR_EQ(expected_state, test_state.process_model<UKF::IntegratorHeun>(0.1));
    EXPECT_VECTOR_EQ(expected_state, test_state.process_model<UKF::IntegratorEuler>(0.1));

    expected_state << 0.1 + (0.5 * 3 * 0.1 * 0.1), 0.2 + (0.5 * 2 * 0.1 * 0.1), 0.3 + (0.5 * 1 * 0.1 * 0.1), 1.3, 2.2, 3.1;
    EXPECT_VECTOR_EQ(expected_state, test_state.process_model(0.1, UKF::Vector<3>(3, 2, 1)));
    EXPECT_VECTOR_EQ(expected_state, test_state.process_model<UKF::IntegratorRK4>(0.1, UKF::Vector<3>(3, 2, 1)));
    EXPECT_VECTOR_EQ(expected_state, test_state.process_model<UKF::IntegratorHeun>(0.1, UKF::Vector<3>(3, 2, 1)));
    expected_state << 0.1, 0.2, 0.3, 1.3, 2.2, 3.1;
    EXPECT_VECTOR_EQ(expected_state, test_state.process_model<UKF::IntegratorEuler>(0.1, UKF::Vector<3>(3, 2, 1)));
}

TEST(StateVectorTest, UpdateDelta) {
    MyStateVector test_state;

    test_state.set_field<LatLon>(UKF::Vector<2>(45, 135));
    test_state.set_field<Velocity>(UKF::Vector<3>(1, 2, 3));
    test_state.set_field<Attitude>(UKF::Quaternion(1, 0, 0, 0));
    test_state.set_field<Altitude>(10);

    MyStateVector::StateVectorDelta test_delta;
    test_delta << 10, -20, 3, 2, 1, 0, 0, 0.5, 1;

    test_state.apply_delta(test_delta);

    MyStateVector expected_state;
    expected_state.set_field<LatLon>(UKF::Vector<2>(55, 115));
    expected_state.set_field<Velocity>(UKF::Vector<3>(4, 4, 4));
    expected_state.set_field<Attitude>(UKF::Quaternion(0.9692, 0, 0, 0.2462));
    expected_state.set_field<Altitude>(11);

    EXPECT_VECTOR_EQ(expected_state, test_state);
}


// Copyright 2019 the Autoware Foundation
//
// 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.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.

#ifndef OPTIMIZATION__NEWTONS_METHOD_OPTIMIZER_HPP_
#define OPTIMIZATION__NEWTONS_METHOD_OPTIMIZER_HPP_

#include <optimization/optimizer.hpp>
#include <optimization/optimization_problem.hpp>
#include <optimization/optimizer_options.hpp>
#include <optimization/line_search/line_search.hpp>
#include <Eigen/Core>
#include <Eigen/SVD>
#include <limits>
#include <memory>
#include <cmath>
#include <iostream>

namespace autoware
{
namespace common
{
namespace optimization
{
/// Optimizer using the Newton method with line search
template<typename LineSearchT>
class OPTIMIZATION_PUBLIC NewtonsMethodOptimizer
  : public Optimizer<NewtonsMethodOptimizer<LineSearchT>>
{
public:
  using StepT = float_t;

  /// Constructor to initialize the line search method
  ///
  /// @param[in]  line_searcher  An instance of a line search class.
  /// @param[in]  options        Options to be used for this optimization.
  ///
  explicit NewtonsMethodOptimizer(
    const LineSearchT & line_searcher,
    const OptimizationOptions & options)
  : m_line_searcher{line_searcher}, m_options{options} {}

  /// Solves `x_out` for an objective `optimization_problem` and an initial value `x0`
  ///
  /// @param      optimization_problem  optimization_problem optimization objective
  /// @param      x0                    initial value
  /// @param      x_out                 optimized value
  ///
  /// @tparam     OptimizationProblemT  Optimization problem type. Must be an implementation of
  ///                                   `common::optimization::OptimizationProblem`.
  /// @tparam     DomainValueT          Type of the parameter
  /// @tparam     EigenSolverT          Type of eigen solver to be used internallt for solving the
  ///                                   necessary linear equations. By default set to `Eigen::LDLT`.
  ///
  /// @return     Summary of this optimization.
  ///
  template<typename OptimizationProblemT, typename DomainValueT, typename EigenSolverT>
  OptimizationSummary solve_(
    OptimizationProblemT & optimization_problem,
    const DomainValueT & x0, DomainValueT & x_out)
  {
    // Get types from the method's templated parameter
    using Value = typename OptimizationProblemT::Value;
    using Jacobian = typename OptimizationProblemT::Jacobian;
    using Hessian = typename OptimizationProblemT::Hessian;
    TerminationType termination_type{TerminationType::NO_CONVERGENCE};
    Value score_previous{0.0};
    Jacobian jacobian{Jacobian{}.setZero()};
    Hessian hessian{Hessian{}.setZero()};
    DomainValueT opt_direction{DomainValueT{}.setZero()};

    if (!x0.allFinite()) {   // Early exit for invalid input.
      return OptimizationSummary{0.0, TerminationType::FAILURE, 0UL};
    }

    // Initialize
    x_out = x0;

    // Get score, Jacobian and Hessian (pre-computed using evaluate)
    optimization_problem.evaluate(x_out, ComputeMode{}.set_score().set_jacobian().set_hessian());
    score_previous = optimization_problem(x_out);
    optimization_problem.jacobian(x_out, jacobian);
    optimization_problem.hessian(x_out, hessian);

    // Early exit if the initial solution is good enough.
    if (jacobian.template lpNorm<Eigen::Infinity>() <= m_options.gradient_tolerance()) {
      // As there's no newton solution yet, jacobian can be a good substitute.
      return OptimizationSummary{jacobian.norm(), TerminationType::CONVERGENCE, 0UL};
    }
    // rubis t0
    auto start_time = omp_get_wtime();

    // Iterate until convergence, error, or maximum number of iterations
    auto nr_iterations = 0UL;
    for (; nr_iterations < m_options.max_num_iterations(); ++nr_iterations) {
      if (!x_out.allFinite() || !jacobian.allFinite() || !hessian.allFinite()) {
        termination_type = TerminationType::FAILURE;
        break;
      }

      // rubis
      if(!para_init) {
        // Eigen::initParallel();
        omp_set_num_threads(1);
        Eigen::setNbThreads(1);
        para_init = true;
        auto n = Eigen::nbThreads();
        std::cerr << "Eigen nbThreads: " << n << std::endl;
        // #pragma omp parallel
        // {  std::cerr << "Hello World from thread" <<  omp_get_thread_num() << std::endl; }
        std::cerr << "rubis" << std::endl;
      }
      if(!rt_init) {

      }

      // Find decent direction using Newton's method
      EigenSolverT solver(hessian);
      opt_direction = solver.solve(-jacobian);

      // Check if there was a problem during Eigen's solve()
      if (!opt_direction.allFinite()) {
        termination_type = TerminationType::FAILURE;
        break;
      }
      // Calculate and apply step length
      // TODO(zozen): with guarnteed sufficient decrease as in [More, Thuente 1994]
      // would need partial results passed to optimization_problem before call, as in:
      // computeStepLengthMT (x0, x_delta, x_delta_norm, transformation_epsilon_/2, ...
      // and would pre-compute score/jacobian/hessian as during init in evaluate!
      // also needs the sign to know the direction of optimization?
      const auto step = m_line_searcher.compute_next_step(
        x_out, opt_direction,
        optimization_problem);
      const auto prev_x_norm = x_out.norm();
      x_out += step;

      // Check change in parameter relative to the parameter value
      // tolerance added to the norm for stability when the norm is close to 0
      // (Inspired from https://github.com/ceres-solver/ceres-solver/blob/4362a2169966e08394252098
      // c80d1f26764becd0/include/ceres/tiny_solver.h#L244)
      const auto parameter_tolerance =
        m_options.parameter_tolerance() * (prev_x_norm + m_options.parameter_tolerance());
      if (step.norm() <= parameter_tolerance) {
        termination_type = TerminationType::CONVERGENCE;
        break;
      }

      // Update value, Jacobian and Hessian (pre-computed using evaluate)
      optimization_problem.evaluate(x_out, ComputeMode{}.set_score().set_jacobian().set_hessian());
      const auto score = optimization_problem(x_out);
      optimization_problem.jacobian(x_out, jacobian);
      optimization_problem.hessian(x_out, hessian);

      // Check if the max-norm of the gradient is small enough.
      if (jacobian.template lpNorm<Eigen::Infinity>() <= m_options.gradient_tolerance()) {
        termination_type = TerminationType::CONVERGENCE;
        break;
      }

      // Check change in cost function.
      if (std::fabs(score - score_previous) <=
        (m_options.function_tolerance() * std::fabs(score_previous)))
      {
        termination_type = TerminationType::CONVERGENCE;
        break;
      }
      score_previous = score;
    }

    // rubis t1
    auto end_time = omp_get_wtime();
    auto response_time = (end_time - start_time) * 1e3;
    std::cerr << "ndt: " << response_time << std::endl;

    // Returning summary consisting of the following three values:
    // estimated_distance_to_optimum, convergence_tolerance_criteria_met, number_of_iterations_made
    return OptimizationSummary{opt_direction.norm(), termination_type, nr_iterations};
  }

private:
  // initialize on construction
  LineSearchT m_line_searcher;
  OptimizationOptions m_options;
  bool para_init = false;
  bool rt_init = false;
};
}  // namespace optimization
}  // namespace common
}  // namespace autoware

#endif  // OPTIMIZATION__NEWTONS_METHOD_OPTIMIZER_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 <nt2/arithmetic/include/functions/two_prod.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/eps.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/functions/unary_minus.hpp>
#include <nt2/include/functions/minus.hpp>
#include <nt2/include/functions/multiplies.hpp>
#include <nt2/include/functions/plus.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/fusion/include/vector_tie.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

NT2_TEST_CASE_TPL( two_prod, NT2_SIMD_REAL_TYPES)
{
  using nt2::two_prod;
  using nt2::tag::two_prod_;
  using boost::simd::native;

  typedef native<T,BOOST_SIMD_DEFAULT_EXTENSION>   vT;

  NT2_TEST_TYPE_IS( (typename boost::dispatch::meta::call<two_prod_(vT,vT)>::type)
                  , (std::pair<vT,vT>)
                  );

  vT inf_    = nt2::Inf<vT>();
  vT zero_   = nt2::Zero<vT>();
  vT one_    = nt2::One<vT>();
  vT eps_    = nt2::Eps<vT>();
  vT meps2_   = -eps_*eps_;

  {
    vT s,r;

    two_prod(inf_,one_, s, r);
    NT2_TEST_EQUAL(s, inf_);
    NT2_TEST_EQUAL(r, zero_);

    two_prod(one_, inf_, s, r);
    NT2_TEST_EQUAL(s, inf_);
    NT2_TEST_EQUAL(r, zero_);

    two_prod(one_ + eps_, one_ - eps_, s, r);
    NT2_TEST_EQUAL(s, one_);
    NT2_TEST_EQUAL(r, meps2_);

    two_prod(one_ - eps_,one_ + eps_, s, r);
    NT2_TEST_EQUAL(s, one_);
    NT2_TEST_EQUAL(r, meps2_);
  }

  {
    vT s,r;

    s = two_prod(inf_,one_, r);
    NT2_TEST_EQUAL(s, inf_);
    NT2_TEST_EQUAL(r, zero_);

    s = two_prod(one_, inf_, r);
    NT2_TEST_EQUAL(s, inf_);
    NT2_TEST_EQUAL(r, zero_);

    s = two_prod(one_ + eps_, one_ - eps_, r);
    NT2_TEST_EQUAL(s, one_);
    NT2_TEST_EQUAL(r, meps2_);

    s = two_prod(one_ - eps_,one_ + eps_, r);
    NT2_TEST_EQUAL(s, one_);
    NT2_TEST_EQUAL(r, meps2_);
  }

  {
    vT s,r;

    boost::fusion::vector_tie(s,r) = two_prod(inf_,one_);
    NT2_TEST_EQUAL(s, inf_);
    NT2_TEST_EQUAL(r, zero_);

    boost::fusion::vector_tie(s,r) = two_prod(one_, inf_);
    NT2_TEST_EQUAL(s, inf_);
    NT2_TEST_EQUAL(r, zero_);

    boost::fusion::vector_tie(s,r) = two_prod(one_ + eps_, one_ - eps_);
    NT2_TEST_EQUAL(s, one_);
    NT2_TEST_EQUAL(r, meps2_);

    boost::fusion::vector_tie(s,r) = two_prod(one_ - eps_,one_ + eps_);
    NT2_TEST_EQUAL(s, one_);
    NT2_TEST_EQUAL(r, meps2_);
  }

  {
    std::pair<vT,vT> p;

    p = two_prod(inf_,one_);
    NT2_TEST_EQUAL(p.first, inf_);
    NT2_TEST_EQUAL(p.second, zero_);

    p = two_prod(one_, inf_);
    NT2_TEST_EQUAL(p.first, inf_);
    NT2_TEST_EQUAL(p.second, zero_);

    p = two_prod(one_ + eps_, one_ - eps_);
    NT2_TEST_EQUAL(p.first, one_);
    NT2_TEST_EQUAL(p.second, meps2_);

    p = two_prod(one_ - eps_,one_ + eps_);
    NT2_TEST_EQUAL(p.first, one_);
    NT2_TEST_EQUAL(p.second, meps2_);
  }
}


//
// Created by Alex Beccaro on 24/01/2019.
//

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../../src/problems/101-150/130/problem130.hpp"

BOOST_AUTO_TEST_SUITE( Problem130 )

    BOOST_AUTO_TEST_CASE( Example ) {
        auto res = problems::problem130::solve(5);
        BOOST_CHECK_EQUAL(res, 1985);
    }

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem130::solve();
        BOOST_CHECK_EQUAL(res, 149253);
    }

BOOST_AUTO_TEST_SUITE_END()


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>

TEST(MathFunctions,logical_or) {
  using stan::math::logical_or;
  EXPECT_TRUE(logical_or(1,0));
  EXPECT_TRUE(logical_or(0,1));
  EXPECT_TRUE(logical_or(1,1));
  EXPECT_TRUE(logical_or(5.7,-1.9));
  EXPECT_TRUE(logical_or(5.7,-1));

  EXPECT_FALSE(logical_or(0,0));
  EXPECT_FALSE(logical_or(0.0, 0.0));
  EXPECT_FALSE(logical_or(0.0, 0));
  EXPECT_FALSE(logical_or(0, 0.0));
}

TEST(MathFunctions, logical_or_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_TRUE(stan::math::logical_or(1.0, nan));
  EXPECT_TRUE(stan::math::logical_or(nan, 2.0));
  EXPECT_TRUE(stan::math::logical_or(nan, nan));
}


#ifndef __AST_BOOL_HPP_DEFINED__
#define __AST_BOOL_HPP_DEFINED__

#include <iostream>
#include <string>
#include <vector>
#include <boost/shared_ptr.hpp>
#include <boost/python.hpp>
#include <boost/multiprecision/cpp_int.hpp>

#include <z3++.h>
#include <assert.h>
#include <type.hpp>

#include <ast.hpp>

namespace ila
{
    class Abstraction;

    // ---------------------------------------------------------------------- //
    // Boolean expressions are derived from this class.
    class BoolExpr : public Node {
    public:
        // constructor.
        BoolExpr();
        // constructor needed for ChoiceExpr
        BoolExpr(NodeType t);
        // destructor.
        virtual ~BoolExpr();
    };

    // ---------------------------------------------------------------------- //
    // Boolean variables.
    class BoolVar : public BoolExpr {
    public:
        BoolVar(const std::string& name);
        virtual ~BoolVar();
        virtual Node* clone() const;
        virtual bool equal(const Node* that) const;
        virtual std::ostream& write(std::ostream& out) const;
    };

    // ---------------------------------------------------------------------- //
    // Boolean constants.
    class BoolConst : public BoolExpr {
    protected:
        static nptr_t true_node;
        static nptr_t false_node;

        bool value;
    private:
        BoolConst(bool value);
        BoolConst(int value);
        BoolConst(const mp_int_t& l);
    public:
        virtual ~BoolConst();
        virtual Node* clone() const;
        virtual bool equal(const Node* that) const;
        virtual boost::python::object getValue() const;
        virtual std::ostream& write(std::ostream& out) const;

        static nptr_t get(bool v) {
            if (v) return true_node;
            else return false_node;
        }

        // helper functions.
        bool val() const { return value; }
    };

    // ---------------------------------------------------------------------- //
    // Boolean operators.
    class BoolOp : public BoolExpr {
    public:
        // Number of operands.
        enum Arity { UNARY, BINARY, TERNARY } arity;

        // What is the operation?
        enum Op {
            // invalid
            INVALID,
            // unary
            NOT,
            // binary.
            AND, OR, XOR, XNOR, NAND, NOR, IMPLY,
            SLT, SGT, SLE, SGE, ULT, UGT, ULE, UGE,
            EQUAL, DISTINCT,
            // ternary
            IF
        } op;

        static const std::string operatorNames[];

        // the operands themselves.
        nptr_vec_t args;

    private:
        // Don't forget to update these helper functions below.
        static bool isUnary(Op op) { return op >= NOT && op <= NOT; }
        static bool isBinary(Op op) { return op >= AND && op <= DISTINCT; }
        static bool isTernary(Op op) { return op >= IF && op <= IF; }
        // helper functions to check argument types.
        static bool checkUnaryOpTypes(Op op, const nptr_t& n);
        static bool checkBinaryOpTypes(Op op, const nptr_t& n1, const nptr_t& n2);
        static int checkTernaryOpTypes(Op op, nptr_vec_t args_);

    public:
        // constructors.
        BoolOp(Op op, const nptr_t& n1);
        BoolOp(Op op, const nptr_t& n1, const nptr_t& n2);
        BoolOp(Op op, nptr_vec_t& args_);
        // kind of like a copy constructor, but use a fresh set of args.
        BoolOp(const BoolOp* other, nptr_vec_t& args_);
        // destructors.
        virtual ~BoolOp();

        // clone.
        virtual Node* clone() const;

        // equal.
        virtual bool equal(const Node* that) const;

        // stream output.
        virtual std::ostream& write(std::ostream& out) const;

        // number of operands.
        virtual unsigned nArgs() const;

        // operand i.
        virtual nptr_t arg(unsigned i) const;

        // operation.
        Op getOp() const { return op; }

        // negate this boolean expression.
        static nptr_t& negate(const nptr_t& n, rwmap_t& cache);
    };
}
#endif


// ----------------------------------------------------------------------------------------------------------
/// @file   ndarray_foo.cpp
/// @brief  Test cases for NDArray.
// ----------------------------------------------------------------------------------------------------------

#define BOOST_TEST_DYN_LINK

#ifdef SEPARATE_TEST
#define BOOST_TEST_MODULE NDArrayTest
#endif

#include <boost/test/unit_test.hpp>
#include <iostream>
#include <vector>

#include "ndarray.h"

BOOST_AUTO_TEST_SUITE(NDArrayTestCases)

BOOST_AUTO_TEST_CASE(NDarrayBasic) {
  using static_ndarray = ndarray::NDArray<int, 48, 3, 32, 32>;
  static_ndarray A;
  auto dim_sizes_a = A.dim_sizes();
  BOOST_CHECK(dim_sizes_a[0] == 48);
  BOOST_CHECK(dim_sizes_a[1] == 3);
  BOOST_CHECK(dim_sizes_a[2] == 32);
  BOOST_CHECK(dim_sizes_a[3] == 32);

  BOOST_CHECK(A.size() == 48 * 3 * 32 * 32);
  BOOST_CHECK(A.rank() == 4);
  BOOST_CHECK(A.dim_size(0) == 48);
  BOOST_CHECK(A.dim_size(1) == 3);
  BOOST_CHECK(A.dim_size(2) == 32);
  BOOST_CHECK(A.dim_size(3) == 32);

  using dynamic_ndarray = ndarray::NDArray<int>;
  dynamic_ndarray B(3);
  BOOST_CHECK(B.rank() == 3);
  dynamic_ndarray C{32, 7, 32, 48};
  auto dim_sizes_c = C.dim_sizes();
  BOOST_CHECK(dim_sizes_c[0] == 32);
  BOOST_CHECK(dim_sizes_c[1] == 7);
  BOOST_CHECK(dim_sizes_c[2] == 32);
  BOOST_CHECK(dim_sizes_c[3] == 48);
  BOOST_CHECK(C.dim_size(0) == 32);
  BOOST_CHECK(C.dim_size(1) == 7);
  BOOST_CHECK(C.dim_size(2) == 32);
  BOOST_CHECK(C.dim_size(3) == 48);
  BOOST_CHECK(C.rank() == 4);
  BOOST_CHECK(C.size() == 32 * 7 * 32 * 48);
}

BOOST_AUTO_TEST_CASE(NDarrayConstructors) {
  // static NDArray
  ndarray::NDArray<int, 48, 3, 32, 32> A;
  auto dim_sizes_a = A.dim_sizes();
  BOOST_CHECK(dim_sizes_a[0] == 48);
  BOOST_CHECK(dim_sizes_a[1] == 3);
  BOOST_CHECK(dim_sizes_a[2] == 32);
  BOOST_CHECK(dim_sizes_a[3] == 32);
  BOOST_CHECK(A.size() == 48 * 3 * 32 * 32);
  BOOST_CHECK(A.rank() == 4);
  BOOST_CHECK(A.dim_size(0) == 48);
  BOOST_CHECK(A.dim_size(1) == 3);
  BOOST_CHECK(A.dim_size(2) == 32);
  BOOST_CHECK(A.dim_size(3) == 32);

  std::array<float, 6> tmp{1.f, 2.f, 3.f, 4.f, 5.f, 6.f};
  ndarray::NDArray<float, 3, 2, 1, 1> B(tmp);
  std::cout << B;
  BOOST_CHECK(B.rank() == 4);
  BOOST_CHECK(B.size() == 6);

  ndarray::NDArray<double, 2, 2> C{1.0, 2.8, 3.7, 4.99};
  std::cout << C;
  BOOST_CHECK(C.rank() == 2);
  BOOST_CHECK(C.size() == 4);
  BOOST_CHECK(C[3] == 4.99);
  BOOST_CHECK(C(1, 1) == 4.99);
  BOOST_CHECK(C.dim_size(0) == 2);
  BOOST_CHECK(C.dim_size(1) == 2);
  BOOST_CHECK(C.dim_sizes()[0] == 2);
  BOOST_CHECK(C.dim_sizes()[1] == 2);

  auto D = B;
  BOOST_CHECK(D.rank() == 4);
  BOOST_CHECK(D.size() == 6);
  BOOST_CHECK(D[0] == 1.f);
  BOOST_CHECK(D(0, 0, 0, 0) == 1.f);

  ndarray::NDArray<float, 3, 2, 1, 1> E(B);
  BOOST_CHECK(E.rank() == 4);
  BOOST_CHECK(E.size() == 6);
  BOOST_CHECK(E[0] == 1.f);
  BOOST_CHECK(E(0, 0, 0, 0) == 1.f);

  auto F(B);
  BOOST_CHECK(F.rank() == 4);
  BOOST_CHECK(F.size() == 6);
  BOOST_CHECK(F[0] == 1.f);
  BOOST_CHECK(F(0, 0, 0, 0) == 1.f);

  // dynamic NDArray
  ndarray::NDArray<int> G(3);
  BOOST_CHECK(G.rank() == 3);
  std::vector<size_t> dim_size{1, 2, 3};
  std::vector<float> data{1.f, 2.f, 3.f, 4.f, 5.f, 6.f};
  ndarray::NDArray<float> H(dim_size, data);
  auto h_data = H.data();
  auto dim = H.dim_sizes();
  BOOST_CHECK(H.rank() == 3);
  BOOST_CHECK(H.size() == 6);
  BOOST_CHECK(H.dim_size(0) == 1);
  BOOST_CHECK(H.dim_size(1) == 2);
  BOOST_CHECK(H.dim_size(2) == 3);
  BOOST_CHECK(dim[0] == 1);
  BOOST_CHECK(dim[1] == 2);
  BOOST_CHECK(dim[2] == 3);
  BOOST_CHECK(h_data[0] == 1.f);
  BOOST_CHECK(h_data[1] == 2.f);
  BOOST_CHECK(h_data[2] == 3.f);
  BOOST_CHECK(h_data[3] == 4.f);
  BOOST_CHECK(h_data[4] == 5.f);
  BOOST_CHECK(h_data[5] == 6.f);

  ndarray::NDArray<float> I({2, 3});
  I(0, 0) = 1.f;
  I(1, 2) = 7.f;
  I[3] = 3.5f;
  BOOST_CHECK(I.rank() == 2);
  BOOST_CHECK(I.size() == 6);
  BOOST_CHECK(I.dim_size(0) == 2);
  BOOST_CHECK(I.dim_size(1) == 3);
  BOOST_CHECK(I[3] == 3.5f);
  BOOST_CHECK(I(0, 0) == 1.f);
  BOOST_CHECK(I(1, 2) == 7.f);
}

BOOST_AUTO_TEST_CASE(NDarrayElementAccess) {
  // static NDArray
  std::array<float, 6> tmp{{1.f, 2.f, 3.f, 4.f, 5.f, 6.f}};
  ndarray::NDArray<float, 3, 2> A(tmp);
  std::cout << A;

  BOOST_CHECK(A.size() == 6);
  BOOST_CHECK(A.rank() == 2);
  BOOST_CHECK(A[0] == 1.f);
  BOOST_CHECK(A[1] == 2.f);
  BOOST_CHECK(A[2] == 3.f);
  BOOST_CHECK(A[3] == 4.f);
  BOOST_CHECK(A[4] == 5.f);
  BOOST_CHECK(A[5] == 6.f);

  BOOST_CHECK(A(0, 0) == 1.f);
  BOOST_CHECK(A(0, 1) == 2.f);
  BOOST_CHECK(A(1, 0) == 3.f);
  BOOST_CHECK(A(1, 1) == 4.f);
  BOOST_CHECK(A(2, 0) == 5.f);
  BOOST_CHECK(A(2, 1) == 6.f);

  BOOST_CHECK(A(0, 0) == A[0]);
  BOOST_CHECK(A(0, 1) == A[1]);
  BOOST_CHECK(A(1, 0) == A[2]);
  BOOST_CHECK(A(1, 1) == A[3]);
  BOOST_CHECK(A(2, 0) == A[4]);
  BOOST_CHECK(A(2, 1) == A[5]);

  A[3] = 3.99f;
  BOOST_CHECK(A[3] == 3.99f);
  A(2, 1) = 99.99f;
  BOOST_CHECK(A(2, 1) == 99.99f);

  // dynamic NDArray
  ndarray::NDArray<float> B({2, 3});
  B(0, 0) = 1.f;
  B(1, 2) = 7.f;
  B[3] = 3.5f;
  BOOST_CHECK(B.rank() == 2);
  BOOST_CHECK(B.size() == 6);
  BOOST_CHECK(B.dim_size(0) == 2);
  BOOST_CHECK(B.dim_size(1) == 3);
  BOOST_CHECK(B[3] == 3.5f);
  BOOST_CHECK(B(0, 0) == 1.f);
  BOOST_CHECK(B(1, 2) == 7.f);
}

BOOST_AUTO_TEST_SUITE_END()


//  (C) Copyright John Maddock 2006.
//  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/math/constants/constants.hpp>
#include <fstream>
#include <boost/math/tools/test_data.hpp>
#include "mp_t.hpp"

using namespace boost::math::tools;
using namespace std;

float external_f;
float force_truncate(const float* f)
{
   external_f = *f;
   return external_f;
}

float truncate_to_float(mp_t r)
{
   float f = boost::math::tools::real_cast<float>(r);
   return force_truncate(&f);
}

struct trig_data_generator
{
   boost::math::tuple<mp_t, mp_t> operator()(mp_t z)
   {
      return boost::math::make_tuple(sin(z * boost::math::constants::pi<mp_t>()), cos(z * boost::math::constants::pi<mp_t>()));
   }
};

int main(int argc, char*argv [])
{
   parameter_info<mp_t> arg1;
   test_data<mp_t> data;

   bool cont;
   std::string line;

   std::cout << "Welcome.\n"
      "This program will generate spot tests for the cos_pi and sin_pi functions:\n";

   do{
      if(0 == get_user_parameter_info(arg1, "a"))
         return 1;
      data.insert(trig_data_generator(), arg1);

      std::cout << "Any more data [y/n]?";
      std::getline(std::cin, line);
      boost::algorithm::trim(line);
      cont = (line == "y");
   }while(cont);

   std::cout << "Enter name of test data file [default=trig_data.ipp]";
   std::getline(std::cin, line);
   boost::algorithm::trim(line);
   if(line == "")
      line = "trig_data.ipp";
   std::ofstream ofs(line.c_str());
   ofs << std::scientific << std::setprecision(40);
   write_code(ofs, data, "trig_data");

   return 0;
}

/* Output for asymptotic limits:

Erf asymptotic limit for 24 bit numbers is 2.8 after approximately 6 terms.
Erfc asymptotic limit for 24 bit numbers is 4.12064 after approximately 17 terms.
Erf asymptotic limit for 53 bit numbers is 4.3 after approximately 11 terms.
Erfc asymptotic limit for 53 bit numbers is 6.19035 after approximately 29 terms.
Erf asymptotic limit for 64 bit numbers is 4.8 after approximately 12 terms.
Erfc asymptotic limit for 64 bit numbers is 7.06004 after approximately 29 terms.
Erf asymptotic limit for 106 bit numbers is 6.5 after approximately 14 terms.
Erfc asymptotic limit for 106 bit numbers is 11.6626 after approximately 29 terms.
Erf asymptotic limit for 113 bit numbers is 6.8 after approximately 14 terms.
Erfc asymptotic limit for 113 bit numbers is 12.6802 after approximately 29 terms.
*/


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <boost/config.hpp>
#include <iostream>
#include <fstream>
#include <string>
#include <boost/tokenizer.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <map>

using namespace boost;

template <typename DistanceMap>
class bacon_number_recorder : public default_bfs_visitor
{
public:
  bacon_number_recorder(DistanceMap dist) : d(dist) { }

  template <typename Edge, typename Graph>
  void tree_edge(Edge e, const Graph& g) const
  {
    typename graph_traits<Graph>::vertex_descriptor
      u = source(e, g), v = target(e, g);
      d[v] = d[u] + 1;
  }
private:
    DistanceMap d;
};

// Convenience function
template < typename DistanceMap >
bacon_number_recorder<DistanceMap>
record_bacon_number(DistanceMap d)
{
  return bacon_number_recorder < DistanceMap > (d);
}

int
main()
{
  std::ifstream datafile("./kevin-bacon.dat");
  if (!datafile) {
    std::cerr << "No ./kevin-bacon.dat file" << std::endl;
    return EXIT_FAILURE;
  }

  typedef adjacency_list < vecS, vecS, undirectedS, property < vertex_name_t,
    std::string >, property < edge_name_t, std::string > > Graph;
  Graph g;

  typedef property_map < Graph, vertex_name_t >::type actor_name_map_t;
  actor_name_map_t actor_name = get(vertex_name, g);
  typedef property_map < Graph, edge_name_t >::type movie_name_map_t;
  movie_name_map_t connecting_movie = get(edge_name, g);

  typedef graph_traits < Graph >::vertex_descriptor Vertex;
  typedef std::map < std::string, Vertex > NameVertexMap;
  NameVertexMap actors;

  for (std::string line; std::getline(datafile, line);) {
    char_delimiters_separator < char >sep(false, "", ";");
    tokenizer <> line_toks(line, sep);
    tokenizer <>::iterator i = line_toks.begin();
    std::string actors_name = *i++;
    NameVertexMap::iterator pos;
    bool inserted;
    Vertex u, v;
    boost::tie(pos, inserted) = actors.insert(std::make_pair(actors_name, Vertex()));
    if (inserted) {
      u = add_vertex(g);
      actor_name[u] = actors_name;
      pos->second = u;
    } else
      u = pos->second;

    std::string movie_name = *i++;

    boost::tie(pos, inserted) = actors.insert(std::make_pair(*i, Vertex()));
    if (inserted) {
      v = add_vertex(g);
      actor_name[v] = *i;
      pos->second = v;
    } else
      v = pos->second;

    graph_traits < Graph >::edge_descriptor e;
    boost::tie(e, inserted) = add_edge(u, v, g);
    if (inserted)
      connecting_movie[e] = movie_name;

  }

  std::vector < int >bacon_number(num_vertices(g));

  Vertex src = actors["Kevin Bacon"];
  bacon_number[src] = 0;

  breadth_first_search(g, src,
                       visitor(record_bacon_number(&bacon_number[0])));

  graph_traits < Graph >::vertex_iterator i, end;
  for (boost::tie(i, end) = vertices(g); i != end; ++i) {
    std::cout << actor_name[*i] << " has a Bacon number of "
      << bacon_number[*i] << std::endl;
  }

  return 0;
}


// g++ -std=c++11 mines.cc

#include <cstdlib>
#include <cstring>
#include <cassert>
#include <iostream>
#include <vector>
#include <string>
#include <exception>
#include <fstream>
#include <boost/algorithm/string.hpp>

using namespace std;

// Describes parsed mine field
struct field_t {
    // Field lines, true means this slot has a mine
    vector<vector<bool>> lines;

    size_t total_lines() const {
        return lines.size();
    }

    const vector<bool>& get_line(size_t num) const {
        return lines[num];
    }

    bool has_mine(size_t line, size_t pos) const {
        return lines[lines][pos];
    }
};

// Parse a single input line
static vector<bool> parse_line(const string& line)
{
    vector<bool> res;

    // Tokenize string with boost
    // It's an overkill for this type of task, but i was given 15 minutes, so overkill it is
    vector<string> tokens;
    boost::split(tokens, line, boost::is_space());

    // Check that all tokens are valid
    for (auto str : tokens) {
        if (str == "X") {
            res.push_back(true);
        } else if (str == "O") {
            res.push_back(false);
        } else {
            throw invalid_argument("Invalid token");
        }
    }

    // Assume we're working with c++11 move semantics
    return res;
}

static field_t parse_mine_field(ifstream& input)
{
    field_t field;

    string str;
    while (!getline(input, str).eof()) {
        vector<bool> line = parse_line(str);

        // skip empty lines
        if (str.size() == 0) {
            continue;
        }

        if (field.total_lines() > 0) {
            // If this is not the first line, double check that all lines have the same length
            if (field.get_line(0).size() != line.size()) {
                throw invalid_argument("Bad line size");
            }

            // Task also specifies an NxN field, so check if we're not overflowing line count
            if (field.get_line(0).size() == field.total_lines()) {
                throw invalid_argument("Field is not NxN");
            }
        }

        field.lines.push_back(std::move(line));
    }

    // Assume we're working with c++11 move semantics
    return field;
}

// Count neighbour mines in a line
static unsigned int count_neighbour_mines_in_line(const vector<bool>& line, size_t pos)
{
    assert(pos < line.size());

    unsigned int res = line[pos];
    if (pos > 0) {
        res += line[pos - 1];
    }

    if ((pos + 1) < line.size()) {
        res += line[pos + 1];
    }

    return res;
}

static unsigned int count_adjacent_mines(const field_t& field, size_t line, size_t pos)
{
    assert(line < field.lines.size());

    unsigned int res = count_neighbour_mines_in_line(field.get_line(line), pos);
    if (line > 0) {
        res += count_neighbour_mines_in_line(field.get_line(line - 1), pos);
    }

    if ((line + 1) < field.lines.size()) {
        res += count_neighbour_mines_in_line(field.get_line(line + 1), pos);
    }

    return res;
}

int main(int argc, char** argv)
{
    if (argc != 2) {
        std::cerr << "Provide a file name" << std::endl;
        return EXIT_FAILURE;
    }

     ifstream input(argv[1]);
     if (!input.is_open()) {
         cerr << "Cant open file " << argv[1] << endl;
         return EXIT_FAILURE;
     }

     field_t field = parse_mine_field(input);

     for (size_t i = 0; i < field.total_lines(); ++i) {
         for (size_t j = 0; j < field.get_line(i).size(); ++j) {
            if (field.has_mine(i, j)) {
                cout << "X ";
            } else {
                cout << count_adjacent_mines(field, i, j) << " ";
            }
         }

         cout << endl;
     }

     return EXIT_SUCCESS;
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    testGaussianISAM.cpp
 * @brief   Unit tests for GaussianISAM
 * @author  Michael Kaess
 */

#include <tests/smallExample.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianISAM.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h>
#include <gtsam/inference/ISAM.h>
#include <gtsam/geometry/Rot2.h>

#include <CppUnitLite/TestHarness.h>

#include <boost/foreach.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;

using namespace std;
using namespace gtsam;
using namespace example;

using symbol_shorthand::X;
using symbol_shorthand::L;

/* ************************************************************************* */
// Some numbers that should be consistent among all smoother tests

static const double tol = 1e-4;

/* ************************************************************************* */
TEST( ISAM, iSAM_smoother )
{
  Ordering ordering;
  for (int t = 1; t <= 7; t++) ordering += X(t);

  // Create smoother with 7 nodes
  GaussianFactorGraph smoother = createSmoother(7, ordering).first;

  // run iSAM for every factor
  GaussianISAM actual;
  BOOST_FOREACH(boost::shared_ptr<GaussianFactor> factor, smoother) {
    GaussianFactorGraph factorGraph;
    factorGraph.push_back(factor);
    actual.update(factorGraph);
  }

  // Create expected Bayes Tree by solving smoother with "natural" ordering
  BayesTree<GaussianConditional>::shared_ptr bayesTree = GaussianMultifrontalSolver(smoother).eliminate();
  GaussianISAM expected(*bayesTree);

  // Verify sigmas in the bayes tree
  BOOST_FOREACH(const GaussianBayesTree::sharedClique& clique, bayesTree->nodes()) {
    GaussianConditional::shared_ptr conditional = clique->conditional();
    size_t dim = conditional->dim();
    EXPECT(assert_equal(gtsam::ones(dim), conditional->get_sigmas(), tol));
  }

  // Check whether BayesTree is correct
  EXPECT(assert_equal(expected, actual));

  // obtain solution
  VectorValues e(VectorValues::Zero(7,2)); // expected solution
  VectorValues optimized = optimize(actual); // actual solution
  EXPECT(assert_equal(e, optimized));
}

/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */


#include <fstream>
#include <iostream>
#include <sstream>
#include <algorithm>
#include <map>

#include <ros/time.h>
#include <tf/tf.h>
#include <tf_conversions/tf_eigen.h>
#include <Eigen/Geometry>

void load_tum(const std::string& path, std::vector<tf::StampedTransform>& out)
{
    std::ifstream in(path);
    std::string line;
    std::stringstream ss;
    double t, x, y, z, qx, qy, qz, qw;
    tf::StampedTransform T;
    while(std::getline(in, line))
    {
        if(line == "") continue;
        ss.clear();
        ss.str(line);
        ss >> t >> x >> y >> z >> qx >> qy >> qz >> qw;
        // z=0;
        T.stamp_.fromSec(t);
        T.setOrigin(tf::Vector3(x, y, z));
        T.setRotation(tf::Quaternion(qx, qy, qz, qw));
        // double roll ,pitch, yaw;
        // tf::Matrix3x3(T.getRotation()).getRPY(roll, pitch, yaw);
        // T.setRotation(tf::Quaternion(yaw,0,0));
        out.push_back(T);
    }
    std::sort(out.begin(), out.end(), [](const tf::StampedTransform& u, const tf::StampedTransform& v){return u.stamp_ <= v.stamp_;});
    std::cout << "load " << out.size() << " poses from " << path << "\n";
}

bool inter_transform(const ros::Time& stamp, const std::vector<tf::StampedTransform>& sequence, const double dt_tolerance, tf::StampedTransform& out)
{
    auto p = std::upper_bound(sequence.begin(), sequence.end(), stamp, [](const ros::Time& stamp, const tf::StampedTransform& T){return stamp < T.stamp_;});
    if(p == sequence.end() || p == sequence.begin()) return false;
    const auto& pre = *(p - 1);
    const auto& next = *p;
    const double dt_pre = (stamp - pre.stamp_).toSec();
    const double dt_next = (next.stamp_ - stamp).toSec();
    if(dt_pre + dt_next > dt_tolerance) return false;
    out.stamp_ = stamp;
    const double f = dt_pre / (dt_pre + dt_next);
    out.setOrigin(pre.getOrigin() + f * (next.getOrigin() - pre.getOrigin()));
    out.setRotation(pre.getRotation().slerp(next.getRotation(), f));
    return true;
}

void calc_rpe(const tf::StampedTransform& ref_i, const tf::StampedTransform& ref_j, const tf::StampedTransform& odom_i, const tf::StampedTransform& odom_j, tf::StampedTransform& out)
{
    out.setData(ref_j.inverse() * ref_i * odom_i.inverse() * odom_j);
}

bool calc_drift(const std::vector<tf::StampedTransform>& odom, const std::vector<tf::StampedTransform>& ref, const double dt_tolerance, tf::StampedTransform& out)
{
    tf::StampedTransform start_pose_odom, end_pose_odom, start_pose_ref, end_pose_ref;
    auto p = odom.begin();
    while (p != odom.end())
    {
        if(inter_transform(p->stamp_, ref, dt_tolerance, start_pose_ref))
        {
            start_pose_odom = *p;
            break;
        }
        ++p;
    }

    if(p == odom.end()) return false;

    auto rp = odom.rbegin();
    while (rp != odom.rend())
    {
        if(inter_transform(rp->stamp_, ref, dt_tolerance, end_pose_ref))
        {
            end_pose_odom = *rp;
            break;
        }
        ++rp;
    }

    if(rp == odom.rend()) return false;

    if(start_pose_odom.stamp_ >= end_pose_odom.stamp_) return false;

    std::cout << std::setprecision(3);
    std::cout << "ref time range: [" << start_pose_ref.stamp_.toSec() << ", " << end_pose_ref.stamp_.toSec() << "] " << " total: " << (end_pose_ref.stamp_ - start_pose_ref.stamp_).toSec() << " second\n";
    std::cout << "odom time range: [" << start_pose_odom.stamp_.toSec() << ", " << end_pose_odom.stamp_.toSec() << "]" << " total: " << (end_pose_odom.stamp_ - start_pose_odom.stamp_).toSec() << " second\n";
    std::cout << std::setprecision(6);
    std::cout << "start ref: [" << start_pose_ref.getOrigin().x() << ", " << start_pose_ref.getOrigin().y() << ", "<< start_pose_ref.getOrigin().z() << "]\n";
    std::cout << "end ref: [" << end_pose_ref.getOrigin().x() << ", " << end_pose_ref.getOrigin().y() << ", "<< end_pose_ref.getOrigin().z() << "]\n";
    std::cout << "start odom: [" << start_pose_odom.getOrigin().x() << ", " << start_pose_odom.getOrigin().y() << ", "<< start_pose_odom.getOrigin().z() << "]\n";
    std::cout << "end odom: [" << end_pose_odom.getOrigin().x() << ", " << end_pose_odom.getOrigin().y() << ", "<< end_pose_odom.getOrigin().z() << "]\n";

    calc_rpe(start_pose_ref, end_pose_ref, start_pose_odom, end_pose_odom, out);

    return true;

}

struct DriftInfo
{
    double distance;
    double seg_length;
    tf::StampedTransform rpe;

    DriftInfo(){}
    DriftInfo(const double dist, const double seg_len, const tf::StampedTransform& T) : distance(dist), seg_length(seg_len), rpe(T){}
}; // struct DriftInfo

void calc_path_distance(const std::vector<tf::StampedTransform>& odom, std::vector<double>& distance)
{
    const auto size = odom.size();
    distance.resize(size, 0);
    for(int i = 1; i < size; ++i)
    {
        distance[i] = distance[i-1] + (odom[i].getOrigin() - odom[i-1].getOrigin()).length();
    }

    std::cout << "total distance: " <<distance.back() << " m\n";

}

tf::Transform convert_to_2d(const tf::Transform& T_3d)
{
    double roll, pitch, yaw;
    tf::Transform T_2d;
    tf::Matrix3x3(T_3d.getRotation()).getRPY(roll, pitch, yaw);
    T_2d.setRotation(tf::createQuaternionFromRPY(0, 0, yaw));
    T_2d.setOrigin(tf::Vector3(T_3d.getOrigin().x(), T_3d.getOrigin().y(), 0));
    return T_2d;
}

void calc_ate(const std::vector<tf::StampedTransform>& odom, const std::vector<tf::StampedTransform>& ref, const double dt_tolerance)
{
    std::vector<tf::StampedTransform> odom_sub, ref_sub;
    tf::StampedTransform T_correspond;
    for(const auto& T : odom)
    {
        if(!inter_transform(T.stamp_, ref, dt_tolerance, T_correspond)) continue;
        odom_sub.push_back(T);
        ref_sub.push_back(T_correspond);
    }
    const auto size = odom_sub.size();
    std::cout << "find " << size << " corresponding poses\n";
    Eigen::Matrix<float, 3, Eigen::Dynamic> src(3, size);
    Eigen::Matrix<float, 3, Eigen::Dynamic> dist(3, size);
    for(int i = 0; i < size; ++i)
    {
        src(0,i) = odom_sub[i].getOrigin().x();
        src(1,i) = odom_sub[i].getOrigin().y();
        src(2,i) = odom_sub[i].getOrigin().z();
        dist(0,i) = ref_sub[i].getOrigin().x();
        dist(1,i) = ref_sub[i].getOrigin().y();
        dist(2,i) = ref_sub[i].getOrigin().z();
    }

    tf::Transform transform;
    tf::poseEigenToTF(Eigen::Isometry3f(Eigen::umeyama(src, dist, false)).cast<double>(), transform);
    tf::Quaternion q(transform.getRotation());

    std::cout << "aligned translation: [" << transform.getOrigin().x() << " " << transform.getOrigin().y() << " " << transform.getOrigin().z() << "]\n"
              << "aligned rotation: [" << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << "]\n";

    double dist_err = 0, dist_err_2d = 0;
    double rotation_err = 0, rotation_err_2d = 0;
    for(int i = 0; i < size; ++i)
    {
        const auto rel_pose = ref_sub[i].inverse() * transform * odom_sub[i];
        dist_err += rel_pose.getOrigin().length();
        rotation_err += rel_pose.getRotation().getAngle();

        const auto rel_pose_2d = convert_to_2d(ref_sub[i]).inverse() * convert_to_2d(transform * odom_sub[i]);
        dist_err_2d += rel_pose_2d.getOrigin().length();
        rotation_err_2d += rel_pose_2d.getRotation().getAngle();
    }
    dist_err /= size;
    rotation_err = 180 / M_PI * rotation_err / size;
    dist_err_2d /= size;
    rotation_err_2d = 180 / M_PI * rotation_err_2d / size;

    std::cout << "ATE: " << dist_err << " m\n";
    std::cout << "ARE: " << rotation_err <<" deg\n";
    std::cout << "ATE(2D): " << dist_err_2d << " m\n";
    std::cout << "ARE(2D): " << rotation_err_2d << " deg\n";
}

void calc_rpe_by_segment(const std::vector<tf::StampedTransform>& odom, const std::vector<double>& distance,const std::vector<tf::StampedTransform>& ref, const double dt_tolerance, const double seg_length, std::vector<DriftInfo>& out)
{
    const auto size = odom.size();

    const auto find_odom = [&](const double dist)->size_t
    {
        auto p = std::lower_bound(distance.begin(), distance.end(), dist);
        return p - distance.begin();
    };

    tf::StampedTransform start_pose_odom, end_pose_odom, start_pose_ref, end_pose_ref, rpe;

    for(int i = 0; i < size; ++i)
    {
        if(!inter_transform(odom[i].stamp_, ref, dt_tolerance, start_pose_ref)) continue;
        const auto next_i = find_odom(distance[i] + seg_length);
        if(next_i == size) continue;
        if(!inter_transform(odom[next_i].stamp_, ref, dt_tolerance, end_pose_ref)) continue;
        start_pose_odom = odom[i];
        end_pose_odom = odom[next_i];
        calc_rpe(start_pose_ref, end_pose_ref, start_pose_odom, end_pose_odom, rpe);
        out.emplace_back(distance[i], distance[next_i] - distance[i], rpe);
    }
}

void calc_average_drift_rate(const std::vector<DriftInfo>& drift , double& translation_drift, double& rotation_drift)
{
    translation_drift = -1;
    if(drift.empty()) return;
    translation_drift = 0;
    rotation_drift = 0;
    for(const auto& info : drift)
    {
        translation_drift += info.rpe.getOrigin().length() / info.seg_length;
        rotation_drift +=  180.0 / M_PI *  info.rpe.getRotation().getAngle() / info.seg_length;
    }
    translation_drift /= drift.size();
    rotation_drift  = rotation_drift / drift.size();
}

int main(int argc, char** argv)
{
    const double inter_dt_threshold = 1;
    tf::StampedTransform drift;
    std::vector<tf::StampedTransform> ref, odom;
    load_tum(argv[1], ref);
    load_tum(argv[2], odom);
    std::vector<double> distance;
    calc_path_distance(odom, distance);
    std::cout << "================= total drift ===================\n";
    std::cout << std::fixed;
    if(calc_drift(odom, ref, inter_dt_threshold, drift))
    {
        std::cout << std::setprecision(6);
        std::cout << "delta t(m):[" << drift.getOrigin().x() << ", " << drift.getOrigin().y() << ", " << drift.getOrigin().z() << "]\n";
        double roll ,pitch, yaw;
        tf::Matrix3x3(drift.getRotation()).getRPY(roll, pitch, yaw);
        std::cout << "delta R(rpy,deg):[" << 180.0 / M_PI * roll << ", " << 180.0 / M_PI * pitch << ", " << 180.0 / M_PI * yaw << "]" << std::endl;
    }
    else
    {
        std::cout << "failed..." << std::endl;
    }
    std::cout << "===================== APE =======================\n";
    calc_ate(odom, ref, inter_dt_threshold);

    std::cout << "===================== RPE =======================\n";
    std::vector<double> seg_len{100, 200, 300, 400, 500, 600, 700, 800, 900};
    std::vector<DriftInfo> drift_info_total, drift_info_seg;
    double translation_drift_rate, rotation_drift_rate;
    std::vector<double> result;

    for(const auto len : seg_len)
    {
        drift_info_seg.clear();
        calc_rpe_by_segment(odom, distance, ref, inter_dt_threshold, len, drift_info_seg);
        calc_average_drift_rate(drift_info_seg, translation_drift_rate, rotation_drift_rate);
        if(translation_drift_rate >= 0)
        {
            drift_info_total.insert(drift_info_total.end(), drift_info_seg.begin(), drift_info_seg.end());
            std::cout << "avg_drift_rate(" << int(len) <<"m, " << drift_info_seg.size() << " poses): "
                      << translation_drift_rate * 100.0 << " %, " << rotation_drift_rate * 100 << " deg/100m\n";
            result.push_back(len);
            result.push_back(translation_drift_rate);
            result.push_back(rotation_drift_rate);
        }
        else
        {
            std::cout << "failed..." << std::endl;
            break;
        }
    }

    calc_average_drift_rate(drift_info_total, translation_drift_rate, rotation_drift_rate);

    if (translation_drift_rate >= 0)
    {
        drift_info_total.insert(drift_info_total.end(), drift_info_seg.begin(), drift_info_seg.end());
        std::cout << "avg_drift_rate(average, " << drift_info_total.size() << " poses): "
                  << translation_drift_rate * 100 <<"%, " << rotation_drift_rate * 100 <<" deg/100m" << std::endl;;
    }
    else
    {
        std::cout << "failed..." << std::endl;
        return EXIT_FAILURE;
    }
    std::cout << "=================== RPE table ===================\n";
    std::cout << "output as table:\n" << std::setw(5) << "len" << "   trans(%)" << "   rot(deg/100m)\n";
    std::cout << std::setprecision(5);
    std::cout << std::setw(5) << 0 << "    " << translation_drift_rate * 100 << "    " << rotation_drift_rate * 100 << "\n";
    for(int i = 0; i < result.size(); i+=3)
    {
        std::cout << std::setw(5) <<int(result[i]) << "    " << result[i+1] * 100 << "    " << result[i+2] * 100 << "\n";
    }
}