#ifndef _POSE2DRANSAC_H
#define _POSE2DRANSAC_H

#include <opencv2/core/core.hpp>

#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <Eigen/Eigen>

namespace pose2d
{
   void pose_translation(const Eigen::Matrix3d& Kinv, const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts,
                         const Eigen::Matrix3d& R, Eigen::Vector3d& translation);

   void pose_translation(const Eigen::Matrix3d& Kinv, const std::vector<cv::Point3d>& train_img_pts,
                         const std::vector<cv::Point3d>& query_img_pts,
                         const Eigen::Matrix3d& R, Eigen::Vector3d& translation);

   void pose_translation(const Eigen::Matrix3d& Kinv, const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts,
                         const double depth, const Eigen::Matrix3d& R, Eigen::Vector3d& translation);

   void pose_translation(const Eigen::Matrix3d& Kinv, const std::vector<cv::Point3d>& train_img_pts,
                         const std::vector<cv::Point3d>& query_img_pts, const double depth,
                         const Eigen::Matrix3d& R, Eigen::Vector3d& translation);
};

namespace pose3d
{
   void pose_translation(const std::vector<cv::Point3d>& world_pts, const std::vector<cv::Point2d>& query_image_pts,
                         const Eigen::Matrix3d& KI, const Eigen::Quaterniond& Q, Eigen::Vector3d& translation);

   void pose_translation(const std::vector<cv::Point3d>& world_pts, const std::vector<cv::Point2d>& query_image_pts,
                         const Eigen::Matrix3d& KI, const Eigen::Matrix3d& R, Eigen::Vector3d& translation);
};

struct GravPoseRansacModel
//=========================
{
   GravPoseRansacModel() : rotation(0, 0, 0, 0), translation(0, 0, 0) {}
   GravPoseRansacModel(const Eigen::Quaterniond& rotation_, const Eigen::Vector3d& translation_) : rotation(rotation_),
                                                                                translation(translation_) {}
   GravPoseRansacModel(const GravPoseRansacModel& other) = default;

   GravPoseRansacModel& operator=(const GravPoseRansacModel &other) = default;

   Eigen::Quaterniond rotation;
   Eigen::Vector3d translation;
};

#ifdef USE_THEIA_RANSAC
#include "sample_consensus_estimator.h"
#include "create_and_initialize_ransac_variant.h"

struct Grav2DRansacEstimator : public theia::Estimator<std::pair<cv::Point3d, cv::Point3d>, GravPoseRansacModel>
//=================================================================================================================
{
   Grav2DRansacEstimator(const cv::Mat& K_, const Eigen::Quaterniond& Q_, const double depth_ =0, int samples =3) :
      Q(Q_), R(Q_.toRotationMatrix()), depth(depth_), sample_size(samples)
   //--------------------------------------------------------------------------------------------
   {
      Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> EK_((double *) K_.data);
      EK = EK_;
      KI = EK.inverse();
   }

   Grav2DRansacEstimator(Eigen::Matrix3d _K, const Eigen::Quaterniond& Q_, const double depth_ =0, int samples =3) :
         EK(std::move(_K)), Q(Q_), R(Q_.toRotationMatrix()), depth(depth_), sample_size(samples) { KI = EK.inverse(); }

   virtual double SampleSize() const override { return sample_size; }

   bool EstimateModel(const std::vector<std::pair<cv::Point3d, cv::Point3d>> &matches,
                      std::vector<GravPoseRansacModel> *models) const override;

   double Error(const std::pair<cv::Point3d, cv::Point3d> &match, const GravPoseRansacModel &model) const override;

   Eigen::Matrix3d EK;
   const Eigen::Quaterniond Q;
   const Eigen::Matrix3d R;
   Eigen::Matrix3d KI;
   const double depth;
   int sample_size;
};

struct Grav3DRansacEstimator : public theia::Estimator<std::pair<cv::Point3d, cv::Point2d>, GravPoseRansacModel>
//=================================================================================================================
{
   Grav3DRansacEstimator(const Eigen::Matrix3d& KI_, const Eigen::Quaterniond& Q_, int samples =3) :
      KI(KI_), K(KI_.inverse()), Q(Q_), R(Q_.toRotationMatrix()), sample_size(samples) { }

   double SampleSize() const override { return sample_size; }

   bool EstimateModel(const std::vector<std::pair<cv::Point3d, cv::Point2d>> &matches,
                      std::vector<GravPoseRansacModel> *models) const override;

   double Error(const std::pair<cv::Point3d, cv::Point2d> &match, const GravPoseRansacModel &model) const override;

   Eigen::Matrix3d KI, K;
   const Eigen::Quaterniond Q;
   const Eigen::Matrix3d R;
   int sample_size;
};
#else
#include "Ransac.hh"

struct Grav2DRansacData
//=====================
{
   Grav2DRansacData(const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts) : pts(pts) {}

   const std::vector<std::pair<cv::Point3d, cv::Point3d>>& pts;
};

struct Grav3DRansacData
//=====================
{
   Grav3DRansacData(const std::vector<std::pair<cv::Point3d, cv::Point2d>>& pts) : pts(pts) {}

   const std::vector<std::pair<cv::Point3d, cv::Point2d>>& pts;
};

template <typename Pt>
static inline size_t RANSAC_copy_points(const Grav2DRansacData& samples, const std::vector<size_t>& sampleIndices,
                                        std::vector<Pt>& train_pts, std::vector<Pt>& query_pts)
//-----------------------------------------------------------------------------------------------------
{
   const size_t no = sampleIndices.size();
   for (size_t i=0; i<no; i++)
   {
      size_t index = sampleIndices[i];
      train_pts.emplace_back(samples.pts[index].first);
      query_pts.emplace_back(samples.pts[index].second);
   }
   return no;
}

static inline size_t copy_points3d(const Grav3DRansacData& samples, const std::vector<size_t>& sampleIndices,
                                   std::vector<cv::Point3d>& world_pts, std::vector<cv::Point2d>& query_pts)
//-----------------------------------------------------------------------------------------------------
{
   const size_t no = sampleIndices.size();
   for (size_t i=0; i<no; i++)
   {
      size_t index = sampleIndices[i];
      const cv::Point3d& wpt = samples.pts[index].first;
      const cv::Point2d& ipt = samples.pts[index].second;
      world_pts.emplace_back(wpt.x, wpt.y, wpt.z);
      query_pts.emplace_back(ipt.x, ipt.y);
   }
   return no;
}

struct PlanarRansacEstimator
//==========================
{
   PlanarRansacEstimator(const cv::Mat& K_, const Eigen::Quaterniond& Q_) : Q(Q_), R(Q_.toRotationMatrix())
   //-------------------------------------------------------------------------------------------
   {
      Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> EK_((double *) K_.data);
      EK = EK_;
      KI = EK.inverse();
   }

   PlanarRansacEstimator(Eigen::Matrix3d K_, const Eigen::Quaterniond& Q_) : EK(std::move(K_)), Q(Q_), R(Q_.toRotationMatrix())
   //-------------------------------------------------------------------------------------------
   {
      KI = EK.inverse();
   }

   Eigen::Matrix3d EK;
   const Eigen::Quaterniond Q;
   const Eigen::Matrix3d R;
   Eigen::Matrix3d KI;

};

struct Grav2DRansacEstimator : public PlanarRansacEstimator
//==============================================================
{
   Grav2DRansacEstimator(const cv::Mat& K_, const Eigen::Quaterniond& Q_) : PlanarRansacEstimator(K_, Q_) {}

   Grav2DRansacEstimator(const Eigen::Matrix3d K_, const Eigen::Quaterniond& Q_) : PlanarRansacEstimator(K_, Q_) {}

   const int estimate(const Grav2DRansacData &samples, const std::vector<size_t> &sampleIndices,
                      templransac::RANSACParams &parameters,
                      std::vector<GravPoseRansacModel> &models) const;

   const void error(const Grav2DRansacData &samples, const std::vector<size_t> &sampleIndices,
                    GravPoseRansacModel &model, std::vector<size_t> &inlier_indices,
                    std::vector<size_t> &outlier_indices, double error_threshold) const;
};

struct Grav2DDepthRansacEstimator : public PlanarRansacEstimator
//==============================================================
{
   Grav2DDepthRansacEstimator(const cv::Mat& K_, const Eigen::Quaterniond& Q_, const double depth_) :
         PlanarRansacEstimator(K_, Q_), depth(depth_) {}

   Grav2DDepthRansacEstimator(const Eigen::Matrix3d K_, const Eigen::Quaterniond& Q_, const double depth_) :
         PlanarRansacEstimator(K_, Q_), depth(depth_) {}

   const int estimate(const Grav2DRansacData& samples, const std::vector<size_t>& sampleIndices,
                      templransac::RANSACParams& parameters,
                      std::vector<GravPoseRansacModel>& models) const;

   const void error(const Grav2DRansacData& samples, const std::vector<size_t>& sampleIndices,
                    GravPoseRansacModel& model, std::vector<size_t>& inlier_indices,
                    std::vector<size_t>& outlier_indices, double error_threshold) const;

   const double depth;
};

struct Grav3DRansacEstimator
//==========================
{
   Grav3DRansacEstimator(const Eigen::Matrix<double, 3, 3>& KI_, const Eigen::Quaterniond& Q_)
         : Q(Q_), KI(KI_), K(KI.inverse()), R(Q_.toRotationMatrix()) { }

   const int estimate(const Grav3DRansacData& samples, const std::vector<size_t>& sampleIndices,
                      templransac::RANSACParams& parameters,
                      std::vector<GravPoseRansacModel>& models) const;

   const void error(const Grav3DRansacData& samples, const std::vector<size_t>& sampleIndices,
                    GravPoseRansacModel& model, std::vector<size_t>& inlier_indices,
                    std::vector<size_t>& outlier_indices, double error_threshold) const;

   const Eigen::Quaterniond Q;
   const Eigen::Matrix3d& KI;
   const Eigen::Matrix3d K, R;
};

#endif // USE_THEIA_RANSAC

#endif //_POSE2DRANSAC_H


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2021,
 *  Max Planck Institute for Intelligent Systems (MPI-IS).
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the MPI-IS nor the names
 *     of its contributors may be used to endorse or promote products
 *     derived from this software without specific prior written
 *     permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Andreas Orthey */

#include <ompl/base/spaces/TorusStateSpace.h>
#include <ompl/tools/config/MagicConstants.h>
#include <cstring>

#include <boost/math/constants/constants.hpp>
using namespace boost::math::double_constants;  // pi
using namespace ompl::base;

TorusStateSampler::TorusStateSampler(const StateSpace *space) : StateSampler(space)
{
}

void TorusStateSampler::sampleUniform(State *state)
{
    // https://stackoverflow.com/questions/26300510/generating-random-points-on-a-surface-of-an-n-dimensional-torus
    // Based on publication "Random selection of points distributed on curved surfaces."
    // Link: https://iopscience.iop.org/article/10.1088/0031-9155/32/10/009/pdf
    const auto *T = static_cast<const TorusStateSpace *>(space_);

    bool acceptedSampleFound = false;
    while (!acceptedSampleFound)
    {
        double u = rng_.uniformReal(-pi, pi);
        double v = rng_.uniformReal(-pi, pi);

        const double &R = T->getMajorRadius();
        const double &r = T->getMinorRadius();

        double vprime = (R + r * cos(v)) / (R + r);

        double mu = rng_.uniformReal(0, 1);
        if (mu <= vprime)
        {
            TorusStateSpace::StateType *T = state->as<TorusStateSpace::StateType>();
            T->setS1S2(u, v);
            acceptedSampleFound = true;
        }
    }
}

void TorusStateSampler::sampleUniformNear(State *state, const State *near, double distance)
{
    TorusStateSpace::StateType *T = state->as<TorusStateSpace::StateType>();
    const TorusStateSpace::StateType *Tnear = near->as<TorusStateSpace::StateType>();
    T->setS1(rng_.uniformReal(Tnear->getS1() - distance, Tnear->getS1() + distance));
    T->setS2(rng_.uniformReal(Tnear->getS2() - distance, Tnear->getS2() + distance));
    space_->enforceBounds(state);
}

void TorusStateSampler::sampleGaussian(State *state, const State *mean, double stdDev)
{
    TorusStateSpace::StateType *T = state->as<TorusStateSpace::StateType>();
    const TorusStateSpace::StateType *Tmean = mean->as<TorusStateSpace::StateType>();
    T->setS1(rng_.gaussian(Tmean->getS1(), stdDev));
    T->setS2(rng_.gaussian(Tmean->getS2(), stdDev));

    space_->enforceBounds(state);
}

TorusStateSpace::TorusStateSpace(double majorRadius, double minorRadius)
  : majorRadius_(majorRadius), minorRadius_(minorRadius)
{
    setName("Torus" + getName());
    type_ = STATE_SPACE_TORUS;
    addSubspace(std::make_shared<SO2StateSpace>(), 1.0);
    addSubspace(std::make_shared<SO2StateSpace>(), 1.0);
    lock();
}

StateSamplerPtr TorusStateSpace::allocDefaultStateSampler() const
{
    return std::make_shared<TorusStateSampler>(this);
}

double TorusStateSpace::distance(const State *state1, const State *state2) const
{
    const auto *cstate1 = static_cast<const CompoundState *>(state1);
    const auto *cstate2 = static_cast<const CompoundState *>(state2);
    double x = components_[0]->distance(cstate1->components[0], cstate2->components[0]);
    double y = components_[1]->distance(cstate1->components[1], cstate2->components[1]);
    return sqrtf(x * x + y * y);
}

State *TorusStateSpace::allocState() const
{
    auto *state = new StateType();
    allocStateComponents(state);
    return state;
}

double TorusStateSpace::getMajorRadius() const
{
    return majorRadius_;
}

double TorusStateSpace::getMinorRadius() const
{
    return minorRadius_;
}

Eigen::Vector3f TorusStateSpace::toVector(const State *state) const
{
    Eigen::Vector3f v;

    const TorusStateSpace::StateType *s = state->as<TorusStateSpace::StateType>();
    float theta = s->getS1();
    float phi = s->getS2();

    const double &R = majorRadius_;
    const double &r = minorRadius_;

    v[0] = (R + r*cos(phi))*cos(theta);
    v[1] = (R + r*cos(phi))*sin(theta);
    v[2] = r*sin(phi);

    return v;
}


/********************************************************************
	created:	2012/08/27
	created:	27:8:2012   15:04
	filename: 	VietesFormulaeTest.cpp
	file path:	test
	file base:	VietesFormulaeTest
	file ext:	cpp
	author:		Han Hu

	purpose:
*********************************************************************/

#include "GAGPL/MATH/VietesFormulae.h"
#include <boost/foreach.hpp>
#include <iostream>

int main()
{
	using namespace msmath;
	using namespace std;

    double mycoef[] = {3, 2, 0, 5, 1};
    PolyCoef poly_eq(mycoef, mycoef + sizeof(mycoef) / sizeof(double));

    VietesFormulae formulae(poly_eq);
    EleSymPolyVec ele_values = formulae.getElementarySymmetricFunctionFromCoef();

    // The correct value should be: 1, -5, 0, -2, 3.
    for(size_t i=0; i<ele_values.size(); i++)
    {
        cout << i << ": " << ele_values.at(i) << endl;
    }

    cout << "The End!" << endl;

    cin.get();

}


//          Copyright Carl Philipp Reh 2009 - 2016.
// 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 <fcppt/algorithm/contains.hpp>
#include <fcppt/math/box/corner_points.hpp>
#include <fcppt/math/box/object_impl.hpp>
#include <fcppt/math/box/output.hpp>
#include <fcppt/math/vector/comparison.hpp>
#include <fcppt/math/vector/object_impl.hpp>
#include <fcppt/math/vector/output.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <array>
#include <iostream>
#include <ostream>
#include <fcppt/config/external_end.hpp>

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	math_box_corner_points
)
{
FCPPT_PP_POP_WARNING

	typedef
	fcppt::math::box::object<
		int,
		2
	>
	box_type;

	box_type box(
		box_type::vector(
			10,
			12
		),
		box_type::dim(24,26)
	);

	std::cout
		<<
		"Checking border points of box: "
		<<
		box
		<<
		'\n';

	typedef
	std::array<
		box_type::vector,
		4
	>
	vertex_array;

	vertex_array const vertices(
		fcppt::math::box::corner_points(
			box
		)
	);

	std::cout
		<<
		"Result: \n";

	for(
		auto const &elem
		:
		vertices
	)
		std::cout
			<<
			elem
			<<
			'\n';

	BOOST_CHECK(
		fcppt::algorithm::contains(
			vertices,
			box_type::vector(
				10,
				12
			)
		)
	);

	BOOST_CHECK(
		fcppt::algorithm::contains(
			vertices,
			box_type::vector(
				34,
				12
			)
		)
	);

	BOOST_CHECK(
		fcppt::algorithm::contains(
			vertices,
			box_type::vector(
				10,
				38
			)
		)
	);

	BOOST_CHECK(
		fcppt::algorithm::contains(
			vertices,
			box_type::vector(
				34,
				38
			)
		)
	);
}


#include <armadillo>

using namespace arma;
using namespace std;

void crearTuplas(string rutaArchivo,
                 int nEntradas,
                 int nSalidas,
                 string rutaNuevoArchivo)
{
    mat datos;
    datos.load(rutaArchivo);

    vec meses = datos.col(0);
    vec ventas = datos.col(1);

    meses = (meses - min(meses)) / (max(meses) - min(meses));
    ventas = (ventas - min(ventas)) / (max(ventas) - min(ventas));

    const int longitudTupla = 1 + nEntradas + nSalidas;

	// FIXME: Cuando la cantidad de salidas es mayor a 1
	// no se calcula bien la cantidad de tuplas
    mat tuplas(ventas.n_elem - nEntradas - nSalidas, longitudTupla);

	for (unsigned int i = 0; i < tuplas.n_rows; ++i) {
        const rowvec tupla = join_horiz(rowvec{meses(i)},
                                        ventas(span(i, i + longitudTupla - 2)).t());
		tuplas.row(i) = tupla;
	}

	tuplas.save(rutaNuevoArchivo, arma::csv_ascii);
}

void crearTuplas(string rutaArchivo1,
                 string rutaArchivo2,
                 string rutaArchivo3,
                 int nEntradas,
                 int nSalidas,
                 string rutaNuevoArchivo)
{

	mat datos1;
	vec datos2;
	vec datos3;
	datos1.load(rutaArchivo1);
	datos2.load(rutaArchivo2);
	datos3.load(rutaArchivo3);

    vec meses = datos1.col(0);
	vec ventas = datos1.col(1);

    // Normalizar datos de exportaciones e importacioes
    meses = (meses - min(meses)) / (max(meses) - min(meses));
	ventas = (ventas - min(ventas)) / (max(ventas) - min(ventas));
	datos2 = (datos2 - min(datos2)) / (max(datos2) - min(datos2));
	datos3 = (datos3 - min(datos3)) / (max(datos3) - min(datos3));

	const int longitudTupla = 1 + nEntradas * 3 + nSalidas;

	mat tuplas(ventas.n_elem - nEntradas - nSalidas - 1, longitudTupla);

	for (unsigned int i = 0; i < tuplas.n_rows; ++i) {
		tuplas(i, 0) = meses(i);

		tuplas(i, span(1, tuplas.n_cols - nSalidas - 1))
		    = join_vert(join_vert(ventas.rows(span(i, i + nEntradas - 1)),
		                          datos2.rows(span(i, i + nEntradas - 1))),
		                datos3.rows(span(i, i + nEntradas - 1)))
		          .t();

		tuplas(i,
		       span(tuplas.n_cols - nSalidas,
		            tuplas.n_cols - 1))
		    = ventas(span(i + nEntradas,
		                  i + nEntradas + nSalidas - 1))
		          .t();
	}

	tuplas.save(rutaNuevoArchivo, arma::csv_ascii);
}


#include "tutorial_shared_path.h"
#include <igl/fast_winding_number.h>
#include <igl/read_triangle_mesh.h>
#include <igl/slice_mask.h>
#include <Eigen/Geometry>
#include <igl/octree.h>
#include <igl/barycenter.h>
#include <igl/knn.h>
#include <igl/random_points_on_mesh.h>
#include <igl/bounding_box_diagonal.h>
#include <igl/per_face_normals.h>
#include <igl/copyleft/cgal/point_areas.h>
#include <igl/opengl/glfw/Viewer.h>
#include <igl/get_seconds.h>
#include <iostream>
#include <cstdlib>

int main(int argc, char *argv[])
{
  const auto time = [](std::function<void(void)> func)->double
  {
    const double t_before = igl::get_seconds();
    func();
    const double t_after = igl::get_seconds();
    return t_after-t_before;
  };

  Eigen::MatrixXd V;
  Eigen::MatrixXi F;
  igl::read_triangle_mesh(argc>1?argv[1]:TUTORIAL_SHARED_PATH "/bunny.off",V,F);
  // Sample mesh for point cloud
  Eigen::MatrixXd P,N;
  {
    Eigen::VectorXi I;
    Eigen::SparseMatrix<double> B;
    igl::random_points_on_mesh(10000,V,F,B,I);
    P = B*V;
    Eigen::MatrixXd FN;
    igl::per_face_normals(V,F,FN);
    N.resize(P.rows(),3);
    for(int p = 0;p<I.rows();p++)
    {
      N.row(p) = FN.row(I(p));
    }
  }
  // Build octree
  std::vector<std::vector<int > > O_PI;
  Eigen::MatrixXi O_CH;
  Eigen::MatrixXd O_CN;
  Eigen::VectorXd O_W;
  igl::octree(P,O_PI,O_CH,O_CN,O_W);
  Eigen::VectorXd A;
  {
    Eigen::MatrixXi I;
    igl::knn(P,20,O_PI,O_CH,O_CN,O_W,I);
    // CGAL is only used to help get point areas
    igl::copyleft::cgal::point_areas(P,I,N,A);
  }

  if(argc<=1)
  {
    // corrupt mesh
    Eigen::MatrixXd BC;
    igl::barycenter(V,F,BC);
    Eigen::MatrixXd OV = V;
    V.resize(F.rows()*3,3);
    for(int f = 0;f<F.rows();f++)
    {
      for(int c = 0;c<3;c++)
      {
        int v = f+c*F.rows();
        // random rotation about barycenter
        Eigen::AngleAxisd R(
          0.5*static_cast <double> (rand()) / static_cast <double> (RAND_MAX),
          Eigen::Vector3d::Random(3,1));
        V.row(v) = (OV.row(F(f,c))-BC.row(f))*R.matrix()+BC.row(f);
        F(f,c) = v;
      }
    }
  }

  // Generate a list of random query points in the bounding box
  Eigen::MatrixXd Q = Eigen::MatrixXd::Random(1000000,3);
  const Eigen::RowVector3d Vmin = V.colwise().minCoeff();
  const Eigen::RowVector3d Vmax = V.colwise().maxCoeff();
  const Eigen::RowVector3d Vdiag = Vmax-Vmin;
  for(int q = 0;q<Q.rows();q++)
  {
    Q.row(q) = (Q.row(q).array()*0.5+0.5)*Vdiag.array() + Vmin.array();
  }

  // Positions of points inside of point cloud P
  Eigen::MatrixXd QiP;
  {
    Eigen::MatrixXd O_CM;
    Eigen::VectorXd O_R;
    Eigen::MatrixXd O_EC;
    printf("  point cloud precomputation: %g secs\n",
      time([&](){igl::fast_winding_number(P,N,A,O_PI,O_CH,2,O_CM,O_R,O_EC);}));
    Eigen::VectorXd WiP;
    printf("      point cloud evaluation: %g secs\n",
      time([&](){igl::fast_winding_number(P,N,A,O_PI,O_CH,O_CM,O_R,O_EC,Q,2,WiP);}));
    igl::slice_mask(Q,WiP.array()>0.5,1,QiP);
  }

  // Positions of points inside of triangle soup (V,F)
  Eigen::MatrixXd QiV;
  {
    igl::FastWindingNumberBVH fwn_bvh;
    printf("triangle soup precomputation: %g secs\n",
      time([&](){igl::fast_winding_number(V.cast<float>(),F,2,fwn_bvh);}));
    Eigen::VectorXf WiV;
    printf("    triangle soup evaluation: %g secs\n",
      time([&](){igl::fast_winding_number(fwn_bvh,2,Q.cast<float>(),WiV);}));
    igl::slice_mask(Q,WiV.array()>0.5,1,QiV);
  }

  // Visualization
  igl::opengl::glfw::Viewer viewer;
  // For dislpaying normals as little line segments
  Eigen::MatrixXd PN(2*P.rows(),3);
  Eigen::MatrixXi E(P.rows(),2);
  const double bbd = igl::bounding_box_diagonal(V);
  for(int p = 0;p<P.rows();p++)
  {
    E(p,0) = 2*p;
    E(p,1) = 2*p+1;
    PN.row(E(p,0)) = P.row(p);
    PN.row(E(p,1)) = P.row(p)+bbd*0.01*N.row(p);
  }

  bool show_P = false;
  int show_Q = 0;

  int query_data = 0;
  viewer.data_list[query_data].set_mesh(V,F);
  viewer.data_list[query_data].clear();
  viewer.data_list[query_data].point_size = 2;
  viewer.append_mesh();
  int object_data = 1;
  viewer.data_list[object_data].set_mesh(V,F);
  viewer.data_list[object_data].point_size = 5;

  const auto update = [&]()
  {
    viewer.data_list[query_data].clear();
    switch(show_Q)
    {
      case 1:
        // show all Q
        viewer.data_list[query_data].set_points(Q,Eigen::RowVector3d(0.996078,0.760784,0.760784));
        break;
      case 2:
        // show all Q inside
        if(show_P)
        {
          viewer.data_list[query_data].set_points(QiP,Eigen::RowVector3d(0.564706,0.847059,0.768627));
        }else
        {
          viewer.data_list[query_data].set_points(QiV,Eigen::RowVector3d(0.564706,0.847059,0.768627));
        }
        break;
    }

    viewer.data_list[object_data].clear();
    if(show_P)
    {
      viewer.data_list[object_data].set_points(P,Eigen::RowVector3d(1,1,1));
      viewer.data_list[object_data].set_edges(PN,E,Eigen::RowVector3d(0.8,0.8,0.8));
    }else
    {
      viewer.data_list[object_data].set_mesh(V,F);
    }
  };

  viewer.callback_key_pressed =
    [&](igl::opengl::glfw::Viewer &, unsigned int key, int mod)
  {
    switch(key)
    {
      default:
        return false;
      case '1':
        show_P = !show_P;
        break;
      case '2':
        show_Q = (show_Q+1) % 3;
        break;
    }
    update();
    return true;
  };

  update();
  viewer.launch();

}


#include <dlib/matrix.h>
#include <dlib/svm.h>
#include <plot.h>

#include "isolation-forest.h"

#include <experimental/filesystem>
#include <iostream>
#include <unordered_map>

using namespace dlib;
namespace fs = std::experimental::filesystem;

const std::vector<std::string> colors{"black", "red",    "blue",  "green",
                                      "cyan",  "yellow", "brown", "magenta"};

using DataType = double;
using Matrix = matrix<DataType>;
using Coords = std::vector<DataType>;
using PointCoords = std::pair<Coords, Coords>;
using Clusters = std::unordered_map<size_t, PointCoords>;

void PlotClusters(const Clusters& clusters,
                  const std::string& name,
                  const std::string& file_name) {
  plotcpp::Plot plt(true);
  // plt.SetTerminal("qt");
  plt.SetTerminal("png");
  plt.SetOutput(file_name);
  plt.SetTitle(name);
  plt.SetXLabel("x");
  plt.SetYLabel("y");
  // plt.SetAutoscale();
  plt.GnuplotCommand("set size square");
  plt.GnuplotCommand("set grid");

  auto draw_state = plt.StartDraw2D<Coords::const_iterator>();
  for (auto& cluster : clusters) {
    std::stringstream params;
    params << "lc rgb '" << colors[cluster.first] << "' pt 7";
    plt.AddDrawing(draw_state,
                   plotcpp::Points(
                       cluster.second.first.begin(), cluster.second.first.end(),
                       cluster.second.second.begin(),
                       std::to_string(cluster.first) + " cls", params.str()));
  }

  plt.EndDraw2D(draw_state);
  plt.Flush();
}

void MultivariateGaussianDist(const Matrix& normal,
                              const Matrix& test,
                              const std::string& file_name) {
  // assume that rows are samples and columns are features

  // calculate per feature mean
  dlib::matrix<double> mu(1, normal.nc());
  dlib::set_all_elements(mu, 0);

  for (long c = 0; c < normal.nc(); ++c) {
    auto col_mean = dlib::mean(dlib::colm(normal, c));
    dlib::set_colm(mu, c) = col_mean;
  }

  // calculate covariance matrix
  dlib::matrix<double> cov(normal.nc(), normal.nc());
  dlib::set_all_elements(cov, 0);
  for (long r = 0; r < normal.nr(); ++r) {
    auto row = dlib::rowm(normal, r);
    cov += dlib::trans(row - mu) * (row - mu);
  }
  cov *= 1.0 / normal.nr();
  double cov_det = dlib::det(cov);
  dlib::matrix<double> cov_inv = dlib::inv(cov);

  auto first_part =
      1. / std::pow(2. * M_PI, normal.nc() / 2.) / std::sqrt(cov_det);

  // define probability function
  auto prob = [&](const dlib::matrix<double>& sample) {
    dlib::matrix<double> s = sample - mu;
    dlib::matrix<double> exp_val_m = s * (cov_inv * dlib::trans(s));
    double exp_val = -0.5 * exp_val_m(0, 0);
    double p = first_part * std::exp(exp_val);
    return p;
  };

  Clusters clusters;  // there will two clusters with normal and anomaly data

  // change this parameter to see descision boundary
  double prob_threshold = 0.001;

  auto detect = [&](auto samples) {
    for (long r = 0; r < samples.nr(); ++r) {
      auto row = dlib::rowm(samples, r);
      double x = row(0, 0);
      double y = row(0, 1);
      auto p = prob(row);
      if (p >= prob_threshold) {
        clusters[0].first.push_back(x);
        clusters[0].second.push_back(y);
      } else {
        clusters[1].first.push_back(x);
        clusters[1].second.push_back(y);
      }
    }
  };

  detect(normal);
  detect(test);
  PlotClusters(clusters, "Multivariate Gaussian Distribution", file_name);
}

void OneClassSvm(const Matrix& normal,
                 const Matrix& test,
                 const std::string& file_name) {
  typedef matrix<double, 0, 1> sample_type;
  typedef radial_basis_kernel<sample_type> kernel_type;
  svm_one_class_trainer<kernel_type> trainer;
  trainer.set_nu(0.5);                   // control smoothness of the solution
  trainer.set_kernel(kernel_type(0.5));  // kernel bandwidth
  std::vector<sample_type> samples;
  for (long r = 0; r < normal.nr(); ++r) {
    auto row = rowm(normal, r);
    samples.push_back(row);
  }
  decision_function<kernel_type> df = trainer.train(samples);
  Clusters clusters;
  double threshold = -2.0;

  auto detect = [&](auto samples) {
    for (long r = 0; r < samples.nr(); ++r) {
      auto row = dlib::rowm(samples, r);
      double x = row(0, 0);
      double y = row(0, 1);
      auto p = df(row);
      if (p > threshold) {
        clusters[0].first.push_back(x);
        clusters[0].second.push_back(y);
      } else {
        clusters[1].first.push_back(x);
        clusters[1].second.push_back(y);
      }
    }
  };

  detect(normal);
  detect(test);
  PlotClusters(clusters, "One Class SVM", file_name);
}

void IsolationForest(const Matrix& normal,
                     const Matrix& test,
                     const std::string& file_name) {
  iforest::Dataset<2> dataset;

  auto put_to_dataset = [&](const Matrix& samples) {
    for (long r = 0; r < samples.nr(); ++r) {
      auto row = dlib::rowm(samples, r);
      double x = row(0, 0);
      double y = row(0, 1);
      dataset.push_back({x, y});
    }
  };

  put_to_dataset(normal);
  put_to_dataset(test);

  iforest::IsolationForest iforest(dataset, 300, 50);

  Clusters clusters;
  double threshold = 0.6;  // change this value to see isolation boundary
  for (auto& s : dataset) {
    auto anomaly_score = iforest.AnomalyScore(s);
    // std::cout << anomaly_score << " " << s[0] << " " << s[1] << std::endl;

    if (anomaly_score < threshold) {
      clusters[0].first.push_back(s[0]);
      clusters[0].second.push_back(s[1]);
    } else {  // anomaly
      clusters[1].first.push_back(s[0]);
      clusters[1].second.push_back(s[1]);
    }
  }

  PlotClusters(clusters, "Isolation Forest", file_name);
}

using Dataset = std::pair<Matrix, Matrix>;

Dataset LoadDataset(const fs::path& file_path) {
  if (fs::exists(file_path)) {
    std::ifstream file(file_path);
    matrix<DataType> data;
    file >> data;

    long n_normal = 50;
    Matrix normal =
        dlib::subm(data, range(0, n_normal - 1), range(0, data.nc() - 1));
    Matrix test = dlib::subm(data, range(n_normal, data.nr() - 1),
                             range(0, data.nc() - 1));
    return {normal, test};
  } else {
    std::string msg = "Dataset file " + file_path.string() + " missed\n";
    throw std::invalid_argument(msg);
  }
}

Dataset CombineDatasets(const Dataset& a, const Dataset& b) {
  Matrix normal(a.first.nr() + b.first.nr(), a.first.nc());
  set_subm(normal, range(0, a.first.nr() - 1), range(0, a.first.nc() - 1)) =
      a.first;
  set_subm(normal, range(a.first.nr(), normal.nr() - 1),
           range(0, a.first.nc() - 1)) = b.first;

  Matrix test(a.second.nr() + b.second.nr(), a.second.nc());
  set_subm(test, range(0, a.second.nr() - 1), range(0, a.second.nc() - 1)) =
      a.second;
  set_subm(test, range(a.second.nr(), test.nr() - 1),
           range(0, a.second.nc() - 1)) = b.second;

  return {normal, test};
}

int main(int argc, char** argv) {
  if (argc > 1) {
    try {
      auto base_dir = fs::path(argv[1]);

      std::string data_name_multi{"multivar.csv"};
      std::string data_name_uni{"univar.csv"};

      auto dataset_multi = LoadDataset(base_dir / data_name_multi);
      auto dataset_uni = LoadDataset(base_dir / data_name_uni);

      MultivariateGaussianDist(dataset_multi.first, dataset_multi.second,
                               "dlib-multi-var.png");
      OneClassSvm(dataset_multi.first, dataset_multi.second, "dlib-ocsvm.png");

      // make dataset with two clusters

      auto dataset_combi = CombineDatasets(dataset_multi, dataset_uni);
      OneClassSvm(dataset_combi.first, dataset_combi.second,
                  "dlib-ocsvm_two.png");

      IsolationForest(dataset_combi.first, dataset_combi.second,
                      "dlib-iforest-two.png");

    } catch (const std::exception& err) {
      std::cerr << err.what();
    }
  } else {
    std::cerr << "Please provider path to the datasets folder\n";
  }
  return 0;
}


#include "floor.hpp"
#include "ast.hpp"
#include "parsers.hpp"
#include "types.hpp"
#include <boost/variant.hpp>
#include <stdexcept>
namespace HT
{
    void floor(PASTNode astnode, ParsersHelper& ph)
    {
        auto myParserHelper(ph);
        if (astnode->ch.size()!=2)
          throw std::runtime_error("floor can only have one parameter");
        auto & secondCh = *astnode->ch.rbegin();
        ph.parse(secondCh);
        if (secondCh->token.tokenType != Complex || ! boost::get<ComplexType>(secondCh->token.info).isReal())
          throw std::runtime_error("The argument of floor must be real");
        auto cast = boost::get<ComplexType>(secondCh->token.info);

        astnode->type = Simple;
        astnode->token.tokenType = Complex;

        if (!cast.exact())
          astnode->token.info = ComplexType(std::floor(cast.getRealD()));
        else
        {
            auto rat = cast.getRealR();
            if (rat.getSign())
              astnode->token.info = ComplexType( rat.getUp() / rat.getDown() );
            else
              if (rat.isInt())
                astnode->token.info = cast;
            else
            {
                auto res = rat.getUp() / rat.getDown();
                res.setSign(false);
                res -= 1;
                astnode->token.info = ComplexType(res);
            }
        }

        astnode->remove();
    }

}


/**
 * Copyright (c) 2018, The Akatsuki(Jacob.lsx). All rights reserved.
 */

#include <iostream>

#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/SVD>

#include <ceres/ceres.h>
#include <ceres/rotation.h>

#include <chrono>

using namespace std;
using namespace cv;

/* find the two photo feature matches points */
void find_feature_matches(const cv::Mat&, const cv::Mat&, std::vector<cv::KeyPoint>&, std::vector<cv::KeyPoint>&, std::vector<cv::DMatch>&);

/* Converts the pixel coordinate system to the normalized imaging plane coordinate system */
cv::Point2d pixel2cam(const cv::Point2d&, const cv::Mat&);

/*  */
void pose_estimation_3d3d(const std::vector<cv::Point3f>&, const std::vector<cv::Point3f>&, cv::Mat&, cv::Mat&);

/*  */
void bundleAdjustment(const std::vector<cv::Point3f>&, const std::vector<cv::Point3f>&, cv::Mat&, cv::Mat&);

int main(int argc, char** argv)
{
    google::InitGoogleLogging(argv[0]);
    if (argc != 5) {
        cout << "usage: pose_estimation_3d3d img1 img2 depth1 depth2." << endl;
        return 1;
    }

    //-- 1st, read photo
    cv::Mat img_1 = cv::imread(argv[1], CV_LOAD_IMAGE_COLOR);
    cv::Mat img_2 = cv::imread(argv[2], CV_LOAD_IMAGE_COLOR);
    if (img_1.empty() || img_2.empty()) {
        cerr << "img1 or img2 is empty." << endl;
        return 1;
    }

    //-- 2nd, find the two photo feature matches points
    std::vector<cv::KeyPoint> keypoints_1, keypoints_2;
    std::vector<cv::DMatch> matches;
    find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
    cout << "feature matches points total is " << matches.size() << endl;

    //-- 3rd, create 3 dimensions point correspondences
    cv::Mat depth1 = cv::imread(argv[3], CV_LOAD_IMAGE_UNCHANGED);      // 深度图为16位无符号数，单通道图像
    cv::Mat depth2 = cv::imread(argv[4], CV_LOAD_IMAGE_UNCHANGED);
    if (depth1.empty() || depth2.empty()) {
        cerr << "depth1 or depth2 is empty." << endl;
        return 1;
    }

    cv::Mat K = (cv::Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    std::vector<cv::Point3f> pts1, pts2;

    for (cv::DMatch m : matches) {
        ushort d1 = depth1.ptr<unsigned short>((int)keypoints_1[m.queryIdx].pt.y)[(int)keypoints_1[m.queryIdx].pt.x];
        ushort d2 = depth2.ptr<unsigned short>((int)keypoints_2[m.trainIdx].pt.y)[(int)keypoints_2[m.trainIdx].pt.x];
        if (d1 == 0 || d2 == 0) {
            continue;
        }
        cv::Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
        cv::Point2d p2 = pixel2cam(keypoints_2[m.trainIdx].pt, K);
        float dd1 = float(d1) / 5000.0;
        float dd2 = float(d2) / 5000.0;
        pts1.push_back(cv::Point3f(p1.x * dd1, p1.y * dd1, dd1));
        pts2.push_back(cv::Point3f(p2.x * dd2, p2.y * dd2, dd2));
    }
    cout << "3d-3d pairs: " << pts1.size() << endl;

    cv::Mat R, t;
    pose_estimation_3d3d(pts1, pts2, R, t);
    cout << "ICP vid SVD results: " << endl;
    cout << "R = " << R << endl;
    cout << "t = " << t << endl;
    cout << "R_inv = " << R.t() << endl;
    cout << "t_inv = " << -R.t() * t << endl;

    cout << "calling bundle adjustment. \r\n";

    bundleAdjustment(pts1, pts2, R, t);

    // verify p1 = R*p2 + t
    for (int i = 0; i < 5; i ++) {
        cout << "p1 = " << pts1[i] << endl;
        cout << "p2 = " << pts2[i] << endl;
        cout << "R*p2 + t = " << R * (Mat_<double>(3, 1) << pts2[i].x, pts2[i].y, pts2[i].z) + t << endl;
        cout << endl;
    }

    return 0;
}

/**
 * find the two photo feature matches points
 */
void find_feature_matches(const cv::Mat& img_1, const cv::Mat& img_2, std::vector<cv::KeyPoint>& keypoints_1, std::vector<cv::KeyPoint>& keypoints_2, std::vector<cv::DMatch>& matches)
{
    //-- Initialize
    cv::Mat descriptors_1, descriptors_2;
    cv::Ptr<cv::FeatureDetector> detector = cv::ORB::create();
    cv::Ptr<cv::DescriptorExtractor> descriptor = cv::ORB::create();
    cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");

    //-- 1st: detect Oriented FAST KeyPoint
    detector->detect(img_1, keypoints_1);
    detector->detect(img_2, keypoints_2);

    //-- 2nd: Calculates BRIEF descriptors with KeyPoint's coordinate
    descriptor->compute(img_1, keypoints_1, descriptors_1);
    descriptor->compute(img_2, keypoints_2, descriptors_2);

    //-- 3rd: 对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    std::vector<cv::DMatch> match;
    matcher->match(descriptors_1, descriptors_2, match);

    //-- 4th: 匹配点对筛选
    double min_dist = 10000, max_dist = 0;

    /* 找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离 */
    for (int i = 0; i < descriptors_1.rows; i++) {
        double dist = match[i].distance;
        if (dist < min_dist) {
            min_dist = dist;
        }
        if (dist > max_dist) {
            max_dist = dist;
        }
    }

    printf("-- Max distance : %f \r\n", max_dist);
    printf("-- Min distance : %f \r\n", min_dist);

    // 当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
    for (int i = 0; i < descriptors_2.rows; i ++) {
        if (match[i].distance <= max(2*min_dist, 30.0)) {
            matches.push_back(match[i]);
        }
    }
}

/**
 * Converts the pixel coordinate system to the normalized imaging plane coordinate system
 */
cv::Point2d pixel2cam(const cv::Point2d& p, const cv::Mat& K)
{
    return cv::Point2d
        (
            (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
            (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
        );
}

void pose_estimation_3d3d(const std::vector<cv::Point3f>& pts1, const std::vector<cv::Point3f>& pts2, cv::Mat& R, cv::Mat& t)
{
    // center of mass
    cv::Point3f p1, p2;
    int N = pts1.size();
    for (int i = 0; i < N; i ++) {
        p1 += pts1[i];
        p2 += pts2[i];
    }
    p1 = cv::Point3f(Vec3f(p1) / N);
    p2 = cv::Point3f(Vec3f(p2) / N);

    // remove the center
    std::vector<cv::Point3f> q1(N), q2(N);
    for (int i = 0; i < N; i ++) {
        q1[i] = pts1[i] - p1;
        q2[i] = pts2[i] - p2;
    }

    // compute q1*q2^T
    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for (int i = 0; i < N; i ++) {
        W += Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z) * Eigen::Vector3d(q2[i].x, q2[i].y, q2[i].z).transpose();
    }
    cout << "W = " << W << endl;

    // SVD on W
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();

    // 利用SVD求解3D-3D变换，需要U和V的行列式同号。换言之，旋转矩阵的行列式只能为1，不能为-1
    if (U.determinant() * V.determinant() < 0) {
        for (int x = 0; x < 3; ++x) {
            U(x, 2) *= -1;
        }
    }
    cout << "U = " << U << endl;
    cout << "V = " << V << endl;

    Eigen::Matrix3d R_ = U * (V.transpose());
    Eigen::Vector3d t_ = Eigen::Vector3d(p1.x, p1.y, p1.z) - R_ * Eigen::Vector3d(p2.x, p2.y, p2.z);

    // convert to cv::mat
    R = (cv::Mat_<double>(3, 3) <<
        R_(0, 0), R_(0, 1), R_(0, 2),
        R_(1, 0), R_(1, 1), R_(1, 2),
        R_(2, 0), R_(2, 1), R_(2, 2)
    );
    t = (cv::Mat_<double>(3, 1) << t_(0, 0), t_(1, 0), t_(2, 0));
}

class ReprojectionError {
public:
    ReprojectionError(const cv::Point3f &point1, const cv::Point3f &point2) : pt1_(point1), pt2_(point2) {}

    template<typename T>
    bool operator() (const T* const r, const T* const t, T* residuals) const
    {
        T p[3];
        T pt2[3] = {(T)pt2_.x, (T)pt2_.y, (T)pt2_.z};

        ceres::AngleAxisRotatePoint(r, pt2, p);

        p[0] += t[0];
        p[1] += t[1];
        p[2] += t[2];

        residuals[0] = p[0] - (T)pt1_.x;
        residuals[1] = p[1] - (T)pt1_.y;
        residuals[2] = p[2] - (T)pt1_.z;

        return true;
    }

    static ceres::CostFunction* create(const cv::Point3f &point1, const cv::Point3f &point2)
    {
        return new ceres::AutoDiffCostFunction<ReprojectionError, 3, 3, 3>(new ReprojectionError(point1, point2));
    }

private:
    cv::Point3f pt1_, pt2_;
};

void bundleAdjustment(const std::vector<cv::Point3f>& pts1, const std::vector<cv::Point3f>& pts2, cv::Mat& R, cv::Mat& t)
{
    cv::Mat r;
    cv::Rodrigues(R, r);

    double rotation[3] = {0.f};
    double translation[3] = {0.f};

#if 1
    for (int i = 0; i < 3; i++) {
        rotation[i] = r.at<double>(i, 0);
        translation[i] = r.at<double>(i, 0);
    }
#endif

    ceres::Problem problem;
    for (int i = 0; i < pts1.size(); i ++) {
        ceres::CostFunction *costFunction = ReprojectionError::create(pts1[i], pts2[i]);
        problem.AddResidualBlock(costFunction, nullptr, rotation, translation);
    }

    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_SCHUR;
    options.minimizer_progress_to_stdout = true;
    ceres::Solver::Summary summary;

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();

    ceres::Solve(options, &problem, &summary);

    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> (t2 - t1);

    cout << summary.BriefReport() << endl;

    cout << "optimization costs time: " << time_used.count() << " seconds." << "\r\n";

    cout << endl << "after optimization: " << endl;

    cv::Mat R_vec = (cv::Mat_<double>(3, 1) << rotation[0], rotation[1], rotation[2]);
    cv::Mat Rotation;
    cv::Rodrigues(R_vec, Rotation);

    cout << "R = \r\n" << Rotation << endl;
    cout << "t = \r\n" << translation[0] << ", " << translation[1] << ", " << translation[2] << endl;

    R = Rotation;
    t = (cv::Mat_<double>(3,1) << translation[0], translation[1], translation[2]);
}


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

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_DIST_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_DIST_HPP_INCLUDED

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

 /*!

    @ingroup group-arithmetic
    Function object implementing dist

    Computes the absolute value of the difference of its parameters.

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

    @code
    T r = dist(x, y);
    @endcode

    is similar to:

    @code
    T r = abs(x-y);
    @endcode

    @par Note

    The result may be negative for signed integers as @ref abs(@ref Valmin) is @ref Valmin.
    To avoid the problem you can apply to dist the saturated_ @ref decorator.

    @par Decorators

     - saturated_ decorator garanties that saturated_(dist)(x, y)) will never be strictly less than 0.

    @see  ulpdist, abs

  **/
  Value dist(Option const& o, Value const & v0, Value const& v1);

  //@overload
  Value dist(Value const & v0, Value const& v1);
} }
#endif

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

#endif


/**
 * Multidimensional Static Matrix C++11
 * Copyright Emanuele Ruffaldi (2015) at Scuola Superiore Sant'Anna Pisa
 *
 * AKA extreme parameter packs
 *
 * Core functionalities ... the rest is "trivial"
 *
 * Under Apache License
 */
#pragma once
#include <Eigen/Dense>
#include <initializer_list>
#include <iostream>
#include <type_traits>
#include "multidim_details.hpp"

namespace multidim
{
	template <class T, class TS>
	class MultiDimNView;

	/**
	 * Base class of Multidimensional Static matrix of elements of type T
	 *
	 * TS is a sequence of types, where each type is (size,step)
	 *
	 */
	template <class T, class TS>
	class MultiDimNBase
	{
	public:
		using value_t = T;
		static constexpr int Ncount = TS::size;
		static constexpr int Ntot = details::productseq<TS>::value;
		using indexvector_t = Eigen::Matrix<int,Ncount,1>; // instead of std::vector<int>

		template <int i>
		using getsizetype = std::integral_constant<int,TS::template pick<i>::xsize>;

		template <int i>
		using getsteptype = std::integral_constant<int,TS::template pick<i>::xstep>;

		template <int i>
		constexpr int getssize() const { return details::saccessorseq<i,TS>::xsize; }

		/// can't move in base
		template <int i>
		constexpr int getsstep() const { return details::saccessorseq<i,TS>::xstep; }

		constexpr int getsize(int i) const { return details::daccessorseq<TS>::type::size(i); }

		constexpr int getstep(int i) const { return details::daccessorseq<TS>::type::step(i); }

		constexpr int ndims() const { return Ncount; }

		constexpr int numel() const { return Ntot; }

		/// compile type via pack: offsetvalue<1,2,3,4>::value
		template<int... I>
		using offsetvalue = typename details::offsetcompute<TS,I...>::type;

		/// returns offset from variadic via initializer list
		template <class...X>
		constexpr int offset(X... I) const
		{
			static_assert(sizeof...(I) == Ncount,"wrong number of dimensions");
			return offset({I...});
		}

		template <class dummy=void>
		auto squeeze() -> MultiDimNView<T, typename TS::template removeif<singletondim> >;

		template <int...neworder>
		auto permutedim() -> MultiDimNView<T, typename TS::template permuted<neworder...> >;

		// TODO: let last dimension free to match
		template <int...N>
		auto reshapeR() -> MultiDimNView<T, typename details::rowmajorstepper<N...> >;

		template <int...N>
		auto reshapeC() -> MultiDimNView<T, typename details::colmajorstepper<N...> >;

		/// via initializer list (bit ugly...)
		/// we loop over i Ncount because is compile time length, while for(int x: L) IS NOT
		int offset(const std::initializer_list<int> & L) const
		{
			assert(L.size() == Ncount); //,"wrong number of dims");
			auto x = L.begin(); /// C++14 constexpr
			int o = 0;
			for(int i = 0; i < Ncount; x++, i++)
				o += *x *getstep(i);
			return o;
		}
	};

	/**
	 * View over a TS (typesequence of sizes)
	 */
	template <class T, class TS>
	class MultiDimNView: public MultiDimNBase<T,TS>
	{
	public:
		using base_t = MultiDimNBase<T,TS>;
		using data_t = T*;
		using map_t = Eigen::Map<Eigen::Matrix<T,base_t::Ntot,1> >;

		MultiDimNView(data_t x):  data_(x)
		{
		}

		constexpr const T * data() const { return data_; }

		T * data() { return data_; }

		void setOnes()
		{
			map_t(data_).setOnes();
		}

		void setZero()
		{
			map_t(data_).setZero();
		}

		/// COMMON ACROSS MultiDimNView and MultiDimN
		/// TODO: replace with curiously recursive pattern
		/// limit by dimension
		/// FUTURE: variant with index compile time
		template<int dim>
	 	auto limit1(int index) -> MultiDimNView<T, typename TS::template drop<dim> >
		{
			return (data()+index*  MultiDimNBase<T,TS>::template getsteptype<dim>::value ); // via implicit construction
		}

		/// for the dimension dim takes from the given index1 up to newsize elements. This is not reducing the number of dimensions
		/// FUTURE: variant with index compile time
		template<int dim, int newsize>
		auto limit1block(int index1) ->
			MultiDimNView<T, typename TS::template replacetype<dim,sspair<newsize,  MultiDimNBase<T,TS>::template getsteptype<dim>::value   > > >
		{
			static_assert(newsize <= MultiDimNBase<T,TS>::template getsizetype<dim>::value,"sub-size cannot be larger than original");

			return data() + index1*MultiDimNBase<T,TS>::template getsteptype<dim>::value;
		}
		template <int ...neworder>
		auto permutedim() -> MultiDimNView<T, typename TS::template permuted<neworder...> >
		{
			static_assert(sizeof...(neworder) == TS::size,"permutation requires same order");
			return data();
		}

		// TODO: let last dimension free to match
		template <int...N>
		auto reshapeR() -> MultiDimNView<T, typename details::rowmajorstepper<N...> >
		{
			using RT = MultiDimNView<T, typename details::rowmajorstepper<N...> >;

			static_assert(RT::Ntot == MultiDimNBase<T,TS>::Ntot,"result requires same number of elements");
			return data();
		}

		template <int...N>
		auto reshapeC() -> MultiDimNView<T, typename details::colmajorstepper<N...> >
		{
			using RT = MultiDimNView<T, typename details::colmajorstepper<N...> >;

			static_assert(RT::Ntot == MultiDimNBase<T,TS>::Ntot,"result requires same number of elements");
			return data();
		}

		auto squeeze() -> MultiDimNView<T, typename TS::template removeif<singletondim> >
		{
			return data();
		}

	private:
		data_t data_;
	};

	// column wise
	template <class T, class TS>
	class MultiDimN: public MultiDimNBase<T,TS>
	{
	public:
		using data_t = Eigen::Matrix<T,MultiDimNBase<T,TS>::Ntot,1>;

		MultiDimN()
		{

		}

		const T * data() const { return data_.data(); }

		T * data() { return data_.data(); }

		void setOnes()
		{
			data_.setOnes();
		}

		void setZero()
		{
			data_.setZero();
		}

		/// COMMON ACROSS MultiDimNView and MultiDimN
		/// TODO: replace with curiously recursive pattern
		template<int dim>
		auto limit1(int index) -> MultiDimNView<T, typename TS::template drop<dim> >
		{
			return (data()+index*  MultiDimNBase<T,TS>::template getsteptype<dim>::value ); // via implicit construction
		}

		/// for the dimension dim takes from the given index1 up to newsize elements. This is not reducing the number of dimensions
		/// FUTURE: variant with index compile time
		template<int dim, int newsize>
		auto limit1block(int index1) ->
			MultiDimNView<T, typename TS::template replacetype<dim,sspair<newsize,  MultiDimNBase<T,TS>::template getsteptype<dim>::value   > > >
		{
			static_assert(newsize <= MultiDimNBase<T,TS>::template getsizetype<dim>::value,"sub-size cannot be larger than original");

			return data() + index1*MultiDimNBase<T,TS>::template getsteptype<dim>::value;
		}

		template <int ...neworder>
		auto permutedim() -> MultiDimNView<T, typename TS::template permuted<neworder...> >
		{
			static_assert(sizeof...(neworder) == TS::size,"permutation requires same order");
			return data();
		}

		// TODO: let last dimension free to match
		template <int...N>
		auto reshapeR() -> MultiDimNView<T, typename details::rowmajorstepper<N...> >
		{
			using RT = MultiDimNView<T, typename details::rowmajorstepper<N...> >;

			static_assert(RT::Ntot == MultiDimNBase<T,TS>::Ntot,"result requires same number of elements");
			return data();
		}

		template <int...N>
		auto reshapeC() -> MultiDimNView<T, typename details::colmajorstepper<N...> >
		{
			using RT = MultiDimNView<T, typename details::colmajorstepper<N...> >;

			static_assert(RT::Ntot == MultiDimNBase<T,TS>::Ntot,"result requires same number of elements");
			return data();
		}

		auto squeeze() -> MultiDimNView<T, typename TS::template removeif<singletondim> >
		{
			return data();
		}

	private:
		data_t data_;
	};

	/// declares a multidim with type T and given dimensions in row-major
	template <class T, int...N>
	using MultiDimNRow = MultiDimN<T, typename details::rowmajorstepper<N...> >;

	/// declares a multidim with type T and given dimensions in col-major
	template <class T, int...N>
	using MultiDimNCol = MultiDimN<T, typename details::colmajorstepper<N...> >;
}


#include "drake/common/trajectories/exponential_plus_piecewise_polynomial.h"

#include <cmath>
#include <random>

#include <Eigen/Core>
#include <gtest/gtest.h>

#include "drake/common/trajectories/test/random_piecewise_polynomial.h"

using std::default_random_engine;
using std::uniform_real_distribution;

namespace drake {
namespace trajectories {

namespace {

template <typename T>
void testSimpleCase() {
  int num_coefficients = 5;
  int num_segments = 1;

  MatrixX<T> K = MatrixX<T>::Random(1, 1);
  MatrixX<T> A = MatrixX<T>::Random(1, 1);
  MatrixX<T> alpha = MatrixX<T>::Random(1, 1);

  default_random_engine generator;
  auto segment_times =
      PiecewiseTrajectory<double>::RandomSegmentTimes(num_segments, generator);
  auto polynomial_part = test::MakeRandomPiecewisePolynomial<T>(
      1, 1, num_coefficients, segment_times);

  ExponentialPlusPiecewisePolynomial<T> expPlusPp(
      K, A, alpha, polynomial_part);
  ExponentialPlusPiecewisePolynomial<T> derivative =
      expPlusPp.derivative();

  uniform_real_distribution<T> uniform(expPlusPp.start_time(),
                                                     expPlusPp.end_time());
  double t = uniform(generator);
  auto check =
      K(0) * std::exp(A(0) * (t - expPlusPp.start_time())) * alpha(0) +
      polynomial_part.scalarValue(t);
  auto derivative_check =
      K(0) * A(0) * std::exp(A(0) * (t - expPlusPp.start_time())) * alpha(0) +
      polynomial_part.derivative().scalarValue(t);

  EXPECT_NEAR(check, expPlusPp.value(t)(0), 1e-8);
  EXPECT_NEAR(derivative_check, derivative.value(t)(0), 1e-8);
}

GTEST_TEST(testExponentialPlusPiecewisePolynomial, BasicTest) {
  testSimpleCase<double>();
}

}  // namespace
}  // namespace trajectories
}  // namespace drake


#include "catch.hpp"

#include "libirc/wilson.h"

#include "libirc/atom.h"
#include "libirc/connectivity.h"
#include "libirc/conversion.h"
#include "libirc/io.h"
#include "libirc/molecule.h"

#include "config.h"

#include <cmath>

#ifdef HAVE_ARMA
#include <armadillo>
using vec3 = arma::vec3;
using vec = arma::vec;
using mat = arma::mat;
#elif HAVE_EIGEN3
#include <Eigen/Dense>
using vec3 = Eigen::Vector3d;
using vec = Eigen::VectorXd;
using mat = Eigen::MatrixXd;
#else
#error
#endif

using namespace irc;

TEST_CASE("Wilson B matrix for single fragments", "[wilson]") {
  using namespace connectivity;
  using namespace molecule;
  using namespace wilson;
  using namespace tools;

  SECTION("H2 stretching") {
    // Define molecule
    Molecule<vec3> mol{{"H", {0., 0., 0.}}, {"H", {1., 0., 0.}}};

    // Molecular connectivity
    mat dd{distances<vec3, mat>(mol)};
    UGraph adj{adjacency_matrix(dd, mol)};
    mat dist{distance_matrix<mat>(adj)};

    // Compute bonds
    std::vector<Bond> B{bonds(dist, mol)};
    REQUIRE(B.size() == 1);

    // Compute Wilson B matrix for H2 analytically
    mat Bwilson =
        wilson_matrix<vec3, vec, mat>(to_cartesian<vec3, vec>(mol), B);

    // Check Wilson B matrix size
    INFO("Wilson B matrix (analytical):\n" << Bwilson);
    REQUIRE(linalg::size(Bwilson) == 6);

    // Compute Wilson B matrix for H2 numerically
    mat BwilsonN = wilson_matrix_numerical<vec3, vec, mat>(
        to_cartesian<vec3, vec>(mol), B);

    // Check Wilson B matrix size
    INFO("Wilson B matrix (numerical):\n" << BwilsonN);
    REQUIRE(linalg::size(BwilsonN) == 6);

    // Check analytical and numerical Wilson matrices are the same
    INFO("Analytical vs Numerical");
    for (std::size_t i{0}; i < 6; i++)
      REQUIRE(Bwilson(i) == Approx(BwilsonN(i)).margin(1e-6));

    INFO("Transformation with bond stretch");
    const double d{0.01};
    vec dx{-d, 0.00, 0.00, d, 0.00, 0.00};

    INFO("Analytical transformation");
    vec analytical_transformation = Bwilson * dx;
    CAPTURE(analytical_transformation);
    REQUIRE(analytical_transformation(0) == Approx(2 * d).margin(1e-5));

    INFO("Numerical transformation");
    vec numerical_transformation = BwilsonN * dx;
    CAPTURE(numerical_transformation);
    REQUIRE(numerical_transformation(0) == Approx(2 * d).margin(1e-5));
  } // H2 stretching

  SECTION("H2O bending") {

    double angle(0.5);
    double angle_rad(angle / 180. * constants::pi);

    const std::vector<mat> R{
        {// Rotation for H1
         {cos(angle_rad), -sin(angle_rad), 0},
         {sin(angle_rad), cos(angle_rad), 0},
         {0, 0, 1}},
        {// Rotation for O
         {1, 0, 0},
         {0, 1, 0},
         {0, 0, 1}},
        {// Rotation for H2
         {cos(-angle_rad), -sin(-angle_rad), 0},
         {sin(-angle_rad), cos(-angle_rad), 0},
         {0, 0, 1}},
    };

    const Molecule<vec3> mol{
        {"H", {1.43, -1.10, 0.00}}, // H1
        {"O", {0.00, 0.00, 0.00}},  // O
        {"H", {-1.43, -1.10, 0.00}} // H2
    };

    // Allocate displacements in cartesian coordinates
    vec dx{linalg::zeros<vec>(3 * mol.size())};

    // Compute displacements
    for (std::size_t i{0}; i < 3; i++) {
      vec3 v{R[i] * mol[i].position - mol[i].position};

      dx(3 * i + 0) = v(0);
      dx(3 * i + 1) = v(1);
      dx(3 * i + 2) = v(2);
    }

    // Molecular connectivity
    mat dd{distances<vec3, mat>(mol)};
    UGraph adj{adjacency_matrix(dd, mol)};
    mat dist{distance_matrix<mat>(adj)};

    // Compute bonds
    const auto B = bonds(dist, mol);
    REQUIRE(B.size() == 2);

    // Compute angles
    const auto A = angles(dist, mol);
    REQUIRE(A.size() == 1);

    // Compute Wilson B matrix for H2O analytically
    mat Bwilson =
        wilson_matrix<vec3, vec, mat>(to_cartesian<vec3, vec>(mol), B, A);
    REQUIRE(linalg::size(Bwilson) == 27);
    INFO("Wilson B matrix (analytical):\n" << Bwilson);

    // Compute Wilson B matrix for H2O numerically
    const mat BwilsonN = wilson_matrix_numerical<vec3, vec, mat>(
        to_cartesian<vec3, vec>(mol), B, A);
    REQUIRE(linalg::size(BwilsonN) == 27);
    INFO("Wilson B matrix (numerical):\n" << BwilsonN);

    // Check analytical and numerical Wilson matrices are the same
    for (std::size_t i{0}; i < 27; i++)
      REQUIRE(Bwilson(i) == Approx(BwilsonN(i)).margin(1e-6));

    INFO("Compute displacements in internal coordinates");
    const vec displacement = Bwilson * dx;
    CAPTURE(displacement);

    INFO("Check bond change");
    REQUIRE(displacement(0) == Approx(0).margin(1e-4));
    REQUIRE(displacement(1) == Approx(0).margin(1e-4));

    INFO("Check angle change");
    REQUIRE(displacement(2) == Approx(2 * angle_rad).margin(1e-3));
  }

  SECTION("H2O2 torsion") {

    const double angle(1.0);
    const double angle_rad(angle / 180. * constants::pi);

    const mat R{{cos(angle_rad), -sin(angle_rad), 0},
                {sin(angle_rad), cos(angle_rad), 0},
                {0, 0, 1}};

    molecule::Molecule<vec3> molecule{
        {"H", {0.000, 0.947, -0.079}}, // H1
        {"O", {0.000, 0.000, 0.000}},  // O1
        {"O", {0.000, 0.000, 1.474}},  // O2
        {"H", {-0.854, -0.407, 1.553}} // H2
    };
    molecule::multiply_positions(molecule, conversion::angstrom_to_bohr);

    // Allocate displacements in cartesian coordinates
    vec dx{linalg::zeros<vec>(3 * molecule.size())};

    // Compute transformation for H1 rotation
    vec3 v{R * molecule[0].position - molecule[0].position};
    dx(0) = v(0);
    dx(1) = v(1);
    dx(2) = v(2);

    // Compute old dihedral (before rotation)
    double d_old{dihedral<vec3>(molecule[0].position,
                                molecule[1].position,
                                molecule[2].position,
                                molecule[3].position)};

    // Compute new dihedral angle (after rotation)
    double d_new{dihedral<vec3>(R * molecule[0].position,
                                molecule[1].position,
                                molecule[2].position,
                                molecule[3].position)};

    // Compute dihedral variation
    double d_diff{d_new - d_old};

    // Compute interatomic distances
    mat dd{distances<vec3, mat>(molecule)};
    UGraph adj{adjacency_matrix(dd, molecule)};
    mat dist{distance_matrix<mat>(adj)};

    // Compute bonds, angles and dihedrals
    const std::vector<Bond> B{bonds(dist, molecule)};
    CHECK(B.size() == 3);
    const std::vector<Angle> A{angles(dist, molecule)};
    CHECK(A.size() == 2);
    const std::vector<Dihedral> D{dihedrals(dist, molecule)};
    CHECK(D.size() == 1);

    // Compute Wilson's B matrix
    const mat Bwilson = wilson_matrix<vec3, vec, mat>(
        to_cartesian<vec3, vec>(molecule), B, A, D);
    REQUIRE(linalg::size(Bwilson) == 72);
    INFO("Wilson B matrix (analytical):\n" << Bwilson);

    // Compute Wilson's B matrix
    const mat BwilsonN = wilson_matrix_numerical<vec3, vec, mat>(
        to_cartesian<vec3, vec>(molecule), B, A, D);
    REQUIRE(linalg::size(BwilsonN) == 72);
    INFO("Wilson B matrix (numerical):\n" << BwilsonN);

    // Check analytical and numerical Wilson matrices are the same
    INFO("Check Analytical vs Numerical Wilson B matrix");
    for (std::size_t i{0}; i < 72; i++) {
      REQUIRE(Bwilson(i) == Approx(BwilsonN(i)).margin(1e-6));
    }

    SECTION("Analytical displacements") {
      // Compute displacement in internal coordinates
      const vec displacement{Bwilson * dx};
      INFO("Displacement (analytical):\n" << displacement);

      INFO("Bonds do not change");
      REQUIRE(displacement(0) == Approx(0).margin(1e-3));
      REQUIRE(displacement(1) == Approx(0).margin(1e-3));
      REQUIRE(displacement(2) == Approx(0).margin(1e-3));

      INFO("Angles do not change");
      REQUIRE(displacement(3) == Approx(0).margin(1e-4));
      REQUIRE(displacement(4) == Approx(0).margin(1e-4));

      INFO("Dihedral should change");
      REQUIRE(displacement(5) == Approx(d_diff).margin(1e-4));
    }

    SECTION("Numerical displacements") {
      // Compute displacement in internal coordinates
      const vec displacement{BwilsonN * dx};
      INFO("Displacement (numerical):\n" << displacement);

      INFO("Bonds do not change");
      REQUIRE(displacement(0) == Approx(0).margin(1e-3));
      REQUIRE(displacement(1) == Approx(0).margin(1e-3));
      REQUIRE(displacement(2) == Approx(0).margin(1e-3));

      INFO("Angles do not change");
      REQUIRE(displacement(3) == Approx(0).margin(1e-4));
      REQUIRE(displacement(4) == Approx(0).margin(1e-4));

      INFO("Dihedral should change");
      REQUIRE(displacement(5) == Approx(d_diff).margin(1e-4));
    }
  }
}

TEST_CASE("Wilson B matrix for formalydehyde", "[wilson]") {
  using namespace connectivity;
  using namespace molecule;
  using namespace tools;
  using namespace wilson;
  using namespace io;

  const auto mol = load_xyz<vec3>(config::molecules_dir + "formaldehyde.xyz");

  // Compute interatomic distances
  mat dd{distances<vec3, mat>(mol)};
  UGraph adj{adjacency_matrix(dd, mol)};
  mat dist{distance_matrix<mat>(adj)};

  // Compute bonds
  std::vector<Bond> B{bonds(dist, mol)};
  std::vector<Angle> A{angles(dist, mol)};
  std::vector<Dihedral> D{dihedrals(dist, mol)};
  std::vector<LinearAngle<vec3>> LA{linear_angles(dist, mol)};
  std::vector<OutOfPlaneBend> OOPB{out_of_plane_bends(dist, mol)};

  CHECK(OOPB.size() == 1);

  const auto q = connectivity::cartesian_to_irc<vec3, vec>(
      to_cartesian<vec3, vec>(mol), B, A, D, LA, OOPB);
  CAPTURE(q);

  const mat wilson_b_analytical = wilson_matrix<vec3, vec, mat>(
      to_cartesian<vec3, vec>(mol), B, A, D, LA, OOPB);
  CAPTURE(wilson_b_analytical);

  const mat wilson_b_numerical = wilson_matrix_numerical<vec3, vec, mat>(
      to_cartesian<vec3, vec>(mol), B, A, D, LA, OOPB);
  CAPTURE(wilson_b_numerical);

  REQUIRE(linalg::size(wilson_b_analytical) ==
          linalg::size(wilson_b_numerical));

  const std::size_t n = linalg::size(wilson_b_analytical);
  for (std::size_t i{0}; i < n; i++) {
    CAPTURE(i);
    REQUIRE(wilson_b_analytical(i) ==
            Approx(wilson_b_numerical(i)).margin(1e-5));
  }
}

TEST_CASE("Wilson B matrix for bent out of plane bend", "[wilson]") {
  using namespace connectivity;
  using namespace molecule;
  using namespace tools;
  using namespace wilson;
  using namespace io;

  molecule::Molecule<vec3> molecule{{"C", {0.200, 0.000, 2.800}},
                                    {"C", {0.000, 0.000, 0.000}},
                                    {"C", {0.000, 2.500, -0.500}},
                                    {"C", {0.000, -2.500, -0.500}}};

  // Compute interatomic distances
  mat dd{distances<vec3, mat>(molecule)};
  UGraph adj{adjacency_matrix(dd, molecule)};
  mat dist{distance_matrix<mat>(adj)};

  std::vector<OutOfPlaneBend> OOPB{out_of_plane_bends(dist, molecule)};
  CHECK(OOPB.size() == 1);

  const auto q = connectivity::cartesian_to_irc<vec3, vec>(
      to_cartesian<vec3, vec>(molecule), {}, {}, {}, {}, OOPB);
  CAPTURE(q);
  CHECK(q.size() == 1);
  CHECK(q(0) == Approx(-0.0713074648));

  const mat wilson_b_analytical = wilson_matrix<vec3, vec, mat>(
      to_cartesian<vec3, vec>(molecule), {}, {}, {}, {}, OOPB);
  CAPTURE(wilson_b_analytical);

  const mat wilson_b_numerical = wilson_matrix_numerical<vec3, vec, mat>(
      to_cartesian<vec3, vec>(molecule), {}, {}, {}, {}, OOPB);
  CAPTURE(wilson_b_numerical);

  REQUIRE(linalg::size(wilson_b_analytical) ==
          linalg::size(wilson_b_numerical));

  const std::size_t n = linalg::size(wilson_b_analytical);
  for (std::size_t i{0}; i < n; i++) {
    CAPTURE(i);
    REQUIRE(wilson_b_analytical(i) ==
            Approx(wilson_b_numerical(i)).margin(1e-5));
  }
}

TEST_CASE("Wilson B matrix for water dimer", "[wilson]") {
  using namespace connectivity;
  using namespace molecule;
  using namespace tools;
  using namespace wilson;
  using namespace io;

  const auto mol = load_xyz<vec3>(config::molecules_dir + "water_dimer_2.xyz");

  // Compute interatomic distances
  mat dd{distances<vec3, mat>(mol)};
  UGraph adj{adjacency_matrix(dd, mol)};
  mat dist{distance_matrix<mat>(adj)};

  // Compute bonds
  std::vector<Bond> B{bonds(dist, mol)};
  std::vector<Angle> A{angles(dist, mol)};
  std::vector<Dihedral> D{dihedrals(dist, mol)};
  std::vector<LinearAngle<vec3>> LA{linear_angles(dist, mol)};
  std::vector<OutOfPlaneBend> OOPB{out_of_plane_bends(dist, mol)};

  const auto q = connectivity::cartesian_to_irc<vec3, vec>(
      to_cartesian<vec3, vec>(mol), B, A, D, LA, OOPB);
  CAPTURE(q);

  const mat wilson_b_analytical = wilson_matrix<vec3, vec, mat>(
      to_cartesian<vec3, vec>(mol), B, A, D, LA, OOPB);
  CAPTURE(wilson_b_analytical);

  const mat wilson_b_numerical = wilson_matrix_numerical<vec3, vec, mat>(
      to_cartesian<vec3, vec>(mol), B, A, D, LA, OOPB);
  CAPTURE(wilson_b_numerical);

  REQUIRE(linalg::size(wilson_b_analytical) ==
          linalg::size(wilson_b_numerical));

  const std::size_t n = linalg::size(wilson_b_analytical);
  for (std::size_t i{0}; i < n; i++) {
    REQUIRE(wilson_b_analytical(i) ==
            Approx(wilson_b_numerical(i)).margin(1e-5));
  }
}

TEST_CASE("Linear angle gradient", "[wilson]") {
  using namespace connectivity;
  using namespace molecule;
  using namespace tools;
  using namespace wilson;
  using namespace io;

  Molecule<vec3> mol{
      {"H", {0.7, 1.1, 2.4}}, {"C", {2.8, 1.2, 2.5}}, {"N", {4.9, 1.3, 2.6}}};
  REQUIRE(mol.size() == 3);

  // Compute interatomic distances
  mat dd{distances<vec3, mat>(mol)};
  UGraph adj{adjacency_matrix(dd, mol)};
  mat dist{distance_matrix<mat>(adj)};

  // Compute bonds
  std::vector<Bond> B{bonds(dist, mol)};
  std::vector<Angle> A{angles(dist, mol)};
  std::vector<Dihedral> D{dihedrals(dist, mol)};
  std::vector<LinearAngle<vec3>> LA{linear_angles(dist, mol)};
  std::vector<OutOfPlaneBend> OOPB{out_of_plane_bends(dist, mol)};
  REQUIRE(LA.size() == 2);

  const auto q = connectivity::cartesian_to_irc<vec3, vec>(
      to_cartesian<vec3, vec>(mol), B, A, D, LA, OOPB);
  CAPTURE(q);

  const mat wilson_b_analytical = wilson_matrix<vec3, vec, mat>(
      to_cartesian<vec3, vec>(mol), B, A, D, LA, OOPB);
  CAPTURE(wilson_b_analytical);

  const mat wilson_b_numerical = wilson_matrix_numerical<vec3, vec, mat>(
      to_cartesian<vec3, vec>(mol), B, A, D, LA, OOPB);
  CAPTURE(wilson_b_numerical);

  REQUIRE(linalg::size(wilson_b_analytical) ==
          linalg::size(wilson_b_numerical));

  const std::size_t n = linalg::size(wilson_b_analytical);
  for (std::size_t i{0}; i < n; i++) {
    REQUIRE(wilson_b_analytical(i) ==
            Approx(wilson_b_numerical(i)).margin(1e-5));
  }
}


#include "utils.hpp"

#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/integer.hpp>
#include <memory>
using namespace std;
using Bint = boost::multiprecision::cpp_int;

Bint f(Bint const& x, Bint const& c, Bint const& n);

class LinkedList{
    public:
        LinkedList(Bint value_);
        ~LinkedList();
        Bint getValue();
        LinkedList* getAfter();
        void setAfter(LinkedList* after_);
    private:
        Bint value;
        LinkedList* after;
};

LinkedList::LinkedList(Bint value_){
    value = value_;
    after = nullptr;
}

LinkedList::~LinkedList(){
    ;
}

Bint LinkedList::getValue(){
    return value;
}

LinkedList* LinkedList::getAfter(){
    return after;
}

void LinkedList::setAfter(LinkedList* after_){
    after = after_;
}

string PollardsRhoFactorizer_cppfunc(string s, long c_){
    Bint n(s);
    Bint c(c_);
    LinkedList* x = new LinkedList(Bint(2));
    LinkedList* y = new LinkedList(f(x->getValue(), c, n));
    x->setAfter(y);
    Bint d = gcd(abs(x->getValue()-y->getValue()), n);
    while(d==1){
        LinkedList* z1 = new LinkedList(f(y->getValue(), c, n));
        LinkedList* z2 = new LinkedList(f(z1->getValue(), c, n));
        y->setAfter(z1);
        z1->setAfter(z2);
        y = z2;

        LinkedList* tmp;
        tmp = x;
        x = x->getAfter();
        delete tmp;
        d = gcd(abs(x->getValue()-y->getValue()), n);
    }

    if(d<n){
        return d.str();
    }
    else{
        return "1";
    }
}

Bint f(Bint const& x, Bint const& c, Bint const& n){
    return (x*x+c)%n;
}


// die.cpp
//
// Copyright (c) 2009
// Steven Watanabe
//
// 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)

//[die
/*`
    For the source of this example see
    [@boost://libs/random/example/die.cpp die.cpp].
    First we include the headers we need for __mt19937
    and __uniform_int.
*/
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/variate_generator.hpp>

/*`
  We use __mt19937 with the default seed as a source of
  randomness.  The numbers produced will be the same
  every time the program is run.  One common method to
  change this is to seed with the current time (`std::time(0)`
  defined in ctime).
*/
boost::mt19937 gen;
/*`
  [note We are using a /global/ generator object here.  This
  is important because we don't want to create a new [prng
  pseudo-random number generator] at every call]
*/
/*`
  Now we can define a function that simulates an ordinary
  six-sided die.
*/
int roll_die() {
    /*<< __mt19937 produces integers in the range [0, 2[sup 32]-1].
        However, we want numbers in the range [1, 6].  The distribution
        __uniform_int performs this transformation.
        [warning Contrary to common C++ usage __uniform_int
        does not take a /half-open range/.  Instead it takes a /closed range/.
        Given the parameters 1 and 6, __uniform_int can
        can produce any of the values 1, 2, 3, 4, 5, or 6.]
    >>*/
    boost::uniform_int<> dist(1, 6);
    /*<< __variate_generator combines a generator with a distribution.
        [important We pass [classref boost::mt19937 boost::mt19937&] to
        __variate_generator instead of just [classref boost::mt19937]
        (note the reference).  Without the reference, __variate_generator
        would make a copy of the generator and would leave the global
        `gen` unchanged.  Consequently, `roll_die` would produce *the same value*
        every time it was called.]
    >>*/
    boost::variate_generator<boost::mt19937&, boost::uniform_int<> > die(gen, dist);
    /*<< A __variate_generator is a function object. >>*/
    return die();
}
//]

#include <iostream>

int main() {
    for(int i = 0; i < 10; ++i) {
        std::cout << roll_die() << std::endl;
    }
}


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/
// http://csclab.murraystate.edu/~bob.pilgrim/445/munkres.html

#pragma once

#include "AlgorithmUtils.hpp"
#include "FluidEigenMappings.hpp"
#include "../../data/FluidIndex.hpp"
#include <Eigen/Core>
#include <cassert>
#include <cmath>

namespace fluid {
namespace algorithm {

class Munkres
{
public:
  using intPair = std::pair<int, int>;
  void init(index rows, index cols)
  {
    using namespace Eigen;
    index N = std::max(rows, cols);
    mCost = ArrayXXd::Zero(N, N);
    mRowMin = ArrayXd::Zero(N);
    mColMin = ArrayXd::Zero(N);
    mMask = ArrayXXi::Zero(N, N);
    mRowCover = ArrayXi::Zero(N);
    mColCover = ArrayXi::Zero(N);
    mPath = ArrayXXi::Zero(2 * N + 1, 2);
  }

  void reset()
  {
    mCost.setZero();
    mRowMin.setZero();
    mColMin.setZero();
    mMask.setZero();
    mRowCover.setZero();
    mColCover.setZero();
    mPath.setZero();
  }

  void process(Eigen::Ref<const Eigen::ArrayXXd> costMatrix,
               Eigen::Ref<Eigen::ArrayXi>        result)
  {
    bool done;
    reset();
    double maxCost = costMatrix.maxCoeff();
    mCost = Eigen::ArrayXXd::Ones(mCost.rows(), mCost.cols());
    mCost = mCost * (10.0 * maxCost);
    mCost.block(0, 0, costMatrix.rows(), costMatrix.cols()) = costMatrix;
    step1();
    step2();
    done = step3();
    while (!done)
    {
      intPair Z = step4();
      while (Z.first < 0)
      {
        step6();
        Z = step4();
      }
      step5(Z);
      done = step3();
    }
    for (int i = 0; i < result.size(); i++)
    { mMask.row(i).maxCoeff(&result(i)); }
  }

  void step1()
  {
    mRowMin = mCost.rowwise().minCoeff();
    mColMin = (mCost.colwise() - mRowMin).colwise().minCoeff();
    for (int i = 0; i < mCost.rows(); i++)
    {
      double min = mCost.row(i).minCoeff();
      mCost.row(i) -= min;
    }
  }

  void step2()
  {
    for (int i = 0; i < mCost.rows(); i++)
    {
      for (int j = 0; j < mCost.cols(); j++)
      {
        if (mCost(i, j) == 0 && mRowCover(i) == 0 && mColCover(j) == 0)
        {
          mMask(i, j) = 1;
          mRowCover(i) = 1;
          mColCover(j) = 1;
        }
      }
    }
    mRowCover.setZero();
    mColCover.setZero();
  }

  bool step3()
  {
    for (int i = 0; i < mMask.rows(); i++)
    {
      for (int j = 0; j < mMask.cols(); j++)
      {
        if (mMask(i, j) == 1) { mColCover(j) = 1; }
      }
    }
    Eigen::Index nCovered = (mColCover == 1).count();
    return nCovered >= mMask.rows() || nCovered >= mMask.cols();
  }

  intPair step4()
  {
    int     row = -1, col = -1;
    intPair result = std::make_pair(row, col);
    while (true)
    {
      intPair pos = findZero();
      row = pos.first;
      col = pos.second;
      if (row < 0) { break; }
      mMask(row, col) = 2;
      Eigen::ArrayXi r = mMask.row(row);
      int            colStar = findValue(r, 1);
      if (colStar >= 0)
      {
        col = colStar;
        mRowCover(row) = 1;
        mColCover(col) = 0;
      }
      else
      {
        result = std::make_pair(row, col);
        break;
      }
    }
    return result;
  }

  void step5(const intPair Z)
  {
    int row = -1, col = -1;
    int pathCount = 0;
    mPath(pathCount, 0) = Z.first;
    mPath(pathCount, 1) = Z.second;
    while (true)
    {
      int            tmp = mPath(pathCount, 1);
      Eigen::ArrayXi tmpCol = mMask.col(tmp);
      row = findValue(tmpCol, 1);
      if (row == -1) break;
      pathCount++;
      mPath(pathCount, 0) = row;
      mPath(pathCount, 1) = mPath(pathCount - 1, 1);
      Eigen::ArrayXi r = mMask.row(mPath(pathCount, 0));
      col = findValue(r, 2);
      pathCount++;
      mPath(pathCount, 0) = mPath(pathCount - 1, 0);
      mPath(pathCount, 1) = col;
    }
    augmentPath(pathCount);
    mRowCover.setZero();
    mColCover.setZero();
    erasePrimes();
  }

  void step6()
  {
    double m = minCost();
    for (int i = 0; i < mCost.rows(); i++)
    {
      for (int j = 0; j < mCost.cols(); j++)
      {
        if (mRowCover(i) == 1) mCost(i, j) += m;
        if (mColCover(j) == 0) mCost(i, j) -= m;
      }
    }
  }

  double minCost()
  {
    double minVal = std::numeric_limits<double>::max();
    for (int i = 0; i < mCost.rows(); i++)
    {
      for (int j = 0; j < mCost.cols(); j++)
      {
        if (mRowCover(i) == 0 && mColCover(j) == 0)
        {
          double v = mCost(i, j);
          minVal = (minVal > v) ? v : minVal;
        }
      }
    }
    return minVal;
  }

  void erasePrimes()
  {
    for (int i = 0; i < mMask.rows(); i++)
    {
      for (int j = 0; j < mMask.cols(); j++)
      {
        if (mMask(i, j) == 2) { mMask(i, j) = 0; }
      }
    }
  }

  intPair findZero()
  {
    for (int i = 0; i < mCost.rows(); i++)
      for (int j = 0; j < mCost.cols(); j++)
      {
        if (mCost(i, j) == 0 && mRowCover(i) == 0 && mColCover(j) == 0)
          return std::make_pair(i, j);
      }
    return std::make_pair(-1, -1);
  }

  int findValue(Eigen::Ref<const Eigen::ArrayXi> vector, const int val)
  {
    for (int i = 0; i < vector.size(); i++)
    {
      if (vector(i) == val) return i;
    }
    return -1;
  }

  void augmentPath(const int count)
  {
    for (int i = 0; i < count + 1; i++)
    {
      int c = mPath(i, 0), r = mPath(i, 1);
      mMask(c, r) = mMask(c, r) == 1 ? 0 : 1;
    }
  }

private:
  Eigen::ArrayXXd mCost;
  Eigen::ArrayXd  mRowMin;
  Eigen::ArrayXd  mColMin;
  Eigen::ArrayXXi mMask;
  Eigen::ArrayXXi mPath;
  Eigen::ArrayXi  mRowCover;
  Eigen::ArrayXi  mColCover;
};
} // namespace algorithm
} // namespace fluid


#include <boost/numeric/odeint.hpp>

typedef std::vector< double > state_type;

const double gam = 0.15;

void harmonic_oscillator( const state_type &x , state_type &dxdt , const double /* t */ )
{
    dxdt[0] = x[1];
    dxdt[1] = -x[0] - gam*x[1];
}

int main()
{
  state_type x(2);
  x[0] = 1.0; // start at x=1.0, p=0.0
  x[1] = 0.0;

  using namespace boost::numeric::odeint;
  size_t steps = integrate( harmonic_oscillator, x, 0.0, 10.0, 0.1 );
  if(steps == 0)
    return 1;

  return 0;
}


#include <iostream>
#include <vector>

#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Twist.h>

#include <Eigen/Dense>
#include <Eigen/Geometry>

//#include <boost/bind.hpp>

tf2_ros::Buffer tfBuffer;
ros::Publisher pub;

bool reset_odom = false;
bool first_pose = true;
Eigen::Matrix4d first_pose_inv;

void inverse(Eigen::Matrix4d& in, Eigen::Matrix4d& out)
{
	// set result matrix to identity matrix
	out.setIdentity();
	// get rotation matrix
	Eigen::Matrix3d rot;
	rot = in.block(0, 0, 3, 3);
	// inverse rotation matrix
	Eigen::Matrix3d rot_inverse;
	rot_inverse = rot.transpose();
	// set inversed rotation matrix
	out.block(0, 0, 3, 3) = rot_inverse;

	// get translation
	Eigen::Vector3d t;
	t = in.block(0, 3, 3, 1);
	// inverse translation
	Eigen::Vector3d t_inv;
	t_inv = -1 * rot.transpose() * t;
	// set inversed translation
	out.block(0, 3, 3, 1) = t_inv;
}

void transform_to_matrix(const geometry_msgs::TransformStamped& transformStamped, Eigen::Matrix4d& matrix)
{
	matrix.setIdentity();

	Eigen::Vector3d t;
	t.x() = transformStamped.transform.translation.x;
	t.y() = transformStamped.transform.translation.y;
	t.z() = transformStamped.transform.translation.z;

	Eigen::Quaterniond q;
	q.x() = transformStamped.transform.rotation.x;
	q.y() = transformStamped.transform.rotation.y;
	q.z() = transformStamped.transform.rotation.z;
	q.w() = transformStamped.transform.rotation.w;

	matrix.block(0,0,3,3) = q.normalized().toRotationMatrix();
	matrix.block(0,3,3,1) = t;
}

void pose_to_matrix(const geometry_msgs::Pose& pose, Eigen::Matrix4d& matrix)
{
	matrix.setIdentity();

	Eigen::Vector3d t;
	t.x() = pose.position.x;
	t.y() = pose.position.y;
	t.z() = pose.position.z;

	Eigen::Quaterniond q;
	q.x() = pose.orientation.x;
	q.y() = pose.orientation.y;
	q.z() = pose.orientation.z;
	q.w() = pose.orientation.w;

	matrix.block(0,0,3,3) = q.normalized().toRotationMatrix();
	matrix.block(0,3,3,1) = t;
}

void matrix_to_pose(const Eigen::Matrix4d& matrix, geometry_msgs::Pose& pose)
{
	pose.position.x = matrix(0, 3);
	pose.position.y = matrix(1, 3);
	pose.position.z = matrix(2, 3);

	Eigen::Matrix3d rot;
	rot = matrix.block(0, 0, 3, 3);
	Eigen::Quaterniond q(rot);

	pose.orientation.x = q.x();
	pose.orientation.y = q.y();
	pose.orientation.z = q.z();
	pose.orientation.w = q.w();
}

void transfrom_to_twist_matrix(const Eigen::Matrix4d& transfrom_matrix_inv, Eigen::MatrixXd& matrix)
{
	matrix.setIdentity();

	Eigen::Vector3d t;
	t = transfrom_matrix_inv.block(0, 3, 3, 1);

	Eigen::Matrix3d t_hat;
	t_hat << 0, -t(2), t(1),
			 t(2), 0, -t(0),
    		-t(1), t(0), 0;

	Eigen::Matrix3d rot;
	rot = transfrom_matrix_inv.block(0, 0, 3, 3);

	matrix.block(0,0,3,3) = rot;
	matrix.block(3,3,3,3) = rot;
	matrix.block(0,3,3,3) = t_hat * rot;
}

void twist_to_vector(const geometry_msgs::Twist& twist, Eigen::VectorXd& twist_vector)
{
	twist_vector(0) = twist.linear.x;
	twist_vector(1) = twist.linear.y;
	twist_vector(2) = twist.linear.z;

	twist_vector(3) = twist.angular.x;
	twist_vector(4) = twist.angular.y;
	twist_vector(5) = twist.angular.z;
}

void vector_to_twist(const Eigen::VectorXd& twist_vector, geometry_msgs::Twist& twist)
{
	twist.linear.x = twist_vector(0);
	twist.linear.y = twist_vector(1);
	twist.linear.z = twist_vector(2);

	twist.angular.x = twist_vector(3);
	twist.angular.y = twist_vector(4);
	twist.angular.z = twist_vector(5);
}

void odomCallback(const nav_msgs::Odometry& msg)
{
	nav_msgs::Odometry msg_result;

	std::string from_frame, to_frame;
	ros::param::get("~from_frame", from_frame);
	ros::param::get("~to_frame", to_frame);

	geometry_msgs::TransformStamped transformStamped;
	try
 	{
    	transformStamped = tfBuffer.lookupTransform(from_frame, to_frame, ros::Time(0));

    	// convert transform to matrix
    	Eigen::Matrix4d transfrom_matrix;
    	transform_to_matrix(transformStamped, transfrom_matrix);
    	// get inverse transform matrix
    	Eigen::Matrix4d transfrom_matrix_inv;
    	inverse(transfrom_matrix, transfrom_matrix_inv);

    	// --------------------- pose transform ---------------------
    	// convert pose to matrix
    	Eigen::Matrix4d pose_matrix;
    	pose_to_matrix(msg.pose.pose, pose_matrix);

    	// apply transfrom on pose
    	Eigen::Matrix4d pose_matrix_result;
    	pose_matrix_result = transfrom_matrix_inv * pose_matrix * transfrom_matrix;

    	// reset odom if needed
    	if (reset_odom) {
    		if (first_pose) {
    			inverse(pose_matrix_result, first_pose_inv);
    			first_pose = false;
    		}
    		pose_matrix_result = first_pose_inv * pose_matrix_result;
    	}

    	matrix_to_pose(pose_matrix_result, msg_result.pose.pose);

    	// --------------------- twist transform ---------------------
    	// convert transform to twist_transfrom
    	Eigen::MatrixXd twist_transfrom(6, 6);
    	transfrom_to_twist_matrix(transfrom_matrix_inv, twist_transfrom);

    	// convert twist to vector
    	Eigen::VectorXd twist_vector(6);
    	twist_to_vector(msg.twist.twist, twist_vector);

    	// apply transfrom on twist
    	Eigen::VectorXd twist_vector_result(6);
    	twist_vector_result = twist_transfrom * twist_vector;
    	vector_to_twist(twist_vector_result, msg_result.twist.twist);

    	// --------------------- header replacement ---------------------
    	// copy header
    	msg_result.header = msg.header;
    	msg_result.header.frame_id = "odom";
    	msg_result.child_frame_id = to_frame;

        // write pose noise covariance if needed
        if(ros::param::has("~covariance"))
        {
            std::vector<double> covariance;
            ros::param::get("~covariance", covariance);
            for (int i = 0; i < 36 && i < covariance.size(); ++i) msg_result.pose.covariance[i] = covariance[i];
        }
    	else
    	{
    		for (int i = 0; i < 36; ++i) msg_result.pose.covariance[i] = msg.pose.covariance[i];
    	}
    	// publish odometry
    	pub.publish(msg_result);
 	}
 	catch (tf2::TransformException &ex)
 	{
   		ROS_WARN("%s",ex.what());
 	}
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "odometry_transformer");
	ros::NodeHandle node;

	std::string in_topic, out_topic;
	ros::param::get("~in_topic", in_topic);
	ros::param::get("~out_topic", out_topic);
    ros::param::get("~reset_odom", reset_odom);

	tf2_ros::TransformListener tfListener(tfBuffer);

 	pub = node.advertise<nav_msgs::Odometry>(out_topic, 10);
 	ros::Subscriber sub = node.subscribe(in_topic, 10, odomCallback);

 	ros::spin();

	return 0;
}


// smooth_feedback: Control theory on Lie groups
// https://github.com/pettni/smooth_feedback
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include <boost/numeric/odeint.hpp>
#include <smooth/compat/odeint.hpp>
#include <smooth/feedback/mpc.hpp>

#include <chrono>

#ifdef ENABLE_PLOTTING
#include <matplot/matplot.h>
#endif

using namespace std::chrono_literals;
using namespace boost::numeric::odeint;

using Time = std::chrono::duration<double>;

template<typename T>
using G = Eigen::Vector2<T>;
template<typename T>
using U = Eigen::Matrix<T, 1, 1>;

using Gd = G<double>;
using Ud = U<double>;

int main()
{
  using std::sin;
  std::srand(5);

  // system variables
  Gd g = Gd::Random();
  Ud u;

  // dynamics
  auto f = []<typename T>(Time, const G<T> & x, const U<T> u) -> smooth::Tangent<G<T>> {
    return {x(1), u(0)};
  };

  // parameters
  smooth::feedback::MPCParams<Gd, Ud> prm{
    .T = 5,
    .K = 20,
    .weights =
      {
        .Q  = Eigen::Matrix2d::Identity(),
        .QT = 0.1 * Eigen::Matrix2d::Identity(),
        .R  = Eigen::Matrix<double, 1, 1>::Constant(0.1),
      },
    .ulim =
      smooth::feedback::ManifoldBounds<Ud>{
        .A = Eigen::Matrix<double, 1, 1>(1),
        .c = Ud::Zero(),
        .l = Eigen::Matrix<double, 1, 1>(-0.5),
        .u = Eigen::Matrix<double, 1, 1>(0.5),
      },
  };

  // create MPC object and set input bounds, and desired trajectories
  smooth::feedback::MPC<Time, Gd, Ud, decltype(f)> mpc(f, prm);
  mpc.set_xdes([]<typename T>(T t) -> G<T> { return G<T>{-0.5 * sin(0.3 * t), 0}; });
  mpc.set_udes([]<typename T>(T) -> U<T> { return U<T>::Zero(); });

  // prepare for integrating the closed-loop system
  runge_kutta4<Gd, double, smooth::Tangent<Gd>, double, vector_space_algebra> stepper{};
  const auto ode = [&f, &u](const Gd & x, smooth::Tangent<Gd> & d, double t) -> void {
    d = f(Time(t), x, u);
  };
  std::vector<double> tvec, xvec, vvec, uvec;

  // integrate closed-loop system
  for (std::chrono::milliseconds t = 0s; t < 60s; t += 50ms) {
    // compute MPC input
    auto [u_mpc, code] = mpc(t, g);
    u                  = u_mpc;
    if (code != smooth::feedback::QPSolutionStatus::Optimal) {
      std::cerr << "Solver failed with code " << static_cast<int>(code) << std::endl;
    }

    // store data
    tvec.push_back(duration_cast<Time>(t).count());
    xvec.push_back(g.x());
    vvec.push_back(g.y());
    uvec.push_back(u(0));

    // step dynamics
    stepper.do_step(ode, g, 0, 0.05);
  }

#ifdef ENABLE_PLOTTING
  matplot::figure();
  matplot::hold(matplot::on);

  matplot::plot(tvec, xvec)->line_width(2);
  matplot::plot(tvec, matplot::transform(tvec, [](auto t) { return -0.5 * sin(0.3 * t); }), "k--")
    ->line_width(2);
  matplot::plot(tvec, vvec)->line_width(2);
  matplot::plot(tvec, uvec)->line_width(2);
  matplot::legend({"x", "x_{des}", "v", "u"});

  matplot::show();
#else
  std::cout << "TRAJECTORY:" << std::endl;
  for (auto i = 0u; i != tvec.size(); ++i) {
    std::cout << "t=" << tvec[i] << ": x=" << xvec[i] << ", v=" << vvec[i] << std::endl;
  }
#endif
}