// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published
// by the Free Software Foundation, either version 3 of the License,
// or (at your option) any later version.
//
// FlexibleSUSY is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

// File generated at Thu 10 May 2018 15:16:03

/**
 * @file MSSM_cxx_diagrams.hpp
 *
 * This file was generated at Thu 10 May 2018 15:16:03 with FlexibleSUSY
 * 2.0.1 and SARAH 4.12.2 .
 */

#ifndef MSSM_CXXDIAGRAMS_H
#define MSSM_CXXDIAGRAMS_H

#include "numerics2.hpp"
#include "wrappers.hpp"

#include <array>
#include <boost/range/join.hpp>
#include <boost/mpl/vector_c.hpp>
#include <boost/fusion/adapted/mpl.hpp>
#include <boost/fusion/include/vector.hpp>
#include <boost/fusion/adapted/boost_array.hpp>
#include <boost/array.hpp>

#include <boost/version.hpp>

#if BOOST_VERSION >= 105800
#include <boost/fusion/include/move.hpp>
#else
#include <boost/fusion/include/copy.hpp>
#endif

#define INPUTPARAMETER(p) context.model.get_input().p
#define MODELPARAMETER(p) context.model.get_##p()
#define DERIVEDPARAMETER(p) context.model.p()
#define PHASE(p) context.model.get_##p()

namespace flexiblesusy {
namespace cxx_diagrams {
namespace impl {
/** \brief Helper template for make_array
 */
template<class ForwardIterator, std::size_t N, class T>
struct make_array_it {
   using decayed_type = typename std::decay<
                        typename std::iterator_traits<ForwardIterator>::value_type
                        >::type;
   static constexpr bool use_cast = !std::is_same<decayed_type, T>::value;

   template<class... Args>
   static auto iterate(ForwardIterator begin, Args &&...args)
   -> typename std::enable_if<
   !use_cast,
   std::array<T, N + sizeof...(Args)>
   >::type {
      ForwardIterator copy(begin);
      return make_array_it<ForwardIterator, N-1, T>::iterate(++begin,
            std::forward<Args>(args)..., *copy);
   }

   template<class... Args>
   static auto iterate(ForwardIterator begin, Args &&...args)
   -> typename std::enable_if<
   use_cast,
   std::array<T, N + sizeof...(Args)>
   >::type {
      ForwardIterator copy(begin);
      return make_array_it<ForwardIterator, N-1, T>::iterate(++begin,
            std::forward<Args>(args)..., T(*copy));
   }
};

/** \brief Specialized helper template for make_array */
template<class ForwardIterator, class T>
struct make_array_it<ForwardIterator, 0, T> {
   template<class... Args>
   static auto iterate(ForwardIterator /* begin */, Args &&...args)
   -> std::array<T, sizeof...(Args)> {
      return std::array<T, sizeof...(Args)>{ std::forward<Args>(args)... };
   }
};

/** \class make_array
 * \brief Facility for easy construction of \a std::array
 *        objects.
 * \tparam N The size of the to be constructed array
 * \tparam T The type of the objects the array should hold
 *           or void (by default) if the type should be
 *           inferred.
 */
template<std::size_t N, class T = void>
struct make_array {
   /** \brief Construct a \a std::array by copying values
    *         from a range.
    * \tparam ForwardIterator The iterator type (usually inferred)
    * \param[in] begin The iterator marking the beginning of
    *                  the range to be copied.
    * \returns A \a std::array holding the desired objects.
    * \warning \a begin must be incrementable sufficiently
    *          often (\a N times) otherwise the behaviour
    *          is undefined.
    */
   template<class ForwardIterator>
   static auto iterate(ForwardIterator begin)
   -> std::array<typename std::conditional<
   std::is_void<T>::value,
       typename std::iterator_traits<ForwardIterator>::value_type,
       T
   >::type, N> {
      using value_type = typename std::conditional<
      std::is_void<T>::value,
      typename std::iterator_traits<ForwardIterator>::value_type,
      T
      >::type;

      return make_array_it<ForwardIterator, N, value_type>::iterate(begin);
   }
};

template<class Array1, class Array2>
auto concatenate(const Array1& a1, const Array2& a2)
-> std::array<
typename std::common_type<
typename Array1::value_type,
         typename Array1::value_type
         >::type,
         std::tuple_size<Array1>::value + std::tuple_size<Array2>::value>
         {
            using value_type = typename std::common_type<
                               typename Array1::value_type,
            typename Array1::value_type
            >::type;
            constexpr auto size = std::tuple_size<Array1>::value + std::tuple_size<Array2>::value;

            auto range1 = boost::make_iterator_range(boost::begin(a1),
                          boost::end(a1));
            auto range2 = boost::make_iterator_range(boost::begin(a2),
                          boost::end(a2));
            auto joined = boost::join(range1, range2);

            return make_array<size, value_type>::iterate(boost::begin(joined));
         }

         /**
          * @class IndexBounds<N>
          * @brief A class representing multiple (N) index ranges.
          *
          * N is a non-negative integer.
          * The ranges are specified the c++ way; The range begins are inclusive
          * and the range ends are exclusive. Misspecified ranges result in
          * undefined behaviour!
          * The intended use is to iterate over an IndexBounds<N> using the normal
          * begin()/end() syntax, which will iterate over every possible combination
          * of the indices within their respective ranges.
          * The ranges are const! Once they are initialized they cannot be changed.
          * Initialization is done like = {{ beg1, beg2, ... }, { end1, end2, ... }}
          */
         template<int N> struct IndexBounds;

/**
 * @class IndexIterator<N>
 * @brief The iterator class used by IndexBounds<N>.
 *
 * It only fulfils the input iterator requirements!
 * The value_type is a const std::array<int, N>
 * containing the current indices.
 * Incrementing the iterator results in a new index combination.
 */
template<int N> struct IndexIterator {
   using difference_type = typename std::array<int, N>::difference_type;
   using value_type = int;
   using pointer = int*;
   using reference = int&;
   using iterator_category = std::input_iterator_tag;

   friend class IndexBounds<N>;
private:
   const IndexBounds<N>& bounds;
   std::array<int, N> indices;

   IndexIterator(const IndexBounds<N>& b, std::array<int, N>&& i)
      : bounds(b), indices(std::move(i)) {}
public:
   const std::array<int, N>& operator*() const
   {
      return indices;
   }

   const std::array<int, N>* operator->() const
   {
      return &indices;
   }

   IndexIterator& operator++()
   {
      for (int i = 0; i != N; i++) {
         indices[i]++;
         if (indices[i] == bounds.indexEnd[i]) {
            indices[i] = bounds.indexBegin[i];
            continue;
         }

         return *this;
      }

      indices = make_array<N>::iterate(bounds.indexEnd);
      return *this;
   }

   IndexIterator operator++(int)
   {
      IndexIterator it(*this);
      this->operator++();
      return it;
   }

   template<int M>
   friend bool operator==(const IndexIterator<M>& it1, const IndexIterator<M>& it2);
};

template<int N>
bool operator==(const IndexIterator<N>& it1, const IndexIterator<N>& it2)
{
   return it1.indices == it2.indices;
}

template<int N>
bool operator!=(const IndexIterator<N>& it1, const IndexIterator<N>& it2)
{
   return !(it1 == it2);
}

template<int N> struct IndexBounds {
   typedef const std::array<int, N> indices_type;

   int indexBegin[N];
   int indexEnd[N];

   typedef IndexIterator<N> const_iterator;

   const_iterator begin() const
   {
      return const_iterator(*this, make_array<N>::iterate(indexBegin));
   }

   const_iterator end() const
   {
      return const_iterator(*this, make_array<N>::iterate(indexEnd));
   }
};

template<> struct IndexBounds<0> {
   using indices_type = const std::array<int, 0>;
   using const_iterator = indices_type*;

   const indices_type dummyIndex = {};

   const_iterator begin() const
   {
      return &dummyIndex;
   }
   const_iterator end() const
   {
      return (begin()+1);
   }
};

} // namespace impl

// Lorentz conjugate fermions
template<class Field> struct bar {
   static constexpr int numberOfGenerations = Field::numberOfGenerations;
   static constexpr int numberOfFieldIndices = Field::numberOfFieldIndices;
   using smFlags = typename Field::smFlags;

   static constexpr double electric_charge = - Field::electric_charge;

   using type = bar<Field>;
   using lorentz_conjugate = Field;
};

// Lorentz conjugate bosons
template<class Field> struct conj {
   static constexpr int numberOfGenerations = Field::numberOfGenerations;
   static constexpr int numberOfFieldIndices = Field::numberOfFieldIndices;
   using smFlags = typename Field::smFlags;

   static constexpr double electric_charge = - Field::electric_charge;

   using type = conj<Field>;
   using lorentz_conjugate = Field;
};

// Double Lorentz conjugation
template<class Field> struct bar<bar<Field>> {
   using type = Field;
};
template<class Field> struct conj<conj<Field>> {
   using type = Field;
};

// Remove Lorentz conjugation
template<class Field> struct remove_lorentz_conjugation {
   using type = Field;
};

template<class Field> struct remove_lorentz_conjugation<bar<Field>> {
   using type = Field;
};

template<class Field> struct remove_lorentz_conjugation<conj<Field>> {
   using type = Field;
};

// Declare a type that can hold the field indices for any given field
template<class Field> struct field_indices {
   using type = std::array<int, Field::numberOfFieldIndices>;
};

template<class Field>
typename std::enable_if<Field::numberOfGenerations != 1, bool>::type
isSMField(const typename field_indices<Field>::type& indices)
{
   boost::array<bool,Field::numberOfGenerations> smFlags;

#if BOOST_VERSION >= 105800
   boost::fusion::move(typename Field::smFlags(), smFlags);
#else
   boost::fusion::copy(typename Field::smFlags(), smFlags);
#endif

   return smFlags[indices[0]];
}

template<class Field>
typename std::enable_if<Field::numberOfGenerations == 1, bool>::type
isSMField(const typename field_indices<Field>::type& /* indices */)
{
   return boost::mpl::at_c<typename Field::smFlags,0>::type::value;
}

struct VG {
   static constexpr int numberOfGenerations = 1;
   using smFlags = boost::mpl::vector_c<bool, true>;
   static constexpr int numberOfFieldIndices = 1;
   static constexpr double electric_charge = 0;
   using lorentz_conjugate = VG;
};

struct gG {
   static constexpr int numberOfGenerations = 1;
   using smFlags = boost::mpl::vector_c<bool, true>;
   static constexpr int numberOfFieldIndices = 1;
   static constexpr double electric_charge = 0;
   using lorentz_conjugate = bar<gG>::type;
};

struct Glu {
   static constexpr int numberOfGenerations = 1;
   using smFlags = boost::mpl::vector_c<bool, false>;
   static constexpr int numberOfFieldIndices = 1;
   static constexpr double electric_charge = 0;
   using lorentz_conjugate = Glu;
};

struct Fv {
   static constexpr int numberOfGenerations = 3;
   using smFlags = boost::mpl::vector_c<bool, true, true, true>;
   static constexpr int numberOfFieldIndices = 1;
   static constexpr double electric_charge = 0;
   using lorentz_conjugate = bar<Fv>::type;
};

struct VP {
   static constexpr int numberOfGenerations = 1;
   using smFlags = boost::mpl::vector_c<bool, true>;
   static constexpr int numberOfFieldIndices = 0;
   static constexpr double electric_charge = 0;
   using lorentz_conjugate = VP;
};

struct VZ {
   static constexpr int numberOfGenerations = 1;
   using smFlags = boost::mpl::vector_c<bool, true>;
   static constexpr int numberOfFieldIndices = 0;
   static constexpr double electric_charge = 0;
   using lorentz_conjugate = VZ;
};

struct gP {
   static constexpr int numberOfGenerations = 1;
   using smFlags = boost::mpl::vector_c<bool, true>;
   static constexpr int numberOfFieldIndices = 0;
   static constexpr double electric_charge = 0;
   using lorentz_conjugate = bar<gP>::type;
};

struct gZ {
   static constexpr int numberOfGenerations = 1;
   using smFlags = boost::mpl::vector_c<bool, true>;
   static constexpr int numberOfFieldIndices = 0;
   static constexpr double electric_charge = 0;
   using lorentz_conjugate = bar<gZ>::type;
};

struct gWm {
   static constexpr int numberOfGenerations = 1;
   using smFlags = boost::mpl::vector_c<bool, true>;
   static constexpr int numberOfFieldIndices = 0;
   static constexpr double electric_charge = -1;
   using lorentz_conjugate = bar<gWm>::type;
};

struct gWmC {
   static constexpr int numberOfGenerations = 1;
   using smFlags = boost::mpl::vector_c<bool, true>;
   static constexpr int numberOfFieldIndices = 0;
   static constexpr double electric_charge = 1;
   using lorentz_conjugate = bar<gWmC>::type;
};

struct Sd {
   static constexpr int numberOfGenerations = 6;
   using smFlags = boost::mpl::vector_c<bool, false, false, false, false, false, false>;
   static constexpr int numberOfFieldIndices = 2;
   static constexpr double electric_charge = -0.3333333333333333;
   using lorentz_conjugate = conj<Sd>::type;
};

struct Sv {
   static constexpr int numberOfGenerations = 3;
   using smFlags = boost::mpl::vector_c<bool, false, false, false>;
   static constexpr int numberOfFieldIndices = 1;
   static constexpr double electric_charge = 0;
   using lorentz_conjugate = conj<Sv>::type;
};

struct Su {
   static constexpr int numberOfGenerations = 6;
   using smFlags = boost::mpl::vector_c<bool, false, false, false, false, false, false>;
   static constexpr int numberOfFieldIndices = 2;
   static constexpr double electric_charge = 0.6666666666666666;
   using lorentz_conjugate = conj<Su>::type;
};

struct Se {
   static constexpr int numberOfGenerations = 6;
   using smFlags = boost::mpl::vector_c<bool, false, false, false, false, false, false>;
   static constexpr int numberOfFieldIndices = 1;
   static constexpr double electric_charge = -1;
   using lorentz_conjugate = conj<Se>::type;
};

struct hh {
   static constexpr int numberOfGenerations = 2;
   using smFlags = boost::mpl::vector_c<bool, false, false>;
   static constexpr int numberOfFieldIndices = 1;
   static constexpr double electric_charge = 0;
   using lorentz_conjugate = hh;
};

struct Ah {
   static constexpr int numberOfGenerations = 2;
   using smFlags = boost::mpl::vector_c<bool, false, false>;
   static constexpr int numberOfFieldIndices = 1;
   static constexpr double electric_charge = 0;
   using lorentz_conjugate = Ah;
};

struct Hpm {
   static constexpr int numberOfGenerations = 2;
   using smFlags = boost::mpl::vector_c<bool, false, false>;
   static constexpr int numberOfFieldIndices = 1;
   static constexpr double electric_charge = -1;
   using lorentz_conjugate = conj<Hpm>::type;
};

struct Chi {
   static constexpr int numberOfGenerations = 4;
   using smFlags = boost::mpl::vector_c<bool, false, false, false, false>;
   static constexpr int numberOfFieldIndices = 1;
   static constexpr double electric_charge = 0;
   using lorentz_conjugate = Chi;
};

struct Cha {
   static constexpr int numberOfGenerations = 2;
   using smFlags = boost::mpl::vector_c<bool, false, false>;
   static constexpr int numberOfFieldIndices = 1;
   static constexpr double electric_charge = -1;
   using lorentz_conjugate = bar<Cha>::type;
};

struct Fe {
   static constexpr int numberOfGenerations = 3;
   using smFlags = boost::mpl::vector_c<bool, true, true, true>;
   static constexpr int numberOfFieldIndices = 1;
   static constexpr double electric_charge = -1;
   using lorentz_conjugate = bar<Fe>::type;
};

struct Fd {
   static constexpr int numberOfGenerations = 3;
   using smFlags = boost::mpl::vector_c<bool, true, true, true>;
   static constexpr int numberOfFieldIndices = 2;
   static constexpr double electric_charge = -0.3333333333333333;
   using lorentz_conjugate = bar<Fd>::type;
};

struct Fu {
   static constexpr int numberOfGenerations = 3;
   using smFlags = boost::mpl::vector_c<bool, true, true, true>;
   static constexpr int numberOfFieldIndices = 2;
   static constexpr double electric_charge = 0.6666666666666666;
   using lorentz_conjugate = bar<Fu>::type;
};

struct VWm {
   static constexpr int numberOfGenerations = 1;
   using smFlags = boost::mpl::vector_c<bool, true>;
   static constexpr int numberOfFieldIndices = 0;
   static constexpr double electric_charge = -1;
   using lorentz_conjugate = conj<VWm>::type;
};

// Named fields
using Photon = VP;
using Electron = Fe;

// Fields that are their own Lorentz conjugates.
template<> struct conj<VG> { using type = VG; };
template<> struct bar<Glu> { using type = Glu; };
template<> struct conj<VP> { using type = VP; };
template<> struct conj<VZ> { using type = VZ; };
template<> struct conj<hh> { using type = hh; };
template<> struct conj<Ah> { using type = Ah; };
template<> struct bar<Chi> { using type = Chi; };

/**
* @class SingleComponentedVertex
* @brief A vertex whose value can be represented by a single complex number
*/
class SingleComponentedVertex {
private:
   std::complex<double> val; ///< The value
public:
   SingleComponentedVertex(std::complex<double> v)
      : val(v) {}

   /**
   * @fn value
   * @brief Returns the value of the vertex.
   */
   std::complex<double> value() const
   {
      return val;
   }

   /**
   * @fn isZero
   * @brief Tests whether the value is numerically significant
   */
   bool isZero() const
   {
      return (is_zero(val.real()) && is_zero(val.imag()));
   }
};

/**
* @class LeftAndRightComponentedVertex
* @brief A vertex whose value can be represented by two complex numbers
*/
class LeftAndRightComponentedVertex {
private:
   std::pair<std::complex<double>, std::complex<double>> value;  ///< The values
public:
   LeftAndRightComponentedVertex(const std::complex<double>& left,
                                 const std::complex<double>& right)
      : value(left, right) {}

   /**
   * @fn left
   * @brief Returns the left component of the vertex.
   */
   std::complex<double> left() const
   {
      return value.first;
   }

   /**
   * @fn right
   * @brief Returns the left component of the vertex.
   */
   std::complex<double> right() const
   {
      return value.second;
   }

   /**
   * @fn isZero
   * @brief Tests whether the values are numerically significant
   */
   bool isZero() const
   {
      return (is_zero(value.first.real()) && is_zero(value.first.imag()) &&
              is_zero(value.second.real()) && is_zero(value.second.imag()));
   }
};

/**
 * @class VertexData<F...>
 * @brief VertexData data for a vertex with the fields specified by F....
 */
template<class ...Fields> struct VertexData;

struct EvaluationContext;

/**
 * @class Vertex<F...>
 * @brief A template that represents a vertex with open field indices.
 *
 * All elements in F... have to be publicly derived from Field.
 * To obtain a conrete value, use the static member function evaluate()
 * along with the desired field indices.
 */
template<class ...F> class Vertex {
   using Data = VertexData<F...>;
   using bounds_type = typename std::decay<decltype(Data::index_bounds)>::type;
public:
   using vertex_type = typename Data::vertex_type;
   using indices_type = typename decltype(Data::index_bounds)::indices_type;

   static constexpr bounds_type index_bounds = Data::index_bounds;

   /**
    * @fn fieldIndices
    * @brief Returns the subset of indices belonging to
    *        a field at a given index.
    * @param indices The complete set of indices for all fields
    *
    * The field indexing is in the same order as the template arguments
    * and starts at 0.
    */
   template<int fieldIndex>
   static
   std::array<int, Data::fieldIndexStart[fieldIndex+1] - Data::fieldIndexStart[fieldIndex]>
   fieldIndices(const indices_type& indices)
   {
      constexpr auto length = Data::fieldIndexStart[fieldIndex+1] - Data::fieldIndexStart[fieldIndex];
      constexpr auto offset = Data::fieldIndexStart[fieldIndex];
      auto begin = indices.begin() + offset;
      return impl::make_array<length>::iterate(begin);
   }

   /**
    * @fn vertex
    * @brief Calculates the vertex for a given set of field indices.
    * @param indices The field indices
    * @param context The evaluation context
    */
   static vertex_type evaluate(const indices_type& indices, const EvaluationContext& context);
};

/**
* @class EvaluationContext
* @brief Represents an evaluation context.
*
* It simply contains a reference to a model object.
* All computational functions are forwarded to that object,
* e.g. mass calculation functions.
*/
struct EvaluationContext {
   MSSM_mass_eigenstates& model; ///< The model object.

   template<class Field>
   double mass(const typename field_indices<Field>::type& indices) const
   {
      using CleanField = typename remove_lorentz_conjugation<Field>::type;
      return mass_impl<CleanField>(indices);
   }

private:
   template<class Field>
   double mass_impl(const typename field_indices<Field>::type& indices) const;
};

template<> struct VertexData<bar<Fe>::type, Ah, Fe>
{
   static constexpr impl::IndexBounds<3> index_bounds = { { 0, 0, 0 }, { 3, 2, 3 } };
   static constexpr int fieldIndexStart[4] = { 0, 1, 2, 3 };
   using vertex_type = LeftAndRightComponentedVertex;
};

template<> struct VertexData<Fe, Ah, bar<Fe>::type>
{
   static constexpr impl::IndexBounds<3> index_bounds = { { 0, 0, 0 }, { 3, 2, 3 } };
   static constexpr int fieldIndexStart[4] = { 0, 1, 2, 3 };
   using vertex_type = LeftAndRightComponentedVertex;
};

template<> struct VertexData<VP, bar<Fe>::type, Fe>
{
   static constexpr impl::IndexBounds<2> index_bounds = { { 0, 0 }, { 3, 3 } };
   static constexpr int fieldIndexStart[4] = { 0, 0, 1, 2 };
   using vertex_type = LeftAndRightComponentedVertex;
};

template<> struct VertexData<bar<Fe>::type, Chi, Se>
{
   static constexpr impl::IndexBounds<3> index_bounds = { { 0, 0, 0 }, { 3, 4, 6 } };
   static constexpr int fieldIndexStart[4] = { 0, 1, 2, 3 };
   using vertex_type = LeftAndRightComponentedVertex;
};

template<> struct VertexData<Fe, Chi, conj<Se>::type>
{
   static constexpr impl::IndexBounds<3> index_bounds = { { 0, 0, 0 }, { 3, 4, 6 } };
   static constexpr int fieldIndexStart[4] = { 0, 1, 2, 3 };
   using vertex_type = LeftAndRightComponentedVertex;
};

template<> struct VertexData<VP, conj<Se>::type, Se>
{
   static constexpr impl::IndexBounds<2> index_bounds = { { 0, 0 }, { 6, 6 } };
   static constexpr int fieldIndexStart[4] = { 0, 0, 1, 2 };
   using vertex_type = SingleComponentedVertex;
};

template<> struct VertexData<bar<Fe>::type, Fv, Hpm>
{
   static constexpr impl::IndexBounds<3> index_bounds = { { 0, 0, 0 }, { 3, 3, 2 } };
   static constexpr int fieldIndexStart[4] = { 0, 1, 2, 3 };
   using vertex_type = LeftAndRightComponentedVertex;
};

template<> struct VertexData<Fe, bar<Fv>::type, conj<Hpm>::type>
{
   static constexpr impl::IndexBounds<3> index_bounds = { { 0, 0, 0 }, { 3, 3, 2 } };
   static constexpr int fieldIndexStart[4] = { 0, 1, 2, 3 };
   using vertex_type = LeftAndRightComponentedVertex;
};

template<> struct VertexData<VP, conj<Hpm>::type, Hpm>
{
   static constexpr impl::IndexBounds<2> index_bounds = { { 0, 0 }, { 2, 2 } };
   static constexpr int fieldIndexStart[4] = { 0, 0, 1, 2 };
   using vertex_type = SingleComponentedVertex;
};

template<> struct VertexData<bar<Fe>::type, hh, Fe>
{
   static constexpr impl::IndexBounds<3> index_bounds = { { 0, 0, 0 }, { 3, 2, 3 } };
   static constexpr int fieldIndexStart[4] = { 0, 1, 2, 3 };
   using vertex_type = LeftAndRightComponentedVertex;
};

template<> struct VertexData<Fe, hh, bar<Fe>::type>
{
   static constexpr impl::IndexBounds<3> index_bounds = { { 0, 0, 0 }, { 3, 2, 3 } };
   static constexpr int fieldIndexStart[4] = { 0, 1, 2, 3 };
   using vertex_type = LeftAndRightComponentedVertex;
};

template<> struct VertexData<bar<Fe>::type, Sv, Cha>
{
   static constexpr impl::IndexBounds<3> index_bounds = { { 0, 0, 0 }, { 3, 3, 2 } };
   static constexpr int fieldIndexStart[4] = { 0, 1, 2, 3 };
   using vertex_type = LeftAndRightComponentedVertex;
};

template<> struct VertexData<Fe, conj<Sv>::type, bar<Cha>::type>
{
   static constexpr impl::IndexBounds<3> index_bounds = { { 0, 0, 0 }, { 3, 3, 2 } };
   static constexpr int fieldIndexStart[4] = { 0, 1, 2, 3 };
   using vertex_type = LeftAndRightComponentedVertex;
};

template<> struct VertexData<VP, bar<Cha>::type, Cha>
{
   static constexpr impl::IndexBounds<2> index_bounds = { { 0, 0 }, { 2, 2 } };
   static constexpr int fieldIndexStart[4] = { 0, 0, 1, 2 };
   using vertex_type = LeftAndRightComponentedVertex;
};

template<> inline
Vertex<bar<Fe>::type, Ah, Fe>::vertex_type
Vertex<bar<Fe>::type, Ah, Fe>::evaluate(const indices_type& indices, const EvaluationContext& context)
{
   const int gt1 = indices[0];
   const int gt2 = indices[1];
   const int gt3 = indices[2];
   const auto Ye = MODELPARAMETER(Ye);
   const auto ZEL = MODELPARAMETER(ZEL);
   const auto ZER = MODELPARAMETER(ZER);
   const auto ZA = MODELPARAMETER(ZA);

   const std::complex<double> left = std::complex<double>(0.,-0.7071067811865475)*SUM(j2,0,2,Conj(ZEL(gt3,j2))*SUM(j1,0,2,Conj(ZER(gt1,j1))*Ye(j1,j2)))*ZA(gt2,0);

   const std::complex<double> right = std::complex<double>(0.,0.7071067811865475)*SUM(j2,0,2,SUM(j1,0,2,Conj(Ye(j1,j2))*ZER(gt3,j1))*ZEL(gt1,j2))*ZA(gt2,0);

   return vertex_type(left, right);
}

template<> inline
Vertex<Fe, Ah, bar<Fe>::type>::vertex_type
Vertex<Fe, Ah, bar<Fe>::type>::evaluate(const indices_type& indices, const EvaluationContext& context)
{
   const int gt1 = indices[0];
   const int gt2 = indices[1];
   const int gt3 = indices[2];
   const auto Ye = MODELPARAMETER(Ye);
   const auto ZEL = MODELPARAMETER(ZEL);
   const auto ZER = MODELPARAMETER(ZER);
   const auto ZA = MODELPARAMETER(ZA);

   const std::complex<double> left = std::complex<double>(0.,-0.7071067811865475)*SUM(j2,0,2,Conj(ZEL(gt1,j2))*SUM(j1,0,2,Conj(ZER(gt3,j1))*Ye(j1,j2)))*ZA(gt2,0);

   const std::complex<double> right = std::complex<double>(0.,0.7071067811865475)*SUM(j2,0,2,SUM(j1,0,2,Conj(Ye(j1,j2))*ZER(gt1,j1))*ZEL(gt3,j2))*ZA(gt2,0);

   return vertex_type(left, right);
}

template<> inline
Vertex<VP, bar<Fe>::type, Fe>::vertex_type
Vertex<VP, bar<Fe>::type, Fe>::evaluate(const indices_type& indices, const EvaluationContext& context)
{
   const int gt2 = indices[0];
   const int gt3 = indices[1];
   const auto g1 = MODELPARAMETER(g1);
   const auto g2 = MODELPARAMETER(g2);
   const auto ThetaW = DERIVEDPARAMETER(ThetaW);

   const std::complex<double> left = 0.5*KroneckerDelta(gt2,gt3)*(0.7745966692414834*g1*Cos(ThetaW) + g2*Sin(ThetaW));

   const std::complex<double> right = 0.7745966692414834*g1*Cos(ThetaW)*KroneckerDelta(gt2,gt3);

   return vertex_type(left, right);
}

template<> inline
Vertex<bar<Fe>::type, Chi, Se>::vertex_type
Vertex<bar<Fe>::type, Chi, Se>::evaluate(const indices_type& indices, const EvaluationContext& context)
{
   const int gt1 = indices[0];
   const int gt2 = indices[1];
   const int gt3 = indices[2];
   const auto g1 = MODELPARAMETER(g1);
   const auto g2 = MODELPARAMETER(g2);
   const auto Ye = MODELPARAMETER(Ye);
   const auto ZN = MODELPARAMETER(ZN);
   const auto ZE = MODELPARAMETER(ZE);
   const auto ZER = MODELPARAMETER(ZER);
   const auto ZEL = MODELPARAMETER(ZEL);

   const std::complex<double> left = -1.0954451150103321*g1*Conj(ZN(gt2,0))*SUM(j1,0,2,Conj(ZE(gt3,3 + j1))*Conj(ZER(gt1,j1))) - Conj(ZN(gt2,2))*SUM(j2,0,2,Conj(ZE(gt3,j2))*SUM(j1,0,2,Conj(ZER(gt1,j1))*Ye(j1,j2)));

   const std::complex<double> right = 0.7071067811865475*SUM(j1,0,2,Conj(ZE(gt3,j1))*ZEL(gt1,j1))*(0.7745966692414834*g1*ZN(gt2,0) + g2*ZN(gt2,1)) - SUM(j2,0,2,SUM(j1,0,2,Conj(Ye(j1,j2))*Conj(ZE(gt3,3 + j1)))*ZEL(gt1,j2))*ZN(gt2,2);

   return vertex_type(left, right);
}

template<> inline
Vertex<Fe, Chi, conj<Se>::type>::vertex_type
Vertex<Fe, Chi, conj<Se>::type>::evaluate(const indices_type& indices, const EvaluationContext& context)
{
   const int gt1 = indices[0];
   const int gt2 = indices[1];
   const int gt3 = indices[2];
   const auto g1 = MODELPARAMETER(g1);
   const auto g2 = MODELPARAMETER(g2);
   const auto Ye = MODELPARAMETER(Ye);
   const auto ZN = MODELPARAMETER(ZN);
   const auto ZEL = MODELPARAMETER(ZEL);
   const auto ZE = MODELPARAMETER(ZE);
   const auto ZER = MODELPARAMETER(ZER);

   const std::complex<double> left = 0.5477225575051661*g1*Conj(ZN(gt2,0))*SUM(j1,0,2,Conj(ZEL(gt1,j1))*ZE(gt3,j1)) + 0.7071067811865475*g2*Conj(ZN(gt2,1))*SUM(j1,0,2,Conj(ZEL(gt1,j1))*ZE(gt3,j1)) - Conj(ZN(gt2,2))*SUM(j2,0,2,Conj(ZEL(gt1,j2))*SUM(j1,0,2,Ye(j1,j2)*ZE(gt3,3 + j1)));

   const std::complex<double> right = -1.0954451150103321*g1*SUM(j1,0,2,ZE(gt3,3 + j1)*ZER(gt1,j1))*ZN(gt2,0) - SUM(j2,0,2,SUM(j1,0,2,Conj(Ye(j1,j2))*ZER(gt1,j1))*ZE(gt3,j2))*ZN(gt2,2);

   return vertex_type(left, right);
}

template<> inline
Vertex<VP, conj<Se>::type, Se>::vertex_type
Vertex<VP, conj<Se>::type, Se>::evaluate(const indices_type& indices, const EvaluationContext& context)
{
   const int gt2 = indices[0];
   const int gt3 = indices[1];
   const auto g1 = MODELPARAMETER(g1);
   const auto g2 = MODELPARAMETER(g2);
   const auto ZE = MODELPARAMETER(ZE);
   const auto ThetaW = DERIVEDPARAMETER(ThetaW);

   const std::complex<double> result = -0.5*(0.7745966692414834*g1*Cos(ThetaW) + g2*Sin(ThetaW))*SUM(j1,0,2,Conj(ZE(gt3,j1))*ZE(gt2,j1)) - 0.7745966692414834*g1*Cos(ThetaW)*SUM(j1,0,2,Conj(ZE(gt3,3 + j1))*ZE(gt2,3 + j1));

   return vertex_type(result);
}

template<> inline
Vertex<bar<Fe>::type, Fv, Hpm>::vertex_type
Vertex<bar<Fe>::type, Fv, Hpm>::evaluate(const indices_type& indices, const EvaluationContext& context)
{
   const int gt1 = indices[0];
   const int gt2 = indices[1];
   const int gt3 = indices[2];
   const auto Ye = MODELPARAMETER(Ye);
   const auto ZER = MODELPARAMETER(ZER);
   const auto ZP = MODELPARAMETER(ZP);

   const std::complex<double> left = SUM(j1,0,2,Conj(ZER(gt1,j1))*Ye(j1,gt2))*ZP(gt3,0);

   const std::complex<double> right = 0;

   return vertex_type(left, right);
}

template<> inline
Vertex<Fe, bar<Fv>::type, conj<Hpm>::type>::vertex_type
Vertex<Fe, bar<Fv>::type, conj<Hpm>::type>::evaluate(const indices_type& indices, const EvaluationContext& context)
{
   const int gt1 = indices[0];
   const int gt2 = indices[1];
   const int gt3 = indices[2];
   const auto Ye = MODELPARAMETER(Ye);
   const auto ZER = MODELPARAMETER(ZER);
   const auto ZP = MODELPARAMETER(ZP);

   const std::complex<double> left = 0;

   const std::complex<double> right = SUM(j1,0,2,Conj(Ye(j1,gt2))*ZER(gt1,j1))*ZP(gt3,0);

   return vertex_type(left, right);
}

template<> inline
Vertex<VP, conj<Hpm>::type, Hpm>::vertex_type
Vertex<VP, conj<Hpm>::type, Hpm>::evaluate(const indices_type& indices, const EvaluationContext& context)
{
   const int gt2 = indices[0];
   const int gt3 = indices[1];
   const auto g1 = MODELPARAMETER(g1);
   const auto g2 = MODELPARAMETER(g2);
   const auto ThetaW = DERIVEDPARAMETER(ThetaW);

   const std::complex<double> result = -0.5*KroneckerDelta(gt2,gt3)*(0.7745966692414834*g1*Cos(ThetaW) + g2*Sin(ThetaW));

   return vertex_type(result);
}

template<> inline
Vertex<bar<Fe>::type, hh, Fe>::vertex_type
Vertex<bar<Fe>::type, hh, Fe>::evaluate(const indices_type& indices, const EvaluationContext& context)
{
   const int gt1 = indices[0];
   const int gt2 = indices[1];
   const int gt3 = indices[2];
   const auto Ye = MODELPARAMETER(Ye);
   const auto ZEL = MODELPARAMETER(ZEL);
   const auto ZER = MODELPARAMETER(ZER);
   const auto ZH = MODELPARAMETER(ZH);

   const std::complex<double> left = -0.7071067811865475*SUM(j2,0,2,Conj(ZEL(gt3,j2))*SUM(j1,0,2,Conj(ZER(gt1,j1))*Ye(j1,j2)))*ZH(gt2,0);

   const std::complex<double> right = -0.7071067811865475*SUM(j2,0,2,SUM(j1,0,2,Conj(Ye(j1,j2))*ZER(gt3,j1))*ZEL(gt1,j2))*ZH(gt2,0);

   return vertex_type(left, right);
}

template<> inline
Vertex<Fe, hh, bar<Fe>::type>::vertex_type
Vertex<Fe, hh, bar<Fe>::type>::evaluate(const indices_type& indices, const EvaluationContext& context)
{
   const int gt1 = indices[0];
   const int gt2 = indices[1];
   const int gt3 = indices[2];
   const auto Ye = MODELPARAMETER(Ye);
   const auto ZEL = MODELPARAMETER(ZEL);
   const auto ZER = MODELPARAMETER(ZER);
   const auto ZH = MODELPARAMETER(ZH);

   const std::complex<double> left = -0.7071067811865475*SUM(j2,0,2,Conj(ZEL(gt1,j2))*SUM(j1,0,2,Conj(ZER(gt3,j1))*Ye(j1,j2)))*ZH(gt2,0);

   const std::complex<double> right = -0.7071067811865475*SUM(j2,0,2,SUM(j1,0,2,Conj(Ye(j1,j2))*ZER(gt1,j1))*ZEL(gt3,j2))*ZH(gt2,0);

   return vertex_type(left, right);
}

template<> inline
Vertex<bar<Fe>::type, Sv, Cha>::vertex_type
Vertex<bar<Fe>::type, Sv, Cha>::evaluate(const indices_type& indices, const EvaluationContext& context)
{
   const int gt1 = indices[0];
   const int gt2 = indices[1];
   const int gt3 = indices[2];
   const auto g2 = MODELPARAMETER(g2);
   const auto Ye = MODELPARAMETER(Ye);
   const auto UM = MODELPARAMETER(UM);
   const auto ZV = MODELPARAMETER(ZV);
   const auto ZER = MODELPARAMETER(ZER);
   const auto ZEL = MODELPARAMETER(ZEL);
   const auto UP = MODELPARAMETER(UP);

   const std::complex<double> left = Conj(UM(gt3,1))*SUM(j2,0,2,Conj(ZV(gt2,j2))*SUM(j1,0,2,Conj(ZER(gt1,j1))*Ye(j1,j2)));

   const std::complex<double> right = -(g2*SUM(j1,0,2,Conj(ZV(gt2,j1))*ZEL(gt1,j1))*UP(gt3,0));

   return vertex_type(left, right);
}

template<> inline
Vertex<Fe, conj<Sv>::type, bar<Cha>::type>::vertex_type
Vertex<Fe, conj<Sv>::type, bar<Cha>::type>::evaluate(const indices_type& indices, const EvaluationContext& context)
{
   const int gt1 = indices[0];
   const int gt2 = indices[1];
   const int gt3 = indices[2];
   const auto g2 = MODELPARAMETER(g2);
   const auto Ye = MODELPARAMETER(Ye);
   const auto UP = MODELPARAMETER(UP);
   const auto ZEL = MODELPARAMETER(ZEL);
   const auto ZV = MODELPARAMETER(ZV);
   const auto ZER = MODELPARAMETER(ZER);
   const auto UM = MODELPARAMETER(UM);

   const std::complex<double> left = -(g2*Conj(UP(gt3,0))*SUM(j1,0,2,Conj(ZEL(gt1,j1))*ZV(gt2,j1)));

   const std::complex<double> right = SUM(j2,0,2,SUM(j1,0,2,Conj(Ye(j1,j2))*ZER(gt1,j1))*ZV(gt2,j2))*UM(gt3,1);

   return vertex_type(left, right);
}

template<> inline
Vertex<VP, bar<Cha>::type, Cha>::vertex_type
Vertex<VP, bar<Cha>::type, Cha>::evaluate(const indices_type& indices, const EvaluationContext& context)
{
   const int gt2 = indices[0];
   const int gt3 = indices[1];
   const auto g2 = MODELPARAMETER(g2);
   const auto g1 = MODELPARAMETER(g1);
   const auto UM = MODELPARAMETER(UM);
   const auto UP = MODELPARAMETER(UP);
   const auto ThetaW = DERIVEDPARAMETER(ThetaW);

   const std::complex<double> left = g2*Conj(UM(gt3,0))*Sin(ThetaW)*UM(gt2,0) + 0.5*Conj(UM(gt3,1))*(0.7745966692414834*g1*Cos(ThetaW) + g2*Sin(ThetaW))*UM(gt2,1);

   const std::complex<double> right = g2*Conj(UP(gt2,0))*Sin(ThetaW)*UP(gt3,0) + 0.5*Conj(UP(gt2,1))*(0.7745966692414834*g1*Cos(ThetaW) + g2*Sin(ThetaW))*UP(gt3,1);

   return vertex_type(left, right);
}

template<> inline
double EvaluationContext::mass_impl<VG>(const std::array<int, 1>& indices) const
{ return model.get_MVG(); }

template<> inline
double EvaluationContext::mass_impl<Glu>(const std::array<int, 1>& indices) const
{ return model.get_MGlu(); }

template<> inline
double EvaluationContext::mass_impl<Fv>(const std::array<int, 1>& indices) const
{ return model.get_MFv(indices[0]); }

template<> inline
double EvaluationContext::mass_impl<VP>(const std::array<int, 0>& indices) const
{ return model.get_MVP(); }

template<> inline
double EvaluationContext::mass_impl<VZ>(const std::array<int, 0>& indices) const
{ return model.get_MVZ(); }

template<> inline
double EvaluationContext::mass_impl<Sd>(const std::array<int, 2>& indices) const
{ return model.get_MSd(indices[0]); }

template<> inline
double EvaluationContext::mass_impl<Sv>(const std::array<int, 1>& indices) const
{ return model.get_MSv(indices[0]); }

template<> inline
double EvaluationContext::mass_impl<Su>(const std::array<int, 2>& indices) const
{ return model.get_MSu(indices[0]); }

template<> inline
double EvaluationContext::mass_impl<Se>(const std::array<int, 1>& indices) const
{ return model.get_MSe(indices[0]); }

template<> inline
double EvaluationContext::mass_impl<hh>(const std::array<int, 1>& indices) const
{ return model.get_Mhh(indices[0]); }

template<> inline
double EvaluationContext::mass_impl<Ah>(const std::array<int, 1>& indices) const
{ return model.get_MAh(indices[0]); }

template<> inline
double EvaluationContext::mass_impl<Hpm>(const std::array<int, 1>& indices) const
{ return model.get_MHpm(indices[0]); }

template<> inline
double EvaluationContext::mass_impl<Chi>(const std::array<int, 1>& indices) const
{ return model.get_MChi(indices[0]); }

template<> inline
double EvaluationContext::mass_impl<Cha>(const std::array<int, 1>& indices) const
{ return model.get_MCha(indices[0]); }

template<> inline
double EvaluationContext::mass_impl<Fe>(const std::array<int, 1>& indices) const
{ return model.get_MFe(indices[0]); }

template<> inline
double EvaluationContext::mass_impl<Fd>(const std::array<int, 2>& indices) const
{ return model.get_MFd(indices[0]); }

template<> inline
double EvaluationContext::mass_impl<Fu>(const std::array<int, 2>& indices) const
{ return model.get_MFu(indices[0]); }

template<> inline
double EvaluationContext::mass_impl<VWm>(const std::array<int, 0>& indices) const
{ return model.get_MVWm(); }

namespace impl {
   static LeftAndRightComponentedVertex unit_charge(const EvaluationContext& context)
   {
      using vertex_type = LeftAndRightComponentedVertex;

      std::array<int, 1> electron_indices = { 0 };
      std::array<int, 0> photon_indices = {};
      std::array<int, 2> indices = concatenate(concatenate(photon_indices, electron_indices), electron_indices);

         const int gt2 = indices[0];
      const int gt3 = indices[1];
      const auto g1 = MODELPARAMETER(g1);
      const auto g2 = MODELPARAMETER(g2);
      const auto ThetaW = DERIVEDPARAMETER(ThetaW);

      const std::complex<double> left = -0.7745966692414834*g1*Cos(ThetaW)*KroneckerDelta(gt2,gt3);

      const std::complex<double> right = -0.5*KroneckerDelta(gt2,gt3)*(0.7745966692414834*g1*Cos(ThetaW) + g2*Sin(ThetaW));

      return vertex_type(left, right);
   }
}

static double unit_charge(const EvaluationContext& context)
{
   return impl::unit_charge(context).left().real() / Electron::electric_charge;
}

} // namespace cxx_diagrams
} // namespace flexiblesusy

#undef INPUTPARAMETER
#undef MODELPARAMETER
#undef DERIVEDPARAMETER
#undef PHASE

#endif


#include <boost/thread.hpp>
#include <boost/thread.hpp>
#include "caffe/layer.hpp"

namespace caffe {

template <typename Dtype>
void Layer<Dtype>::InitMutex() {
  forward_mutex_.reset(new boost::mutex());
}

template <typename Dtype>
void Layer<Dtype>::Lock() {
  if (IsShared()) {
    forward_mutex_->lock();
  }
}

template <typename Dtype>
void Layer<Dtype>::Unlock() {
  if (IsShared()) {
    forward_mutex_->unlock();
  }
}

/*
 * 聚类的目的是得到各个weight属于的中心以及这个中心的值
 */
template<typename Dtype>
void Layer<Dtype>::kmeans_cluster(vector<int> &cLabel, vector<Dtype> &cCentro, Dtype *cWeights, int nWeights, \
  vector<bool> &mask, int nCluster,  int max_iter){

  Dtype maxWeight = std::numeric_limits<Dtype>::min();
  Dtype minWeight = std::numeric_limits<Dtype>::max();

  //find min max
  for(int k = 0; k < nWeights; ++k){
    if(mask[k]){
      if(cWeights[k] > maxWeight)
        maxWeight = cWeights[k];
      if(cWeights[k] < minWeight)
        minWeight = cWeights[k];
    }
    cLabel[k] = -1; //initialize all label to -1
  }

  // generate initial centroids linearly
  for (int k = 0; k < nCluster; k++)
    cCentro[k] = minWeight + (maxWeight - minWeight) * k / (nCluster - 1);

  Dtype *ptrC = new Dtype[nCluster]; // 用来存各个中心的权重的和，和下面的相除得到各个中心新的值
  int *ptrS = new int[nCluster]; // 用来存各个中心的权重的个数
  int iter = 0;
  double mCurDistance = 0.0;
  double mPreDistance = std::numeric_limits<double>::max();
  Dtype distance, mindistance; // 临时变量，用于存每个weight到中心的距离和最短距离

  // clustering
  while (iter < max_iter)
  {
    // check convergence
    if (fabs(mPreDistance - mCurDistance) / mPreDistance < 0.01)  break;
    mPreDistance = mCurDistance; mCurDistance = 0.0;
    for (int n = 0; n < nWeights; n++){
      if (!mask[n])  continue; // 如果mask为0，则跳过这个值
      mindistance = std::numeric_limits<Dtype>::max();
      for (int k = 0; k < nCluster; k++){
        distance = fabs(cWeights[n] - cCentro[k]);
        if (distance < mindistance){
          mindistance = distance;
          cLabel[n] = k;
        }
      }
      mCurDistance = mCurDistance + mindistance;
    }

    // 更新中心，初始化为0
    for (int k = 0; k < nCluster; k++){
      ptrC[k] = 0.f;
      ptrS[k] = 0;
    }

    for (int n = 0; n < nWeights; n++){
      if (mask[n]){
        ptrC[cLabel[n]] += cWeights[n];
        ptrS[cLabel[n]] += 1;
      }
    }

    for (int k = 0; k < nCluster; k++)
      cCentro[k] = ptrC[k]/ptrS[k]; // 更新中心值

    iter++;
  }
  delete[] ptrC; delete[] ptrS;
}

  INSTANTIATE_CLASS(Layer);

}  // namespace caffe


/*
 * @Descripttion:
 * @version: 1.0版本
 * @Author: Frank.Wu
 * @Date: 2020-01-09 15:25:57
 * @LastEditors: Frank.Wu
 * @LastEditTime: 2020-11-01 10:36:33
 */
#ifdef _USE_PCL_
#include "LidarFeaturePoints.h"

#include "../LidarGeometry/Geometry.h"
#include "../LidarGeometry/GeometryAlgorithm.h"

#include <boost/thread/thread.hpp>
#include <pcl/range_image/range_image.h>
#include <pcl/features/range_image_border_extractor.h>
#include <pcl/registration/correspondence_rejection_sample_consensus.h>

#include <pcl/console/time.h>
#include <pcl/point_types.h>
#include <pcl/common/io.h>
#include <pcl/keypoints/sift_keypoint.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/keypoints/iss_3d.h>
#include<ceres/ceres.h>
using namespace ceres;
using namespace GeometryLas;

typedef pcl::PointXYZ PointType;
/**
 * @brief  重构一下取值函数，默认取值为去强度值，没有这个部分则报错：
           error: ‘const struct pcl::PointXYZ’ has no member named ‘intensity’
 * @note
 * @retval None
 */
namespace pcl
{
  template<>
    struct SIFTKeypointFieldSelector<PointXYZ>
    {
      inline float
      operator () (const PointXYZ &p) const
      {
    return p.z;
      }
    };
}

/**
 * @brief  代码运行过程会有内存泄露的问题，感觉应该是库的问题，但是不知道问题在哪里
            经过检查是RangeImage这段代码的问题，感觉跟内核有关，估计是虚拟机运行
            不支持，或者是指令集的问题
            2019-07-21:经过查看资料发现是PCL库编译的问题，存在Boost库与STD冲突的问题
            编译的时候添加C++11标准用Release编译就可以解决这个问题
            2019-12-26:一直提取特征Narf点数量为0，不知道问题出现在哪里,需要进一步研究
 * @note
 * @param  input_cloud:  输入点云
 * @param  &narfIndex:   特征点点云
 * @param  &narfDesc:    特征点描述
 * @retval
 */
long LidarFeaturePoints::LidarFeature_NARF(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud,pcl::PointCloud<int> &narfIndex,pcl::PointCloud<pcl::Narf36> &narfDesc)
{
    pcl::PointCloud<PointType>& point_cloud = *input_cloud;
    float angular_resolution = 0.5f;           ////angular_resolution为模拟的深度传感器的角度分辨率，即深度图像中一个像素对应的角度大小
    float support_size = 4.0f;                 //点云大小的设置
    pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;     //设置坐标系
    bool setUnseenToMaxRange = false;
    bool rotation_invariant = true;
    pcl::PointCloud<pcl::PointWithViewpoint> far_ranges;
    Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ());

    // -----------------------------------------------
	// -----Create RangeImage from the PointCloud-----
	// -----------------------------------------------
    float noise_level = 0.0;
    float min_range = 0.0f;
    int border_size = 1;
    boost::shared_ptr<pcl::RangeImage> range_image_ptr (new pcl::RangeImage());
    pcl::RangeImage& range_image = *range_image_ptr;
    range_image.createFromPointCloud (point_cloud, angular_resolution, pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
                                   scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
    range_image.integrateFarRanges (far_ranges);

    if (setUnseenToMaxRange)
        range_image.setUnseenToMaxRange ();

    /*提取特征点*/
    pcl::RangeImageBorderExtractor range_image_border_extractor;   //用来提取边缘
    pcl::NarfKeypoint narf_keypoint_detector;      //用来检测关键点
    narf_keypoint_detector.setRangeImageBorderExtractor (&range_image_border_extractor);   //
    narf_keypoint_detector.setRangeImage (&range_image);
    narf_keypoint_detector.getParameters ().support_size = support_size;    //设置NARF的参数
    narf_keypoint_detector.compute (narfIndex);

    /*提取特征描述*/
    std::vector<int> keypoint_indices2;
    keypoint_indices2.resize (narfIndex.points.size ());
    for (unsigned int i=0; i<narfIndex.size (); ++i) // This step is necessary to get the right vector type
        keypoint_indices2[i]=narfIndex.points[i];
    pcl::NarfDescriptor narf_descriptor (&range_image, &keypoint_indices2);
    narf_descriptor.getParameters ().support_size = support_size;
    narf_descriptor.getParameters ().rotation_invariant = rotation_invariant;
    narf_descriptor.compute (narfDesc);
    printf("feature points count:%d\n",narfIndex.size());
    return 0;
}

long LidarFeaturePoints::LidarFeature_Sift(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud,
                                           pcl::PointCloud<int> &siftPointIdx,
                                           pcl::PointCloud<pcl::FPFHSignature33>::Ptr siftFPFH)
{
    // Parameters for sift computation
    float min_scale = 2.0f;       //the standard deviation of the smallest scale in the scale space
    int n_octaves   = 8;           //the number of octaves (i.e. doublings of scale) to compute
    int n_scales_per_octave = 4;   //the number of scales to compute within each octave
    float min_contrast = 0.5f;    //the minimum contrast required for detection

    pcl::console::TicToc time;
    time.tic();

    // Estimate the sift interest points using z values from xyz as the Intensity variants
    //pcl::PointCloud<pcl::PointXYZ>::Ptr feauture_cloud
    pcl::SIFTKeypoint<pcl::PointXYZ, pcl::PointWithScale> sift;
    pcl::PointCloud<pcl::PointWithScale> result;
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ> ());

    sift.setSearchMethod(tree);
    sift.setScales(min_scale, n_octaves, n_scales_per_octave);
    sift.setMinimumContrast(min_contrast);
    sift.setInputCloud(input_cloud);
    sift.compute(result);

    //pcl::PointIndicesConstPtr keypoints_indices = sift.getKeypointsIndices();
    //计算点云特征
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>());
    pcl::NormalEstimation<pcl::PointXYZ,pcl::Normal> est_normal;
    est_normal.setInputCloud(input_cloud);
    est_normal.setSearchMethod(tree);
    est_normal.setKSearch(10);          //近邻10个点
    est_normal.compute(*normals);       //计算法线

    //创建FPFH估计对象fpfh，并把输入数据集cloud和法线normals传递给它。
    pcl::FPFHEstimation<pcl::PointXYZ,pcl::Normal,pcl::FPFHSignature33> fpfh;
    pcl::search::KdTree<pcl::PointXYZ>::Ptr treeFPFH(new pcl::search::KdTree<pcl::PointXYZ> ());
    fpfh.setInputCloud(input_cloud);
    fpfh.setInputNormals(normals);
    fpfh.setSearchMethod(treeFPFH);
    //fpfh.setIndices(keypoints_indices);
    pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs(new pcl::PointCloud<pcl::FPFHSignature33>());
    fpfh.setKSearch(10);        //近邻10个点
    fpfh.compute(*fpfhs);       //计算特征

    //判断SIFT点在原始点云中的index
    const int searchNum=1;
    std::vector<int> pointIdxNKNSearch(searchNum);
    std::vector<float> pointNKNSquaredDistance(searchNum);
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    kdtree.setInputCloud(input_cloud);
    for(auto resPoint : result.points)
    {
        pcl::PointXYZ searchPoint;
        searchPoint.x=resPoint.x;
        searchPoint.y=resPoint.y;
        searchPoint.z=resPoint.z;

        if (kdtree.nearestKSearch(searchPoint, searchNum, pointIdxNKNSearch, pointNKNSquaredDistance) > 0 )
        {
            siftPointIdx.push_back(pointIdxNKNSearch[0]);
            siftFPFH->push_back(fpfhs->points[pointIdxNKNSearch[0]]);
        }
    }

    std::cout << "Computing Points: "<<input_cloud->size()<<std::endl;
    std::cout << "Computing FPFH Features: "<<siftFPFH->size()<<std::endl;
    std::cout << "Computing the SIFT points takes "<<time.toc()/1000<<"seconds"<<std::endl;
    std::cout << "No. of SIFT points in the result are " << siftPointIdx.size () << std::endl;

/*
    std::cout << "Computing Points: "<<input_cloud->size()<<std::endl;
    std::cout << "Computing FPFH Features: "<<fpfhs->size()<<std::endl;
    std::cout << "Computing Normals: "<<normals->size()<<std::endl;
    std::cout << "Computing the SIFT points takes "<<time.toc()/1000<<"seconds"<<std::endl;
    std::cout << "No. of SIFT points in the result are " << result.points.size () << std::endl;
 */

    // Copying the pointwithscale to pointxyz so as visualize the cloud
    //copyPointCloud(result, *feauture_cloud);
    //std::cout << "SIFT points in the result are " << feauture_cloud->points.size () << std::endl;

#ifdef _DEBUG
    FILE *fs = fopen("../data/test/ptSIFT.txt","w+");
    if(fs!=nullptr)
    {
        for(int i=0;i<siftPointIdx.points.size ();++i)
            fprintf(fs,"%lf  %lf  %lf  %lf  %lf  %lf\n",input_cloud->points[siftPointIdx[i]].x,
                                        input_cloud->points[siftPointIdx[i]].y,
                                        input_cloud->points[siftPointIdx[i]].z,
                                        255.0f,255.0f,0.0f);
        fclose(fs);
    }else
    {
        printf("create test output failed!\n");
    }
#endif
    return 0;
}

long LidarFeaturePoints::LidarFeature_ISS(pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud,
                                          pcl::PointCloud<int> &siftPointIdx,
                                          pcl::PointCloud<pcl::FPFHSignature33>::Ptr siftFPFH)
{
    pcl::console::TicToc time;
    time.tic();
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_src_is(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::ISSKeypoint3D<pcl::PointXYZ, pcl::PointXYZ> iss_det;
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_1(new pcl::search::KdTree<pcl::PointXYZ>());

    double model_solution = 0.4;//参数小，采取的关键点少，论文中为500左右

    //参数设置
    iss_det.setSearchMethod(tree_1);
    iss_det.setSalientRadius(6*model_solution);//
    iss_det.setNonMaxRadius(4*model_solution);//
    iss_det.setThreshold21 (0.975);
    iss_det.setThreshold32 (0.975);
    iss_det.setMinNeighbors(5);
    iss_det.setNumberOfThreads(4);
    iss_det.setInputCloud(input_cloud);
    iss_det.compute(*cloud_src_is);

    clock_t end = clock();
       //pcl::PointIndicesConstPtr keypoints_indices = sift.getKeypointsIndices();
    //计算点云特征
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>());
    pcl::NormalEstimation<pcl::PointXYZ,pcl::Normal> est_normal;
    est_normal.setInputCloud(input_cloud);
    est_normal.setSearchMethod(tree_1);
    est_normal.setKSearch(10);          //近邻10个点
    est_normal.compute(*normals);       //计算法线

    //创建FPFH估计对象fpfh，并把输入数据集cloud和法线normals传递给它。
    pcl::FPFHEstimation<pcl::PointXYZ,pcl::Normal,pcl::FPFHSignature33> fpfh;
    pcl::search::KdTree<pcl::PointXYZ>::Ptr treeFPFH(new pcl::search::KdTree<pcl::PointXYZ> ());
    fpfh.setInputCloud(input_cloud);
    fpfh.setInputNormals(normals);
    fpfh.setSearchMethod(treeFPFH);
    //fpfh.setIndices(keypoints_indices);
    pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs(new pcl::PointCloud<pcl::FPFHSignature33>());
    fpfh.setKSearch(10);        //近邻10个点
    fpfh.compute(*fpfhs);       //计算特征

    //判断SIFT点在原始点云中的index
    const int searchNum=1;
    std::vector<int> pointIdxNKNSearch(searchNum);
    std::vector<float> pointNKNSquaredDistance(searchNum);
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    kdtree.setInputCloud(input_cloud);

    for(auto resPoint : cloud_src_is->points)
    {
        pcl::PointXYZ searchPoint;
        searchPoint.x = resPoint.x;
        searchPoint.y = resPoint.y;
        searchPoint.z = resPoint.z;

        if (kdtree.nearestKSearch(searchPoint, searchNum, pointIdxNKNSearch, pointNKNSquaredDistance) > 0 )
        {
            siftPointIdx.push_back(pointIdxNKNSearch[0]);
            siftFPFH->push_back(fpfhs->points[pointIdxNKNSearch[0]]);
        }
    }

    std::cout << "Computing Points: "<<input_cloud->size()<<std::endl;
    std::cout << "Computing ISS Features: "<<siftFPFH->size()<<std::endl;
    std::cout << "Computing the ISS points takes "<<time.toc()/1000<<"seconds"<<std::endl;
    std::cout << "No. of ISS points in the result are " << siftPointIdx.size () << std::endl;

#ifdef _DEBUG
    FILE *fs = fopen("../data/test/ptISS.txt","w+");
    if(fs!=nullptr)
    {
        for(int i=0;i<siftPointIdx.points.size ();++i)
            fprintf(fs,"%lf  %lf  %lf  %lf  %lf  %lf\n",input_cloud->points[siftPointIdx[i]].x,
                                        input_cloud->points[siftPointIdx[i]].y,
                                        input_cloud->points[siftPointIdx[i]].z,
                                        255.0f,255.0f,0.0f);
        fclose(fs);
    }else
    {
        printf("create test output failed!\n");
    }
#endif
    return 0;

}

// 直接通过kdtree查找比较快
// inline double disFpfh(pcl::FPFHSignature33 pt1,pcl::FPFHSignature33 pt2)
// {
//     double dis = 0;
//     for(int i=0;i<pt1.descriptorSize();++i)
//     {
//         dis+=(pt1.histogram[i]-pt2.histogram[i])*(pt1.histogram[i]-pt2.histogram[i]);
//     }
//     return sqrt(dis);
// }

long LidarFeatureRegistration::LidarRegistration_SiftFPFHMatch(pcl::PointCloud<pcl::FPFHSignature33>::Ptr siftFPFH1,
                                pcl::PointCloud<pcl::FPFHSignature33>::Ptr siftFPFH2,
                                std::vector<MATCHHISTRODIS> &matches)
{
    pcl::KdTreeFLANN<pcl::FPFHSignature33> kdtree;
    kdtree.setInputCloud(siftFPFH1);
    const int searchNum=1;
    std::vector<int> pointIdxNKNSearch(searchNum);
    std::vector<float> pointNKNSquaredDistance(searchNum);

    //特征匹配
    int num=0;
    for(auto fpfhTar:siftFPFH2->points)
    {
        if (kdtree.nearestKSearch(fpfhTar, searchNum, pointIdxNKNSearch, pointNKNSquaredDistance) > 0 )
        {
            MATCHHISTRODIS matchTmp;
            matchTmp.relation=-1;
            matchTmp.idx2 = num;
            matchTmp.idx1 = pointIdxNKNSearch[0];
            matches.push_back(matchTmp);
            num++;
        }
    }
}

long LidarFeatureRegistration::LidarRegistration_Match(pcl::PointCloud<int> idxList1,
                                 pcl::PointCloud<int> idxList2,
                                 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1,
                                 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2,
                                 int matchNum,
                                 std::vector<MATCHHISTRODIS> &matches)
{
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree1;
	kdtree1.setInputCloud(cloud1);
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree2;
	kdtree2.setInputCloud(cloud2);

    for(int i=0;i<idxList1.size();++i)
    {
        //find the most match point
        MATCHHISTRODIS matchTmp;
        matchTmp.relation=-1;
        matchTmp.idx1 = idxList1[i];
        matchTmp.idx2 = idxList2[0];
        for(int j=0;j<idxList2.size();++j)
        {
            pcl::PointXYZ pnt1=cloud1->points[idxList1[i]];
            pcl::PointXYZ pnt2=cloud2->points[idxList2[j]];
            double rel=LidarRegistration_CorrelationMatch(pnt1,pnt2,kdtree1,cloud1,kdtree2,cloud2,matchNum);
            matchTmp.idx2 = matchTmp.relation>rel?matchTmp.idx2:idxList2[j];
            matchTmp.relation=max(rel,matchTmp.relation);
        }
        if(matchTmp.relation<40)
        {
            // printf("%lf\n",matchTmp.relation);
            matches.push_back(matchTmp);
        }
    }
    printf("match point count is : %d\n",matches.size());
    return 0;
}

long LidarFeatureRegistration::LidarRegistration_RANSC(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1,
                             pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2,
                             pcl::PointCloud<int> siftPointIdx1,
                             pcl::PointCloud<int> siftPointIdx2,
                             int type,
                             std::vector<MATCHHISTRODIS> &matches)
{

    boost::shared_ptr<pcl::Correspondences> correspondence_all(new pcl::Correspondences);
    boost::shared_ptr<pcl::Correspondences> correspondence_inliner(new pcl::Correspondences);
    pcl::Correspondences correspondences;

    for(int i=0;i<matches.size();++i)
    {
        //output match point list
        int id1,id2;
        if(type==0)
        {

            if(siftPointIdx1[matches[i].idx1]>cloud1->points.size())
            {
                printf("%d\n",siftPointIdx1[matches[i].idx1]);
                continue;
            }

            if(siftPointIdx2[matches[i].idx2]>cloud2->points.size())
            {
                printf("%d\n",siftPointIdx2[matches[i].idx2]);
                continue;
            }
            id1 = siftPointIdx1[matches[i].idx1];
            id2 = siftPointIdx2[matches[i].idx2];
        }
        else if(type==1)
        {
            id1 = matches[i].idx1;
            id2 = matches[i].idx2;
        }

        pcl::Correspondence Correspondence;
		Correspondence.index_match = id2;   //目标点云
		Correspondence.index_query = id1;//源点云
        // printf("%d-%d\n",id2,id1);
		correspondences.push_back(Correspondence);
    }
    *correspondence_all = correspondences;

    printf("process ransc\n");
    pcl::registration::CorrespondenceRejectorSampleConsensus<pcl::PointXYZ> ransac;
	ransac.setInputSource(cloud1);
	ransac.setInputTarget(cloud2);
	ransac.setMaximumIterations(400);
	ransac.setInlierThreshold(40);
	ransac.getRemainingCorrespondences(*correspondence_all, *correspondence_inliner);

    matches.clear();
    for (int i = 0; i < correspondence_inliner->size(); i++)
	{
        MATCHHISTRODIS matchdis;
        matchdis.idx1 = correspondence_inliner->at(i).index_match;
        matchdis.idx2 = correspondence_inliner->at(i).index_query;
        matchdis.relation = -1;
        matches.push_back(matchdis);
    }
    return 0;
}

void LidarFeatureRegistration::LidarRegistration_OutputTest(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1,
                                 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2,
                                 pcl::PointCloud<int> siftPointIdx1,
                                 pcl::PointCloud<int> siftPointIdx2,
                                 int type,
                                 std::vector<MATCHHISTRODIS> matches)
{
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree1;
	kdtree1.setInputCloud(cloud1);
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree2;
	kdtree2.setInputCloud(cloud2);
    const int matchNum=20;
    for(int i=0;i<matches.size();++i)
    {
        //output match point list
        int id1,id2;
        if(type==0)
        {
            if(siftPointIdx1[matches[i].idx1]>cloud1->points.size())
            {
                printf("%d\n",siftPointIdx1[matches[i].idx1]);
                continue;
            }
            if(siftPointIdx2[matches[i].idx2]>cloud2->points.size())
            {
                printf("%d\n",siftPointIdx2[matches[i].idx2]);
                continue;
            }
            id1 = siftPointIdx1[matches[i].idx1];
            id2 = siftPointIdx2[matches[i].idx2];
        }
        else if(type==1)
        {
            id1 = matches[i].idx1;
            id2 = matches[i].idx2;
        }

        pcl::PointXYZ pnt1=cloud1->points[id1];
        pcl::PointXYZ pnt2=cloud2->points[id2];

        std::vector<int> pointIdxNKNSearch1(matchNum);
        std::vector<int> pointIdxNKNSearch2(matchNum);
        std::vector<float> pointNKNSquaredDistance(matchNum);

        kdtree1.nearestKSearch(pnt1, matchNum, pointIdxNKNSearch1, pointNKNSquaredDistance);
        kdtree2.nearestKSearch(pnt2, matchNum, pointIdxNKNSearch2, pointNKNSquaredDistance);

        std::string path="../data/"+to_string(i)+".txt";
        FILE* fs = fopen(path.c_str(),"w+");
        fprintf(fs,"%lf,%lf,%lf,%lf,%lf,%lf\n", pnt1.x,pnt1.y,pnt1.z,0,0,255.0);
        fprintf(fs,"%lf,%lf,%lf,%lf,%lf,%lf\n", pnt2.x,pnt2.y,pnt2.z,0,0,255.0);
        // for(int tIdx=0;tIdx<matchNum;++tIdx)
        // {
        //     fprintf(fs,"%lf,%lf,%lf\n", cloud1->points[pointIdxNKNSearch1[tIdx]].x,
        //                     cloud1->points[pointIdxNKNSearch1[tIdx]].y,
        //                     cloud1->points[pointIdxNKNSearch1[tIdx]].z);
        //     fprintf(fs,"%lf,%lf,%lf\n", cloud2->points[pointIdxNKNSearch2[tIdx]].x,
        //                     cloud2->points[pointIdxNKNSearch2[tIdx]].y,
        //                     cloud2->points[pointIdxNKNSearch1[tIdx]].z);
        // }
        fclose(fs);
    }
}

void LidarFeatureRegistration::LidarRegistration_Check(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1,
                                 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2,
                                 pcl::PointCloud<int> siftPointIdx1,
                                 pcl::PointCloud<int> siftPointIdx2,
                                 int type,
                                 std::vector<MATCHHISTRODIS> matches,
                                 GeometryLas::Point3D pntCenter,
                                 double *r_t)
{
    double totalRMS=0;
    for(int i=0;i<matches.size();++i)
    {
        //output match point list
        int id1,id2;
        if(type==0)
        {
            if(siftPointIdx1[matches[i].idx1]>cloud1->points.size())
            {
                printf("%d\n",siftPointIdx1[matches[i].idx1]);
                continue;
            }

            if(siftPointIdx2[matches[i].idx2]>cloud2->points.size())
            {
                printf("%d\n",siftPointIdx2[matches[i].idx2]);
                continue;
            }
            id1 = siftPointIdx1[matches[i].idx1];
            id2 = siftPointIdx2[matches[i].idx2];
        }
        else if(type==1)
        {
            id1 = matches[i].idx1;
            id2 = matches[i].idx2;
        }

        pcl::PointXYZ pnt1=cloud1->points[id1];
        pcl::PointXYZ pnt2=cloud2->points[id2];

        double x1 = pnt1.x-pntCenter.x,y1 = pnt1.y-pntCenter.y,z1=pnt1.z-pntCenter.z;
        double x2 = pnt2.x,y2 = pnt2.y,z2=pnt2.z;
        // Debug
        // printf("center x: %lf y: %lf z: %lf\n",pntCenter.x,pntCenter.y,pntCenter.z);

        MatrixXd rotMat = MatrixXd::Identity(3,3);
        rotMat(0,0) = cos(r_t[1])*cos(r_t[2]);
        rotMat(0,1) = sin(r_t[0])*sin(r_t[1])*cos(r_t[2]) - cos(r_t[0])*sin(r_t[2]);
        rotMat(0,2) = cos(r_t[0])*sin(r_t[1])*cos(r_t[2]) + sin(r_t[0])*sin(r_t[2]);
        rotMat(1,0) = cos(r_t[1])*sin(r_t[2]);
        rotMat(1,1) = sin(r_t[0])*sin(r_t[1])*sin(r_t[2]) + cos(r_t[0])*cos(r_t[2]);
        rotMat(1,2) = cos(r_t[0])*sin(r_t[1])*sin(r_t[2]) - sin(r_t[0])*cos(r_t[2]);
        rotMat(2,0) = -sin(r_t[1]);
        rotMat(2,1) = sin(r_t[0])*cos(r_t[1]);
        rotMat(2,2) = cos(r_t[0])*cos(r_t[1]);

        MatrixXd ptMat(3,1);
        ptMat(0,0) = x1;
        ptMat(1,0) = y1;
        ptMat(2,0) = z1;
        MatrixXd transMat=rotMat*ptMat;

        double res1=((transMat(0,0)-r_t[3]-x2+pntCenter.x)*(transMat(0,0)-r_t[3]-x2+pntCenter.x));
        double res2=((transMat(1,0)-r_t[4]-y2+pntCenter.y)*(transMat(1,0)-r_t[4]-y2+pntCenter.y));
        double res3=((transMat(2,0)-r_t[5]-z2+pntCenter.z)*(transMat(2,0)-r_t[5]-z2+pntCenter.z));
        printf("RMS of x=%lf,y=%lf,z=%lf,total=%lf\n",res1,res2,res3,sqrt(res1+res2+res3)/3);
        totalRMS+=sqrt(res1+res2+res3)/3;
    }

    printf("total RMS error:%lf\n",totalRMS/matches.size());
}

double LidarFeatureRegistration::LidarRegistration_CorrelationMatch(pcl::PointXYZ pnt1,pcl::PointXYZ pnt2,
                                                                pcl::KdTreeFLANN<pcl::PointXYZ> kdtree1,
                                                                pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1,
                                                                pcl::KdTreeFLANN<pcl::PointXYZ> kdtree2,
                                                                pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2,
                                                                int nearPointsNum)
{
    std::vector<int> pointIdxNKNSearch1(nearPointsNum);
    std::vector<int> pointIdxNKNSearch2(nearPointsNum);
    std::vector<float> pointNKNSquaredDistance(nearPointsNum);

    kdtree1.nearestKSearch(pnt1, nearPointsNum, pointIdxNKNSearch1, pointNKNSquaredDistance);
    kdtree2.nearestKSearch(pnt2, nearPointsNum, pointIdxNKNSearch2, pointNKNSquaredDistance);

    double *distro1 = new double[nearPointsNum];
    double *distro2 = new double[nearPointsNum];

    // 获取距离直方图
    LidarRegistration_DisHistro(pointIdxNKNSearch1[0],pointIdxNKNSearch1,cloud1,distro1);
    LidarRegistration_DisHistro(pointIdxNKNSearch2[0],pointIdxNKNSearch2,cloud2,distro2);

    //距离直方图叠加角度直方图
    // LidarRegistration_DisAngleHistro(pointIdxNKNSearch1[0],pointIdxNKNSearch1,cloud1,distro1);
    // LidarRegistration_DisAngleHistro(pointIdxNKNSearch2[0],pointIdxNKNSearch2,cloud2,distro2);
    double colRel=LidarRegistration_Correlation(distro1,distro2,nearPointsNum);

    delete []distro1;distro1=nullptr;
    delete []distro2;distro1=nullptr;
    return colRel;

    // std::vector<int> match;
    // double matchRatio=0;
    // for(int i=0;i<nearPointsNum;++i)
    // {
    //     LidarRegistration_DisHistro(pointIdxNKNSearch1[i],pointIdxNKNSearch1,cloud1,distro1);
    //     double maxRel = -99999;
    //     match.push_back(i);
    //     int maxInd=0;
    //     for(int j=0;j<nearPointsNum;++j)
    //     {
    //         LidarRegistration_DisHistro(pointIdxNKNSearch2[j],pointIdxNKNSearch2,cloud2,distro2);
    //         double colRel=LidarRegistration_Correlation(distro1,distro2,nearPointsNum);
    //         maxInd = maxRel>colRel?maxInd:j;
    //         maxRel=max(maxRel,colRel);
    //     }
    //     match.push_back(maxInd);
    //     matchRatio+=maxRel;
    // }
    // // printf("%lf\n",matchRatio);
    // delete []distro1;distro1=nullptr;
    // delete []distro2;distro1=nullptr;
    // return matchRatio/nearPointsNum;
}

double LidarFeatureRegistration::LidarRegistration_Correlation(double* data1,double *data2,int num)
{
    double sumA(0.0), sumB(0.0), aveA(0.0), aveB(0.0);

	//求和
	sumA = std::accumulate(data1,data1+num, 0.0);
	sumB = std::accumulate(data2,data2+num, 0.0);

	//求平均值
	aveA = sumA / double(num);
	aveB = sumB / double(num);

	//计算相关系数
	double R1(0), R2(0), R3(0);
	for (long i = 0; i < num; i++)
	{
		R1 += (data1[i] - aveA) * (data2[i] - aveB);
		R2 += pow((data1[i] - aveA), 2);
		R3 += pow((data2[i] - aveB), 2);
	}

    //计算距离
    double dis=0;
    for(long i=0;i<num;++i)
    {
        dis+=(data1[i]-data2[i])*(data1[i]-data2[i]);
    }
    dis=sqrt(dis/num);
    double ratio=1.0;

	return (/*R1 / sqrt(R2*R3)*/1/dis*ratio);
}

long LidarFeatureRegistration::LidarRegistration_DisHistro(int idxPnt,vector<int> idxPntAllNear,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1,double *disHistro)
{
    memset(disHistro,sizeof(double)*idxPntAllNear.size(),0);
    Point3D pt2(cloud1->points[idxPnt].x,cloud1->points[idxPnt].y,cloud1->points[idxPnt].z);
    for(int i=0;i<idxPntAllNear.size();++i)
    {
        Point3D pt1(cloud1->points[idxPntAllNear[i]].x,cloud1->points[idxPntAllNear[i]].y,cloud1->points[idxPntAllNear[i]].z);
        disHistro[i] = DistanceComputation::Distance(pt1,pt2);
    }
    std::sort(disHistro,disHistro+idxPntAllNear.size());
    return 0;
}

long LidarFeatureRegistration::LidarRegistration_DisAngleHistro(int idxPnt,vector<int> idxPntAllNear,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1,double *disHistro)
{
    double *histro = new double[idxPntAllNear.size()];
    memset(histro,sizeof(double)*idxPntAllNear.size(),0);
    memset(disHistro,sizeof(double)*idxPntAllNear.size(),0);
    Point3D pt2(cloud1->points[idxPnt].x,cloud1->points[idxPnt].y,cloud1->points[idxPnt].z);
    for(int i=0;i<idxPntAllNear.size();++i)
    {
        Point3D pt1(cloud1->points[idxPntAllNear[i]].x,cloud1->points[idxPntAllNear[i]].y,cloud1->points[idxPntAllNear[i]].z);
        histro[i] = DistanceComputation::Distance(pt1,pt2);
    }

    vector<int> idx(idxPntAllNear.size());
    for (int i = 0; i < idxPntAllNear.size(); i++)idx[i] = i;
    sort(histro,histro+idxPntAllNear.size());
    sort(idx.begin(),idx.end(),[histro](int i1, int i2) {return histro[i1] < histro[i2]; });

    double vx1=cloud1->points[idx[0]].x-cloud1->points[idxPnt].x;
    double vy1=cloud1->points[idx[0]].y-cloud1->points[idxPnt].y;
    double vz1=cloud1->points[idx[0]].z-cloud1->points[idxPnt].z;

    double down1 = vx1*vx1+vy1*vy1+vz1*vz1;
    disHistro[0] = histro[0];
    for (int i = 1; i < idxPntAllNear.size(); i++)
    {

        double vx2=cloud1->points[idx[i]].x-cloud1->points[idxPnt].x;
        double vy2=cloud1->points[idx[i]].y-cloud1->points[idxPnt].y;
        double vz2=cloud1->points[idx[i]].z-cloud1->points[idxPnt].y;

        double up = vx1*vx2+vy1*vy2+vz1*vz2;
        double down2 = vx2*vx2+vy2*vy2+vz2*vz2;
        disHistro[i] = histro[i]*up/sqrt(down1)/sqrt(down2);
        // angleHistro[i-1]=up/sqrt(down1)*sqrt(down2);
    }
    delete []histro;histro=nullptr;
}

#ifdef _USE_CERES_
/**
 * @name: ceres 自动求导,注意，各个库采用的编译器类型要一致，
 *        PCL采用C++14 则ceres也采用c++14，否则在link的时候会出问题
 * @msg:
 * @return:
 */
struct CostFunctorRotTrans{

	CostFunctorRotTrans(double x1,double y1,double z1,double x2,double y2,double z2)
                        :m_x1(x1), m_y1(y1), m_z1(z1), m_x2(x2), m_y2(y2), m_z2(z2)
    {}

	template <typename T>
    bool operator()(const T* r_t, T* residual) const
    {
        T a11 = cos(r_t[1])*cos(r_t[2]);
        T a12 = sin(r_t[0])*sin(r_t[1])*cos(r_t[2]) - cos(r_t[0])*sin(r_t[2]);
        T a13 = cos(r_t[0])*sin(r_t[1])*cos(r_t[2]) + sin(r_t[0])*sin(r_t[2]);
        T b11 = cos(r_t[1])*sin(r_t[2]);
        T b12 = sin(r_t[0])*sin(r_t[1])*sin(r_t[2]) + cos(r_t[0])*cos(r_t[2]);
        T b13 = cos(r_t[0])*sin(r_t[1])*sin(r_t[2]) - sin(r_t[0])*cos(r_t[2]);
        T c11 = -sin(r_t[1]);
        T c12 = sin(r_t[0])*cos(r_t[1]);
        T c13 = cos(r_t[0])*cos(r_t[1]);

        residual[0]=(a11*m_x1+a12*m_y1+a13*m_z1+r_t[3]-m_x2)*
                    (a11*m_x1+a12*m_y1+a13*m_z1+r_t[3]-m_x2);
        residual[1]=(b11*m_x1+b12*m_y1+b13*m_z1+r_t[4]-m_y2)*
                    (b11*m_x1+b12*m_y1+b13*m_z1+r_t[4]-m_y2);
        residual[2]=(c11*m_x1+c12*m_y1+c13*m_z1+r_t[5]-m_z2)*
                    (c11*m_x1+c12*m_y1+c13*m_z1+r_t[5]-m_z2);

        return true;
	}

	// static ceres::CostFunction* Create(const double observed_depth) {
	// 	return (new ceres::AutoDiffCostFunction<CostFunctorRotTrans, 6, 3>(
	// 		new CostFunctorRotTrans(m_obs,m_match)));
	// }

    const double m_x1;
    const double m_y1;
    const double m_z1;
    const double m_x2;
    const double m_y2;
    const double m_z2;
};

/**
 * @name: 获取数值中点
 * @param {type}
 * @return:
 */
static Point3D GetMaxMinCenterPoint(pcl::PointCloud<pcl::PointXYZ>::Ptr pts_cloud)
{
    Point3D ptMax(-999999999,-999999999,-999999999);
    Point3D ptMin(999999999,999999999,999999999);

    for (int i = 0; i < pts_cloud->points.size(); ++i)
    {
        ptMax.x=max(double(pts_cloud->points[i].x),ptMax.x);
        ptMax.y=max(double(pts_cloud->points[i].y),ptMax.y);
        ptMax.z=max(double(pts_cloud->points[i].z),ptMax.z);

        ptMin.x=min(double(pts_cloud->points[i].x),ptMin.x);
        ptMin.y=min(double(pts_cloud->points[i].y),ptMin.y);
        ptMin.z=min(double(pts_cloud->points[i].z),ptMin.z);
    }

    return Point3D((ptMax.x+ptMin.x)/2,(ptMax.y+ptMin.y)/2,(ptMax.z+ptMin.z)/2);
}

//TODO:
//功能是做完了，最后也能够收敛，但是计算结果的残差太大，而且在匹配过程中可能存在误匹配的店
//第一步是需要处理误匹配的店，然后再找到减小残差的办法
long LidarFeatureRegistration::LidarRegistration_RotTrans(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1,
                                                          pcl::PointCloud<int> ptSiftIdx1,
                                                          pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2,
                                                          pcl::PointCloud<int> ptSiftIdx2,
                                                          pcl::PointCloud<int> siftMatchPointIdx,
                                                          double *r_t)
{
    Problem problem;
    for(int i=0;i<6;++i)
        r_t[i]=0;
    Point3D ptCent1 = GetMaxMinCenterPoint(cloud1);
    Point3D ptCent2 = GetMaxMinCenterPoint(cloud2);

    double *obs=new double[3*siftMatchPointIdx.points.size()];
    double *match=new double[3*siftMatchPointIdx.points.size()];

    for (int i = 0; i < siftMatchPointIdx.points.size(); ++i)
    {
        const int idx1 = ptSiftIdx2.points[i];
        const int idx2 = ptSiftIdx1.points[siftMatchPointIdx.points[i]];

        obs[i*3+0]=cloud1->points[idx1].x-ptCent1.x;
        obs[i*3+1]=cloud1->points[idx1].y-ptCent1.y;
        obs[i*3+2]=cloud1->points[idx1].z-ptCent1.z;

        match[i*3+0]=cloud2->points[idx2].x-ptCent1.x;
        match[i*3+1]=cloud2->points[idx2].y-ptCent1.y;
        match[i*3+2]=cloud2->points[idx2].z-ptCent1.z;
    }

    for (int i = 0; i < siftMatchPointIdx.points.size(); ++i)
    {
        CostFunction* cost_function =
            new AutoDiffCostFunction<CostFunctorRotTrans, 3,6>(
                new CostFunctorRotTrans(obs[i+3+0],obs[i+3+1],obs[i+3+2],match[i*3+0],match[i*3+1],match[i*3+2]));
        problem.AddResidualBlock(cost_function, nullptr, r_t);
    }

    Solver::Options options;
    options.linear_solver_type=ceres::DENSE_QR;
    options.minimizer_progress_to_stdout=true;
    options.max_num_iterations = 500;
    Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);

    delete obs;  obs=nullptr;
    delete match;match=nullptr;

    return 0;
}
#endif

#endif