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

#define BOOST_MATH_OVERFLOW_ERROR_POLICY ignore_error

#include <boost/math/concepts/real_concept.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/test/included/test_exec_monitor.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/tools/stats.hpp>
#include <boost/math/tools/test.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/type_traits/is_floating_point.hpp>
#include <boost/array.hpp>
#include "functor.hpp"

#include "test_gamma_hooks.hpp"
#include "handle_test_result.hpp"

//
// DESCRIPTION:
// ~~~~~~~~~~~~
//
// This file tests the incomplete gamma functions tgamma,
// tgamma_lower, gamma_p and gamma_q. There are two sets of tests, spot
// tests which compare our results with selected values computed
// using the online special function calculator at
// functions.wolfram.com, while the bulk of the accuracy tests
// use values generated with NTL::RR at 1000-bit precision
// and our generic versions of these functions.
//
// Note that when this file is first run on a new platform many of
// these tests will fail: the default accuracy is 1 epsilon which
// is too tight for most platforms.  In this situation you will
// need to cast a human eye over the error rates reported and make
// a judgement as to whether they are acceptable.  Either way please
// report the results to the Boost mailing list.  Acceptable rates of
// error are marked up below as a series of regular expressions that
// identify the compiler/stdlib/platform/data-type/test-data/test-function
// along with the maximum expected peek and RMS mean errors for that
// test.
//

void expected_results()
{
   //
   // Define the max and mean errors expected for
   // various compilers and platforms.
   //
   const char* largest_type;
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   if(boost::math::policies::digits<double, boost::math::policies::policy<> >() == boost::math::policies::digits<long double, boost::math::policies::policy<> >())
   {
      largest_type = "(long\\s+)?double";
   }
   else
   {
      largest_type = "long double";
   }
#else
   largest_type = "(long\\s+)?double";
#endif
   //
   // Linux:
   //
   // These should not really be needed, but on *some* Linux
   // versions these error rates are quite large and appear to
   // be related to the accuracy of powl and expl.  On Itanium
   // or Xeon machines the error rates are much lower than this.
   // Worst cases appear to be AMD64 machines.
   //
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "linux",                          // platform
      largest_type,                     // test type(s)
      "[^|]*medium[^|]*",               // test data group
      "[^|]*", 1000, 200);                 // test function
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "linux",                          // platform
      largest_type,                     // test type(s)
      "[^|]*integer[^|]*",               // test data group
      "[^|]*", 1000, 200);                 // test function
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "linux",                          // platform
      "real_concept",                   // test type(s)
      "[^|]*medium[^|]*",               // test data group
      "[^|]*", 600, 200);                // test function
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "linux",                          // platform
      "real_concept",                   // test type(s)
      "[^|]*integer[^|]*",               // test data group
      "[^|]*", 600, 200);                // test function

   //
   // Mac OS X:
   // It's not clear why these should be required, but see notes above
   // about Linux.
   //
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "Mac OS",                          // platform
      largest_type,                     // test type(s)
      "[^|]*medium[^|]*",               // test data group
      "[^|]*", 5000, 1000);                 // test function
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "Mac OS",                          // platform
      largest_type,                     // test type(s)
      "[^|]*small[^|]*",               // test data group
      "[^|]*", 40, 15);                 // test function
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "Mac OS",                          // platform
      largest_type,                     // test type(s)
      "[^|]*integer[^|]*",               // test data group
      "[^|]*", 2000, 300);                 // test function
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "Mac OS",                          // platform
      "real_concept",                   // test type(s)
      "[^|]*medium[^|]*",               // test data group
      "[^|]*", 5000, 1000);                // test function
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "Mac OS",                          // platform
      "real_concept",                     // test type(s)
      "[^|]*small[^|]*",               // test data group
      "[^|]*", 40, 15);                 // test function
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "Mac OS",                          // platform
      "real_concept",                   // test type(s)
      "[^|]*integer[^|]*",               // test data group
      "[^|]*", 2000, 300);                // test function
   //
   // HP-UX:
   //
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "HP-UX",                          // platform
      largest_type,                     // test type(s)
      "[^|]*medium[^|]*",               // test data group
      "[^|]*", 500, 50);                 // test function
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "HP-UX",                          // platform
      "real_concept",                   // test type(s)
      "[^|]*medium[^|]*",               // test data group
      "[^|]*", 500, 100);                // test function
   //
   // Sun OS:
   //
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "Sun.*",                          // platform
      largest_type,                     // test type(s)
      "[^|]*medium[^|]*",               // test data group
      "[^|]*", 500, 100);               // test function
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "Sun.*",                          // platform
      largest_type,                     // test type(s)
      "[^|]*integer[^|]*",              // test data group
      "[^|]*", 100, 30);                // test function
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "Sun.*",                          // platform
      "real_concept",                   // test type(s)
      "[^|]*medium[^|]*",               // test data group
      "[^|]*", 500, 100);                // test function
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "Sun.*",                          // platform
      "real_concept",                   // test type(s)
      "[^|]*integer[^|]*",               // test data group
      "[^|]*", 100, 30);                // test function

   //
   // Mac OS X:
   //
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "Mac OS",                          // platform
      largest_type,                     // test type(s)
      "[^|]*medium[^|]*",               // test data group
      "[^|]*", 100, 50);                 // test function

   //
   // Large exponent range causes more extreme test cases to be evaluated:
   //
   if(std::numeric_limits<long double>::max_exponent > std::numeric_limits<double>::max_exponent)
   {
      add_expected_result(
         "[^|]*",                          // compiler
         "[^|]*",                          // stdlib
         "[^|]*",                          // platform
         largest_type,                     // test type(s)
         "[^|]*large[^|]*",                // test data group
         ".*", 40000, 3000);  // test function
   }

   //
   // Catch all cases come last:
   //
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "[^|]*",                          // platform
      largest_type,                     // test type(s)
      "[^|]*medium[^|]*",               // test data group
      "[^|]*", 50, 20);                 // test function
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "[^|]*",                          // platform
      largest_type,                     // test type(s)
      "[^|]*small[^|]*",                // test data group
      "[^|]*", 20, 10);                  // test function
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "[^|]*",                          // platform
      largest_type,                     // test type(s)
      "[^|]*large[^|]*",                // test data group
      "boost::math::gamma_q", 500, 50);  // test function
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "Cygwin",                         // platform
      largest_type,                     // test type(s)
      "[^|]*large[^|]*",                // test data group
      "boost::math::gamma_p", 700, 50);  // test function
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "[^|]*",                          // platform
      largest_type,                     // test type(s)
      "[^|]*large[^|]*",                // test data group
      "boost::math::gamma_p", 350, 50);  // test function
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "[^|]*",                          // platform
      largest_type,                     // test type(s)
      "[^|]*integer[^|]*",              // test data group
      ".*", 20, 10);                    // test function
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "[^|]*",                          // platform
      "real_concept",                   // test type(s)
      "[^|]*medium[^|]*",               // test data group
      "[^|]*", 200, 50);                // test function
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "[^|]*",                          // platform
      "real_concept",                   // test type(s)
      "[^|]*small[^|]*",                // test data group
      ".*", 20, 10);                  // test function
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "[^|]*",                          // platform
      "real_concept",                   // test type(s)
      "[^|]*large[^|]*",                // test data group
      ".*", 1000000, 100000);        // test function
   add_expected_result(
      "[^|]*",                          // compiler
      "[^|]*",                          // stdlib
      "[^|]*",                          // platform
      "real_concept",                   // test type(s)
      "[^|]*integer[^|]*",              // test data group
      ".*", 40, 10);                    // test function

   //
   // Finish off by printing out the compiler/stdlib/platform names,
   // we do this to make it easier to mark up expected error rates.
   //
   std::cout << "Tests run with " << BOOST_COMPILER << ", "
      << BOOST_STDLIB << ", " << BOOST_PLATFORM << std::endl;
}

template <class T>
void do_test_gamma_2(const T& data, const char* type_name, const char* test_name)
{
   typedef typename T::value_type row_type;
   typedef typename row_type::value_type value_type;

   typedef value_type (*pg)(value_type, value_type);
#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
   pg funcp = boost::math::tgamma<value_type, value_type>;
#else
   pg funcp = boost::math::tgamma;
#endif

   boost::math::tools::test_result<value_type> result;

   std::cout << "Testing " << test_name << " with type " << type_name
      << "\n~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n";

   //
   // test tgamma(T, T) against data:
   //
   if(data[0][2] > 0)
   {
      result = boost::math::tools::test(
         data,
         bind_func(funcp, 0, 1),
         extract_result(2));
      handle_test_result(result, data[result.worst()], result.worst(), type_name, "boost::math::tgamma", test_name);
      //
      // test tgamma_lower(T, T) against data:
      //
#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
      funcp = boost::math::tgamma_lower<value_type, value_type>;
#else
      funcp = boost::math::tgamma_lower;
#endif
      result = boost::math::tools::test(
         data,
         bind_func(funcp, 0, 1),
         extract_result(4));
      handle_test_result(result, data[result.worst()], result.worst(), type_name, "boost::math::tgamma_lower", test_name);
   }
   //
   // test gamma_q(T, T) against data:
   //
#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
   funcp = boost::math::gamma_q<value_type, value_type>;
#else
   funcp = boost::math::gamma_q;
#endif
   result = boost::math::tools::test(
      data,
      bind_func(funcp, 0, 1),
      extract_result(3));
   handle_test_result(result, data[result.worst()], result.worst(), type_name, "boost::math::gamma_q", test_name);
#if defined(TEST_CEPHES) || defined(TEST_GSL)
   //
   // test other gamma_q(T, T) against data:
   //
   if(boost::is_floating_point<value_type>::value)
   {
      funcp = other::gamma_q;
      result = boost::math::tools::test(
         data,
         bind_func(funcp, 0, 1),
         extract_result(3));
      print_test_result(result, data[result.worst()], result.worst(), type_name, "other::gamma_q");
   }
#endif
   //
   // test gamma_p(T, T) against data:
   //
#if defined(BOOST_MATH_NO_DEDUCED_FUNCTION_POINTERS)
   funcp = boost::math::gamma_p<value_type, value_type>;
#else
   funcp = boost::math::gamma_p;
#endif
   result = boost::math::tools::test(
      data,
      bind_func(funcp, 0, 1),
      extract_result(5));
   handle_test_result(result, data[result.worst()], result.worst(), type_name, "boost::math::gamma_p", test_name);
#if defined(TEST_CEPHES) || defined(TEST_GSL)
   //
   // test other gamma_p(T, T) against data:
   //
   if(boost::is_floating_point<value_type>::value)
   {
      funcp = other::gamma_p;
      result = boost::math::tools::test(
         data,
         bind_func(funcp, 0, 1),
         extract_result(5));
      print_test_result(result, data[result.worst()], result.worst(), type_name, "other::gamma_p");
   }
#endif
   std::cout << std::endl;
}

template <class T>
void test_gamma(T, const char* name)
{
   //
   // The actual test data is rather verbose, so it's in a separate file
   //
   // First the data for the incomplete gamma function, each
   // row has the following 6 entries:
   // Parameter a, parameter z,
   // Expected tgamma(a, z), Expected gamma_q(a, z)
   // Expected tgamma_lower(a, z), Expected gamma_p(a, z)
   //
#  include "igamma_med_data.ipp"

   do_test_gamma_2(igamma_med_data, name, "tgamma(a, z) medium values");

#  include "igamma_small_data.ipp"

   do_test_gamma_2(igamma_small_data, name, "tgamma(a, z) small values");

#  include "igamma_big_data.ipp"

   do_test_gamma_2(igamma_big_data, name, "tgamma(a, z) large values");

#  include "igamma_int_data.ipp"

   do_test_gamma_2(igamma_int_data, name, "tgamma(a, z) integer and half integer values");
}

template <class T>
void test_spots(T)
{
   //
   // basic sanity checks, tolerance is 10 epsilon expressed as a percentage:
   //
   T tolerance = boost::math::tools::epsilon<T>() * 1000;
#if (defined(macintosh) || defined(__APPLE__) || defined(__APPLE_CC__))
   tolerance *= 10;
#endif
   BOOST_CHECK_CLOSE(::boost::math::tgamma(static_cast<T>(5), static_cast<T>(1)), static_cast<T>(23.912163676143750903709045060494956383977723517065L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::tgamma(static_cast<T>(5), static_cast<T>(5)), static_cast<T>(10.571838841565097874621959975919877646444998907920L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::tgamma(static_cast<T>(5), static_cast<T>(10)), static_cast<T>(0.70206451384706574414638719662835463671916532623256L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::tgamma(static_cast<T>(5), static_cast<T>(100)), static_cast<T>(3.8734332808745531496973774140085644548465762343719e-36L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::tgamma(static_cast<T>(0.5), static_cast<T>(0.5)), static_cast<T>(0.56241823159440712427949495730204306902676756479651L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::tgamma(static_cast<T>(0.5), static_cast<T>(9)/10), static_cast<T>(0.31853210360412109873859360390443790076576777747449L), tolerance*10);
   BOOST_CHECK_CLOSE(::boost::math::tgamma(static_cast<T>(0.5), static_cast<T>(5)), static_cast<T>(0.0027746032604128093194908357272603294120210079791437L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::tgamma(static_cast<T>(0.5), static_cast<T>(100)), static_cast<T>(3.7017478604082789202535664481339075721362102520338e-45L), tolerance);

   BOOST_CHECK_CLOSE(::boost::math::tgamma_lower(static_cast<T>(5), static_cast<T>(1)), static_cast<T>(0.087836323856249096290954939505043616022276482935091L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::tgamma_lower(static_cast<T>(5), static_cast<T>(5)), static_cast<T>(13.428161158434902125378040024080122353555001092080L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::tgamma_lower(static_cast<T>(5), static_cast<T>(10)), static_cast<T>(23.297935486152934255853612803371645363280834673767L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::tgamma_lower(static_cast<T>(5), static_cast<T>(100)), static_cast<T>(23.999999999999999999999999999999999996126566719125L), tolerance);

   BOOST_CHECK_CLOSE(::boost::math::gamma_q(static_cast<T>(5), static_cast<T>(1)), static_cast<T>(0.99634015317265628765454354418728984933240514654437L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::gamma_q(static_cast<T>(5), static_cast<T>(5)), static_cast<T>(0.44049328506521241144258166566332823526854162116334L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::gamma_q(static_cast<T>(5), static_cast<T>(10)), static_cast<T>(0.029252688076961072672766133192848109863298555259690L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::gamma_q(static_cast<T>(5), static_cast<T>(100)), static_cast<T>(1.6139305336977304790405739225035685228527400976549e-37L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::gamma_q(static_cast<T>(1.5), static_cast<T>(2)), static_cast<T>(0.26146412994911062220282207597592120190281060919079L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::gamma_q(static_cast<T>(20.5), static_cast<T>(22)), static_cast<T>(0.34575332043467326814971590879658406632570278929072L), tolerance);

   BOOST_CHECK_CLOSE(::boost::math::gamma_p(static_cast<T>(5), static_cast<T>(1)), static_cast<T>(0.0036598468273437123454564558127101506675948534556288L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::gamma_p(static_cast<T>(5), static_cast<T>(5)), static_cast<T>(0.55950671493478758855741833433667176473145837883666L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::gamma_p(static_cast<T>(5), static_cast<T>(10)), static_cast<T>(0.97074731192303892732723386680715189013670144474031L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::gamma_p(static_cast<T>(5), static_cast<T>(100)), static_cast<T>(0.9999999999999999999999999999999999998386069466302L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::gamma_p(static_cast<T>(1.5), static_cast<T>(2)), static_cast<T>(0.73853587005088937779717792402407879809718939080921L), tolerance);
   BOOST_CHECK_CLOSE(::boost::math::gamma_p(static_cast<T>(20.5), static_cast<T>(22)), static_cast<T>(0.65424667956532673185028409120341593367429721070928L), tolerance);

   // naive check on derivative function:
   using namespace std;  // For ADL of std functions
   tolerance = boost::math::tools::epsilon<T>() * 5000; // 50 eps
   BOOST_CHECK_CLOSE(::boost::math::gamma_p_derivative(static_cast<T>(20.5), static_cast<T>(22)),
      exp(static_cast<T>(-22)) * pow(static_cast<T>(22), static_cast<T>(19.5)) / boost::math::tgamma(static_cast<T>(20.5)), tolerance);

}

int test_main(int, char* [])
{
   expected_results();
   BOOST_MATH_CONTROL_FP;

#ifndef BOOST_MATH_BUGGY_LARGE_FLOAT_CONSTANTS
   test_spots(0.0F);
#endif
   test_spots(0.0);
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   test_spots(0.0L);
#if !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x582))
   test_spots(boost::math::concepts::real_concept(0.1));
#endif
#endif

#ifndef BOOST_MATH_BUGGY_LARGE_FLOAT_CONSTANTS
   test_gamma(0.1F, "float");
#endif
   test_gamma(0.1, "double");
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   test_gamma(0.1L, "long double");
#ifndef BOOST_MATH_NO_REAL_CONCEPT_TESTS
#if !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x582))
   test_gamma(boost::math::concepts::real_concept(0.1), "real_concept");
#endif
#endif
#else
   std::cout << "<note>The long double tests have been disabled on this platform "
      "either because the long double overloads of the usual math functions are "
      "not available at all, or because they are too inaccurate for these tests "
      "to pass.</note>" << std::cout;
#endif
   return 0;
}


#ifndef LEPP3_OBSTACLE_EVALUATOR_H_
#define LEPP3_OBSTACLE_EVALUATOR_H_
#include <boost/filesystem.hpp>
#include <iostream>
#include <sstream>
#include "lepp3/FrameData.hpp"
#include "lepp3/util/util.h"
/**
 * A class that computes the volume of a given model.
 * It is a `ModelVisitor` implementation and it assumes one model as input,
 * regardless of how many sub-parts the approximation has, a.k.a the
 * `CompositeModel`.
 *
 * The volume computation is done by creating a 3D grid around the model and
 * estimating how many points on the grid are occupied.
 */
class VolumeEstimator : public ModelVisitor {
public:
  VolumeEstimator()
      : num_splits_(0) {
    min_p_.x = std::numeric_limits<int>::max();
    min_p_.y = std::numeric_limits<int>::max();
    min_p_.z = std::numeric_limits<int>::max();
    max_p_.x = std::numeric_limits<int>::min();
    max_p_.y = std::numeric_limits<int>::min();
    max_p_.z = std::numeric_limits<int>::min();
  }
  /**
   * Implementation of the `ModelVisitor` interface. It will draw the given
   * sphere onto the `PCLVisualizer` to which it holds a reference.
   */
  void visitSphere(lepp::SphereModel& sphere);
  void visitCapsule(lepp::CapsuleModel& capsule);
  int getSplitCount() { return num_splits_; }
  /**
   * Estimates the volume of the approximated model. This is achieved by
   * creating a bounding box around the min/max points of the approximation and
   * subdivide the region into small grids.
   */
  int estimateVolume();
private:
  /**
   * Checks if the current point on the 3D grid (a.k.a the bounding box around
   * the model) is inside the model.
   */
  bool isVoxelOccupied(double const& x, double const& y, double const& z);
  /**
   * Minimum/Maximum points of the bounding box surrounding the model.
   */
  Coordinate min_p_, max_p_;
  /**
   * Container to hold track of sphere models in a `CompositeModel`
   */
  std::vector<SphereModel> spheres_;
  /**
   * Container to hold track of capsule models in a `CompositeModel`
   */
  std::vector<CapsuleModel> capsules_;
  /**
   * Number of sub-models in this model. This determines how many split operations
   * have been executed
   */
  int num_splits_;
};
void VolumeEstimator::visitSphere(lepp::SphereModel& sphere) {
  ++num_splits_;
  // Store the current Sphere model.
  spheres_.push_back(sphere);
  Coordinate const center = sphere.center_point();
  double const max_x = center.x + sphere.radius();
  double const max_y = center.y + sphere.radius();
  double const max_z = center.z + sphere.radius();
  double const min_x = center.x - sphere.radius();
  double const min_y = center.y - sphere.radius();
  double const min_z = center.z - sphere.radius();
  max_p_.x = std::max(max_p_.x, max_x);
  max_p_.y = std::max(max_p_.y, max_y);
  max_p_.z = std::max(max_p_.z, max_z);
  min_p_.x = std::min(min_p_.x, min_x);
  min_p_.y = std::min(min_p_.y, min_y);
  min_p_.z = std::min(min_p_.z, min_z);
  double volume = 4/3*M_PI*sphere.radius()*sphere.radius()*sphere.radius();
  //std::cout << volume << std::endl;
}
void VolumeEstimator::visitCapsule(lepp::CapsuleModel& capsule) {
  ++num_splits_;
  // Store the current Capsule model.
  capsules_.push_back(capsule);
  Coordinate first = capsule.first();
  Coordinate second = capsule.second();
  double a = std::sqrt((first.x - second.x)*(first.x - second.x) + (first.y - second.y)*(first.y - second.y) + (first.z - second.z)*(first.z - second.z));
  double volume = M_PI*capsule.radius()*capsule.radius()*(4/3*capsule.radius() + a);
  //std::cout << volume << std::endl;
  Coordinate min, max;
  min.x = std::min(first.x, second.x);
  min.y = std::min(first.y, second.y);
  min.z = std::min(first.z, second.z);
  max.x = std::max(first.x, second.x);
  max.y = std::max(first.y, second.y);
  max.z = std::max(first.z, second.z);
  // Add the capsule radius to min/max
  min.x -= capsule.radius();
  min.y -= capsule.radius();
  min.z -= capsule.radius();
  max.x += capsule.radius();
  max.y += capsule.radius();
  max.z += capsule.radius();
  // Find the global min/max
  min_p_.x = std::min(min_p_.x, min.x);
  min_p_.y = std::min(min_p_.y, min.y);
  min_p_.z = std::min(min_p_.z, min.z);
  max_p_.x = std::max(max_p_.x, max.x);
  max_p_.y = std::max(max_p_.y, max.y);
  max_p_.z = std::max(max_p_.z, max.z);
}
bool VolumeEstimator::isVoxelOccupied(
    double const& x, double const& y, double const& z) {
  // Go through all spheres of the current model and check if the point is
  // inside one of them.
  {
    size_t sz = spheres_.size();
    for (size_t i=0; i<sz; ++i) {
      double const r = spheres_[i].radius();
      Coordinate const& center = spheres_[i].center_point();
      double const distance =
          (center.x - x) * (center.x - x) + (center.y - y) * (center.y - y) + (center.z - z) * (center.z - z);
      if (distance <= r * r)
        return true;
    }
  }
  // Go through all capsules of the current model and check if the point is
  // inside one of them.
  {
    size_t sz = capsules_.size();
    for (size_t i=0; i<sz; ++i) {
      double r = capsules_[i].radius();
      /*
       * Prepare the pcl::sqrPointToLineDistance input arguments.
       */
      // line_dir: line direction vector
      Coordinate l = capsules_[i].second() - capsules_[i].first();
      Eigen::Vector4f line_dir(l.x, l.y, l.z, 0.);
      // line_pt: a point on the line
      Eigen::Vector4f line_pt(
          capsules_[i].second().x,
          capsules_[i].second().y,
          capsules_[i].second().z,
          0.);
      Eigen::Vector4f pt(x, y, z, 0);
      double const distance = pcl::sqrPointToLineDistance(pt, line_pt, line_dir);
      if (distance < r*r)
        return true;
    }
  }
  return false;
}
int VolumeEstimator::estimateVolume() {
  // NOTE: All the values are in METERS
  // Create a 3D grid and check the distance of each point on grid to the object
  // models.
  // TODO: 3D grid creation should depend on the point cloud resolution
  double const step_size = 0.01;
  int volume = 0;
  for (double x = min_p_.x; x < max_p_.x; x += step_size) {
    for (double y = min_p_.y; y < max_p_.y; y += step_size) {
      for (double z = min_p_.z; z < max_p_.z; z += step_size) {
        if (isVoxelOccupied(x, y, z))
          ++volume;
      }
    }
  }
  return volume;
}
/**
 *
 */
class ObstacleEvaluator : public FrameDataObserver {
public:
  ObstacleEvaluator();
  ObstacleEvaluator(int vol);
  /**
   * ObstacleAggregator interface implementation: processes the current models.
   */
  void updateFrame(FrameDataPtr frameData);
private:
  void init();
  bool evaluate(ObjectModelPtr const& model, double x, double y, double z);
  std::string file_path_;
  int ref_volume_;
};
ObstacleEvaluator::ObstacleEvaluator()
    : ref_volume_(0) {
  init();
}
ObstacleEvaluator::ObstacleEvaluator(int vol)
    : ref_volume_(vol) {
  init();
}
void ObstacleEvaluator::init() {
  namespace bfs = boost::filesystem;
  std::stringstream ss;
  // Create the evaluation directory
  ss << "../evaluation/";
  std::string dir = ss.str();
  if ( !bfs::exists(bfs::path(dir)) )
    bfs::create_directory(bfs::path(dir));
  // Create the sub directory based on current timestamp
  ss << lepp::get_current_timestamp();
  dir = ss.str();
  bfs::create_directory(bfs::path(dir));
  // Prepare the csv file path
  ss  << "/eval.csv";
  file_path_ = ss.str();
  std::cout << "file_path: " << file_path_ << std::endl;
  // Create the file header
  std::ofstream tf_fout;
  tf_fout.open(file_path_.c_str(), std::ofstream::app);
  tf_fout << "model_id,"
          << "volume,"
          << "sim_veloc_x,"
          << "sim_veloc_y,"
          << "sim_veloc_z,"
          << std::endl;
  tf_fout.close();
}
bool ObstacleEvaluator::evaluate(ObjectModelPtr const& model, double x, double y, double z) {
  // TODO incorporate try-catch scheme
  VolumeEstimator estimator;
  // Find the minimum and maximum point of the approximated model
  model->accept(estimator);
  // Compute the volume of the current model
  int vol = estimator.estimateVolume();
  //std::cout << "Volume Tester " << vol << std::endl;
  // Evaluate the approximation based on the reference information
  float ratio = -1;
  if (ref_volume_ != 0)
    ratio = vol / static_cast<float>(ref_volume_);

  bool has_vel = false;
  boost::shared_ptr<CompositeModel> cm = boost::static_pointer_cast<CompositeModel>(model);
  for( auto& m : cm->models() )
  {
    boost::shared_ptr<CompositeModel> cmm = boost::static_pointer_cast<CompositeModel>(m);

    for ( auto& mm : cmm->models() )
    {
      std::cout << model->id() << ": " << mm->velocity().x << ", "
                << mm->velocity().y << ", " << mm->velocity().z << std::endl;
      if (!std::isnan(mm->velocity().x) &&
          !std::isnan(mm->velocity().y) &&
          !std::isnan(mm->velocity().z))
      {

        // Save the current approximation information
        std::stringstream ss;
        ss << model->id() << ","
           << vol << ","
           << mm->velocity().x << ","
           << mm->velocity().y << ","
           << mm->velocity().z
           << std::endl;
        std::ofstream tf_fout;
        // open the file and add the current model evaluation to the end of it
        tf_fout.open(file_path_.c_str(), std::ofstream::app);
        tf_fout << ss.str();
        tf_fout.close();
        has_vel = true;
        break;
      }
    }
  }

  // if obstacle did not have a velocity estimate, log zero
  if (!has_vel)
  {
      std::ofstream tf_out;
      tf_out.open(file_path_.c_str(), std::ofstream::app);
      tf_out << model->id() << ","
             << vol << ","
             << "0,0,0"
             << std::endl;
  }

  return true;
}
void ObstacleEvaluator::updateFrame(FrameDataPtr frameData) {
  size_t sz = frameData->obstacles.size();
  for (size_t i=0; i<sz; ++i) {
    //evaluate(frameData->obstacles[i]);
    double x = frameData->obstacles[i]->velocity().x;
    double y = frameData->obstacles[i]->velocity().y;
    double z = frameData->obstacles[i]->velocity().z;
    evaluate(frameData->obstacles[i], x, y, z);
  }
}
#endif // LEPP3_OBSTACLE_EVALUATOR_H_


/* ----------------------------------------------------------------------------
 * Copyright 2017, Massachusetts Institute of Technology,
 * Cambridge, MA 02139
 * All Rights Reserved
 * Authors: Luca Carlone, et al. (see THANKS for the full author list)
 * See LICENSE for the license information
 * -------------------------------------------------------------------------- */

/**
 * @file   testParallelPlaneRegularBasicFactor.cpp
 * @brief  test ParallelPlaneRegularBasicFactor
 * @author Antoni Rosinol Vidal
 */

#include <algorithm>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <random>

#include <gtsam/base/numericalDerivative.h>
#include <boost/assign/std/vector.hpp>
#include <boost/bind.hpp>

#include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/PriorFactor.h>

#include <gflags/gflags.h>
#include <glog/logging.h>
#include <gtest/gtest.h>

#include "kimera-vio/factors/ParallelPlaneRegularFactor.h"
#include "kimera-vio/factors/PointPlaneFactor.h"

using namespace std;
using namespace gtsam;

static const double tol = 1e-5;
static const double der_tol = 1e-5;

/**
 * Test that error does give the right result when it is zero.
 */
TEST(testParallelPlaneRegularBasicFactor, ErrorIsZero) {
  /// Plane keys.
  Key plane_key_1(1);
  Key plane_key_2(2);

  /// Noise model for cosntraint between the two planes.
  noiseModel::Diagonal::shared_ptr parallel_plane_noise =
      noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));

  /// Parallelism constraint between Plane 1 and Plane 2.
  ParallelPlaneRegularBasicFactor factor(plane_key_1, plane_key_2,
                                         parallel_plane_noise);

  /// Planes.
  OrientedPlane3 plane_1(0.1, 0.1, 0.9, 0.9);
  OrientedPlane3 plane_2(0.1, 0.1, 0.9, 0.1);

  /// Calculate error.
  Vector error = factor.evaluateError(plane_1, plane_2);

  /// Expected error.
  Vector3 expected_error = Vector3::Constant(0.0);

  ASSERT_TRUE(assert_equal(expected_error, error, tol));
}

/**
 * Test that error does give the right result when it is not zero.
 */
TEST(testParallelPlaneRegularBasicFactor, ErrorOtherThanZero) {
  /// Plane keys.
  Key plane_key_1(1);
  Key plane_key_2(2);

  /// Noise model for cosntraint between the two planes.
  noiseModel::Diagonal::shared_ptr parallel_plane_noise =
      noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));

  /// Parallelism constraint between Plane 1 and Plane 2.
  ParallelPlaneRegularBasicFactor factor(plane_key_1, plane_key_2,
                                         parallel_plane_noise);

  /// Planes.
  OrientedPlane3 plane_1(0.3, 0.2, 1.9, 0.9);
  OrientedPlane3 plane_2(0.1, 0.1, 0.9, 0.1);

  /// Calculate error.
  Vector error = factor.evaluateError(plane_1, plane_2);

  /// Expected error.
  Vector3 expected_error;
  expected_error << 0.045362, -0.00634672, -0.00541173;

  ASSERT_TRUE(assert_equal(expected_error, error, tol));
}

/**
 * Test that analytical jacobians equal numerical ones.
 *
 */
TEST(testParallelPlaneRegularFactor, Jacobians) {
  /// Plane keys.
  Key plane_key_1(1);
  Key plane_key_2(2);

  /// Noise model for cosntraint between the two planes.
  noiseModel::Diagonal::shared_ptr parallel_plane_noise =
      noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));

  /// Parallelism constraint between Plane 1 and Plane 2.
  ParallelPlaneRegularBasicFactor factor(plane_key_1, plane_key_2,
                                         parallel_plane_noise);

  /// Planes.
  OrientedPlane3 plane_1(0.3, 0.2, 1.9, 0.9);
  OrientedPlane3 plane_2(0.1, 0.1, 0.9, 0.1);

  // Use the factor to calculate the Jacobians
  gtsam::Matrix H1Actual, H2Actual;
  factor.evaluateError(plane_1, plane_2, H1Actual, H2Actual);

  // Calculate numerical derivatives
  Matrix H1Expected =
      numericalDerivative21<Vector, OrientedPlane3, OrientedPlane3>(
          boost::bind(&ParallelPlaneRegularBasicFactor::evaluateError, &factor,
                      _1, _2, boost::none, boost::none),
          plane_1, plane_2, der_tol);

  Matrix H2Expected =
      numericalDerivative22<Vector, OrientedPlane3, OrientedPlane3>(
          boost::bind(&ParallelPlaneRegularBasicFactor::evaluateError, &factor,
                      _1, _2, boost::none, boost::none),
          plane_1, plane_2, der_tol);

  // Verify the Jacobians are correct
  ASSERT_TRUE(assert_equal(H1Expected, H1Actual, tol));
  ASSERT_TRUE(assert_equal(H2Expected, H2Actual, tol));
}

/* ************************************************************************* */
TEST(testParallelPlaneRegularBasicFactor, PlanePrior) {
  /// Three landmarks, with prior factors, and a plane constrained together
  /// using the landmark-plane factor.
  NonlinearFactorGraph graph;

  /// Keys
  Key plane_key_1 = 1;

  /// Shared noise for all landmarks.
  noiseModel::Diagonal::shared_ptr prior_noise =
      noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));

  OrientedPlane3 priorMeanPlane(0.0, 0.0, 1.0, 0.0);
  graph.emplace_shared<PriorFactor<OrientedPlane3> >(
      plane_key_1, priorMeanPlane, prior_noise);

  // graph.print("\nFactor Graph:\n");

  Values initial;
  initial.insert(plane_key_1, OrientedPlane3(0.1, 0.2, 0.9, 0.8));

  GaussNewtonParams params;
  params.setVerbosity("ERROR");
  params.setMaxIterations(20);
  params.setRelativeErrorTol(-std::numeric_limits<double>::max());
  // params.setErrorTol(-std::numeric_limits<double>::max());
  params.setAbsoluteErrorTol(-std::numeric_limits<double>::max());

  Values result = GaussNewtonOptimizer(graph, initial, params).optimize();
  // Values result = LevenbergMarquardtOptimizer(graph, initial,
  // params).optimize();

  Values expected;
  expected.insert(plane_key_1, OrientedPlane3(0.0, 0.0, 1.0, 0.0));

  ASSERT_TRUE(assert_equal(expected, result, tol));
}

/**
 * Test that optimization works.
 * A plane and a landmark with prior factors, and a second plane constrained
 * together with the first plane using the ParallelPlaneRegularBasic factor.
 *
 *              Prior                      +-------+    +-+
 *               +-+                       | Lmk 1 +----+ | Prior
 *               +-+        Parallelism    +---+---+    +-+
 *                |           factor           |
 *            +---+---+        +-+         +---+---+
 *            |Plane 1+--------+ +---------+Plane 2|
 *            +-------+        +-+         +-------+
 *
 */
TEST(testParallelPlaneRegularBasicFactor, PlaneOptimization) {
  NonlinearFactorGraph graph;

  /// Keys
  Key landmark_key = 1;
  Key plane_key_1 = 2;
  Key plane_key_2 = 3;

  /// Shared noise for all landmarks.
  noiseModel::Diagonal::shared_ptr prior_noise =
      noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));

  Point3 priorMeanLandmark1(0.0, 0.0, 0.0);
  graph.emplace_shared<PriorFactor<Point3> >(landmark_key, priorMeanLandmark1,
                                             prior_noise);

  OrientedPlane3 priorMeanPlane1(0.0, 0.0, 1.0, 1.0);
  graph.emplace_shared<PriorFactor<OrientedPlane3> >(
      plane_key_1, priorMeanPlane1, prior_noise);

  /// Shared noise for all constraints between landmarks and planes.
  noiseModel::Isotropic::shared_ptr regularity_noise =
      noiseModel::Isotropic::Sigma(1, 0.5);

  /// Plane 2 to landmark.
  graph.emplace_shared<PointPlaneFactor>(landmark_key, plane_key_2,
                                         regularity_noise);

  /// Noise model for cosntraint between the two planes.
  noiseModel::Diagonal::shared_ptr parallel_plane_noise =
      noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));

  /// Parallelism constraint between Plane 1 and Plane 2.
  graph.emplace_shared<ParallelPlaneRegularBasicFactor>(
      plane_key_1, plane_key_2, parallel_plane_noise);

  // graph.print("\nFactor Graph:\n");

  Values initial;
  initial.insert(landmark_key, Point3(0.0, 0.2, 0.1));
  initial.insert(plane_key_1, OrientedPlane3(0.1, 0.1, 0.9, 0.9));
  initial.insert(plane_key_2, OrientedPlane3(0.1, 0.1, 0.8, 0.1));

  // GaussianFactorGraph gfg = *graph.linearize(initial);
  // gfg.print("\nFactor Graph:\n");

  GaussNewtonParams params;
  // params.setVerbosity("LINEAR");
  params.setMaxIterations(20);
  params.setRelativeErrorTol(-std::numeric_limits<double>::max());
  // params.setErrorTol(-std::numeric_limits<double>::max());
  params.setAbsoluteErrorTol(-std::numeric_limits<double>::max());

  Values result = GaussNewtonOptimizer(graph, initial, params).optimize();
  // Values result = LevenbergMarquardtOptimizer(graph, initial,
  // params).optimize();

  Values expected;
  expected.insert(landmark_key, priorMeanLandmark1);
  expected.insert(plane_key_1, priorMeanPlane1);
  expected.insert(plane_key_2, OrientedPlane3(0.0, 0.0, 1.0, 0.0));

  ASSERT_TRUE(assert_equal(expected, result, tol));
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2013   MetaScale SAS
//
//          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
//==============================================================================
#ifndef BOOST_SIMD_META_NEXT_POWER_OF_2_HPP_INCLUDED
#define BOOST_SIMD_META_NEXT_POWER_OF_2_HPP_INCLUDED

/*!
  @file
  @brief Defines and implements next_power_of_2 and next_power_of_2_c
**/

#include <cstddef>
#include <boost/mpl/size_t.hpp>
#include <boost/mpl/integral_c.hpp>

namespace boost { namespace simd {  namespace details
{
  // Implementation courtesy from :
  // http://graphics.stanford.edu/~seander/bithacks.html#RoundUpPowerOf2
  template<std::size_t N> struct next_power_of_2_impl
  {
    BOOST_STATIC_CONSTANT(std::size_t, x0    = N-1             );
    BOOST_STATIC_CONSTANT(std::size_t, x1    = x0 | (x0 >>  1) );
    BOOST_STATIC_CONSTANT(std::size_t, x2    = x1 | (x1 >>  2) );
    BOOST_STATIC_CONSTANT(std::size_t, x3    = x2 | (x2 >>  4) );
    BOOST_STATIC_CONSTANT(std::size_t, x4    = x3 | (x3 >>  8) );
    BOOST_STATIC_CONSTANT(std::size_t, x5    = x4 | (x4 >> 16) );
    BOOST_STATIC_CONSTANT(std::size_t, value = x5 + 1          );
  };

  // Required for MSVC
  template<> struct next_power_of_2_impl<0>
  {
    BOOST_STATIC_CONSTANT(std::size_t, value = 0 );
  };
} } }

namespace boost { namespace simd {  namespace meta
{
  /*!
    @brief Evaluates next power of 2

    Computes the power of two greater or equal to any given integral value @c N.

    @par Semantic:
    For any given integral value @c N:

    @code
    typedef next_power_of_2_c<N>::type r;
    @endcode

    is equivalent to:

    @code
    typedef mpl::size_t<M> r;
    @endcode

    Where @c M is greater or equal to N and so that it exists a given @c P so
    that @c M is equal to 2 at the power of @c P.

    @usage{meta/next_power_of_2_c.cpp}

    @tparam N Integral value to upgrade
  **/
  template<std::size_t N> struct  next_power_of_2_c
#if !defined(DOXYGEN_ONLY)
        : boost::mpl::size_t<details::next_power_of_2_impl<N>::value>
#endif
  {};

  /*!
    @brief Evaluates next power of 2

    Computes the power of two greater or equal to any given @mplint @c N.

    @par Semantic:
    For any given @mplint @c N:

    @code
    typedef next_power_of_2<N>::type r;
    @endcode

    is equivalent to:

    @code
    typedef boost::mpl::integral_c< N::value_type
                                  , next_power_of_2_c<N::value>::value
                                  > r;
    @endcode

    @par Models:

    @metafunction

    @usage{meta/next_power_of_2.cpp}

    @tparam N @mplint to downgrade
  **/
  template<class N> struct  next_power_of_2
#if !defined(DOXYGEN_ONLY)
        : boost::mpl::integral_c< typename N::value_type
                                , details::next_power_of_2_impl<N::value>::value
                                >
#endif
  {};
} } }

#endif


/* ----------------------------------------------------------------------------
 * Copyright 2018, Ross Hartley <m.ross.hartley@gmail.com>
 * All Rights Reserved
 * See LICENSE for the license information
 * -------------------------------------------------------------------------- */

/**
 *  @file   left_vs_right_error_dynamics.cpp
 *  @author Ross Hartley
 *  @brief  Test to make sure the left and right error dynamics are identical
 *  @date   February 18, 2019
 **/

#include <iostream>
#include <fstream>
#include <string>
#include <cstdlib>
#include <Eigen/Dense>
#include <boost/algorithm/string.hpp>
#include <vector>
#include <random>
#include <chrono>
#include "inekf/inekf.hpp"

using namespace std;
using namespace inekf;

int main() {
    typedef std::chrono::high_resolution_clock myclock;
    myclock::time_point beginning = myclock::now();

    //  ---- Initialize invariant extended Kalman filter ----- //
    RobotState initial_state;

    // Initialize state mean
    Eigen::Matrix3d R0;
    Eigen::Vector3d v0, p0, bg0, ba0;
    R0 << 1, 0, 0, // initial orientation
          0, -1, 0, // IMU frame is rotated 90deg about the x-axis
          0, 0, -1;
    v0 << 1,2,3; // initial velocity
    p0 << 4,5,6; // initial position
    bg0 << 0,0,0; // initial gyroscope bias
    ba0 << 0,0,0; // initial accelerometer bias
    initial_state.setRotation(R0);
    initial_state.setVelocity(v0);
    initial_state.setPosition(p0);
    initial_state.setGyroscopeBias(bg0);
    initial_state.setAccelerometerBias(ba0);

    // Initialize noise params
    NoiseParams noise_params;
    noise_params.setGyroscopeNoise(0.0);
    noise_params.setAccelerometerNoise(0.0);
    noise_params.setGyroscopeBiasNoise(0.0);
    noise_params.setAccelerometerBiasNoise(0.0);
    noise_params.setContactNoise(0.0);

    // Initial Covariance and Adjoint
    Eigen::Matrix<double,15,15> P = Eigen::Matrix<double,15,15>::Identity();
    Eigen::MatrixXd Adj = Eigen::MatrixXd::Identity(initial_state.dimP(),initial_state.dimP());
    Adj.block(0,0,initial_state.dimP()-initial_state.dimTheta(),initial_state.dimP()-initial_state.dimTheta()) = Adjoint_SEK3(initial_state.getX());

    // Left invariant filter
    initial_state.setP(P);
    InEKF LI_filter(initial_state, noise_params, ErrorType::LeftInvariant);

    // Right invariant filter
    initial_state.setP(Adj*P*Adj.transpose());
    InEKF RI_filter(initial_state, noise_params, ErrorType::RightInvariant);

    // obtain a seed from the timer
    myclock::duration d = myclock::now() - beginning;
    unsigned seed = d.count();
    std::default_random_engine generator(seed);
    std::normal_distribution<double> distribution(0,1);

    // ----- Propagate using random data ------
    cout << "\n\n ------ Propagate using random data -------\n\n";
    const int NUM_PROPAGATE = 100;
    Eigen::Matrix<double,6,1> imu;
    for (int i=0; i<NUM_PROPAGATE; ++i) {
        for (int j=0; j<6; ++j) {
            imu(j) = distribution(generator);
        }
        double dt = distribution(generator);
        LI_filter.Propagate(imu, dt);
        RI_filter.Propagate(imu, dt);
    }

    // Print covariances
    RobotState LI_state = LI_filter.getState();
    RobotState RI_state = RI_filter.getState();
    cout << "Left Invariant State: \n" << LI_state << endl;
    cout << "Right Invariant State: \n" << RI_state << endl;
    cout << "Left Invariant Covariance: \n" << LI_state.getP() << endl << endl;
    cout << "Right Invariant Covariance: \n" << RI_state.getP() << endl << endl;
    Adj = Eigen::MatrixXd::Identity(LI_state.dimP(),LI_state.dimP());
    Adj.block(0,0,LI_state.dimP()-LI_state.dimTheta(),LI_state.dimP()-LI_state.dimTheta()) = Adjoint_SEK3(LI_state.getX());
    cout << "Difference between right invariant covariance (left is mapped using adjoint): \n" << (RI_state.getP() - (Adj * LI_state.getP() * Adj.transpose()).eval()).norm() << endl << endl;
    Eigen::MatrixXd AdjInv = Eigen::MatrixXd::Identity(RI_state.dimP(),RI_state.dimP());
    AdjInv.block(0,0,RI_state.dimP()-RI_state.dimTheta(),RI_state.dimP()-RI_state.dimTheta()) = Adjoint_SEK3(RI_state.calcXinv());
    cout << "Difference between left invariant covariance (right is mapped using adjoint inverse): \n" << (LI_state.getP() - (AdjInv * RI_state.getP() * AdjInv.transpose()).eval()).norm() << endl << endl;
    cout << "Difference between state estimates: \n" << (LI_state.getX() - RI_state.getX()).norm() << endl << endl;

    // ----- Correct using random data ------
    cout << "\n\n ------ Correct using random data -------\n\n";
    // Set filter's contact state
    vector<pair<int,bool> > contacts;
    contacts.push_back(pair<int,bool> (0, true));
    contacts.push_back(pair<int,bool> (1, true));
    LI_filter.setContacts(contacts);
    RI_filter.setContacts(contacts);

    // Correct state using kinematic measurements
    const int NUM_CORRECT = 10;
    for (int i=0; i<NUM_CORRECT; ++i) {
        vectorKinematics measured_kinematics;
        Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
        Eigen::Vector3d p = Eigen::Vector3d::Zero();
        Eigen::Matrix<double,6,6> covariance = Eigen::Matrix<double,6,6>::Identity();
        p <<  distribution(generator), distribution(generator), distribution(generator);
        pose.block<3,1>(0,3) = p;
        measured_kinematics.push_back(Kinematics(0, pose, covariance));
        p <<  distribution(generator), distribution(generator), distribution(generator);
        pose.block<3,1>(0,3) = p;
        measured_kinematics.push_back(Kinematics(1, pose, covariance));
        LI_filter.CorrectKinematics(measured_kinematics);
        RI_filter.CorrectKinematics(measured_kinematics);
    }

    // Print covariances
    LI_state = LI_filter.getState();
    RI_state = RI_filter.getState();
    cout << "Left Invariant State: \n" << LI_state << endl;
    cout << "Right Invariant State: \n" << RI_state << endl;
    cout << "Left Invariant Covariance: \n" << LI_state.getP() << endl << endl;
    cout << "Right Invariant Covariance: \n" << RI_state.getP() << endl << endl;
    Adj = Eigen::MatrixXd::Identity(LI_state.dimP(),LI_state.dimP());
    Adj.block(0,0,LI_state.dimP()-LI_state.dimTheta(),LI_state.dimP()-LI_state.dimTheta()) = Adjoint_SEK3(LI_state.getX());
    cout << "Difference between right invariant covariance (left is mapped using adjoint): \n" << (RI_state.getP() - (Adj * LI_state.getP() * Adj.transpose()).eval()).norm() << endl << endl;
    AdjInv = Eigen::MatrixXd::Identity(RI_state.dimP(),RI_state.dimP());
    AdjInv.block(0,0,RI_state.dimP()-RI_state.dimTheta(),RI_state.dimP()-RI_state.dimTheta()) = Adjoint_SEK3(RI_state.calcXinv());
    cout << "Difference between left invariant covariance (right is mapped using adjoint inverse): \n" << (LI_state.getP() - (AdjInv * RI_state.getP() * AdjInv.transpose()).eval()).norm() << endl << endl;
    cout << "Difference between state estimates: \n" << (LI_state.getX() - RI_state.getX()).norm() << endl << endl;

    return 0;
}


//
// Copyright (c) 2016 - 2017 Mesh Consultants Inc.
// 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 "Geomlib_TriMeshEdgeCollapse.h"

#include <vector>

#pragma warning(push, 0)
#include <igl/remove_unreferenced.h>
#include <igl/edge_flaps.h>
#include <igl/collapse_edge.h>
#include <igl/is_edge_manifold.h>
#include <igl/is_vertex_manifold.h>
#include <igl/is_boundary_edge.h>
#include <igl/is_border_vertex.h>
#pragma warning(pop)

#include <Eigen/Core>

#include "TriMesh.h"
#include "ConversionUtilities.h"

using Urho3D::Variant;

namespace {

void collapse_edges_below_length(
	const Eigen::MatrixXd& OV,
	const Eigen::MatrixXi& OF,
	Eigen::MatrixXd& NV,
	Eigen::MatrixXi& NF,
	double L
)
{
	Eigen::MatrixXd V = OV;
	Eigen::MatrixXi F = OF;

	// Prepare array-based edge data structures
	Eigen::VectorXi EMAP;
	Eigen::MatrixXi E, EF, EI;
	igl::edge_flaps(F, E, EMAP, EF, EI);

	Eigen::VectorXi B;
	std::vector<bool> border_vertices = igl::is_border_vertex(V, F);
	assert(border_vertices.size() == V.rows());

	Eigen::RowVector3d mid;
	double len;
	bool b;

	int numc = -1;
	while (numc != 0) {
		numc = 0;
		for (int e = 0; e < E.rows(); ++e) {

			bool edge_totally_inside = false;
			if (
				border_vertices[E(e, 0)] == false &&
				border_vertices[E(e, 1)] == false
				)
			{
				edge_totally_inside = true;
			}

			len = (V.row(E(e, 0)) - V.row(E(e, 1))).norm();
			if (len < L && edge_totally_inside) {
				mid = 0.5 * (V.row(E(e, 0)) + V.row(E(e, 1)));
				b = igl::collapse_edge(e, mid, V, F, E, EMAP, EF, EI);
				if (b) {
					++numc;
				}
			}
		}
	}

	Eigen::VectorXi _1;
	igl::remove_unreferenced(V, F, NV, NF, _1);

	std::vector<int> facePatch;
	for (int i = 0; i < NF.rows(); ++i) {
		if ((NF(i, 0) == 0) && (NF(i, 1) == 0) && (NF(i, 2) == 0)) {
		}
		else {
			facePatch.push_back(NF(i, 0));
			facePatch.push_back(NF(i, 1));
			facePatch.push_back(NF(i, 2));
		}
	}
	Eigen::MatrixXi NF2(facePatch.size() / 3, 3);
	for (int i = 0; i < facePatch.size() / 3; ++i) {
		NF2(i, 0) = facePatch[3 * i];
		NF2(i, 1) = facePatch[3 * i + 1];
		NF2(i, 2) = facePatch[3 * i + 2];
	}
	NF.setZero(NF2.rows(), 3);
	NF = NF2;
}

} // namespace

Urho3D::Variant Geomlib::TriMesh_CollapseShortEdges(
	const Urho3D::Variant& tri_mesh,
	float collapse_threshold
)
{
	if (!TriMesh_Verify(tri_mesh)) {
		return Variant();
	}

	Eigen::MatrixXf V;
	Eigen::MatrixXi F;
	IglMeshToMatrices(tri_mesh, V, F);

	if (!igl::is_edge_manifold(F)) {
		return Variant();
	}

	Eigen::VectorXi B;
	if (!igl::is_vertex_manifold(F, B)) {
		return Variant();
	}

	// Convert from float to double
	Eigen::MatrixXd V_d = IglFloatToDouble(V);

	Eigen::MatrixXd NV_d;
	Eigen::MatrixXi NF;
	collapse_edges_below_length(V_d, F, NV_d, NF, collapse_threshold);

	Eigen::MatrixXf NV = IglDoubleToFloat(NV_d);

	return TriMesh_Make(NV, NF);
}


/*
LICENSE: see isogeometric_application/LICENSE.txt
*/

//
//   Project Name:        Kratos
//   Last modified by:    $Author: hbui $
//   Date:                $Date: Nov 24, 2017 $
//   Revision:            $Revision: 1.0 $
//
//

// System includes
#include <string>

// External includes
#include <boost/foreach.hpp>
#include <boost/python.hpp>
#include <boost/python/stl_iterator.hpp>
#include <boost/python/operators.hpp>

// Project includes
#include "includes/define.h"
#include "python/pointer_vector_set_python_interface.h"
#include "custom_utilities/patch.h"
#include "custom_utilities/multipatch.h"
#include "custom_utilities/tsplines/tcell.h"
#include "custom_utilities/nurbs/pbbsplines_basis_function.h"
#include "custom_utilities/nurbs/pbbsplines_fespace.h"
#include "custom_utilities/import_export/multi_pbbsplines_patch_matlab_exporter.h"
#include "custom_python/iga_define_python.h"
#include "custom_python/add_pbbsplines_to_python.h"
#include "custom_python/add_point_based_control_grid_to_python.h"
#include "custom_python/add_import_export_to_python.h"

namespace Kratos
{

namespace Python
{

using namespace boost::python;

template<int TDim>
void IsogeometricApplication_AddPBBSplinesSpaceToPython()
{

    std::stringstream ss;

    ss.str(std::string());
    ss << "PBBSplinesBasisFunction" << TDim << "D";
    typedef PBBSplinesBasisFunction<TDim, TCell> PBBSplinesBasisFunctionType;
    class_<PBBSplinesBasisFunctionType, typename PBBSplinesBasisFunctionType::Pointer, boost::noncopyable>
    (ss.str().c_str(), init<const std::size_t&>())
    .add_property("Id", Isogeometric_GetId<PBBSplinesBasisFunctionType>, Isogeometric_DoNotSetId<PBBSplinesBasisFunctionType>)
    .add_property("EquationId", Isogeometric_GetEquationId<PBBSplinesBasisFunctionType>, Isogeometric_SetEquationId<PBBSplinesBasisFunctionType>)
    .def("Weight", &PBBSplinesBasisFunctionType::Weight)
    .def(self_ns::str(self))
    ;

    ss.str(std::string());
    ss << "PBBSplinesFESpace" << TDim << "D";
    typedef FESpace<TDim> FESpaceType;
    typedef PBBSplinesFESpace<TDim, PBBSplinesBasisFunctionType, BCellManager<TDim, typename PBBSplinesBasisFunctionType::CellType> > PBBSplinesFESpaceType;
    class_<PBBSplinesFESpaceType, typename PBBSplinesFESpaceType::Pointer, bases<FESpaceType>, boost::noncopyable>
    (ss.str().c_str(), init<>())
    .def("__getitem__", &FESpace_GetItem<PBBSplinesFESpaceType>)
    .def("UpdateCells", &PBBSplinesFESpaceType::UpdateCells)
    .def(self_ns::str(self))
    ;

    IsogeometricApplication_AddPointBasedControlGrid_Helper<Variable<double>, PBBSplinesFESpaceType>::Execute();
    IsogeometricApplication_AddPointBasedControlGrid_Helper<Variable<array_1d<double, 3> >, PBBSplinesFESpaceType>::Execute();
    IsogeometricApplication_AddPointBasedControlGrid_Helper<Variable<Vector>, PBBSplinesFESpaceType>::Execute();

}

////////////////////////////////////////

void IsogeometricApplication_AddPBBSplinesToPython()
{

    /////////////////////////////////////////////////////////////////
    ///////////////////////Point-based BSplines//////////////////////
    /////////////////////////////////////////////////////////////////

    IsogeometricApplication_AddPBBSplinesSpaceToPython<1>();
    IsogeometricApplication_AddPBBSplinesSpaceToPython<2>();
    IsogeometricApplication_AddPBBSplinesSpaceToPython<3>();

    class_<MultiPBBSplinesPatchMatlabExporter, MultiPBBSplinesPatchMatlabExporter::Pointer, boost::noncopyable>
    ("MultiPBBSplinesPatchMatlabExporter", init<>())
    .def("Export", &MultiPatchExporter_Export<1, MultiPBBSplinesPatchMatlabExporter, Patch<1> >)
    .def("Export", &MultiPatchExporter_Export<2, MultiPBBSplinesPatchMatlabExporter, Patch<2> >)
    .def("Export", &MultiPatchExporter_Export<3, MultiPBBSplinesPatchMatlabExporter, Patch<3> >)
    .def("Export", &MultiPatchExporter_Export<1, MultiPBBSplinesPatchMatlabExporter, MultiPatch<1> >)
    .def("Export", &MultiPatchExporter_Export<2, MultiPBBSplinesPatchMatlabExporter, MultiPatch<2> >)
    .def("Export", &MultiPatchExporter_Export<3, MultiPBBSplinesPatchMatlabExporter, MultiPatch<3> >)
    .def(self_ns::str(self))
    ;

}

}  // namespace Python.

} // Namespace Kratos


/**
 * \file      miscellaneous-algorithms.hpp
 * \author    Mehdi Benallegue
 * \date       2013
 * \brief      Gathers many kinds of algorithms
 *
 *
 *
 */

#ifndef STATEOBSERVATIONTOOLSMISCELANEOUSALGORITHMS
#define STATEOBSERVATIONTOOLSMISCELANEOUSALGORITHMS
#include <boost/utility.hpp>
#include <cmath>

#include <state-observation/api.h>
#include <state-observation/tools/definitions.hpp>

namespace stateObservation
{
namespace tools
{

/// computes the square of a value of any type
template<class T>
inline T square(const T & x)
{
  return T(x * x);
}

/// derivates any type with finite differences
template<class T>
inline T derivate(const T & o1, const T & o2, double dt)
{
  T o(o2 - o1);
  return o * (1 / dt);
}

/// gives the sign of a variable (1, 0 or -1)
template<typename T>
inline int signum(T x)
{
  return (T(0) < x) - (x < T(0));
}

template<typename T>
inline std::string toString(T val)
{
  std::stringstream ss("");
  ss << val;
  return ss.str();
}

/// @brief checks if the vector is already normalized or not
///
/// @param v                  the vector to normalize
/// @return true              the vector is normalized
/// @return false             The vector is not normalized
inline bool checkIfNormalized(const Vector3 & v)
{
  if(fabs(v.squaredNorm() - 1) > cst::epsilon1)
  {
    return false;
  }
  else
  {
    return true;
  }
}

/// @brief checks if the vector is already normalized or not
///
/// @param v                  the vector to normalize
/// @param outputSquaredNorm  the squared norm as an output
/// @return true              the vector is normalized
/// @return false             The vector is not normalized
inline bool checkIfNormalized(const Vector3 & v, double & outputSquaredNorm)
{
  outputSquaredNorm = v.squaredNorm();
  if(fabs(outputSquaredNorm - 1) > cst::epsilon1)
  {
    return false;
  }
  else
  {
    return true;
  }
}

/// @brief returns the value clamped between min and max
///
/// @tparam T The type of the scalar
/// @param x the input value
/// @param max the max value
/// @param min the min value
/// @return T the calmped value
template<typename T>
inline T clampScalar(const T & x, const T & max, const T & min)
{
  if(x > max)
  {
    return max;
  }
  else
  {
    if(x < min)
    {
      return min;
    }
    else
    {
      return x;
    }
  }
}

/// @brief returns the value clamped between limit and -limit
///
/// @tparam T The type of the scalar
/// @param x the input value
/// @param limit is the maximum absolute value allowed, has to be positive
/// @return T the clamped value
template<typename T>
inline T clampScalar(const T & x, const T & limit)
{
  return clampScalar(x, limit, -limit);
}

/// @brief normalize the vector only if it is not normalized already. Useful if the vector is likely to be normalized
///
/// @param v the input vector
/// @return Vector3
inline Vector3 normalizedLazy(const Vector3 & v)
{
  double squaredNorm;
  if(checkIfNormalized(v, squaredNorm))
  {
    return v;
  }
  else
  {
    return v / sqrt(squaredNorm);
  }
}

/// provides an acceleration giving a finite time convergence to zero
/// the state is the position x and the derivative xd and the output is the
/// acceleration. The gains kp, kv must be negative
inline double STATE_OBSERVATION_DLLAPI finiteTimeAccControl(double x, double xd, double kp = -1, double kv = -1)
{
  double sax = sqrt(fabs(x));
  double xdr = kp * signum(x) * sax;
  double y = xd - xdr;
  double ydr = -kv * signum(y) * sqrt(fabs(y));
  return ydr - kp * xd / (2 * sax);
}

/// sqme as the scalar version but for every member of the vector
inline Vector STATE_OBSERVATION_DLLAPI finiteTimeAccControl(const Vector & x,
                                                            const Vector & xd,
                                                            double kp = -1,
                                                            double kv = -1)
{
  Vector xdd(x.size());
  for(Index i = 1; i < x.size(); ++i)
  {
    xdd(i) = finiteTimeAccControl(x(i), xd(i), kp, kv);
  }
  return xdd;
}

namespace Detail
{
double constexpr sqrtNewtonRaphson(double x, double curr, double prev)
{
  return curr == prev ? curr : sqrtNewtonRaphson(x, 0.5 * (curr + x / curr), curr);
}
} // namespace Detail

/// @brief Constexpr version of the square root
/// @details For a finite and non-negative value of "x", returns an approximation for the square root of "x"
///-Otherwise, returns NaN
///
/// @param x
/// @return double constexpr
double constexpr sqrt(double x)
{
  return x >= 0 && x < std::numeric_limits<double>::infinity() ? Detail::sqrtNewtonRaphson(x, x, 0)
                                                               : std::numeric_limits<double>::quiet_NaN();
}

} // namespace tools

} // namespace stateObservation

#endif // STATEOBSERVATIONTOOLSMISCELANEOUSALGORITHMS


// ----------------------------------------------------------------------------
// -                        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 "open3d/utility/Eigen.h"

#include <Eigen/Geometry>
#include <Eigen/Sparse>

#include "open3d/utility/Console.h"

namespace open3d {
namespace utility {

/// Function to solve Ax=b
std::tuple<bool, Eigen::VectorXd> SolveLinearSystemPSD(
        const Eigen::MatrixXd &A,
        const Eigen::VectorXd &b,
        bool prefer_sparse /* = false */,
        bool check_symmetric /* = false */,
        bool check_det /* = false */,
        bool check_psd /* = false */) {
    // PSD implies symmetric
    check_symmetric = check_symmetric || check_psd;
    if (check_symmetric && !A.isApprox(A.transpose())) {
        LogWarning("check_symmetric failed, empty vector will be returned");
        return std::make_tuple(false, Eigen::VectorXd::Zero(b.rows()));
    }

    if (check_det) {
        double det = A.determinant();
        if (fabs(det) < 1e-6 || std::isnan(det) || std::isinf(det)) {
            LogWarning("check_det failed, empty vector will be returned");
            return std::make_tuple(false, Eigen::VectorXd::Zero(b.rows()));
        }
    }

    // Check PSD: https://stackoverflow.com/a/54569657/1255535
    if (check_psd) {
        Eigen::LLT<Eigen::MatrixXd> A_llt(A);
        if (A_llt.info() == Eigen::NumericalIssue) {
            LogWarning("check_psd failed, empty vector will be returned");
            return std::make_tuple(false, Eigen::VectorXd::Zero(b.rows()));
        }
    }

    Eigen::VectorXd x(b.size());

    if (prefer_sparse) {
        Eigen::SparseMatrix<double> A_sparse = A.sparseView();
        // TODO: avoid deprecated API SimplicialCholesky
        Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> A_chol;
        A_chol.compute(A_sparse);
        if (A_chol.info() == Eigen::Success) {
            x = A_chol.solve(b);
            if (A_chol.info() == Eigen::Success) {
                // Both decompose and solve are successful
                return std::make_tuple(true, std::move(x));
            } else {
                LogWarning("Cholesky solve failed, switched to dense solver");
            }
        } else {
            LogWarning("Cholesky decompose failed, switched to dense solver");
        }
    }

    x = A.ldlt().solve(b);
    return std::make_tuple(true, std::move(x));
}

Eigen::Matrix4d TransformVector6dToMatrix4d(const Eigen::Vector6d &input) {
    Eigen::Matrix4d output;
    output.setIdentity();
    output.block<3, 3>(0, 0) =
            (Eigen::AngleAxisd(input(2), Eigen::Vector3d::UnitZ()) *
             Eigen::AngleAxisd(input(1), Eigen::Vector3d::UnitY()) *
             Eigen::AngleAxisd(input(0), Eigen::Vector3d::UnitX()))
                    .matrix();
    output.block<3, 1>(0, 3) = input.block<3, 1>(3, 0);
    return output;
}

Eigen::Vector6d TransformMatrix4dToVector6d(const Eigen::Matrix4d &input) {
    Eigen::Vector6d output;
    Eigen::Matrix3d R = input.block<3, 3>(0, 0);
    double sy = sqrt(R(0, 0) * R(0, 0) + R(1, 0) * R(1, 0));
    if (!(sy < 1e-6)) {
        output(0) = atan2(R(2, 1), R(2, 2));
        output(1) = atan2(-R(2, 0), sy);
        output(2) = atan2(R(1, 0), R(0, 0));
    } else {
        output(0) = atan2(-R(1, 2), R(1, 1));
        output(1) = atan2(-R(2, 0), sy);
        output(2) = 0;
    }
    output.block<3, 1>(3, 0) = input.block<3, 1>(0, 3);
    return output;
}

std::tuple<bool, Eigen::Matrix4d> SolveJacobianSystemAndObtainExtrinsicMatrix(
        const Eigen::Matrix6d &JTJ, const Eigen::Vector6d &JTr) {
    bool solution_exist;
    Eigen::Vector6d x;
    std::tie(solution_exist, x) = SolveLinearSystemPSD(JTJ, -JTr);

    if (solution_exist) {
        Eigen::Matrix4d extrinsic = TransformVector6dToMatrix4d(x);
        return std::make_tuple(solution_exist, std::move(extrinsic));
    }
    return std::make_tuple(false, Eigen::Matrix4d::Identity());
}

std::tuple<bool, std::vector<Eigen::Matrix4d, Matrix4d_allocator>>
SolveJacobianSystemAndObtainExtrinsicMatrixArray(const Eigen::MatrixXd &JTJ,
                                                 const Eigen::VectorXd &JTr) {
    std::vector<Eigen::Matrix4d, Matrix4d_allocator> output_matrix_array;
    output_matrix_array.clear();
    if (JTJ.rows() != JTr.rows() || JTJ.cols() % 6 != 0) {
        LogWarning(
                "[SolveJacobianSystemAndObtainExtrinsicMatrixArray] "
                "Unsupported matrix format.");
        return std::make_tuple(false, std::move(output_matrix_array));
    }

    bool solution_exist;
    Eigen::VectorXd x;
    std::tie(solution_exist, x) = SolveLinearSystemPSD(JTJ, -JTr);

    if (solution_exist) {
        int nposes = (int)x.rows() / 6;
        for (int i = 0; i < nposes; i++) {
            Eigen::Matrix4d extrinsic =
                    TransformVector6dToMatrix4d(x.block<6, 1>(i * 6, 0));
            output_matrix_array.push_back(extrinsic);
        }
        return std::make_tuple(solution_exist, std::move(output_matrix_array));
    } else {
        return std::make_tuple(false, std::move(output_matrix_array));
    }
}

template <typename MatType, typename VecType>
std::tuple<MatType, VecType, double> ComputeJTJandJTr(
        std::function<void(int, VecType &, double &, double &)> f,
        int iteration_num,
        bool verbose /*=true*/) {
    MatType JTJ;
    VecType JTr;
    double r2_sum = 0.0;
    JTJ.setZero();
    JTr.setZero();
#pragma omp parallel
    {
        MatType JTJ_private;
        VecType JTr_private;
        double r2_sum_private = 0.0;
        JTJ_private.setZero();
        JTr_private.setZero();
        VecType J_r;
        double r;
        double w = 0.0;
#pragma omp for nowait
        for (int i = 0; i < iteration_num; i++) {
            f(i, J_r, r, w);
            JTJ_private.noalias() += J_r * w * J_r.transpose();
            JTr_private.noalias() += J_r * w * r;
            r2_sum_private += r * r;
        }
#pragma omp critical
        {
            JTJ += JTJ_private;
            JTr += JTr_private;
            r2_sum += r2_sum_private;
        }
    }
    if (verbose) {
        LogDebug("Residual : {:.2e} (# of elements : {:d})",
                 r2_sum / (double)iteration_num, iteration_num);
    }
    return std::make_tuple(std::move(JTJ), std::move(JTr), r2_sum);
}

template <typename MatType, typename VecType>
std::tuple<MatType, VecType, double> ComputeJTJandJTr(
        std::function<
                void(int,
                     std::vector<VecType, Eigen::aligned_allocator<VecType>> &,
                     std::vector<double> &)> f,
        int iteration_num,
        bool verbose /*=true*/) {
    MatType JTJ;
    VecType JTr;
    double r2_sum = 0.0;
    JTJ.setZero();
    JTr.setZero();
#pragma omp parallel
    {
        MatType JTJ_private;
        VecType JTr_private;
        double r2_sum_private = 0.0;
        JTJ_private.setZero();
        JTr_private.setZero();
        std::vector<double> r;
        std::vector<VecType, Eigen::aligned_allocator<VecType>> J_r;
#pragma omp for nowait
        for (int i = 0; i < iteration_num; i++) {
            f(i, J_r, r);
            for (int j = 0; j < (int)r.size(); j++) {
                JTJ_private.noalias() += J_r[j] * J_r[j].transpose();
                JTr_private.noalias() += J_r[j] * r[j];
                r2_sum_private += r[j] * r[j];
            }
        }
#pragma omp critical
        {
            JTJ += JTJ_private;
            JTr += JTr_private;
            r2_sum += r2_sum_private;
        }
    }
    if (verbose) {
        LogDebug("Residual : {:.2e} (# of elements : {:d})",
                 r2_sum / (double)iteration_num, iteration_num);
    }
    return std::make_tuple(std::move(JTJ), std::move(JTr), r2_sum);
}

// clang-format off
template std::tuple<Eigen::Matrix6d, Eigen::Vector6d, double> ComputeJTJandJTr(
        std::function<void(int, Eigen::Vector6d &, double &, double &)> f,
        int iteration_num, bool verbose);

template std::tuple<Eigen::Matrix6d, Eigen::Vector6d, double> ComputeJTJandJTr(
        std::function<void(int,
                           std::vector<Eigen::Vector6d, Vector6d_allocator> &,
                           std::vector<double> &)> f,
        int iteration_num, bool verbose);
// clang-format on

Eigen::Matrix3d RotationMatrixX(double radians) {
    Eigen::Matrix3d rot;
    rot << 1, 0, 0, 0, std::cos(radians), -std::sin(radians), 0,
            std::sin(radians), std::cos(radians);
    return rot;
}

Eigen::Matrix3d RotationMatrixY(double radians) {
    Eigen::Matrix3d rot;
    rot << std::cos(radians), 0, std::sin(radians), 0, 1, 0, -std::sin(radians),
            0, std::cos(radians);
    return rot;
}

Eigen::Matrix3d RotationMatrixZ(double radians) {
    Eigen::Matrix3d rot;
    rot << std::cos(radians), -std::sin(radians), 0, std::sin(radians),
            std::cos(radians), 0, 0, 0, 1;
    return rot;
}

Eigen::Vector3uint8 ColorToUint8(const Eigen::Vector3d &color) {
    Eigen::Vector3uint8 rgb;
    for (int i = 0; i < 3; ++i) {
        rgb[i] = uint8_t(
                std::round(std::min(1., std::max(0., color(i))) * 255.));
    }
    return rgb;
}

Eigen::Vector3d ColorToDouble(uint8_t r, uint8_t g, uint8_t b) {
    return Eigen::Vector3d(r, g, b) / 255.0;
}

Eigen::Vector3d ColorToDouble(const Eigen::Vector3uint8 &rgb) {
    return ColorToDouble(rgb(0), rgb(1), rgb(2));
}

template <typename IdxType>
Eigen::Matrix3d ComputeCovariance(const std::vector<Eigen::Vector3d> &points,
                                  const std::vector<IdxType> &indices) {
    Eigen::Matrix3d covariance;
    Eigen::Matrix<double, 9, 1> cumulants;
    cumulants.setZero();
    for (const auto &idx : indices) {
        const Eigen::Vector3d &point = points[idx];
        cumulants(0) += point(0);
        cumulants(1) += point(1);
        cumulants(2) += point(2);
        cumulants(3) += point(0) * point(0);
        cumulants(4) += point(0) * point(1);
        cumulants(5) += point(0) * point(2);
        cumulants(6) += point(1) * point(1);
        cumulants(7) += point(1) * point(2);
        cumulants(8) += point(2) * point(2);
    }
    cumulants /= (double)indices.size();
    covariance(0, 0) = cumulants(3) - cumulants(0) * cumulants(0);
    covariance(1, 1) = cumulants(6) - cumulants(1) * cumulants(1);
    covariance(2, 2) = cumulants(8) - cumulants(2) * cumulants(2);
    covariance(0, 1) = cumulants(4) - cumulants(0) * cumulants(1);
    covariance(1, 0) = covariance(0, 1);
    covariance(0, 2) = cumulants(5) - cumulants(0) * cumulants(2);
    covariance(2, 0) = covariance(0, 2);
    covariance(1, 2) = cumulants(7) - cumulants(1) * cumulants(2);
    covariance(2, 1) = covariance(1, 2);
    return covariance;
}

template <typename IdxType>
std::tuple<Eigen::Vector3d, Eigen::Matrix3d> ComputeMeanAndCovariance(
        const std::vector<Eigen::Vector3d> &points,
        const std::vector<IdxType> &indices) {
    Eigen::Vector3d mean;
    Eigen::Matrix3d covariance;
    Eigen::Matrix<double, 9, 1> cumulants;
    cumulants.setZero();
    for (const auto &idx : indices) {
        const Eigen::Vector3d &point = points[idx];
        cumulants(0) += point(0);
        cumulants(1) += point(1);
        cumulants(2) += point(2);
        cumulants(3) += point(0) * point(0);
        cumulants(4) += point(0) * point(1);
        cumulants(5) += point(0) * point(2);
        cumulants(6) += point(1) * point(1);
        cumulants(7) += point(1) * point(2);
        cumulants(8) += point(2) * point(2);
    }
    cumulants /= (double)indices.size();
    mean(0) = cumulants(0);
    mean(1) = cumulants(1);
    mean(2) = cumulants(2);
    covariance(0, 0) = cumulants(3) - cumulants(0) * cumulants(0);
    covariance(1, 1) = cumulants(6) - cumulants(1) * cumulants(1);
    covariance(2, 2) = cumulants(8) - cumulants(2) * cumulants(2);
    covariance(0, 1) = cumulants(4) - cumulants(0) * cumulants(1);
    covariance(1, 0) = covariance(0, 1);
    covariance(0, 2) = cumulants(5) - cumulants(0) * cumulants(2);
    covariance(2, 0) = covariance(0, 2);
    covariance(1, 2) = cumulants(7) - cumulants(1) * cumulants(2);
    covariance(2, 1) = covariance(1, 2);
    return std::make_tuple(mean, covariance);
}

template Eigen::Matrix3d ComputeCovariance(
        const std::vector<Eigen::Vector3d> &points,
        const std::vector<size_t> &indices);
template std::tuple<Eigen::Vector3d, Eigen::Matrix3d> ComputeMeanAndCovariance(
        const std::vector<Eigen::Vector3d> &points,
        const std::vector<size_t> &indices);
template Eigen::Matrix3d ComputeCovariance(
        const std::vector<Eigen::Vector3d> &points,
        const std::vector<int> &indices);
template std::tuple<Eigen::Vector3d, Eigen::Matrix3d> ComputeMeanAndCovariance(
        const std::vector<Eigen::Vector3d> &points,
        const std::vector<int> &indices);
}  // namespace utility
}  // namespace open3d


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestTransform
#include <boost/test/unit_test.hpp>

#include <boost/compute/lambda.hpp>
#include <boost/compute/system.hpp>
#include <boost/compute/function.hpp>
#include <boost/compute/functional.hpp>
#include <boost/compute/algorithm/transform.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/iterator/counting_iterator.hpp>
#include <boost/compute/functional/field.hpp>

#include "check_macros.hpp"
#include "context_setup.hpp"

namespace bc = boost::compute;
namespace compute = boost::compute;

BOOST_AUTO_TEST_CASE(transform_int_abs)
{
    int data[] = { 1, -2, -3, -4, 5 };
    bc::vector<int> vector(data, data + 5, queue);
    CHECK_RANGE_EQUAL(int, 5, vector, (1, -2, -3, -4, 5));

    bc::transform(vector.begin(),
                  vector.end(),
                  vector.begin(),
                  bc::abs<int>(),
                  queue);
    CHECK_RANGE_EQUAL(int, 5, vector, (1, 2, 3, 4, 5));
}

BOOST_AUTO_TEST_CASE(transform_float_sqrt)
{
    float data[] = { 1.0f, 4.0f, 9.0f, 16.0f };
    bc::vector<float> vector(data, data + 4, queue);
    CHECK_RANGE_EQUAL(float, 4, vector, (1.0f, 4.0f, 9.0f, 16.0f));

    bc::transform(vector.begin(),
                  vector.end(),
                  vector.begin(),
                  bc::sqrt<float>(),
                  queue);
    queue.finish();
    BOOST_CHECK_CLOSE(float(vector[0]), 1.0f, 1e-4f);
    BOOST_CHECK_CLOSE(float(vector[1]), 2.0f, 1e-4f);
    BOOST_CHECK_CLOSE(float(vector[2]), 3.0f, 1e-4f);
    BOOST_CHECK_CLOSE(float(vector[3]), 4.0f, 1e-4f);
}

BOOST_AUTO_TEST_CASE(transform_float_clamp)
{
    float data[] = { 10.f, 20.f, 30.f, 40.f, 50.f };
    bc::vector<float> vector(data, data + 5, queue);
    CHECK_RANGE_EQUAL(float, 5, vector, (10.0f, 20.0f, 30.0f, 40.0f, 50.0f));

    bc::transform(vector.begin(),
                  vector.end(),
                  vector.begin(),
                  clamp(bc::_1, 15.f, 45.f),
                  queue);
    CHECK_RANGE_EQUAL(float, 5, vector, (15.0f, 20.0f, 30.0f, 40.0f, 45.0f));
}

BOOST_AUTO_TEST_CASE(transform_add_int)
{
    int data1[] = { 1, 2, 3, 4 };
    bc::vector<int> input1(data1, data1 + 4, queue);

    int data2[] = { 10, 20, 30, 40 };
    bc::vector<int> input2(data2, data2 + 4, queue);

    bc::vector<int> output(4, context);
    bc::transform(input1.begin(),
                  input1.end(),
                  input2.begin(),
                  output.begin(),
                  bc::plus<int>(),
                  queue);
    CHECK_RANGE_EQUAL(int, 4, output, (11, 22, 33, 44));

    bc::transform(input1.begin(),
                  input1.end(),
                  input2.begin(),
                  output.begin(),
                  bc::multiplies<int>(),
                  queue);
    CHECK_RANGE_EQUAL(int, 4, output, (10, 40, 90, 160));
}

BOOST_AUTO_TEST_CASE(transform_pow4)
{
    float data[] = { 1.0f, 2.0f, 3.0f, 4.0f };
    bc::vector<float> vector(data, data + 4, queue);
    CHECK_RANGE_EQUAL(float, 4, vector, (1.0f, 2.0f, 3.0f, 4.0f));

    bc::vector<float> result(4, context);
    bc::transform(vector.begin(),
                  vector.end(),
                  result.begin(),
                  pown(bc::_1, 4),
                  queue);
    queue.finish();
    BOOST_CHECK_CLOSE(float(result[0]), 1.0f, 1e-4f);
    BOOST_CHECK_CLOSE(float(result[1]), 16.0f, 1e-4f);
    BOOST_CHECK_CLOSE(float(result[2]), 81.0f, 1e-4f);
    BOOST_CHECK_CLOSE(float(result[3]), 256.0f, 1e-4f);
}

BOOST_AUTO_TEST_CASE(transform_custom_function)
{
    float data[] = { 9.0f, 7.0f, 5.0f, 3.0f };
    bc::vector<float> vector(data, data + 4, queue);

    BOOST_COMPUTE_FUNCTION(float, pow3add4, (float x),
    {
        return pow(x, 3.0f) + 4.0f;
    });

    bc::vector<float> result(4, context);
    bc::transform(vector.begin(),
                  vector.end(),
                  result.begin(),
                  pow3add4,
                  queue);
    queue.finish();
    BOOST_CHECK_CLOSE(float(result[0]), 733.0f, 1e-4f);
    BOOST_CHECK_CLOSE(float(result[1]), 347.0f, 1e-4f);
    BOOST_CHECK_CLOSE(float(result[2]), 129.0f, 1e-4f);
    BOOST_CHECK_CLOSE(float(result[3]), 31.0f, 1e-4f);
}

BOOST_AUTO_TEST_CASE(extract_vector_component)
{
    using bc::int2_;

    int data[] = { 1, 2,
                   3, 4,
                   5, 6,
                   7, 8 };
    bc::vector<int2_> vector(
        reinterpret_cast<int2_ *>(data),
        reinterpret_cast<int2_ *>(data) + 4,
        queue
    );
    CHECK_RANGE_EQUAL(
        int2_, 4, vector,
        (int2_(1, 2), int2_(3, 4), int2_(5, 6), int2_(7, 8))
    );

    bc::vector<int> x_components(4, context);
    bc::transform(vector.begin(),
                  vector.end(),
                  x_components.begin(),
                  bc::get<0>(),
                  queue);
    CHECK_RANGE_EQUAL(int, 4, x_components, (1, 3, 5, 7));

    bc::vector<int> y_components(4, context);
    bc::transform(vector.begin(),
                  vector.end(),
                  y_components.begin(),
                  bc::get<1>(),
                  queue);
    CHECK_RANGE_EQUAL(int, 4, y_components, (2, 4, 6, 8));
}

BOOST_AUTO_TEST_CASE(transform_pinned_vector)
{
    int data[] = { 2, -3, 4, -5, 6, -7 };
    std::vector<int> vector(data, data + 6);

    bc::buffer buffer(context,
                      vector.size() * sizeof(int),
                      bc::buffer::read_write | bc::buffer::use_host_ptr,
                      &vector[0]);

    bc::transform(bc::make_buffer_iterator<int>(buffer, 0),
                  bc::make_buffer_iterator<int>(buffer, 6),
                  bc::make_buffer_iterator<int>(buffer, 0),
                  bc::abs<int>(),
                  queue);

    void *ptr = queue.enqueue_map_buffer(buffer,
                                         bc::command_queue::map_read,
                                         0,
                                         buffer.size());
    BOOST_VERIFY(ptr == &vector[0]);
    BOOST_CHECK_EQUAL(vector[0], 2);
    BOOST_CHECK_EQUAL(vector[1], 3);
    BOOST_CHECK_EQUAL(vector[2], 4);
    BOOST_CHECK_EQUAL(vector[3], 5);
    BOOST_CHECK_EQUAL(vector[4], 6);
    BOOST_CHECK_EQUAL(vector[5], 7);
    queue.enqueue_unmap_buffer(buffer, ptr);
}

BOOST_AUTO_TEST_CASE(transform_popcount)
{
    using boost::compute::uint_;

    uint_ data[] = { 0, 1, 2, 3, 4, 45, 127, 5000, 789, 15963 };
    bc::vector<uint_> input(data, data + 10, queue);
    bc::vector<uint_> output(input.size(), context);

    bc::transform(
        input.begin(),
        input.end(),
        output.begin(),
        bc::popcount<uint_>(),
        queue
    );
    CHECK_RANGE_EQUAL(uint_, 10, output, (0, 1, 1, 2, 1, 4, 7, 5, 5, 10));
}

// generates the first 25 fibonacci numbers in parallel using the
// rounding-based fibonacci formula
BOOST_AUTO_TEST_CASE(generate_fibonacci_sequence)
{
    using boost::compute::uint_;

    boost::compute::vector<uint_> sequence(25, context);

    BOOST_COMPUTE_FUNCTION(uint_, nth_fibonacci, (const uint_ n),
    {
        const float golden_ratio = (1.f + sqrt(5.f)) / 2.f;
        return floor(pown(golden_ratio, n) / sqrt(5.f) + 0.5f);
    });

    boost::compute::transform(
        boost::compute::make_counting_iterator(uint_(0)),
        boost::compute::make_counting_iterator(uint_(sequence.size())),
        sequence.begin(),
        nth_fibonacci,
        queue
    );
    CHECK_RANGE_EQUAL(
        uint_, 25, sequence,
        (0, 1, 1, 2, 3, 5, 8, 13, 21, 34, 55, 89, 144, 233, 377, 610,
         987, 1597, 2584, 4181, 6765, 10946, 17711, 28657, 46368)
    );
}

BOOST_AUTO_TEST_CASE(field)
{
    using compute::uint2_;
    using compute::uint4_;
    using compute::field;

    unsigned int data[] = { 1, 2, 3, 4, 5, 6, 7, 8 };
    compute::vector<uint4_> input(
        reinterpret_cast<uint4_ *>(data),
        reinterpret_cast<uint4_ *>(data) + 2,
        queue
    );
    compute::vector<uint2_> output(input.size(), context);

    compute::transform(
        input.begin(),
        input.end(),
        output.begin(),
        compute::field<uint2_>("xz"),
        queue
    );

    queue.finish();

    BOOST_CHECK_EQUAL(uint2_(output[0]), uint2_(1, 3));
    BOOST_CHECK_EQUAL(uint2_(output[1]), uint2_(5, 7));
}

BOOST_AUTO_TEST_CASE(transform_abs_doctest)
{
//! [transform_abs]
int data[] = { -1, -2, -3, -4 };
boost::compute::vector<int> vec(data, data + 4, queue);

using boost::compute::abs;

// calculate the absolute value for each element in-place
boost::compute::transform(
    vec.begin(), vec.end(), vec.begin(), abs<int>(), queue
);

// vec == { 1, 2, 3, 4 }
//! [transform_abs]

    CHECK_RANGE_EQUAL(int, 4, vec, (1, 2, 3, 4));
}

BOOST_AUTO_TEST_CASE(abs_if_odd)
{
    // return absolute value only for odd values
    BOOST_COMPUTE_FUNCTION(int, abs_if_odd, (int x),
    {
        if(x & 1){
            return abs(x);
        }
        else {
            return x;
        }
    });

    int data[] = { -2, -3, -4, -5, -6, -7, -8, -9 };
    compute::vector<int> vector(data, data + 8, queue);

    compute::transform(
        vector.begin(), vector.end(), vector.begin(), abs_if_odd, queue
    );

    CHECK_RANGE_EQUAL(int, 8, vector, (-2, +3, -4, +5, -6, +7, -8, +9));
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include <ros/ros.h>
#include "roboy_communication_middleware/InverseKinematics.h"
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/SVD>
#include <vector>
#include <boost/numeric/odeint.hpp>
#include "common_utilities/CommonDefinitions.h"
#include <boost/thread/thread.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <thread>
#include <mutex>
#include "common_utilities/rviz_visualization.hpp"

using namespace Eigen;
using namespace std;

class PaBiRoboyInverseKinematics:public rviz_visualization{
public:
    PaBiRoboyInverseKinematics();
    ~PaBiRoboyInverseKinematics(){};

    template<typename _Matrix_Type_>
    _Matrix_Type_ pseudoInverse(const _Matrix_Type_ &a, double epsilon = std::numeric_limits<double>::epsilon())
    {
        Eigen::JacobiSVD< _Matrix_Type_ > svd(a ,Eigen::ComputeThinU | Eigen::ComputeThinV);
        double tolerance = epsilon * std::max(a.cols(), a.rows()) *svd.singularValues().array().abs()(0);
        return svd.matrixV() *  (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0).matrix().asDiagonal() * svd.matrixU().adjoint();
    }

    bool inverseKinematics(roboy_communication_middleware::InverseKinematics::Request &req,
                               roboy_communication_middleware::InverseKinematics::Response &res);
     /**
      * Calculates the Jacobian of a lighthouse sensor
      * @param out 3x5 Matrix will be filled with the values
      * @param sensor for this sensor
      * @param ankle_left
      * @param ankle_right
      * @param theta0 knee_left angle
      * @param theta1 hip_left angle
      * @param theta2 hip_right angle
      * @param theta3 knee_right angle
      * @param phi angle of lower left leg wrt to inertial_frame.z
      */
    void Jacobian(double *out, int sensor, double ankle_left, double ankle_right, double theta0, double theta1, double theta2, double theta3, double phi);
    /**
     * forward kinematics of joints
     * @param out position
     * @param joint for this joint
     * @param ankle_x ankle_left position x
     * @param ankle_y ankle_left position y
     * @param theta0 knee_left angle
     * @param theta1 hip_left angle
     * @param theta2 hip_right angle
     * @param theta3 knee_right angle
     * @param phi angle of lower left leg wrt to inertial_frame.z
     */
    void joint_position(double *out, int joint, double ankle_x, double ankle_y, double theta0, double theta1, double theta2, double theta3, double phi);

    /**
     * forward kinematics of lighthouse sensors
     * @param out
     * @param sensor for this lighthouse sensor
     * @param ankle_x ankle_left position x
     * @param ankle_y ankle_left position y
     * @param theta0 knee_left angle
     * @param theta1 hip_left angle
     * @param theta2 hip_right angle
     * @param theta3 knee_right angle
     * @param phi angle of lower left leg wrt to inertial_frame.z
     */
    void lighthouse_sensor(double *out, int sensor, double ankle_x, double ankle_y, double phi, double theta0, double theta1, double theta2, double theta3);
    geometry_msgs::Vector3 lighthouse_sensor(int sensor, double ankle_x, double ankle_y, double phi, double theta0, double theta1, double theta2, double theta3);

    void visualize();

    enum JOINTS{
        ANKLE_LEFT = 0,
        KNEE_LEFT,
        HIP_LEFT,
        HIP_CENTER,
        HIP_RIGHT,
        KNEE_RIGHT,
        ANKLE_RIGHT,
        ANKLE_RIGHT_JACOBIAN = 9,
    };
private:
    ros::NodeHandlePtr nh;
    ros::ServiceServer ik_srv;
    const double l1 = 0.34;
    const double l2 = 0.40;
    const double l3 = 0.18;
    const double error_threshold = 0.005, max_number_iterations = 100;
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
    map<string,Vector3d> initial_position, result_position;
};

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