// Copyright 2006-2009 Daniel James.
// 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 "../helpers/prefix.hpp"

#include <boost/unordered_set.hpp>
#include <boost/unordered_map.hpp>
#include "../helpers/test.hpp"
#include "../helpers/random_values.hpp"
#include "../helpers/tracker.hpp"

namespace rehash_tests
{

test::seed_t seed(2974);

template <class X>
bool postcondition(X const& x, BOOST_DEDUCED_TYPENAME X::size_type n)
{
    return static_cast<double>(x.bucket_count()) >
        static_cast<double>(x.size()) / x.max_load_factor() &&
        x.bucket_count() >= n;
}

template <class X>
void rehash_empty_test1(X* = 0)
{
    X x;

    x.rehash(10000);
    BOOST_TEST(postcondition(x, 10000));

    x.rehash(0);
    BOOST_TEST(postcondition(x, 0));
}

template <class X>
void rehash_empty_test2(X* = 0,
    test::random_generator generator = test::default_generator)
{
    test::random_values<X> v(1000, generator);
    test::ordered<X> tracker;

    X x;

    x.rehash(10000);
    BOOST_TEST(postcondition(x, 10000));

    tracker.insert_range(v.begin(), v.end());
    x.insert(v.begin(), v.end());
    tracker.compare(x);

    BOOST_TEST(postcondition(x, 10000));
}

template <class X>
void rehash_empty_test3(X* = 0,
    test::random_generator generator = test::default_generator)
{
    test::random_values<X> v(1000, generator);
    test::ordered<X> tracker;

    X x;

    x.rehash(0);
    BOOST_TEST(postcondition(x, 0));

    tracker.insert_range(v.begin(), v.end());
    x.insert(v.begin(), v.end());
    tracker.compare(x);

    BOOST_TEST(postcondition(x, 0));
}

template <class X>
void rehash_test1(X* = 0,
    test::random_generator generator = test::default_generator)
{
    test::random_values<X> v(1000, generator);
    test::ordered<X> tracker;
    tracker.insert_range(v.begin(), v.end());
    X x(v.begin(), v.end());

    x.rehash(0); BOOST_TEST(postcondition(x, 0));
    tracker.compare(x);

    x.max_load_factor(0.25);
    x.rehash(0); BOOST_TEST(postcondition(x, 0));
    tracker.compare(x);

    x.max_load_factor(50.0);
    x.rehash(0); BOOST_TEST(postcondition(x, 0));
    tracker.compare(x);

    x.rehash(1000); BOOST_TEST(postcondition(x, 1000));
    tracker.compare(x);
}

boost::unordered_set<int>* int_set_ptr;
boost::unordered_multiset<int>* int_multiset_ptr;
boost::unordered_map<int, int>* int_map_ptr;
boost::unordered_multimap<int, int>* int_multimap_ptr;

UNORDERED_TEST(rehash_empty_test1,
    ((int_set_ptr)(int_multiset_ptr)(int_map_ptr)(int_multimap_ptr))
)
UNORDERED_TEST(rehash_empty_test2,
    ((int_set_ptr)(int_multiset_ptr)(int_map_ptr)(int_multimap_ptr))
)
UNORDERED_TEST(rehash_empty_test3,
    ((int_set_ptr)(int_multiset_ptr)(int_map_ptr)(int_multimap_ptr))
)
UNORDERED_TEST(rehash_test1,
    ((int_set_ptr)(int_multiset_ptr)(int_map_ptr)(int_multimap_ptr))
)

}

RUN_TESTS()


#include <boost/program_options.hpp>

#include "base/init.hpp"
#include "ridgelet/basis.hpp"
#include "ridgelet/lambda.hpp"
#include "ridgelet/ridgelet_frame.hpp"

using namespace std;

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

  SOURCE_INFO();
  namespace po = boost::program_options;

  po::options_description options("options");
  options.add_options()("help", "produce help message")
      ("Jxy,J", po::value<int>(&J)->default_value(3), "J")
      ("rho, r", po::value<int>(&rho)->default_value(1), "rho_x")
      ("no-dump", "dump coefficients to hdf5")
      ("list", "list basis and sizes");
  po::variables_map vm;

  try {
    po::store(po::parse_command_line(argc, argv, options), vm);
    po::notify(vm);
    if (vm.count("help")) {
      std::cout << options << "\n";
      return 0;
    }
  } catch (std::exception& e) {
    cout << e.what() << "\n";
  }

  RidgeletFrame frame(J, J, rho, rho);

  RTBasis<> rt_basis;
  auto& lambdas = frame.lambdas();
  for (auto& lam : lambdas) {
    unsigned int tx = translation_size(lam.t, lam.j, rho, 0);
    unsigned int ty = translation_size(lam.t, lam.j, rho, 1);

    unsigned int incx = std::max(tx / 4, 1u);
    unsigned int incy = std::max(ty / 4, 1u);
    cout << "incx: " << incx << "\n";
    cout << "incy: " << incy << "\n";

    for (unsigned int i = 0; i < tx; i += incx) {
      for (unsigned int j = 0; j < ty; j += incy) {
        rt_basis.insert(lam, i / double(tx), j / double(ty));
      }
    }
  }

  auto& elements = rt_basis.get_elements();

  cout << "basis size: " << rt_basis.size() << "\n";

  for (unsigned int i = 0; i < elements.size(); ++i) {
    auto index = rt_basis.get_index(elements[i]);
    BOOST_VERIFY(index == i);
  }

  return 0;
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/maybe.hpp>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_LAMBDA auto plus = [](auto x, auto y) {
        return x + y;
    };

    BOOST_HANA_CONSTEXPR_ASSERT(foldr(nothing, 1, plus) == 1);
    BOOST_HANA_CONSTEXPR_ASSERT(foldr(just(4), 1, plus) == 5);
    //! [main]
}


#include "dlib/matrix.h"
#include <dlib/image_processing/frontal_face_detector.h>
#include <dlib/image_processing/render_face_detections.h>
#include <dlib/image_processing.h>
#include <dlib/image_io.h>
#include "call_matlab.h"

using namespace dlib;
using namespace std;

void mex_function(
	const array2d<rgb_pixel >& img,
	matrix<double>& bboxes,
	matrix<double>& shape
)
{
	//开启检测模块
	frontal_face_detector detector = get_frontal_face_detector();

	//轮廓预测器
	shape_predictor sp;
	deserialize("shape_predictor_68_face_landmarks.dat") >> sp;

	//下面一行为放大图片来检测更小人脸，可以加上
	//pyramid_up(img);
	//人脸检测
	std::vector<dlib::rectangle> det = detector(img);
	std::vector<full_object_detection> shapes;

	//人脸数目
	int faces = det.size();
	bboxes = ones_matrix<double>(faces, 4);

	for (int i = 0; i < faces; i++)
	{
		bboxes(i,0)=det[i].left();
		bboxes(i,1)=det[i].top();
		bboxes(i,2)=det[i].width();
		bboxes(i,3)=det[i].height();
		shapes.push_back(sp(img, det[i]));
	}

	//一张图像中存在人脸个数
	cout << "Number of faces detected: " << faces << endl;

	int shape_colum = faces * 68;
	int k = 0;
	shape = ones_matrix<double>(shape_colum, 2);

	if (!shapes.empty())
	{
		for (int j = 0; j < faces; j++)
			for (int i = 0; i < 68; i++, k++)
			{
				shape(k, 0) = shapes[j].part(i).x();
				shape(k, 1) = shapes[j].part(i).y();
			}
	}
	cout << "finish!" << endl;
}
#include "mex_wrapper.cpp"


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

#include <nt2/sdk/functor/meta/call.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/include/functions/splat.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/simd/sdk/simd/io.hpp>

#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>

NT2_TEST_CASE_TPL ( exp2,  NT2_SIMD_REAL_TYPES)
{
  using nt2::exp2;
  using nt2::tag::exp2_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;

  typedef typename nt2::meta::call<exp2_(vT)>::type r_t;
  typedef vT wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_ULP_EQUAL(exp2(nt2::Inf<vT>()), nt2::Inf<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(exp2(nt2::Minf<vT>()), nt2::Zero<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(exp2(nt2::Nan<vT>()), nt2::Nan<r_t>(), 0);
#endif
  NT2_TEST_ULP_EQUAL(exp2(nt2::Mone<vT>()), nt2::Half<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(exp2(nt2::One<vT>()), nt2::Two<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(exp2(nt2::Zero<vT>()), nt2::One<r_t>(), 0);
}


/*
 * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
 * 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 Autonomous Systems Lab, ETH Zurich 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 Christian Gehring, Hannes Sommer, Paul Furgale,
 * Remo Diethelm 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.
 *
*/
#ifndef KINDR_ROTATIONS_ROTATION_EIGEN_FUNCTIONS_HPP_
#define KINDR_ROTATIONS_ROTATION_EIGEN_FUNCTIONS_HPP_

#include <cmath>

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

#include "kindr/common/common.hpp"

namespace kindr {
namespace rotations {
namespace eigen_impl {
namespace eigen_internal {

// 1) Output: AngleAxis

template<typename T, typename TReturn = T>
static Eigen::AngleAxis<TReturn> getAngleAxisFromQuaternion(const Eigen::Quaternion<T>& p_BI)
{
  // Bad precision!
  return Eigen::AngleAxis<TReturn>(p_BI.template cast<TReturn>());
}

template<typename T, typename TReturn = T>
static Eigen::AngleAxis<TReturn> getAngleAxisFromTransformationMatrix(const Eigen::Matrix<T,3,3>& A_IB)
{
  // Bad precision!
  return Eigen::AngleAxis<TReturn>(A_IB.template cast<TReturn>());
}

template<typename T, typename TReturn = T>
static Eigen::AngleAxis<TReturn> getAngleAxisFromRotationMatrix(const Eigen::Matrix<T,3,3>& R_BI)
{
  // Bad precision!
  return Eigen::AngleAxis<TReturn>(R_BI.template cast<TReturn>());
}

//template<typename T, typename TReturn = T>
//static Eigen::Matrix<TReturn,3,1> getRotationVectorFromRotationMatrix(const Eigen::Matrix<T,3,3>& R_BI)
//{
//  const Eigen::Matrix<TReturn,3,3> rotationMatrix = R_BI.template cast<TReturn>();
//
//  // Bad precision!
//  return Eigen::Matrix<TReturn, 3, 1>();
//}

template<typename T, typename TReturn = T>
static Eigen::AngleAxis<TReturn> getAngleAxisFromRpy(const Eigen::Matrix<T,3,1>& rpy_BI)
{
  // Bad precision!
  return Eigen::AngleAxis<TReturn>(
    Eigen::AngleAxis<TReturn>((TReturn)rpy_BI(0), Eigen::Matrix<TReturn, 3, 1>::UnitX()) *
    Eigen::AngleAxis<TReturn>((TReturn)rpy_BI(1), Eigen::Matrix<TReturn, 3, 1>::UnitY()) *
    Eigen::AngleAxis<TReturn>((TReturn)rpy_BI(2), Eigen::Matrix<TReturn, 3, 1>::UnitZ()));
}

template<typename T, typename TReturn = T>
static Eigen::AngleAxis<TReturn> getAngleAxisFromYpr(const Eigen::Matrix<T,3,1>& ypr_BI)
{
  // Bad precision!
  return Eigen::AngleAxis<TReturn>(
    Eigen::AngleAxis<TReturn>((TReturn)ypr_BI(0), Eigen::Matrix<TReturn, 3, 1>::UnitZ()) *
    Eigen::AngleAxis<TReturn>((TReturn)ypr_BI(1), Eigen::Matrix<TReturn, 3, 1>::UnitY()) *
    Eigen::AngleAxis<TReturn>((TReturn)ypr_BI(2), Eigen::Matrix<TReturn, 3, 1>::UnitX()));
}

// 2) Output: Quaternion

template<typename T, typename TReturn = T>
static Eigen::Quaternion<TReturn> getQuaternionFromAngleAxis(const Eigen::AngleAxis<T>& aa_BI)
{
  return Eigen::Quaternion<TReturn>(aa_BI.template cast<TReturn>());
}

template<typename T, typename TReturn = T>
static Eigen::Quaternion<TReturn> getQuaternionFromTransformationMatrix(const Eigen::Matrix<T,3,3>& A_IB)
{
//  // Untested
//  // Bad precision!
//  double w;
//  double x;
//  double y;
//  double z;
//  w = sqrt(1+mat(0,0)+mat(1,1)+mat(2,2))/2;
//  if(w>0.02){
//    x = 1/4/w*(mat(2,1)-mat(1,2));
//    y = 1/4/w*(mat(0,2)-mat(2,0));
//    z = 1/4/w*(mat(1,0)-mat(0,1));
//  } else {
//    x = sqrt(1+mat(0,0)-mat(1,1)-mat(2,2))/2;
//    y = 1/4/x*(mat(0,1)+mat(1,0));
//    z = 1/4/x*(mat(0,2)+mat(2,0));
//    w = 1/4/x*(mat(2,1)-mat(1,2));
//  }
//  q.w() = w;
//  q.x() = x;
//  q.y() = y;
//  q.z() = z;
//  q.normalize();

  return Eigen::Quaternion<TReturn>(A_IB.template cast<TReturn>());
}

template<typename T, typename TReturn = T>
static Eigen::Quaternion<TReturn> getQuaternionFromRotationMatrix(const Eigen::Matrix<T,3,3>& R_BI)
{
//  // Untested
//  // Bad precision!
//  double w;
//  double x;
//  double y;
//  double z;
//  w = sqrt(1+mat(0,0)+mat(1,1)+mat(2,2))/2;
//  if(w>0.02){
//    x = 1/4/w*(mat(2,1)-mat(1,2));
//    y = 1/4/w*(mat(0,2)-mat(2,0));
//    z = 1/4/w*(mat(1,0)-mat(0,1));
//  } else {
//    x = sqrt(1+mat(0,0)-mat(1,1)-mat(2,2))/2;
//    y = 1/4/x*(mat(0,1)+mat(1,0));
//    z = 1/4/x*(mat(0,2)+mat(2,0));
//    w = 1/4/x*(mat(2,1)-mat(1,2));
//  }
//  q.w() = w;
//  q.x() = x;
//  q.y() = y;
//  q.z() = z;
//  q.normalize();

  return Eigen::Quaternion<TReturn>(R_BI.template cast<TReturn>());
}

template<typename T, typename TReturn = T>
static Eigen::Quaternion<TReturn> getQuaternionFromRpy(const Eigen::Matrix<T,3,1>& rpy_BI)
{
//  // Tested and Working
//  Eigen::Quaternion<T> p_BI;
//
//  const T sr = sin(rpy_BI(0)/2);
//  const T cr = cos(rpy_BI(0)/2);
//  const T sp = sin(rpy_BI(1)/2);
//  const T cp = cos(rpy_BI(1)/2);
//  const T sy = sin(rpy_BI(2)/2);
//  const T cy = cos(rpy_BI(2)/2);
//
//  const T srsp = sr*sp;
//  const T srcp = sr*cp;
//  const T crsp = cr*sp;
//  const T crcp = cr*cp;
//
//  p_BI.w() = -srsp*sy+crcp*cy;
//  p_BI.x() = crsp*sy+srcp*cy;
//  p_BI.y() = crsp*cy-srcp*sy;
//  p_BI.z() = srsp*cy+crcp*sy;
////  p_BI.normalize();
//
//  return p_BI;

  return Eigen::Quaternion<TReturn>(
    Eigen::AngleAxis<TReturn>((TReturn)rpy_BI(0), Eigen::Matrix<TReturn, 3, 1>::UnitX()) *
    Eigen::AngleAxis<TReturn>((TReturn)rpy_BI(1), Eigen::Matrix<TReturn, 3, 1>::UnitY()) *
    Eigen::AngleAxis<TReturn>((TReturn)rpy_BI(2), Eigen::Matrix<TReturn, 3, 1>::UnitZ()));
}

template<typename T, typename TReturn = T>
static Eigen::Quaternion<TReturn> getQuaternionFromYpr(const Eigen::Matrix<T,3,1>& ypr_BI)
{
//  // Tested and Working
//  Eigen::Quaternion<T> p_BI;
//
//  const T sy = sin(ypr_BI(0)/2);
//  const T cy = cos(ypr_BI(0)/2);
//  const T sp = sin(ypr_BI(1)/2);
//  const T cp = cos(ypr_BI(1)/2);
//  const T sr = sin(ypr_BI(2)/2);
//  const T cr = cos(ypr_BI(2)/2);
//
//  const T sysp = sy*sp;
//  const T sycp = sy*cp;
//  const T cysp = cy*sp;
//  const T cycp = cy*cp;
//
//  p_BI.w() = sysp*sr+cycp*cr;
//  p_BI.x() = -sysp*cr+cycp*sr;
//  p_BI.y() = sycp*sr+cysp*cr;
//  p_BI.z() = sycp*cr-cysp*sr;
////  p_BI.normalize();
//
//  return p_BI;

  return Eigen::Quaternion<TReturn>(
    Eigen::AngleAxis<TReturn>((TReturn)ypr_BI(0), Eigen::Matrix<TReturn, 3, 1>::UnitZ()) *
    Eigen::AngleAxis<TReturn>((TReturn)ypr_BI(1), Eigen::Matrix<TReturn, 3, 1>::UnitY()) *
    Eigen::AngleAxis<TReturn>((TReturn)ypr_BI(2), Eigen::Matrix<TReturn, 3, 1>::UnitX()));
}

// 3) Output: Transformation Matrix

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,3> getTransformationMatrixFromAngleAxis(const Eigen::AngleAxis<T>& aa_BI)
{
  return (aa_BI.template cast<TReturn>()).toRotationMatrix(); // A_IB
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,3> getTransformationMatrixFromQuaternion(const Eigen::Quaternion<T>& p_BI)
{
  return (p_BI.template cast<TReturn>()).toRotationMatrix(); // A_IB
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,3> getTransformationMatrixFromRotationMatrix(const Eigen::Matrix<T,3,3>& R_BI)
{
  return R_BI.template cast<TReturn>(); // A_IB
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,3> getTransformationMatrixFromRpy(const Eigen::Matrix<T,3,1>& rpy_BI)
{
  Eigen::Matrix<TReturn,3,3> A_IB;

  const TReturn sr = sin(rpy_BI(0));
  const TReturn cr = cos(rpy_BI(0));
  const TReturn sp = sin(rpy_BI(1));
  const TReturn cp = cos(rpy_BI(1));
  const TReturn sy = sin(rpy_BI(2));
  const TReturn cy = cos(rpy_BI(2));

  const TReturn srsy = sr*sy;
  const TReturn srcy = sr*cy;
  const TReturn crsy = cr*sy;
  const TReturn crcy = cr*cy;

  A_IB(0,0) = cp*cy;
  A_IB(0,1) = -cp*sy;
  A_IB(0,2) = sp;
  A_IB(1,0) = crsy+srcy*sp;
  A_IB(1,1) = crcy-srsy*sp;
  A_IB(1,2) = -sr*cp;
  A_IB(2,0) = srsy-crcy*sp;
  A_IB(2,1) = srcy+crsy*sp;
  A_IB(2,2) = cr*cp;

  return A_IB;
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,3> getTransformationMatrixFromYpr(const Eigen::Matrix<T,3,1>& ypr_BI)
{
  Eigen::Matrix<TReturn,3,3> A_IB;

  const TReturn sy = sin(ypr_BI(0));
  const TReturn cy = cos(ypr_BI(0));
  const TReturn sp = sin(ypr_BI(1));
  const TReturn cp = cos(ypr_BI(1));
  const TReturn sr = sin(ypr_BI(2));
  const TReturn cr = cos(ypr_BI(2));

  const TReturn sysr = sy*sr;
  const TReturn sycr = sy*cr;
  const TReturn cysr = cy*sr;
  const TReturn cycr = cy*cr;

  A_IB(0,0) = cy*cp;
  A_IB(0,1) = cysr*sp-sycr;
  A_IB(0,2) = sysr+cycr*sp;
  A_IB(1,0) = cp*sy;
  A_IB(1,1) = sysr*sp+cycr;
  A_IB(1,2) = sycr*sp-cysr;
  A_IB(2,0) = -sp;
  A_IB(2,1) = cp*sr;
  A_IB(2,2) = cp*cr;

  return A_IB;
}

// 4) Output: Rotation Matrix

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,3> getRotationMatrixFromAngleAxis(const Eigen::AngleAxis<T>& aa_BI)
{
  return (aa_BI.template cast<TReturn>()).toRotationMatrix();
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,3> getRotationMatrixFromQuaternion(const Eigen::Quaternion<T>& p_BI)
{
  return (p_BI.template cast<TReturn>()).toRotationMatrix();
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,3> getRotationMatrixFromTransformationMatrix(const Eigen::Matrix<T,3,3>& A_IB)
{
  return A_IB.template cast<TReturn>(); // R_BI
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,3> getRotationMatrixFromRpy(const Eigen::Matrix<T,3,1>& rpy_BI)
{
  Eigen::Matrix<TReturn,3,3> R_BI;

  const TReturn sr = sin(rpy_BI(0));
  const TReturn cr = cos(rpy_BI(0));
  const TReturn sp = sin(rpy_BI(1));
  const TReturn cp = cos(rpy_BI(1));
  const TReturn sy = sin(rpy_BI(2));
  const TReturn cy = cos(rpy_BI(2));

  const TReturn srsy = sr*sy;
  const TReturn srcy = sr*cy;
  const TReturn crsy = cr*sy;
  const TReturn crcy = cr*cy;

  R_BI(0,0) = cp*cy;
  R_BI(0,1) = -cp*sy;
  R_BI(0,2) = sp;
  R_BI(1,0) = crsy+srcy*sp;
  R_BI(1,1) = crcy-srsy*sp;
  R_BI(1,2) = -sr*cp;
  R_BI(2,0) = srsy-crcy*sp;
  R_BI(2,1) = srcy+crsy*sp;
  R_BI(2,2) = cr*cp;

  return R_BI;
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,3> getRotationMatrixFromYpr(const Eigen::Matrix<T,3,1>& ypr_BI)
{
  Eigen::Matrix<TReturn,3,3> R_BI;

  const TReturn sy = sin(ypr_BI(0));
  const TReturn cy = cos(ypr_BI(0));
  const TReturn sp = sin(ypr_BI(1));
  const TReturn cp = cos(ypr_BI(1));
  const TReturn sr = sin(ypr_BI(2));
  const TReturn cr = cos(ypr_BI(2));

  const TReturn sysr = sy*sr;
  const TReturn sycr = sy*cr;
  const TReturn cysr = cy*sr;
  const TReturn cycr = cy*cr;

  R_BI(0,0) = cy*cp;
  R_BI(0,1) = cysr*sp-sycr;
  R_BI(0,2) = sysr+cycr*sp;
  R_BI(1,0) = cp*sy;
  R_BI(1,1) = sysr*sp+cycr;
  R_BI(1,2) = sycr*sp-cysr;
  R_BI(2,0) = -sp;
  R_BI(2,1) = cp*sr;
  R_BI(2,2) = cp*cr;

  return R_BI;
}

// 5) Output: Roll-Pitch-Yaw

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,1> getRpyFromAngleAxis(const Eigen::AngleAxis<T>& aa_BI)
{
  return (aa_BI.toRotationMatrix().eulerAngles(0, 1, 2)).template cast<TReturn>();
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,1> getRpyFromQuaternion(const Eigen::Quaternion<T>& p_BI)
{
//  Eigen::Matrix<T,3,1> rpy_BI;
//
//  const T w = p_BI.w();
//  const T x = p_BI.x();
//  const T y = p_BI.y();
//  const T z = p_BI.z();
//
////  rpy_BI(1) = -asin(2*(x*z-w*y));
////
//////  std::cout << 2*(x*z-w*y)+1 << std::endl;
////
////  if(cos(rpy_BI(1)) == 0) // roll and yaw axis are the same -> yaw angle can be set to zero, roll does the whole rotation
////  {
////    rpy_BI(0) = -asin(2*(y*z-w*x));
////    rpy_BI(2) = 0;
////  }
////  else
////  {
////    rpy_BI(0) = atan2(2*(y*z+w*x),1-2*(x*x+y*y));
////    rpy_BI(2) = atan2(2*(x*y+w*z),1-2*(y*y+z*z));
////  }
//
//
//  // NEW
//
////  const T rpy_BI(1) = atan2(-2*(x*z-w*y),sqrt((1-2*(y*y+z*z))*(1-2*(y*y+z*z))+4*(x*y+w*z)*(x*y+w*z)));
//  const T test = 2*(x*z-w*y);
//
//  if(test > 0.999999999) // roll and yaw axis are the same -> yaw angle can be set to zero, roll does the whole rotation
//  {
////    rpy_BI(0) = -asin(2*(y*z-w*x));
////    rpy_BI(0) = acos(1-2*(x*x+z*z));
////    rpy_BI(0) = atan2(2*(y*z-w*x),1-2*(x*x+z*z));
//    rpy_BI(0) = 2*atan2(x,w);
//    rpy_BI(1) = -M_PI/2; // if test is a slight bit larger than 1, asin(test) does not work anymore
//    rpy_BI(2) = 0;
//  }
//  else if(test < -0.999999999)
//  {
////    rpy_BI(0) = -asin(2*(y*z-w*x));
////    rpy_BI(0) = acos(1-2*(x*x+z*z));
////    rpy_BI(0) = atan2(2*(y*z-w*x),1-2*(x*x+z*z));
//    rpy_BI(0) = -2*atan2(x,w);
//    rpy_BI(1) = M_PI/2;
//    rpy_BI(2) = 0;
//  }
//  else
//  {
//    rpy_BI(0) = atan2(2*(y*z+w*x),1-2*(x*x+y*y));
//    rpy_BI(1) = -asin(test);
//    rpy_BI(2) = atan2(2*(x*y+w*z),1-2*(y*y+z*z));
//  }
//
//  return rpy_BI;

  return (p_BI.toRotationMatrix().eulerAngles(0, 1, 2)).template cast<TReturn>();
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,1> getRpyFromTransformationMatrix(const Eigen::Matrix<T,3,3>& A_IB)
{
  return (A_IB.eulerAngles(0, 1, 2)).template cast<TReturn>(); // rpy_BI
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,1> getRpyFromRotationMatrix(const Eigen::Matrix<T,3,3>& R_BI)
{
  return (R_BI.eulerAngles(0, 1, 2)).template cast<TReturn>(); // rpy_BI
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,1> getRpyFromYpr(const Eigen::Matrix<T,3,1>& ypr_BI)
{
  return getRpyFromQuaternion(getQuaternionFromYpr(ypr_BI)).template cast<TReturn>();
//  return getRpyFromQuaternion(getQuaternionFromYpr(ypr_BI.template cast<TReturn>()));
}

// 6) Output: Yaw-Pitch-Roll

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,1> getYprFromAngleAxis(const Eigen::AngleAxis<T>& aa_BI)
{
  return (aa_BI.toRotationMatrix().eulerAngles(2, 1, 0)).template cast<TReturn>();
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,1> getYprFromQuaternion(const Eigen::Quaternion<T>& p_BI)
{
//  Eigen::Matrix<T,3,1> ypr_BI;
//
//  const T w = p_BI.w();
//  const T x = p_BI.x();
//  const T y = p_BI.y();
//  const T z = p_BI.z();
//
////  ypr_BI(1) = asin(2*(x*z+w*y));
////
////  if(cos(ypr_BI(1)) == 0) // yaw and roll axis are the same -> roll angle can be set to zero, yaw does the whole rotation
////  {
////    ypr_BI(0) = asin(2*(y*z+w*x));
////    ypr_BI(2) = 0;
////  }
////  else
////  {
////    ypr_BI(0) = -atan2(2*(x*y-w*z),1-2*(y*y+z*z));
////    ypr_BI(2) = -atan2(2*(y*z-w*x),1-2*(x*x+y*y));
////  }
//
//
//  // NEW
//
////  const T rpy_BI(1) = atan2(-2*(x*z-w*y),sqrt((1-2*(y*y+z*z))*(1-2*(y*y+z*z))+4*(x*y+w*z)*(x*y+w*z)));
//  const T test = 2*(x*z+w*y);
//
//  if(test > 0.999999999) // roll and yaw axis are the same -> yaw angle can be set to zero, roll does the whole rotation
//  {
////    ypr_BI(0) = asin(2*(y*z+w*x));
//    ypr_BI(0) = 2*atan2(x,w);
//    ypr_BI(1) = M_PI/2; // if test is a slight bit larger than 1, asin(test) does not work anymore
//    ypr_BI(2) = 0;
//  }
//  else if(test < -0.999999999)
//  {
////    ypr_BI(0) = -asin(2*(y*z+w*x));
//    ypr_BI(0) = -2*atan2(x,w);
//    ypr_BI(1) = -M_PI/2;
//    ypr_BI(2) = 0;
//  }
//  else
//  {
//    ypr_BI(0) = -atan2(2*(x*y-w*z),1-2*(y*y+z*z));
//    ypr_BI(1) = asin(test);
//    ypr_BI(2) = -atan2(2*(y*z-w*x),1-2*(x*x+y*y));
//  }
//
//  return ypr_BI;

//  const TReturn q0 = p_BI.w();
//  const TReturn q1 = p_BI.x();
//  const TReturn q2 = p_BI.y();
//  const TReturn q3 = p_BI.z();
//  return Eigen::Matrix<TReturn,3,1>(atan2(2.0*q1*q2 + 2.0*q0*q3, q1*q1 + q0*q0 - q3*q3 - q2*q2),
//                                    -asin(2.0*q1*q3 - 2.0*q0*q2),
//                                    atan2(2.0*q2*q3 + 2.0*q0*q1, q3*q3 - q2*q2 - q1*q1 + q0*q0));

  return (p_BI.toRotationMatrix().eulerAngles(2, 1, 0)).template cast<TReturn>();
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,1> getYprFromTransformationMatrix(const Eigen::Matrix<T,3,3>& A_IB)
{
  return (A_IB.eulerAngles(2, 1, 0)).template cast<TReturn>(); // ypr_BI
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,1> getYprFromRotationMatrix(const Eigen::Matrix<T,3,3>& R_BI)
{
  const TReturn r23 = R_BI(1,2);
  const TReturn r33 = R_BI(2,2);
  const TReturn r13 = R_BI(0,2);
  const TReturn r12 = R_BI(0,1);
  const TReturn r11 = R_BI(0,0);

  return Eigen::Matrix<TReturn,3,1>(atan2(r12,r11), -asin(r13), atan2(r23,r33));
//  return (R_BI.eulerAngles(2, 1, 0)).template cast<TReturn>(); // original
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,1> getYprFromRpy(const Eigen::Matrix<T,3,1>& rpy_BI)
{
  return getYprFromQuaternion(getQuaternionFromRpy(rpy_BI)).template cast<TReturn>();
//  return getYprFromQuaternion(getQuaternionFromRpy(rpy_BI.template cast<TReturn>()));
}

// TODO inverse will be deleted in future

// 7) Output: Inverses

//template<typename T, typename TReturn = T>
//static Eigen::AngleAxis<TReturn> getInverseAngleAxis(const Eigen::AngleAxis<T>& aa_BI)
//{
//  return (aa_BI.template cast<TReturn>()).inverse();
//}
//
template<typename T, typename TReturn = T>
static Eigen::Quaternion<TReturn> getInverseQuaternion(const Eigen::Quaternion<T>& p_BI)
{
  return (p_BI.conjugate()).template cast<TReturn>();
}
//
//template<typename T, typename TReturn = T>
//static Eigen::Matrix<TReturn,3,3> getInverseTransformationMatrix(const Eigen::Matrix<T,3,3>& A_IB)
//{
//  return (A_IB.template cast<TReturn>()).transpose();
//}
//
//template<typename T, typename TReturn = T>
//static Eigen::Matrix<TReturn,3,3> getInverseRotationMatrix(const Eigen::Matrix<T,3,3>& R_BI)
//{
//  return (R_BI.template cast<TReturn>()).transpose();
//}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,1> getInverseRpy(const Eigen::Matrix<T,3,1>& rpy_BI)
{
  return getRpyFromQuaternion(getInverseQuaternion(getQuaternionFromRpy(rpy_BI.template cast<TReturn>())));
}

template<typename T, typename TReturn = T>
static Eigen::Matrix<TReturn,3,1> getInverseYpr(const Eigen::Matrix<T,3,1>& ypr_BI)
{
  return getYprFromQuaternion(getInverseQuaternion(getQuaternionFromYpr(ypr_BI.template cast<TReturn>())));
}

/*

Matrix3x4d get_H_bar(const Vector4d &p)
{
  Matrix3x4d H_bar = Matrix3x4d::Zero();
  const double w = p(0);
  const Vector3d e = p.tail(3);
  const Matrix3x3d eye3 = Matrix3x3d::Identity();

  H_bar.col(0) = -e;
  H_bar.block<3,3>(0,1) = -skewsymm(e)+w*eye3;

  return H_bar;
}

MatrixDyn get_F(const VectorDyn &q)
{
  const double dim_q = q.rows();
  const double dim_u = dim_q - 1;

  MatrixDyn F = MatrixDyn::Zero(dim_q,dim_u);
  const Vector4d p = q.block<4,1>(3,0);

  F.block<3,3>(0,0) = Matrix3x3d::Identity();
  F.block<4,3>(3,3) = 0.5*get_H_bar(p).transpose();
  if(dim_u > 6)
  {
    F.block(7,6,dim_u-6,dim_u-6) = MatrixDyn::Identity(dim_u-6,dim_u-6);
  }

  return F;
}

VectorDyn get_dqdt(const VectorDyn &q, const VectorDyn &u)
{
  const double dim_q = q.rows();
  const double dim_u = dim_q - 1;

  VectorDyn dqdt = VectorDyn::Zero(dim_q);
  const Vector4d p = q.block<4,1>(3,0);

  dqdt.head(3) = u.head(3);
  dqdt.block<4,1>(3,0) = 0.5*get_H_bar(p).transpose()*u.block<3,1>(3,0);
  if(dim_u > 6)
  {
    dqdt.tail(dim_u-6) = u.tail(dim_u-6);
  }

  return dqdt;
}

VectorDyn get_dqdt_2(const VectorDyn &q, const VectorDyn &u) // older version, slower
{
  return get_F(q)*u;
}

void prox1D(double &y, const double &x, const double &min, const double &max)
{
  if     (x < min) {y = min;}
  else if(x > max) {y = max;}
  else             {y = x;  }
}

void prox2D(double &y1, double &y2, const double &x1, const double &x2, const double &max)
{
  const double r = sqrt(x1*x1 + x2*x2);
  if(r > max)
  {
    y1 = max*x1/r;
    y2 = max*x2/r;
  }
  else
  {
    y1 = x1;
    y2 = x2;
  }
}

*/

/*

Vector4d multiplyQuaternion(const Vector4d &p_CB, const Vector4d &p_BA) // same in eigen
{
  // p_CA = p_CB*p_BA
  Vector4d p_CA = Vector4d::Zero();

  p_CA(0) = p_CB(0)*p_BA(0) - p_CB.tail(3).transpose()*p_BA.tail(3);
  p_CA.tail(3) = p_CB(0)*p_BA.tail(3) + p_BA(0)*p_CB.tail(3) + skewsymm(p_BA.tail(3))*p_CB.tail(3);

  p_CA.normalize();

  return p_CA;
}

double w_to_angleVel(const Vector3d &K_w_JK, const Vector3d &n)
{
//  if(n(0) == 1) // around x
//  {
//    return K_w_JK(0);
//  }
//  else
//  if(n(1) == 1) // around y
//  {
//    return K_w_JK(1);
//  }
//  else
//  if(n(2) == 1) // around z
//  {
//    return K_w_JK(2);
//  }

  const Vector3d n_norm = n.normalized();

  return n_norm.dot(K_w_JK); // attention: dot product doesn't eliminate small errors
}

Vector3d omega2kardan(const Vector3d &K_w_IK, const Vector3d &abc) // quaternion: rotation from I to K, kardan: x-y-z with alpha-beta-gamma
{
  double alpha = abc(0);
  double beta = abc(1);
  double gamma = abc(2);

  Vector3d dadbdc = Vector3d::Zero();
  Matrix3x3d H = Matrix3x3d::Zero();

  if(cos(beta) == 0)
  {
    H << std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), 0, sin(gamma), cos(gamma), 0, -cos(gamma)*tan(beta), sin(gamma)*tan(beta), 1;
  }
  else
  {
    H << cos(gamma)/cos(beta), -sin(gamma)/cos(beta), 0, sin(gamma), cos(gamma), 0, -cos(gamma)*tan(beta), sin(gamma)*tan(beta), 1;
  }

  dadbdc = H*K_w_IK;

  return dadbdc;

  */

} // namespace eigen_internal
} // namespace eigen_impl
} // namespace rotations
} // namespace rm

#endif /* KINDR_ROTATIONS_ROTATION_EIGEN_FUNCTIONS_HPP_ */


// Boost.Signals library
//
// Copyright (C) 2001 Doug Gregor (gregod@cs.rpi.edu)
//
// Permission to copy, use, sell and distribute this software is granted
// provided this copyright notice appears in all copies.
// Permission to modify the code and to distribute modified code is granted
// provided this copyright notice appears in all copies, and a notice
// that the code was modified is included with the copyright notice.
//
// This software is provided "as is" without express or implied warranty,
// and with no claim as to its suitability for any purpose.

// For more information, see http://www.boost.org

#include <iostream>
#include <boost/signals/signal2.hpp>
#include <string>
#include <functional>

void print_sum(int x, int y)
{
  std::cout << "Sum = " << x+y << std::endl;
}

void print_product(int x, int y)
{
  std::cout << "Product = " << x*y << std::endl;
}

void print_quotient(float x, float y)
{
  std::cout << "Quotient = " << x/y << std::endl;
}

int main()
{
  typedef boost::signal2<void, int, int, boost::last_value<void>,
                         std::string, std::less<std::string>,
                         void (*)(int, int)> sig_type;

  sig_type sig;
  sig.connect(&print_sum);
  sig.connect(&print_product);

  sig(3, 5); // print sum and product of 3 and 5

  // should fail
  //   sig.connect(&print_quotient);
}


/**
@file

Copyright John Reid 2013

*/

#include <boost/multi_array.hpp>
#include <iostream>

using boost::extents;
using boost::indices;
using namespace std;

typedef boost::multi_array<double, 2> array_t;
typedef array_t::array_view< 2 >::type view;
typedef array_t::const_array_view< 2 >::type const_view;
typedef boost::multi_array_types::index_range range;

const int W = 8;

/// A reverse complement view
view
make_view( array_t a ) {
    return a[ indices[ range( W - 1, -1, -1 ) ] [range( 3, -1, -1 ) ] ];
}

/// A reverse complement view
const_view
make_const_view( array_t a ) {
    return static_cast< const array_t & >( a )[ indices[ range( W - 1, -1, -1 ) ] [range( 3, -1, -1 ) ] ];
}

template< typename Array >
void
print( Array a ) {
    for( array_t::index w = 0; w != W; ++w ) {
        for( array_t::index b = 0; b != 4; ++b ) {
            cout << a[ w ][ b ] << " ";
        }
        cout << "\n";
    }
    cout << "\n";
}

int
main( int argc, char * argv[] ) {
    //
    // Initialise our array_t
    //
    double data[] = {
        .7,.1,.1,.1,
        .1,.1,.1,.7,
        .1,.7,.1,.1,
        .1,.1,.7,.1,
        .7,.1,.1,.1,
        .1,.1,.1,.7,
        .1,.7,.1,.1,
        .1,.1,.7,.1,
    };
    array_t a( extents[ W ][ 4 ] );
    a.assign( data, data + W * 4) ;
    print( a );
    print( make_view( a ) );
    print( make_const_view( a ) );

    return 0;
}


#pragma once

#include "types.hpp"

#include <boost/date_time/gregorian/gregorian.hpp>

namespace opossum {

/**
 *  Takes delimited date string with order year-month-day (ISO 8601 extended format), e.g., "2001-01-01".
 *  Notably, Boost's gregorian dates do not support years < 1400 or > 9999.
 */
std::optional<boost::gregorian::date> string_to_date(const std::string& date_string);

/**
 * This also handles edge cases with days that are the end of a month.
 * E.g., March 31 + one month == April 30, and vice versa.
 * This also applies to leap years.
 */
boost::gregorian::date date_interval(const boost::gregorian::date& start_date, int64_t offset, DatetimeComponent unit);

// ISO 8601 extended format representation of the date
std::string date_to_string(const boost::gregorian::date& date);

}  // namespace opossum


// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2018 www.open3d.org
//
// 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 <Eigen/Dense>
#include <iostream>
#include <memory>

#include "open3d/Open3D.h"

using namespace open3d;

std::tuple<std::shared_ptr<geometry::PointCloud>,
           std::shared_ptr<pipelines::registration::Feature>>
PreprocessPointCloud(const char *file_name) {
    auto pcd = open3d::io::CreatePointCloudFromFile(file_name);
    auto pcd_down = pcd->VoxelDownSample(0.05);
    pcd_down->EstimateNormals(
            open3d::geometry::KDTreeSearchParamHybrid(0.1, 30));
    auto pcd_fpfh = pipelines::registration::ComputeFPFHFeature(
            *pcd_down, open3d::geometry::KDTreeSearchParamHybrid(0.25, 100));
    return std::make_tuple(pcd_down, pcd_fpfh);
}

void VisualizeRegistration(const open3d::geometry::PointCloud &source,
                           const open3d::geometry::PointCloud &target,
                           const Eigen::Matrix4d &Transformation) {
    std::shared_ptr<geometry::PointCloud> source_transformed_ptr(
            new geometry::PointCloud);
    std::shared_ptr<geometry::PointCloud> target_ptr(new geometry::PointCloud);
    *source_transformed_ptr = source;
    *target_ptr = target;
    source_transformed_ptr->Transform(Transformation);
    visualization::DrawGeometries({source_transformed_ptr, target_ptr},
                                  "Registration result");
}

int main(int argc, char *argv[]) {
    using namespace open3d;

    utility::SetVerbosityLevel(utility::VerbosityLevel::Debug);

    if (argc != 3 && argc != 4) {
        utility::LogInfo(
                "Usage : RegistrationRANSAC [path_to_first_point_cloud] "
                "[path_to_second_point_cloud] --visualize");
        return 1;
    }

    bool visualize = false;
    if (utility::ProgramOptionExists(argc, argv, "--visualize"))
        visualize = true;

    std::shared_ptr<geometry::PointCloud> source, target;
    std::shared_ptr<pipelines::registration::Feature> source_fpfh, target_fpfh;
    std::tie(source, source_fpfh) = PreprocessPointCloud(argv[1]);
    std::tie(target, target_fpfh) = PreprocessPointCloud(argv[2]);

    std::vector<std::reference_wrapper<
            const pipelines::registration::CorrespondenceChecker>>
            correspondence_checker;
    auto correspondence_checker_edge_length =
            pipelines::registration::CorrespondenceCheckerBasedOnEdgeLength(
                    0.9);
    auto correspondence_checker_distance =
            pipelines::registration::CorrespondenceCheckerBasedOnDistance(
                    0.075);
    auto correspondence_checker_normal =
            pipelines::registration::CorrespondenceCheckerBasedOnNormal(
                    0.52359878);

    correspondence_checker.push_back(correspondence_checker_edge_length);
    correspondence_checker.push_back(correspondence_checker_distance);
    correspondence_checker.push_back(correspondence_checker_normal);
    auto registration_result =
            pipelines::registration::RegistrationRANSACBasedOnFeatureMatching(
                    *source, *target, *source_fpfh, *target_fpfh, 0.075,
                    pipelines::registration::
                            TransformationEstimationPointToPoint(false),
                    4, correspondence_checker,
                    pipelines::registration::RANSACConvergenceCriteria(4000000,
                                                                       1000));

    if (visualize)
        VisualizeRegistration(*source, *target,
                              registration_result.transformation_);

    return 0;
}


// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_control/blob/master/LICENSE

#ifndef CBR_CONTROL__FIRST_ORDER_FILTER_HPP_
#define CBR_CONTROL__FIRST_ORDER_FILTER_HPP_

#include <Eigen/Core>

#include <cbr_utils/cyber_timer.hpp>

#include <cmath>
#include <memory>
#include <type_traits>
#include <utility>

namespace cbr
{

namespace fof_details
{

template<typename T>
struct zero_value
{
  static T value()
  {
    if constexpr (std::is_base_of_v<Eigen::MatrixBase<T>, T>) {
      return T::Zero();
    } else {
      return T{0};
    }
  }
};

}  // namespace fof_details

template<typename T, typename _clock_t = std::chrono::high_resolution_clock>
class FirstOrderFilter
{
public:
  using clock_t = _clock_t;
  using timer_t = CyberTimerNoAvg<std::ratio<1>, double, clock_t>;

  FirstOrderFilter() = default;
  FirstOrderFilter(const FirstOrderFilter &) = default;
  FirstOrderFilter(FirstOrderFilter &) = default;
  FirstOrderFilter(FirstOrderFilter &&) = default;
  FirstOrderFilter & operator=(const FirstOrderFilter &) = default;
  FirstOrderFilter & operator=(FirstOrderFilter &) = default;
  FirstOrderFilter & operator=(FirstOrderFilter &&) = default;
  ~FirstOrderFilter() = default;

  template<typename T1>
  explicit FirstOrderFilter(T1 && clock, const double tau = 1.)
  : timer_(std::forward<T1>(clock)),
    tau_(tau)
  {}

  explicit FirstOrderFilter(const double tau)
  : tau_(tau)
  {}

  template<typename T1>
  void set_clock(T1 && clock)
  {
    timer_.set_clock(std::forward<T1>(clock));
  }

  void set_params(const double tau)
  {
    tau_ = tau;
  }

  double get_params() const
  {
    return tau_;
  }

  const T & update(const T & val, const typename timer_t::time_point tNow)
  {
    if (tau_ <= 0.0) {
      return valNm1_ = val;
    }

    if (init_) {
      const double dt = timer_.toctic(tNow);
      const double e = std::exp(-dt / tau_);
      return valNm1_ = valNm1_ * e + (1 - e) * val;
    }

    init_ = true;
    timer_.tic(tNow);
    return valNm1_ = val;
  }

  const T & update(const T & val)
  {
    return update(val, timer_.now());
  }
  const T & operator()(const T & val)
  {
    return update(val);
  }

  const T & getValue() const
  {
    return valNm1_;
  }

  void reset()
  {
    init_ = false;
  }

  const T & reset(const T & val, const typename timer_t::time_point tNow)
  {
    timer_.tic(tNow);
    init_ = true;
    return valNm1_ = val;
  }

  const T & reset(const T & val)
  {
    return reset(val, timer_.now());
  }

protected:
  timer_t timer_ = timer_t(clock_t{});
  double tau_{1.0};
  T valNm1_{fof_details::zero_value<T>::value()};
  bool init_{false};
};

}  // namespace cbr

#endif  // CBR_CONTROL__FIRST_ORDER_FILTER_HPP_


#define BOOST_TEST_MODULE pcraster com clone
#include <boost/test/unit_test.hpp>
#include "stddefx.h"
#include <memory>
#include "com_intervaltypes.h"

namespace com {

  typedef Interval<double> IntervalD;
  static IntervalD *create(const char *msg)
  {
    return com::createIntervalFromLookupTableKey<double>(std::string(msg));
  }

  typedef std::auto_ptr< com::Interval<double> > IVap;
}

BOOST_AUTO_TEST_CASE(from_lookup_table_key_correct_format)
{
  using namespace com;

 {
  IVap a(create("[ ,]"));
  BOOST_CHECK(a->min() == a->minLimit());
  BOOST_CHECK(a->valid(a->min()));
  BOOST_CHECK(a->max() == a->maxLimit());
  BOOST_CHECK(a->valid(a->max()));
 }
 {
  IVap a(create("6")); // EqualTo a(6);
  BOOST_CHECK(a->min() == 6);
  BOOST_CHECK(a->valid(a->min()));
  BOOST_CHECK(a->max() == 6);
  BOOST_CHECK(a->valid(a->max()));
 }

// THE HALF OPEN LIMITS
 {IVap a(create("[6, >")); // GreaterThanEqualTo a(6);
  BOOST_CHECK(a->min() == 6);
  BOOST_CHECK(a->valid(a->min()));
  BOOST_CHECK(a->max() == a->maxLimit());
  BOOST_CHECK(a->valid(a->max()));
 }
 {IVap a(create("<68, ]")); // GreaterThan a(68);
  BOOST_CHECK(a->min() == 68);
  BOOST_CHECK(!a->valid(a->min()));
  BOOST_CHECK(a->max() == a->maxLimit());
  BOOST_CHECK(a->valid(a->max()));
 }

// THE HALF OPEN LIMITS
 {IVap a(create("<,61]")); // LessThanEqualTo a(61);
  BOOST_CHECK(a->min() == a->minLimit());
  BOOST_CHECK(a->valid(a->min()));
  BOOST_CHECK(a->max() == 61);
  BOOST_CHECK(a->valid(a->max()));
 }
 {IVap a(create("<,6>")); // LessThan a(6);
  BOOST_CHECK(a->min() == a->minLimit());
  BOOST_CHECK(a->valid(a->min()));
  BOOST_CHECK(a->max() == 6);
  BOOST_CHECK(!a->valid(a->max()));
 }

 {
 IVap a(create("[4, 6>"));
  BOOST_CHECK(a->min() == 4);
  BOOST_CHECK(a->valid(a->min()));
  BOOST_CHECK(a->max() == 6);
  BOOST_CHECK(!a->valid(a->max()));
 }
 {
  IVap a(create("<4, 6>"));
  BOOST_CHECK(a->min() == 4);
  BOOST_CHECK(!a->valid(a->min()));
  BOOST_CHECK(a->max() == 6);
  BOOST_CHECK(!a->valid(a->max()));
 }
 {
 IVap a(create("[4, 6]"));
  BOOST_CHECK(a->min() == 4);
  BOOST_CHECK(a->valid(a->min()));
  BOOST_CHECK(a->max() == 6);
  BOOST_CHECK(a->valid(a->max()));
 }
 {
   try {
   // test tabs as whitespace
   // test trailing whitespace (failure introduce from boost 1.33 - 1.34)
   // IVap a(create(" <4, 6	    ]  "));
   IVap a(create(" <4, 6	    ] "));
    BOOST_CHECK(a->min() == 4);
    BOOST_CHECK(!a->valid(a->min()));
    BOOST_CHECK(a->max() == 6);
    BOOST_CHECK(a->valid(a->max()));
   } catch (const com::BadIntervalFormat& /*e*/) {
     // bug67
     bool fixSpiritParserBug67=false;
     BOOST_CHECK(fixSpiritParserBug67);
   }
 }
 {
 IVap a(create("<0,0.2]"));
  BOOST_CHECK(a->min() == 0);
  BOOST_CHECK(!a->valid(a->min()));
  BOOST_CHECK(a->max() == 0.2);
  BOOST_CHECK(a->valid(a->max()));
  BOOST_CHECK(a->valid(0.2));
 }
}

BOOST_AUTO_TEST_CASE(from_lookup_table_key_wrong_format)
{
  using namespace com;

  const char *fmts[] = {
    "[ , ",
    "a6",
    "[6, >d",
    "d<68, ]",
    "=,61]",
    ",,>",
    "<03, 34,>",
    "[ 0 ] ",
    "<2, 1>", // <- low is larger than high
    "<1, 1>", // <- low is larger than high
    "[1, 1>", // <- low is larger than high
    "<1, 1]", // <- low is larger than high
  };
  for (size_t i=0; i< ARRAY_SIZE(fmts); i++) {
    bool catched=false;
    try {
       IVap a(create(fmts[i]));
    } catch (const com::BadIntervalFormat& /*e*/) {
       catched=true;
    }
    // if (!catched)
    //  std::cerr << "expect wrong fmt|" << fmts[i] << "|\n";
    BOOST_CHECK(catched);
  }
}

BOOST_AUTO_TEST_CASE(test_equal_to)
{
  using namespace com;

 {
  IVap a(create("2"));
  BOOST_CHECK(a->equalTo());
 }
 {
  IVap a(create("[2,2]"));
  BOOST_CHECK( a->equalTo());
 }
 {
  IVap a(create("[2,>"));
  BOOST_CHECK(!a->equalTo());
 }
 {
  IVap a(create("[ , >"));
  BOOST_CHECK(!a->equalTo());
 }
}

BOOST_AUTO_TEST_CASE(test_min_max)
{
  using namespace com;

  {// docs of virtual double  min()const=0;
   GreaterThanEqualTo<> ge(0);
   BOOST_CHECK( ge.valid(ge.min()));
   GreaterThan<> gt(0);
   BOOST_CHECK(!gt.valid(gt.min()));
  }
 {
  AnythingInterval<> a;
  BOOST_CHECK(a.min() == a.minLimit());
  BOOST_CHECK(a.valid(a.min()));
  BOOST_CHECK(a.max() == a.maxLimit());
  BOOST_CHECK(a.valid(a.max()));
 }
 {
  EqualTo<> a(6);
  BOOST_CHECK(a.min() == 6);
  BOOST_CHECK(a.valid(a.min()));
  BOOST_CHECK(a.max() == 6);
  BOOST_CHECK(a.valid(a.max()));
 }

// THE HALF OPEN LIMITS
 {GreaterThanEqualTo<> a(6);
  BOOST_CHECK(a.min() == 6);
  BOOST_CHECK(a.valid(a.min()));
  BOOST_CHECK(a.max() == a.maxLimit());
  BOOST_CHECK(a.valid(a.max()));
 }
 {GreaterThan<> a(6);
  BOOST_CHECK(a.min() == 6);
  BOOST_CHECK(!a.valid(a.min()));
  BOOST_CHECK(a.max() == a.maxLimit());
  BOOST_CHECK(a.valid(a.max()));
 }

// THE HALF OPEN LIMITS
 {LessThanEqualTo<> a(6);
  BOOST_CHECK(a.min() == a.minLimit());
  BOOST_CHECK(a.valid(a.min()));
  BOOST_CHECK(a.max() == 6);
  BOOST_CHECK(a.valid(a.max()));
 }
 {LessThan<> a(6);
  BOOST_CHECK(a.min() == a.minLimit());
  BOOST_CHECK(a.valid(a.min()));
  BOOST_CHECK(a.max() == 6);
  BOOST_CHECK(!a.valid(a.max()));
 }

 {
  BetweenLimits<> a(GreaterThanEqualTo<>(4),LessThan<>(6)); // [4, 6>
  BOOST_CHECK(a.min() == 4);
  BOOST_CHECK(a.valid(a.min()));
  BOOST_CHECK(a.max() == 6);
  BOOST_CHECK(!a.valid(a.max()));
 }
 {
  BetweenLimits<> a(GreaterThan<>(4),LessThan<>(6)); // <4, 6>
  BOOST_CHECK(a.min() == 4);
  BOOST_CHECK(!a.valid(a.min()));
  BOOST_CHECK(a.max() == 6);
  BOOST_CHECK(!a.valid(a.max()));
 }
 {
  BetweenLimits<> a(GreaterThanEqualTo<>(4),LessThanEqualTo<>(6)); // [4, 6]
  BOOST_CHECK(a.min() == 4);
  BOOST_CHECK(a.valid(a.min()));
  BOOST_CHECK(a.max() == 6);
  BOOST_CHECK(a.valid(a.max()));
 }
 {
  BetweenLimits<> a(GreaterThan<>(4),LessThanEqualTo<>(6)); // <4, 6]
  BOOST_CHECK(a.min() == 4);
  BOOST_CHECK(!a.valid(a.min()));
  BOOST_CHECK(a.max() == 6);
  BOOST_CHECK(a.valid(a.max()));
 }
}

BOOST_AUTO_TEST_CASE(between_limits)
{
  using namespace com;

  BetweenLimits<> b(GreaterThanEqualTo<>(0),LessThan<>(4));
  BOOST_CHECK(!b.valid(-1));
  BOOST_CHECK( b.valid(0));
  BOOST_CHECK( b.valid(2));
  BOOST_CHECK(!b.valid(4));
  BOOST_CHECK(!b.valid(5));

  // was bug, now works
  BetweenLimits<> copy(b);
  BOOST_CHECK(copy.valid(2));

  BetweenLimits<> assignTo(GreaterThanEqualTo<>(8),LessThan<>(12));
  BOOST_CHECK(!assignTo.valid(2));
  assignTo = b;
  BOOST_CHECK( assignTo.valid(2));
}

BOOST_AUTO_TEST_CASE(less_double_operator)
{
  using namespace com;

// Anything

  BOOST_CHECK(! (AnythingInterval<>()   < 2));
  // DOES NOT COMPILE?
  // BOOST_CHECK(! (AnythingInterval<>() > 2));
  BOOST_CHECK(! AnythingInterval<>().operator>(2));
  AnythingInterval<> a;
  BOOST_CHECK(! (a                     > 2));

// EqualTo

  BOOST_CHECK(! (EqualTo<>(0) < -2));   //   0  < -2
  BOOST_CHECK(! (EqualTo<>(0) <  0));   //   0  <  0
  BOOST_CHECK(  (EqualTo<>(0) <  1));   //   0  <  1
  BOOST_CHECK(  (EqualTo<>(0) > -2));   //   0  > -2
  BOOST_CHECK(! (EqualTo<>(0) >  0));   //   0  >  0
  BOOST_CHECK(! (EqualTo<>(0) >  1));   //   0  >  1

// THE HALF OPEN LIMITS

  BOOST_CHECK(! (GreaterThanEqualTo<>(0) < -2));   //   [0->  < -2
  BOOST_CHECK(! (GreaterThanEqualTo<>(0) <  0));   //   [0->  <  0
  BOOST_CHECK(! (GreaterThanEqualTo<>(0) <  1));   //   [0->  <  1
  BOOST_CHECK(  (GreaterThanEqualTo<>(0) > -2));   //   [0->  > -2
  BOOST_CHECK(! (GreaterThanEqualTo<>(0) >  0));   //   [0->  >  0
  BOOST_CHECK(! (GreaterThanEqualTo<>(0) >  1));   //   [0->  >  1

  BOOST_CHECK(! (GreaterThan<>     (0) < -2));   //   <0->  < -2
  BOOST_CHECK(! (GreaterThan<>     (0) <  0));   //   <0->  <  0
  BOOST_CHECK(! (GreaterThan<>     (0) <  1));   //   <0->  <  1
  BOOST_CHECK(  (GreaterThan<>     (0) > -2));   //   <0->  > -2
  BOOST_CHECK(  (GreaterThan<>     (0) >  0));   //   <0->  >  0
  BOOST_CHECK(! (GreaterThan<>     (0) >  1));   //   <0->  >  1

  BOOST_CHECK(! (LessThanEqualTo<>(0) < -2));   //   <-0]  < -2
  BOOST_CHECK(! (LessThanEqualTo<>(0) <  0));   //   <-0]  <  0
  BOOST_CHECK(  (LessThanEqualTo<>(0) <  1));   //   <-0]  <  1
  BOOST_CHECK(! (LessThanEqualTo<>(0) > -2));   //   <-0]  > -2
  BOOST_CHECK(! (LessThanEqualTo<>(0) >  0));   //   <-0]  >  0
  BOOST_CHECK(! (LessThanEqualTo<>(0) >  1));   //   <-0]  >  1

  BOOST_CHECK(! (LessThan<>     (0) < -2));   //   <-0>  < -2
  BOOST_CHECK(  (LessThan<>     (0) <  0));   //   <-0>  <  0
  BOOST_CHECK(  (LessThan<>     (0) <  1));   //   <-0>  <  1
  BOOST_CHECK(! (LessThan<>     (0) > -2));   //   <-0>  > -2
  BOOST_CHECK(! (LessThan<>     (0) >  0));   //   <-0>  >  0
  BOOST_CHECK(! (LessThan<>     (0) >  1));   //   <-0>  >  1

  {
  BetweenLimits<> b(GreaterThanEqualTo<>(0),LessThan<>(4));
  BOOST_CHECK(! (b < -2));   //   [0,4> < -2
  BOOST_CHECK(! (b <  0));   //   [0,4> <  0
  BOOST_CHECK(! (b <  1));   //   [0,4> <  1
  BOOST_CHECK(  (b <  4));   //   [0,4> <  4
  BOOST_CHECK(  (b <  6));   //   [0,4> <  6
  BOOST_CHECK(  (b > -2));   //   [0,4> > -2
  BOOST_CHECK(! (b >  0));   //   [0,4> >  0
  BOOST_CHECK(! (b >  1));   //   [0,4> >  1
  BOOST_CHECK(! (b >  4));   //   [0,4> >  4
  BOOST_CHECK(! (b >  6));   //   [0,4> >  6
  }

  {
  BetweenLimits<> b(GreaterThan<>(0),LessThan<>(4));
  BOOST_CHECK(! (b < -2));   //    <0,4> < -2
  BOOST_CHECK(! (b <  0));   //    <0,4> <  0
  BOOST_CHECK(! (b <  1));   //    <0,4> <  1
  BOOST_CHECK(  (b <  4));   //    <0,4> <  4
  BOOST_CHECK(  (b <  6));   //    <0,4> <  6
  BOOST_CHECK(  (b > -2));   //    <0,4> > -2
  BOOST_CHECK(  (b >  0));   //    <0,4> >  0
  BOOST_CHECK(! (b >  1));   //    <0,4> >  1
  BOOST_CHECK(! (b >  4));   //    <0,4> >  4
  BOOST_CHECK(! (b >  6));   //    <0,4> >  6
  }

  {
  BetweenLimits<> b(GreaterThanEqualTo<>(0),LessThanEqualTo<>(4));
  BOOST_CHECK(! (b < -2));   //   [0,4] < -2
  BOOST_CHECK(! (b <  0));   //   [0,4] <  0
  BOOST_CHECK(! (b <  1));   //   [0,4] <  1
  BOOST_CHECK(! (b <  4));   //   [0,4] <  4
  BOOST_CHECK(  (b <  6));   //   [0,4] <  6
  BOOST_CHECK(  (b > -2));   //   [0,4] > -2
  BOOST_CHECK(! (b >  0));   //   [0,4] >  0
  BOOST_CHECK(! (b >  1));   //   [0,4] >  1
  BOOST_CHECK(! (b >  4));   //   [0,4] >  4
  BOOST_CHECK(! (b >  6));   //   [0,4] >  6
  }

  {
  BetweenLimits<> b(GreaterThan<>(0),LessThanEqualTo<>(4));
  BOOST_CHECK(! (b < -2));   //    <0,4] < -2
  BOOST_CHECK(! (b <  0));   //    <0,4] <  0
  BOOST_CHECK(! (b <  1));   //    <0,4] <  1
  BOOST_CHECK(! (b <  4));   //    <0,4] <  4
  BOOST_CHECK(  (b <  6));   //    <0,4] <  6
  BOOST_CHECK(  (b > -2));   //    <0,4] > -2
  BOOST_CHECK(  (b >  0));   //    <0,4] >  0
  BOOST_CHECK(! (b >  1));   //    <0,4] >  1
  BOOST_CHECK(! (b >  4));   //    <0,4] >  4
  BOOST_CHECK(! (b >  6));   //    <0,4] >  6
  }

}

BOOST_AUTO_TEST_CASE(less_operator)
{
  using namespace com;

  BOOST_CHECK(!(GreaterThan<>(0).less(LessThanEqualTo<>(4))));

  // < 0   vs.  >= 4
  BOOST_CHECK( (LessThan<>(0).less(GreaterThanEqualTo<>(4))));

  // <= 4   vs.  >= 4
  BOOST_CHECK(!(LessThanEqualTo<>(4).less(GreaterThanEqualTo<>(4))));
  BOOST_CHECK( (LessThanEqualTo<>(4).less(GreaterThan<>(4))));
  BOOST_CHECK( (LessThan<>(4).less(GreaterThanEqualTo<>(4))));

  BOOST_CHECK(!(LessThanEqualTo<>(4).less(AnythingInterval<>())));
  BOOST_CHECK(!(AnythingInterval<>().less(AnythingInterval<>())));

  BOOST_CHECK( (EqualTo<>(4).less(GreaterThan<>(4))));
  BOOST_CHECK(!(EqualTo<>(4).less(GreaterThanEqualTo<>(4))));

  BOOST_CHECK( (EqualTo<>(3).less(GreaterThan<>(4))));
  BOOST_CHECK( (EqualTo<>(3).less(GreaterThanEqualTo<>(4))));

  BOOST_CHECK(!GreaterThan<>(4).less(GreaterThan<>(5)));
}

BOOST_AUTO_TEST_CASE(eq_operator)
{
  using namespace com;

  BOOST_CHECK((GreaterThan<>(0)!= (LessThanEqualTo<>(4))));
  BOOST_CHECK(GreaterThan<>(0) == GreaterThan<>(0));
  BOOST_CHECK(LessThanEqualTo<>(4) != AnythingInterval<>());
  BOOST_CHECK(AnythingInterval<>() == AnythingInterval<>());
  BOOST_CHECK(EqualTo<>(4)         == EqualTo<>(4));
  BOOST_CHECK(EqualTo<>(4)         != EqualTo<>(10));
}

BOOST_AUTO_TEST_CASE(round_error)
{
  using namespace com;

  double v(0.2);
  float  vf(0.2F);
  // assumptions:
  // ! v in < 0.2, 0.4]
  //   v in < 0  , 0.2]
  BOOST_CHECK(!
    BetweenLimits<>(GreaterThan<>(0.2),LessThanEqualTo<>(0.4)).valid(v));
  BOOST_CHECK(
    BetweenLimits<>(GreaterThan<>(0),LessThanEqualTo<>(0.2)).valid(v));
  {
  typedef float CT; // choose template
  BOOST_CHECK(!
    BetweenLimits<CT>(GreaterThan<CT>(0.2F),LessThanEqualTo<CT>(0.4F)).valid(vf));
  BOOST_CHECK(
    BetweenLimits<CT>(GreaterThan<CT>(0.0F),LessThanEqualTo<CT>(0.2F)).valid(vf));
  }
  {
    // ROUNDING ERROR comparing float(0.2) ?= double(0.2)
  typedef double CT; // choose template
  BOOST_CHECK(   // <-- this is not what we expect
    BetweenLimits<CT>(GreaterThan<CT>(0.2),LessThanEqualTo<CT>(0.4)).valid(vf));
  BOOST_CHECK(! // <-- this is not what we expect
    BetweenLimits<CT>(GreaterThan<CT>(0.0),LessThanEqualTo<CT>(0.2)).valid(vf));
  }
  { // OK float instantation
  typedef double CT; // choose template
  BOOST_CHECK(!
    BetweenLimits<CT>(GreaterThan<CT>(0.2f),LessThanEqualTo<CT>(0.4f)).valid(vf));
  BOOST_CHECK(
    BetweenLimits<CT>(GreaterThan<CT>(0.0f),LessThanEqualTo<CT>(0.2f)).valid(vf));
  }
  {
    // the solution, ctor argument must be a float if the valid argument is a float
  typedef double CT; // choose template
  BOOST_CHECK(!
    BetweenLimits<CT>(GreaterThan<CT>(vf),LessThanEqualTo<CT>(0.4)).valid(vf));
  BOOST_CHECK(
    BetweenLimits<CT>(GreaterThan<CT>(0),LessThanEqualTo<CT>(vf)).valid(vf));
  }
}

BOOST_AUTO_TEST_CASE(limit)
{
  using namespace com;

  {
   typedef Interval<double> D; // choose template
   BOOST_CHECK(D::maxLimit()+D::minLimit()==0);
  }
  {
   typedef Interval<float> F; // choose template
   BOOST_CHECK(F::maxLimit()+F::minLimit()==0);
  }
}


#pragma once

#if defined(_OPENMP)
#include <omp.h>
#endif

#include <cmath>
#include <functional>
#include <map>
#include <vector>

#include <Eigen/Geometry>

namespace common_robotics_utilities
{
namespace math
{
///////////////////////////////////////////////////////////////
//// Typedefs for aligned STL containers using Eigen types ////
///////////////////////////////////////////////////////////////

using VectorVector2f
  = std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f>>;
using VectorVector2d
  = std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>>;
using VectorVector3f
  = std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>>;
using VectorVector3d
  = std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>>;
using VectorVector4f
  = std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f>>;
using VectorVector4d
  = std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d>>;
using VectorQuaternionf
  = std::vector<Eigen::Quaternionf,
                Eigen::aligned_allocator<Eigen::Quaternionf>>;
using VectorQuaterniond
  = std::vector<Eigen::Quaterniond,
                Eigen::aligned_allocator<Eigen::Quaterniond>>;
using VectorIsometry3f
  = std::vector<Eigen::Isometry3f,
                Eigen::aligned_allocator<Eigen::Isometry3f>>;
using VectorIsometry3d
  = std::vector<Eigen::Isometry3d,
                Eigen::aligned_allocator<Eigen::Isometry3d>>;
using MapStringVector2f
  = std::map<std::string, Eigen::Vector2f, std::less<std::string>,
  Eigen::aligned_allocator<std::pair<const std::string, Eigen::Vector2f>>>;
using MapStringVector2d
  = std::map<std::string, Eigen::Vector2d, std::less<std::string>,
  Eigen::aligned_allocator<std::pair<const std::string, Eigen::Vector2d>>>;
using MapStringVector3f
  = std::map<std::string, Eigen::Vector3f, std::less<std::string>,
  Eigen::aligned_allocator<std::pair<const std::string, Eigen::Vector3f>>>;
using MapStringVector3d
  = std::map<std::string, Eigen::Vector3d, std::less<std::string>,
  Eigen::aligned_allocator<std::pair<const std::string, Eigen::Vector3d>>>;
using MapStringVector4f
  = std::map<std::string, Eigen::Vector4f, std::less<std::string>,
  Eigen::aligned_allocator<std::pair<const std::string, Eigen::Vector4f>>>;
using MapStringVector4d
  = std::map<std::string, Eigen::Vector4d, std::less<std::string>,
  Eigen::aligned_allocator<std::pair<const std::string, Eigen::Vector4d>>>;
using MapStringQuaternionf
  = std::map<std::string, Eigen::Quaternionf, std::less<std::string>,
  Eigen::aligned_allocator<std::pair<const std::string, Eigen::Quaternionf>>>;
using MapStringQuaterniond
  = std::map<std::string, Eigen::Quaterniond, std::less<std::string>,
  Eigen::aligned_allocator<std::pair<const std::string, Eigen::Quaterniond>>>;
using MapStringIsometry3f
  = std::map<std::string, Eigen::Isometry3f, std::less<std::string>,
  Eigen::aligned_allocator<std::pair<const std::string, Eigen::Isometry3f>>>;
using MapStringIsometry3d
  = std::map<std::string, Eigen::Isometry3d, std::less<std::string>,
  Eigen::aligned_allocator<std::pair<const std::string, Eigen::Isometry3d>>>;

bool Equal3d(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2);

bool Equal4d(const Eigen::Vector4d& v1, const Eigen::Vector4d& v2);

bool CloseEnough(const double p1, const double p2, const double threshold);

bool CloseEnough(const Eigen::Vector3d& v1,
                 const Eigen::Vector3d& v2,
                 const double threshold);

Eigen::Vector3d RotateVector(const Eigen::Quaterniond& quat,
                             const Eigen::Vector3d& vec);

Eigen::Vector3d RotateVectorReverse(const Eigen::Quaterniond& quat,
                                    const Eigen::Vector3d& vec);

double EnforceContinuousRevoluteBounds(const double value);

Eigen::VectorXd SafeNormal(const Eigen::VectorXd& vec);

double SquaredNorm(const std::vector<double>& vec);

double Norm(const std::vector<double>& vec);

std::vector<double> Abs(const std::vector<double>& vec);

std::vector<double> Multiply(const std::vector<double>& vec,
                             const double scalar);

std::vector<double> Multiply(const std::vector<double>& vec1,
                             const std::vector<double>& vec2);

std::vector<double> Divide(const std::vector<double>& vec,
                           const double scalar);

std::vector<double> Divide(const std::vector<double>& vec1,
                           const std::vector<double>& vec2);

std::vector<double> Add(const std::vector<double>& vec,
                        const double scalar);

std::vector<double> Add(const std::vector<double>& vec1,
                        const std::vector<double>& vec2);

std::vector<double> Sub(const std::vector<double>& vec,
                        const double scalar);

std::vector<double> Sub(const std::vector<double>& vec1,
                        const std::vector<double>& vec2);

double Sum(const std::vector<double>& vec);

Eigen::Matrix3d Skew(const Eigen::Vector3d& vector);

Eigen::Vector3d Unskew(const Eigen::Matrix3d& matrix);

Eigen::Matrix4d TwistHat(const Eigen::Matrix<double, 6, 1>& twist);

Eigen::Matrix<double, 6, 1> TwistUnhat(const Eigen::Matrix4d& hatted_twist);

Eigen::Matrix<double, 6, 6> AdjointFromTransform(
    const Eigen::Isometry3d& transform);

Eigen::Matrix<double, 6, 1> TransformTwist(
    const Eigen::Isometry3d& transform,
    const Eigen::Matrix<double, 6, 1>& initial_twist);

Eigen::Matrix<double, 6, 1> TwistBetweenTransforms(
    const Eigen::Isometry3d& start,
    const Eigen::Isometry3d& end);

Eigen::Matrix3d ExpMatrixExact(const Eigen::Matrix3d& hatted_rot_velocity,
                               const double delta_t);

Eigen::Isometry3d ExpTwist(const Eigen::Matrix<double, 6, 1>& twist,
                           const double delta_t);

double Interpolate(const double p1, const double p2, const double ratio);

double InterpolateContinuousRevolute(const double p1,
                                     const double p2,
                                     const double ratio);

std::vector<double> Interpolate(const std::vector<double>& v1,
                                const std::vector<double>& v2,
                                const double ratio);

Eigen::Quaterniond Interpolate(const Eigen::Quaterniond& q1,
                               const Eigen::Quaterniond& q2,
                               const double ratio);

Eigen::VectorXd InterpolateXd(const Eigen::VectorXd& v1,
                              const Eigen::VectorXd& v2,
                              const double ratio);

Eigen::Vector3d Interpolate3d(const Eigen::Vector3d& v1,
                              const Eigen::Vector3d& v2,
                              const double ratio);

Eigen::Vector4d Interpolate4d(const Eigen::Vector4d& v1,
                              const Eigen::Vector4d& v2,
                              const double ratio);

Eigen::Isometry3d Interpolate(const Eigen::Isometry3d& t1,
                              const Eigen::Isometry3d& t2,
                              const double ratio);

double SquaredDistance(const Eigen::Vector2d& v1,
                       const Eigen::Vector2d& v2);

double Distance(const Eigen::Vector2d& v1,
                const Eigen::Vector2d& v2);

double SquaredDistance(const Eigen::Vector3d& v1,
                       const Eigen::Vector3d& v2);

double Distance(const Eigen::Vector3d& v1,
                const Eigen::Vector3d& v2);

double SquaredDistance(const Eigen::VectorXd& v1,
                       const Eigen::VectorXd& v2);

double Distance(const Eigen::VectorXd& v1,
                const Eigen::VectorXd& v2);

double Distance(const Eigen::Quaterniond& q1,
                const Eigen::Quaterniond& q2);

double Distance(const Eigen::Isometry3d& t1,
                const Eigen::Isometry3d& t2,
                const double alpha=0.5);

double SquaredDistance(const std::vector<double>& p1,
                       const std::vector<double>& p2);

double Distance(const std::vector<double>& p1, const std::vector<double>& p2);

double ContinuousRevoluteSignedDistance(const double p1, const double p2);

double ContinuousRevoluteDistance(const double p1, const double p2);

double AddContinuousRevoluteValues(const double start, const double change);

double GetContinuousRevoluteRange(const double start, const double end);

bool CheckInContinuousRevoluteRange(const double start,
                                    const double range,
                                    const double val);

bool CheckInContinuousRevoluteBounds(const double start,
                                     const double end,
                                     const double val);

double AverageStdVectorDouble(
    const std::vector<double>& values,
    const std::vector<double>& weights=std::vector<double>());

double ComputeStdDevStdVectorDouble(const std::vector<double>& values,
                                    const double mean);

double ComputeStdDevStdVectorDouble(const std::vector<double>& values);

double WeightedDotProduct(const Eigen::VectorXd& vec1,
                          const Eigen::VectorXd& vec2,
                          const Eigen::VectorXd& weights);

double WeightedSquaredNorm(const Eigen::VectorXd& vec,
                           const Eigen::VectorXd weights);

double WeightedNorm(const Eigen::VectorXd& vec,
                    const Eigen::VectorXd& weights);

double WeightedCosineAngleBetweenVectors(const Eigen::VectorXd& vec1,
                                         const Eigen::VectorXd& vec2,
                                         const Eigen::VectorXd& weights);

double WeightedAngleBetweenVectors(const Eigen::VectorXd& vec1,
                                   const Eigen::VectorXd& vec2,
                                   const Eigen::VectorXd& weights);

Eigen::Vector3d AverageEigenVector3d(
    const VectorVector3d& vectors,
    const std::vector<double>& weights=std::vector<double>());

Eigen::Vector4d AverageEigenVector4d(
    const VectorVector4d& vectors,
    const std::vector<double>& weights=std::vector<double>());

Eigen::VectorXd AverageEigenVectorXd(
    const std::vector<Eigen::VectorXd>& vectors,
    const std::vector<double>& weights=std::vector<double>());

Eigen::Quaterniond AverageEigenQuaterniond(
    const VectorQuaterniond& quaternions,
    const std::vector<double>& weights=std::vector<double>());

Eigen::Isometry3d AverageEigenIsometry3d(
    const VectorIsometry3d& transforms,
    const std::vector<double>& weights=std::vector<double>());

// This function does not actually deal with the continuous revolute
// space correctly, it just assumes a normal real Euclidean space
double AverageContinuousRevolute(
    const std::vector<double>& angles,
    const std::vector<double>& weights=std::vector<double>());

template <typename Derived>
inline Eigen::MatrixXd ClampNorm(
    const Eigen::MatrixBase<Derived>& item_to_clamp, const double max_norm)
{
  const double current_norm = item_to_clamp.norm();
  if (current_norm > max_norm)
  {
    return item_to_clamp * (max_norm / current_norm);
  }
  return item_to_clamp;
}

// This function is really only going to work well for "almost continuous"
// types, i.e. floats and doubles, due to the implementation
template<typename ScalarType, int Rows,
         typename Allocator=std::allocator<Eigen::Matrix<ScalarType, Rows, 1>>>
inline Eigen::Matrix<ScalarType, Rows, 1> AverageEigenVector(
    const std::vector<Eigen::Matrix<ScalarType, Rows, 1>, Allocator>& vectors,
    const std::vector<double>& weights = std::vector<double>())
{
  // Get the weights
  if (vectors.empty())
  {
    throw std::invalid_argument("vectors is empty");
  }
  if ((weights.size() > 0) && (vectors.size() != weights.size()))
  {
    throw std::invalid_argument("weights.size() > 0 != vectors.size()");
  }
  const bool use_weights = (weights.size() != 0);
  // Find the first element with non-zero weight
  size_t starting_idx = 0;
  while (starting_idx < weights.size() && weights[starting_idx] == 0.0)
  {
    starting_idx++;
  }
  // If all weights are zero, result is undefined
  if (starting_idx >= vectors.size())
  {
    throw std::invalid_argument("All provided weights are zero");
  }
  // Start the recursive definition with the base case
  Eigen::Matrix<ScalarType, Rows, 1> avg_vector = vectors[starting_idx];
  const double starting_weight = use_weights ? std::abs(weights[starting_idx])
                                             : 1.0;
  double weights_running_sum = starting_weight;
  // Do the weighted averaging on the rest of the vectors
  for (size_t idx = starting_idx + 1; idx < vectors.size(); ++idx)
  {
    const double weight = use_weights ? std::abs(weights[idx]) : 1.0;
    weights_running_sum += weight;
    const double effective_weight = weight / weights_running_sum;
    const Eigen::Matrix<ScalarType, Rows, 1> prev_avg_vector = avg_vector;
    const Eigen::Matrix<ScalarType, Rows, 1>& current = vectors[idx];
    avg_vector = prev_avg_vector
                 + (effective_weight * (current - prev_avg_vector));
  }
  return avg_vector;
}

// Projects vector_to_project onto base_vector and
// returns the portion that is parallel to base_vector
template <typename DerivedB, typename DerivedV>
inline Eigen::Matrix<typename DerivedB::Scalar, Eigen::Dynamic, 1>
VectorProjection(
    const Eigen::MatrixBase<DerivedB>& base_vector,
    const Eigen::MatrixBase<DerivedV>& vector_to_project)
{
  EIGEN_STATIC_ASSERT_VECTOR_ONLY(DerivedB);
  EIGEN_STATIC_ASSERT_VECTOR_ONLY(DerivedV);
  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(DerivedB, DerivedV)
  static_assert(std::is_same<typename DerivedB::Scalar,
                typename DerivedV::Scalar>::value,
                "vectors must have the same data type");
  // Perform projection
  const typename DerivedB::Scalar b_squared_norm = base_vector.squaredNorm();
  if (b_squared_norm > 0)
  {
    return (base_vector.dot(vector_to_project) / b_squared_norm) * base_vector;
  }
  else
  {
    return Eigen::Matrix<typename DerivedB::Scalar, Eigen::Dynamic, 1>
        ::Zero(base_vector.rows());
  }
}

// Projects vector_to_project onto base_vector and
// returns the portion that is perpendicular to base_vector
template <typename DerivedB, typename DerivedV>
inline Eigen::Matrix<typename DerivedB::Scalar, Eigen::Dynamic, 1>
VectorRejection(
    const Eigen::MatrixBase<DerivedB>& base_vector,
    const Eigen::MatrixBase<DerivedV>& vector_to_reject)
{
  // Rejection is defined in relation to projection
  return vector_to_reject - VectorProjection(base_vector, vector_to_reject);
}

template <typename DerivedV>
inline Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1>
GetArbitraryOrthogonalVector(const Eigen::MatrixBase<DerivedV>& vector)
{
  // We're going to try arbitrary possibilities until one of them works
  const ssize_t vector_size = vector.size();
  if (vector_size > 0)
  {
    for (ssize_t idx = 0; idx < vector_size; idx++)
    {
      Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1> test_vector
          = Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1>
          ::Zero(vector_size);
      test_vector(idx) = static_cast<typename DerivedV::Scalar>(1.0);
      const auto rejected_vector = VectorRejection(vector, test_vector);
      const typename DerivedV::Scalar rejected_vector_squared_norm
          = rejected_vector.squaredNorm();
      if (rejected_vector_squared_norm > 0)
      {
        return rejected_vector;
      }
    }
    throw std::runtime_error("Vector rejection failed to find orthogonal"
                             " vector, probably numerical error");
  }
  else
  {
    throw std::invalid_argument("Vector size is zero");
  }
}

template <typename DerivedV>
inline Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 1>
GetArbitraryOrthogonalVectorToPlane(
    const Eigen::MatrixBase<DerivedV>& plane_vector1,
    const Eigen::MatrixBase<DerivedV>& plane_vector2,
    const Eigen::MatrixBase<DerivedV>& vector)
{
  const ssize_t vector_size = vector.size();
  if ((vector_size > 0)
      && (vector_size == plane_vector1.size())
      && (vector_size == plane_vector2.size()))
  {
    const Eigen::MatrixBase<DerivedV> unit_plane_vector1
        = plane_vector1 / plane_vector1.norm();
    const Eigen::MatrixBase<DerivedV> unit_plane_vector2
        = plane_vector2 / plane_vector2.norm();
    const typename DerivedV::Scalar plane_vector_dot_product_mag
        = std::abs(unit_plane_vector1.dot(unit_plane_vector2));
    if (plane_vector_dot_product_mag == 1.0)
    {
      throw std::invalid_argument("Plane vectors do not define a valid plane");
    }
    else
    {
      // Try both plane vectors
      // (by definition, one of the two MUST have an orthogonal component!)
      const auto rejected_vector1 = VectorRejection(vector, plane_vector1);
      const typename DerivedV::Scalar rejected_vector1_squared_norm
          = rejected_vector1.squaredNorm();
      if (rejected_vector1_squared_norm > 0)
      {
        return rejected_vector1;
      }
      else
      {
        const auto rejected_vector2 = VectorRejection(vector, plane_vector2);
        const typename DerivedV::Scalar rejected_vector2_squared_norm
            = rejected_vector2.squaredNorm();
        if (rejected_vector2_squared_norm > 0)
        {
          return rejected_vector2;
        }
        else
        {
          throw std::runtime_error("Vector rejection failed to find orthogonal"
                                   " vector, probably numerical error");
        }
      }
    }
  }
  else
  {
    throw std::invalid_argument("Vector size is zero");
  }
}

template<typename DataType, typename Container=std::vector<DataType>>
Eigen::MatrixXd BuildPairwiseDistanceMatrixParallel(
    const Container& data,
    const std::function<double(const DataType&, const DataType&)>& distance_fn)
{
  Eigen::MatrixXd distance_matrix(data.size(), data.size());
#if defined(_OPENMP)
#pragma omp parallel for
#endif
  for (size_t idx = 0; idx < data.size(); idx++)
  {
    for (size_t jdx = idx; jdx < data.size(); jdx++)
    {
      if (idx != jdx)
      {
        const double distance = distance_fn(data[idx], data[jdx]);
        distance_matrix(static_cast<ssize_t>(idx), static_cast<ssize_t>(jdx))
            = distance;
        distance_matrix(static_cast<ssize_t>(jdx), static_cast<ssize_t>(idx))
            = distance;
      }
      else
      {
        distance_matrix(static_cast<ssize_t>(idx), static_cast<ssize_t>(jdx))
            = 0.0;
        distance_matrix(static_cast<ssize_t>(jdx), static_cast<ssize_t>(idx))
            = 0.0;
      }
    }
  }
  return distance_matrix;
}

template<typename DataType, typename Container=std::vector<DataType>>
Eigen::MatrixXd BuildPairwiseDistanceMatrixSerial(
    const Container& data,
    const std::function<double(const DataType&, const DataType&)>& distance_fn)
{
  Eigen::MatrixXd distance_matrix(data.size(), data.size());
  for (size_t idx = 0; idx < data.size(); idx++)
  {
    for (size_t jdx = idx; jdx < data.size(); jdx++)
    {
      if (idx != jdx)
      {
        const double distance = distance_fn(data[idx], data[jdx]);
        distance_matrix(static_cast<ssize_t>(idx), static_cast<ssize_t>(jdx))
            = distance;
        distance_matrix(static_cast<ssize_t>(jdx), static_cast<ssize_t>(idx))
            = distance;
      }
      else
      {
        distance_matrix(static_cast<ssize_t>(idx), static_cast<ssize_t>(jdx))
            = 0.0;
        distance_matrix(static_cast<ssize_t>(jdx), static_cast<ssize_t>(idx))
            = 0.0;
      }
    }
  }
  return distance_matrix;
}

template<typename DataType, typename Container=std::vector<DataType>>
Eigen::MatrixXd BuildPairwiseDistanceMatrix(
    const Container& data,
    const std::function<double(const DataType&, const DataType&)>& distance_fn,
    const bool use_parallel = false)
{
  if (use_parallel)
  {
    return BuildPairwiseDistanceMatrixParallel(data, distance_fn);
  }
  else
  {
    return BuildPairwiseDistanceMatrixSerial(data, distance_fn);
  }
}

template<typename FirstDataType, typename SecondDataType,
         typename FirstContainer=std::vector<FirstDataType>,
         typename SecondContainer=std::vector<SecondDataType>>
Eigen::MatrixXd BuildPairwiseDistanceMatrixParallel(
    const FirstContainer& data1, const SecondContainer& data2,
    const std::function<double(const FirstDataType&,
                               const SecondDataType&)>& distance_fn)
{
  Eigen::MatrixXd distance_matrix(data1.size(), data2.size());
#if defined(_OPENMP)
#pragma omp parallel for
#endif
  for (size_t idx = 0; idx < data1.size(); idx++)
  {
    for (size_t jdx = 0; jdx < data2.size(); jdx++)
    {
      const double distance = distance_fn(data1[idx], data2[jdx]);
      distance_matrix(static_cast<ssize_t>(idx), static_cast<ssize_t>(jdx))
          = distance;
    }
  }
  return distance_matrix;
}

template<typename FirstDataType, typename SecondDataType,
         typename FirstContainer=std::vector<FirstDataType>,
         typename SecondContainer=std::vector<SecondDataType>>
Eigen::MatrixXd BuildPairwiseDistanceMatrixSerial(
    const FirstContainer& data1, const SecondContainer& data2,
    const std::function<double(const FirstDataType&,
                               const SecondDataType&)>& distance_fn)
{
  Eigen::MatrixXd distance_matrix(data1.size(), data2.size());
  for (size_t idx = 0; idx < data1.size(); idx++)
  {
    for (size_t jdx = 0; jdx < data2.size(); jdx++)
    {
      const double distance = distance_fn(data1[idx], data2[jdx]);
      distance_matrix(static_cast<ssize_t>(idx), static_cast<ssize_t>(jdx))
          = distance;
    }
  }
  return distance_matrix;
}

template<typename FirstDataType, typename SecondDataType,
         typename FirstContainer=std::vector<FirstDataType>,
         typename SecondContainer=std::vector<SecondDataType>>
Eigen::MatrixXd BuildPairwiseDistanceMatrix(
    const FirstContainer& data1, const SecondContainer& data2,
    const std::function<double(const FirstDataType&,
                               const SecondDataType&)>& distance_fn,
    const bool use_parallel = false)
{
  if (use_parallel)
  {
    return BuildPairwiseDistanceMatrixParallel(data1, data2, distance_fn);
  }
  else
  {
    return BuildPairwiseDistanceMatrixSerial(data1, data2, distance_fn);
  }
}

class Hyperplane
{
private:
  Eigen::VectorXd plane_origin_;
  Eigen::VectorXd plane_normal_;

public:

  Hyperplane(const Eigen::VectorXd& origin, const Eigen::VectorXd& normal)
      : plane_origin_(origin), plane_normal_(normal)
  {
    if (plane_origin_.size() != plane_normal_.size())
    {
      throw std::invalid_argument("origin.size() != normal.size()");
    }
  }

  Hyperplane() {}

  size_t GetDimensionality() const
  {
    return static_cast<size_t>(plane_origin_.size());
  }

  const Eigen::VectorXd& GetOrigin() const { return plane_origin_; }

  const Eigen::VectorXd& GetNormal() const { return plane_normal_; }

  double GetNormedDotProduct(const Eigen::VectorXd& point) const
  {
    const Eigen::VectorXd check_vector = point - GetOrigin();
    const Eigen::VectorXd check_vector_normed = SafeNormal(check_vector);
    const double dot_product = check_vector_normed.dot(GetNormal());
    return dot_product;
  }

  double GetRawDotProduct(const Eigen::VectorXd& point) const
  {
    const Eigen::VectorXd check_vector = point - GetOrigin();
    const double dot_product = check_vector.dot(GetNormal());
    return dot_product;
  }

  Eigen::VectorXd RejectVectorOntoPlane(const Eigen::VectorXd& vector) const
  {
    return VectorProjection(GetNormal(), vector);
  }

  double GetSquaredDistanceToPlane(const Eigen::VectorXd& point) const
  {
    const Eigen::VectorXd origin_to_point_vector = point - GetOrigin();
    return VectorProjection(GetNormal(), origin_to_point_vector).squaredNorm();
  }

  double GetDistanceToPlane(const Eigen::VectorXd& point) const
  {
    const Eigen::VectorXd origin_to_point_vector = point - GetOrigin();
    return VectorProjection(GetNormal(), origin_to_point_vector).norm();
  }

  Eigen::VectorXd ProjectVectorOntoPlane(const Eigen::VectorXd& vector) const
  {
    return VectorRejection(GetNormal(), vector);
  }

  Eigen::VectorXd ProjectPointOntoPlane(const Eigen::VectorXd& point) const
  {
    const Eigen::VectorXd origin_to_point_vector = point - GetOrigin();
    const Eigen::VectorXd projected_to_point_vector =
        VectorRejection(GetNormal(), origin_to_point_vector);
    const Eigen::VectorXd projected_point =
        GetOrigin() + projected_to_point_vector;
    return projected_point;
  }
};

Hyperplane FitPlaneToPoints(const std::vector<Eigen::VectorXd>& points);
}  // namespace math
}  // namespace common_robotics_utilities


#pragma once

#include <unordered_map>
#include <functional>
#include <memory>
#include <boost/numeric/ublas/matrix.hpp>
#include <vector>
#include "core/Environment.hpp" // IWYU pragma: export
#include "core/Agent.hpp"
#include "misc.hpp"

// IWYU pragma: no_include <boost/numeric/ublas/functional.hpp>
// IWYU pragma: no_include <boost/numeric/ublas/storage.hpp>
// IWYU pragma: no_include "util/UniqueIdMixin.hpp"

namespace aima::core {
    class Percept;

    class Action;
}

namespace aima::vacuum {
    /**
     * The world contains two locations. Each is either clean or dirty.
     * The agent can be in either location. The agent can move left, move right, or suck up dirt at its current location
     * The goal is to clean all the dirt
     */
    class BasicVacuumEnvironment : public core::Environment {
    public:
        struct AgentState {
            Location location;
            bool     sucking;
        };

        using AgentStates = std::unordered_map<std::reference_wrapper<const core::Agent>,
                                               AgentState,
                                               core::Agent::hash>;
        using Locations = boost::numeric::ublas::matrix<LocationState>;

        static const core::Action ACTION_MOVE_LEFT;
        static const core::Action ACTION_MOVE_RIGHT;
        static const core::Action ACTION_MOVE_UP;
        static const core::Action ACTION_MOVE_DOWN;
        static const core::Action ACTION_SUCK;

        /**
         * Construct a vacuum environment with two locations named A and B and dirt is placed at random
         */
        explicit BasicVacuumEnvironment( unsigned long x = 2, unsigned long y = 1 );

        std::unique_ptr<core::Environment> clone() override;

        std::unique_ptr<core::Percept> getPerceptSeenBy( const core::Agent& agent ) override;

        const Location& getAgentLocation( const core::Agent& agent ) const { return agentStates.at( agent ).location; }

        const AgentStates& getAgentStates() const noexcept { return agentStates; }

        std::vector<Location> getAgentLocationsList() const;

        void setAgentLocation( const core::Agent& agent, const Location& location );

        void removeAgent( const core::Agent& agent ) override;

        bool isDone() const override;

        const Locations& getLocations() const noexcept { return locations; }

        LocationState getLocationState( const Location& location ) const;

        void setLocationState( const Location& location, LocationState state );

        bool addAgent( core::Agent& agent ) override;

        void executeAction( const core::Agent& agent, const core::Action& action ) override;

        void createExogenousChange() override;

        unsigned long getX() const { return locations.size1(); }

        unsigned long getY() const { return locations.size2(); }

    protected:
        BasicVacuumEnvironment( const BasicVacuumEnvironment& ) = default;

        Location& getAgentLocationByRef( const core::Agent& agent ) { return agentStates[ agent ].location; }

        Location randomLocation() const;

        constexpr unsigned maxSteps() const noexcept { return 1000; }

        bool agentStopped() const noexcept { return agentStopped_; }

        void agentStopped( bool stopped ) { agentStopped_ = stopped; }

    private:
        bool        agentStopped_ = false;
        Locations   locations;
        AgentStates agentStates;
    };
}


#include "block_convolver_test_utils.hpp"
#include <Eigen/Core>
#include <cassert>
#include <cmath>
#include <fstream>
#include <random>

/// Generate random impulses.
/// @param len length of returned buffer
/// @param num_nonzero The number of values to randomise
/// @param seedval Seed for rng.
Eigen::VectorXf generate_random(size_t len, size_t num_nonzero,
                                long int seedval) {
  Eigen::VectorXf data = Eigen::VectorXf::Zero(len);

  std::mt19937 gen(seedval);
  for (size_t i = 0; i < num_nonzero; i++) {
    data[std::uniform_int_distribution<int>(0, (int)len - 1)(gen)] =
        std::uniform_real_distribution<float>()(gen);
  }

  return data;
}

/// Simple convolution implementation.
/// @param len output and input lengths
/// @param out[out] output data
/// @param out[in] input data
/// @param ir_len length of impulse response
/// @param ir impulse response data
void convolve(size_t len, float *out, float *in, size_t ir_len, float *ir) {
  for (size_t i = 0; i < len; i++) {
    float total = 0.0;
    for (size_t j = 0; j < ir_len; j++) {
      if (j > i) break;
      total += in[i - j] * ir[j];
    }
    out[i] = total;
  }
}

/// Fade up linearly over a block.
void fade_up(float *out, float *in, size_t n) {
  float i_scale = 1.0f / n;

  for (size_t i = 0; i < n; i++) {
    float a_v = (float)i * i_scale;
    out[i] = a_v * in[i];
  }
}

/// Fade down linearly over a block.
void fade_down(float *out, float *in, size_t n) {
  float i_scale = 1.0f / n;

  for (size_t i = 0; i < n; i++) {
    float a_v = (float)i * i_scale;
    out[i] = (1 - a_v) * in[i];
  }
}

/// Write a float array to a file, readable by numpy.loadtext
void write_array(const std::string &base, const std::string &name, size_t n,
                 float *x) {
  if (base != "") {
    std::ofstream out(base + name);
    assert(out.is_open());
    for (size_t i = 0; i < n; i++) out << x[i] << std::endl;
  }
}


/*
 * test_CVAL.cpp
 *
 *  Created on: 2017年6月25日
 *      Author: Administrator
 */

#ifdef TEST_ALL_IN_ONE
    #include <boost/test/unit_test.hpp>
#else
    #define BOOST_TEST_MODULE test_hikyuu_indicator_suite
    #include <boost/test/unit_test.hpp>
#endif

#include <hikyuu/StockManager.h>
#include <hikyuu/indicator/crt/CVAL.h>
#include <hikyuu/indicator/crt/PRICELIST.h>

using namespace hku;

/**
 * @defgroup test_indicator_CVAL test_indicator_CVAL
 * @ingroup test_hikyuu_indicator_suite
 * @{
 */

/** @par 检测点 */
BOOST_AUTO_TEST_CASE( test_CVAL ) {
    Indicator result;

    PriceList d;
    d.push_back(6063);
    d.push_back(6041);
    d.push_back(6065);
    d.push_back(6078);
    d.push_back(6114);
    d.push_back(6121);
    d.push_back(6106);
    d.push_back(6101);
    d.push_back(6166);
    d.push_back(6169);
    d.push_back(6195);
    d.push_back(6222);
    d.push_back(6186);
    d.push_back(6214);
    d.push_back(6185);
    d.push_back(6209);
    d.push_back(6221);
    d.push_back(6278);
    d.push_back(6326);
    d.push_back(6347);

    /** @arg operator(ind) */
    Indicator ind = PRICELIST(d);
    BOOST_CHECK(ind.size() == 20);
    result = CVAL(ind, 100);
    BOOST_CHECK(result.getParam<double>("value") == 100);
    BOOST_CHECK(result.size() == 20);
    BOOST_CHECK(result.empty() == false);
    BOOST_CHECK(result.discard() == 0);
    BOOST_CHECK(result.getResultNumber() == 1);
    for (size_t i = 0; i < ind.size(); ++i) {
        BOOST_CHECK(result[i] == 100);
    }

    /** @arg operator() */
    result = CVAL(100);
    BOOST_CHECK(result.getParam<double>("value") == 100);
    BOOST_CHECK(result.size() == 0);
    BOOST_CHECK(result.empty() == true);
    BOOST_CHECK(result.discard() == 0);
    BOOST_CHECK(result.getResultNumber() == 1);

    /** @arg 生成指定长度的常量Indicator, discard=0 */
    result = CVAL(100, 5);
    BOOST_CHECK(result.getParam<double>("value") == 100);
    BOOST_CHECK(result.size() == 5);
    BOOST_CHECK(result.empty() == false);
    BOOST_CHECK(result.discard() == 0);
    BOOST_CHECK(result.getResultNumber() == 1);
    for (int i = 0; i < 5; i++) {
        BOOST_CHECK(result[i] == 100);
    }

    /** @arg 生成指定长度的常量Indicator, discard=2 */
    result = CVAL(100, 5, 2);
    BOOST_CHECK(result.getParam<double>("value") == 100);
    BOOST_CHECK(result.size() == 5);
    BOOST_CHECK(result.empty() == false);
    BOOST_CHECK(result.discard() == 2);
    BOOST_CHECK(result.getResultNumber() == 1);
    BOOST_CHECK(result[0] == Null<price_t>());
    BOOST_CHECK(result[1] == Null<price_t>());
    for (int i = 2; i < 5; i++) {
        BOOST_CHECK(result[i] == 100);
    }

}

/** @} */