/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2017, 2018 Klaus Spanderen

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program 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 license for more details.
*/

/*! \file andreasenhugelocalvolatility.cpp
*/

#include <ql/timegrid.hpp>
#include <ql/exercise.hpp>
#include <ql/utilities/null.hpp>
#include <ql/math/array.hpp>
#include <ql/math/functional.hpp>
#include <ql/math/comparison.hpp>
#include <ql/math/interpolations/cubicinterpolation.hpp>
#include <ql/math/interpolations/backwardflatinterpolation.hpp>
#include <ql/instruments/vanillaoption.hpp>
#include <ql/pricingengines/blackcalculator.hpp>
#include <ql/methods/finitedifferences/tridiagonaloperator.hpp>
#include <ql/methods/finitedifferences/meshers/fdmmeshercomposite.hpp>
#include <ql/methods/finitedifferences/meshers/concentrating1dmesher.hpp>
#include <ql/methods/finitedifferences/operators/firstderivativeop.hpp>
#include <ql/methods/finitedifferences/operators/fdmlinearoplayout.hpp>
#include <ql/methods/finitedifferences/operators/secondderivativeop.hpp>
#include <ql/termstructures/yieldtermstructure.hpp>
#include <ql/termstructures/volatility/equityfx/andreasenhugevolatilityinterpl.hpp>

#include <boost/math/special_functions/fpclassify.hpp>

#include <limits>

namespace QuantLib {

    class AndreasenHugeCostFunction : public CostFunction {
      public:
        AndreasenHugeCostFunction(
            const Array& marketNPVs,
            const Array& marketVegas,
            const Array& lnMarketStrikes,
            const Array& previousNPVs,
            const boost::shared_ptr<FdmMesherComposite> mesher,
            Time dT,
            AndreasenHugeVolatilityInterpl::InterpolationType interpolationType)
        : marketNPVs_(marketNPVs),
          marketVegas_(marketVegas),
          lnMarketStrikes_(lnMarketStrikes),
          previousNPVs_(previousNPVs),
          mesher_(mesher),
          nGridPoints_(mesher->layout()->size()),
          dT_(dT),
          interpolationType_(
              (lnMarketStrikes_.size() > 1)
                  ? interpolationType
                  : AndreasenHugeVolatilityInterpl::PiecewiseConstant),
          dxMap_ (FirstDerivativeOp(0, mesher_)),
          dxxMap_(SecondDerivativeOp(0, mesher_)),
          d2CdK2_(dxMap_.mult(Array(mesher->layout()->size(), -1.0))
                        .add(dxxMap_)),
          mapT_  (0, mesher_) {
        }

        Disposable<Array> d2CdK2(const Array& c) const {
            return d2CdK2_.apply(c);
        }

        Disposable<Array> solveFor(
            Time dT, const Array& sig, const Array& b) const {

            Array x(lnMarketStrikes_.size());
            Interpolation sigInterpl;

            switch (interpolationType_) {
              case AndreasenHugeVolatilityInterpl::CubicSpline:
                sigInterpl = CubicNaturalSpline(
                    lnMarketStrikes_.begin(), lnMarketStrikes_.end(),
                    sig.begin());
                break;
              case AndreasenHugeVolatilityInterpl::Linear:
                sigInterpl = LinearInterpolation(
                    lnMarketStrikes_.begin(), lnMarketStrikes_.end(),
                    sig.begin());
                break;
              case AndreasenHugeVolatilityInterpl::PiecewiseConstant:
                for (Size i=0; i < x.size()-1; ++i)
                    x[i] = 0.5*(lnMarketStrikes_[i] + lnMarketStrikes_[i+1]);
                x.back() = lnMarketStrikes_.back();

                sigInterpl = BackwardFlatInterpolation(
                    x.begin(), x.end(), sig.begin());
                break;
              default:
                QL_FAIL("unknown interpolation type");
            }

            const boost::shared_ptr<FdmLinearOpLayout> layout =
                mesher_->layout();
            const FdmLinearOpIterator endIter = layout->end();

            Array z(layout->size());
            for (FdmLinearOpIterator iter = layout->begin();
                 iter!=endIter; ++iter) {
                const Size i = iter.index();
                const Real lnStrike = mesher_->location(iter, 0);

                const Real vol = sigInterpl(
                    std::min(std::max(lnStrike, lnMarketStrikes_.front()),
                            lnMarketStrikes_.back()), true);

                z[i] = 0.5*vol*vol;
            }

            mapT_.axpyb(z, dxMap_, dxxMap_.mult(-z), Array());
            return mapT_.mult(Array(z.size(), dT)).solve_splitting(b, 1.0);

        }

        Disposable<Array> apply(const Array& c) const {
            return -mapT_.apply(c);
        }

        Disposable<Array> values(const Array& sig) const {
            Array newNPVs = solveFor(dT_, sig, previousNPVs_);

            const std::vector<Real>& gridPoints =
                mesher_->getFdm1dMeshers().front()->locations();

            const MonotonicCubicNaturalSpline interpl(
                gridPoints.begin(), gridPoints.end(), newNPVs.begin());

            Array retVal(lnMarketStrikes_.size());
            for (Size i=0; i < retVal.size(); ++i) {
                const Real strike = lnMarketStrikes_[i];
                retVal[i] = interpl(strike) - marketNPVs_[i];
            }
            return retVal;
        }

        Disposable<Array> vegaCalibrationError(const Array& sig) const {
            return values(sig)/marketVegas_;
        }

        Disposable<Array> initialValues() const {
            Array retVal(lnMarketStrikes_.size(), 0.25);
            return retVal;
        }

      private:
        const Array marketNPVs_, marketVegas_;
        const Array lnMarketStrikes_, previousNPVs_;
        const boost::shared_ptr<FdmMesherComposite> mesher_;
        const Size nGridPoints_;
        const Time dT_;
        const AndreasenHugeVolatilityInterpl::InterpolationType
            interpolationType_;

        const FirstDerivativeOp  dxMap_;
        const TripleBandLinearOp dxxMap_;
        const TripleBandLinearOp d2CdK2_;
        mutable TripleBandLinearOp mapT_;
    };

    class CombinedCostFunction : public CostFunction {
      public:
        CombinedCostFunction(
            const boost::shared_ptr<AndreasenHugeCostFunction>& putCostFct,
            const boost::shared_ptr<AndreasenHugeCostFunction>& callCostFct)
      : putCostFct_(putCostFct),
        callCostFct_(callCostFct) { }

        Disposable<Array> values(const Array& sig) const {
            if (putCostFct_ && callCostFct_) {
                const Array pv = putCostFct_->values(sig);
                const Array cv = callCostFct_->values(sig);

                Array retVal(pv.size() + cv.size());
                std::copy(pv.begin(), pv.end(), retVal.begin());
                std::copy(cv.begin(), cv.end(), retVal.begin() + cv.size());

                return retVal;
            }
            else if (putCostFct_)
                return putCostFct_->values(sig);
            else if (callCostFct_)
                return callCostFct_->values(sig);
            else
                QL_FAIL("internal error: cost function not set");
        }

        Disposable<Array> initialValues() const {
            if (putCostFct_ && callCostFct_)
                return 0.5*(  putCostFct_->initialValues()
                            + callCostFct_->initialValues());
            else if (putCostFct_)
                return putCostFct_->initialValues();
            else if (callCostFct_)
                return callCostFct_->initialValues();
            else
                QL_FAIL("internal error: cost function not set");
        }

      private:
        const boost::shared_ptr<AndreasenHugeCostFunction> putCostFct_;
        const boost::shared_ptr<AndreasenHugeCostFunction> callCostFct_;
    };

    AndreasenHugeVolatilityInterpl::AndreasenHugeVolatilityInterpl(
        const CalibrationSet& calibrationSet,
        const Handle<Quote>& spot,
        const Handle<YieldTermStructure>& rTS,
        const Handle<YieldTermStructure>& qTS,
        InterpolationType interplationType,
        CalibrationType calibrationType,
        Size nGridPoints,
        Real _minStrike,
        Real _maxStrike,
        const boost::shared_ptr<OptimizationMethod>& optimizationMethod,
        const EndCriteria& endCriteria)
    : spot_(spot),
      rTS_(rTS),
      qTS_(qTS),
      interpolationType_(interplationType),
      calibrationType_(calibrationType),
      nGridPoints_(nGridPoints),
      minStrike_(_minStrike),
      maxStrike_(_maxStrike),
      optimizationMethod_(optimizationMethod),
      endCriteria_(endCriteria) {
        QL_REQUIRE(nGridPoints > 2 && calibrationSet.size() > 0,
                "undefined grid or calibration set");

        std::set<Real> strikes;
        std::set<Date> expiries;

        calibrationSet_.reserve(calibrationSet.size());
        for (Size i=0; i < calibrationSet.size(); ++i) {

            const boost::shared_ptr<Exercise> exercise =
                calibrationSet[i].first->exercise();

            QL_REQUIRE(exercise->type() == Exercise::European,
                    "European option required");

            const Date expiry = exercise->lastDate();
            expiries.insert(expiry);

            const boost::shared_ptr<PlainVanillaPayoff> payoff =
                boost::dynamic_pointer_cast<PlainVanillaPayoff>(
                    calibrationSet[i].first->payoff());

            QL_REQUIRE(payoff, "plain vanilla payoff required");

            const Real strike = payoff->strike();
            strikes.insert(strike);

            calibrationSet_.push_back(
                std::make_pair(
                    boost::make_shared<VanillaOption>(payoff, exercise),
                    calibrationSet[i].second)
            );

            registerWith(calibrationSet[i].second);
        }

        strikes_.assign(strikes.begin(), strikes.end());
        expiries_.assign(expiries.begin(), expiries.end());

        dT_.resize(expiries_.size());
        expiryTimes_.resize(expiries_.size());

        calibrationMatrix_ = std::vector< std::vector<Size> >(
            expiries.size(), std::vector<Size>(strikes.size(), Null<Size>()));

        for (Size i=0; i < calibrationSet.size(); ++i) {
            const Date expiry =
                calibrationSet[i].first->exercise()->lastDate();

            const Size l = std::distance(expiries.begin(),
                std::lower_bound(expiries.begin(), expiries.end(), expiry));

            const Real strike =
                boost::dynamic_pointer_cast<PlainVanillaPayoff>(
                    calibrationSet[i].first->payoff())->strike();

            const Size k = std::distance(strikes_.begin(),
                std::find_if(strikes_.begin(), strikes_.end(),
                    std::bind2nd(std::ptr_fun(close_enough), strike)));

            calibrationMatrix_[l][k] = i;
        }

        registerWith(spot_);
        registerWith(rTS_);
        registerWith(qTS_);
    }

    boost::shared_ptr<AndreasenHugeCostFunction>
        AndreasenHugeVolatilityInterpl::buildCostFunction(
        Size iExpiry, Option::Type optionType,
        const Array& previousNPVs) const {

        if (calibrationType_ != CallPut
            && (   (calibrationType_ == Call && optionType ==Option::Put)
                || (calibrationType_ == Put  && optionType ==Option::Call)))
            return boost::shared_ptr<AndreasenHugeCostFunction>();

        const Time expiryTime = expiryTimes_[iExpiry];

        const DiscountFactor discount = rTS_->discount(expiryTime);
        const Real fwd = spot_->value()*qTS_->discount(expiryTime)/discount;

        const Size nOptions = std::count_if(
            calibrationMatrix_[iExpiry].begin(),
            calibrationMatrix_[iExpiry].end(),
            std::bind2nd(std::not_equal_to<Size>(), Null<Size>()));

        Array lnMarketStrikes(nOptions),
            marketNPVs(nOptions), marketVegas(nOptions);

        // calculate undiscounted market prices
        for (Size j=0, k=0; j < strikes_.size(); ++j) {
            const Size idx = calibrationMatrix_[iExpiry][j];

            if (idx != Null<Size>()) {

                const Volatility vol = calibrationSet_[idx].second->value();
                const Real stdDev = vol*std::sqrt(expiryTime);

                const BlackCalculator calculator(
                    optionType, strikes_[j], fwd, stdDev, discount);

                const Real npv = calculator.value();
                const Real vega = calculator.vega(expiryTime);

                marketNPVs[k] = npv/(discount*fwd);
                marketVegas[k] = vega/(discount*fwd);
                lnMarketStrikes[k++] = std::log(strikes_[j]/fwd);
            }
        }

        return boost::make_shared<AndreasenHugeCostFunction>(
            marketNPVs,
            marketVegas,
            lnMarketStrikes,
            previousNPVs,
            mesher_,
            dT_[iExpiry],
            interpolationType_);
    }

    void AndreasenHugeVolatilityInterpl::performCalculations() const {
        QL_REQUIRE(maxStrike() > minStrike(),
            "max strike must be greater than min strike");

        const DayCounter dc = rTS_->dayCounter();
        for (Size i=0; i < expiryTimes_.size(); ++i) {
            expiryTimes_[i] =
                dc.yearFraction(rTS_->referenceDate(), expiries_[i]);
            dT_[i] = expiryTimes_[i] - ( (i==0)? 0.0 : expiryTimes_[i-1]);
        }

        mesher_ =
            boost::make_shared<FdmMesherComposite>(
                boost::make_shared<Concentrating1dMesher>(
                    std::log(minStrike()/spot_->value()),
                    std::log(maxStrike()/spot_->value()),
                    nGridPoints_,
                    std::pair<Real, Real>(0.0, 0.025)));

        gridPoints_ = mesher_->locations(0);
        gridInFwd_ = Exp(gridPoints_)*spot_->value();

        localVolCache_.clear();
        calibrationResults_.clear();

        avgError_ = 0.0;
        minError_ = std::numeric_limits<Real>::max();
        maxError_ = 0.0;

        calibrationResults_.reserve(expiries_.size());

        Array npvPuts(nGridPoints_);
        Array npvCalls(nGridPoints_);

        for (Size i=0; i < nGridPoints_; ++i) {
            const Real strike = std::exp(gridPoints_[i]);
            npvPuts[i] = PlainVanillaPayoff(Option::Put, strike)(1.0);
            npvCalls[i]= PlainVanillaPayoff(Option::Call, strike)(1.0);
        }

        for (Size i=0; i < expiries_.size(); ++i) {
            const boost::shared_ptr<AndreasenHugeCostFunction> putCostFct =
                buildCostFunction(i, Option::Put, npvPuts);
            const boost::shared_ptr<AndreasenHugeCostFunction> callCostFct =
                buildCostFunction(i, Option::Call, npvCalls);

            CombinedCostFunction costFunction(putCostFct, callCostFct);

            PositiveConstraint positiveConstraint;
            Problem problem(costFunction,
                positiveConstraint, costFunction.initialValues());

            optimizationMethod_->minimize(problem, endCriteria_);

            const Array& sig = problem.currentValue();

            const SingleStepCalibrationResult calibrationResult = {
                npvPuts, npvCalls, sig,
                (calibrationType_ == Call)? callCostFct : putCostFct
            };

            calibrationResults_.push_back(calibrationResult);

            Array vegaDiffs(sig.size());
            switch (calibrationType_) {
              case CallPut: {
                const Array vegaPutDiffs =
                    putCostFct->vegaCalibrationError(sig);
                const Array vegaCallDiffs =
                    callCostFct->vegaCalibrationError(sig);

                const Real fwd = spot_->value()*
                    qTS_->discount(expiryTimes_[i])/rTS_->discount(expiryTimes_[i]);

                for (Size j=0; j < vegaDiffs.size(); ++j)
                    vegaDiffs[j] = std::fabs(
                        (fwd > gridInFwd_[j])? vegaPutDiffs[j] : vegaCallDiffs[j]);
              }
              break;
              case Put:
                vegaDiffs = Abs(putCostFct->vegaCalibrationError(sig));
              break;
              case Call:
                vegaDiffs = Abs(callCostFct->vegaCalibrationError(sig));
              break;
              default:
                QL_FAIL("unknown calibration type");
            }

            avgError_ +=
                std::accumulate(vegaDiffs.begin(), vegaDiffs.end(), 0.0);
            minError_ = std::min(minError_,
                *std::min_element(vegaDiffs.begin(), vegaDiffs.end()));
            maxError_ = std::max(maxError_,
                *std::max_element(vegaDiffs.begin(), vegaDiffs.end()));

            if (putCostFct)
                npvPuts = putCostFct->solveFor(dT_[i], sig, npvPuts);
            if (callCostFct)
                npvCalls= callCostFct->solveFor(dT_[i], sig, npvCalls);
        }

        avgError_ /= calibrationSet_.size();
    }

    Date AndreasenHugeVolatilityInterpl::maxDate() const {
        return expiries_.back();
    }

    Real AndreasenHugeVolatilityInterpl::minStrike() const {
        return (minStrike_ == Null<Real>())
            ? 1/8.0*strikes_.front() : minStrike_;
    }

    Real AndreasenHugeVolatilityInterpl::maxStrike() const {
        return (maxStrike_ == Null<Real>())
            ? 8.0*strikes_.back() : maxStrike_;
    }

    Real AndreasenHugeVolatilityInterpl::fwd(Time t) const {
        return spot_->value()*qTS_->discount(t)/rTS_->discount(t);
    }

    const Handle<YieldTermStructure>&
    AndreasenHugeVolatilityInterpl::riskFreeRate() const {
        return rTS_;
    }

    boost::tuple<Real, Real, Real>
    AndreasenHugeVolatilityInterpl::calibrationError() const {
        calculate();

        return boost::make_tuple<Real, Real, Real>(
            minError_, maxError_, avgError_);
    }

    Size AndreasenHugeVolatilityInterpl::getExerciseTimeIdx(Time t) const {
        return std::min<Size>(expiryTimes_.size()-1,
            std::distance(expiryTimes_.begin(),
                std::upper_bound(
                    expiryTimes_.begin(), expiryTimes_.end(), t)));
    }

    Real AndreasenHugeVolatilityInterpl::getCacheValue(
        Real strike, const TimeValueCacheType::const_iterator& f) const {

        const Real fwd = f->second.get<0>();
        const Real k = std::log(strike / fwd);

        const Real s = std::max(gridPoints_[1],
            std::min(*(gridPoints_.end()-2), k));

        return f->second.get<2>()->operator()(s);
    }

    Disposable<Array> AndreasenHugeVolatilityInterpl::getPriceSlice(
        Time t, Option::Type optionType) const {

        const Size iu = getExerciseTimeIdx(t);

        return calibrationResults_[iu].costFunction->solveFor(
            (iu == 0) ? t : t-expiryTimes_[iu-1],
            calibrationResults_[iu].sigmas,
            (optionType == Option::Call)? calibrationResults_[iu].callNPVs
                                        : calibrationResults_[iu].putNPVs);
    }

    Real AndreasenHugeVolatilityInterpl::optionPrice(
        Time t, Real strike, Option::Type optionType) const {

        TimeValueCacheType::const_iterator f = priceCache_.find(t);

        const DiscountFactor df = rTS_->discount(t);

        if (f != priceCache_.end()) {
            const Real fwd = f->second.get<0>();

            Real price = getCacheValue(strike, f);

            if (optionType == Option::Put
                && (calibrationType_ == Call || calibrationType_ == CallPut))
                price = price + strike/fwd - 1.0;
            else if (optionType == Option::Call && calibrationType_ == Put)
                price = 1.0 - strike/fwd + price;

            return price*df*fwd;
        }

        calculate();

        const boost::shared_ptr<Array> prices(
            boost::make_shared<Array>(gridPoints_));

        switch (calibrationType_) {
          case Put:
            (*prices) = getPriceSlice(t, Option::Put);
          break;
          case Call:
          case CallPut:
            (*prices) = getPriceSlice(t, Option::Call);
          break;
          default:
            QL_FAIL("unknown calibration type");
        }

        const Real fwd = spot_->value()*qTS_->discount(t)/df;

        priceCache_[t] = boost::make_tuple<
            Real,
            boost::shared_ptr<Array>,
            boost::shared_ptr<Interpolation> >(
                fwd, prices,
                boost::make_shared<CubicNaturalSpline>(
                    gridPoints_.begin()+1, gridPoints_.end()-1,
                    prices->begin()+1));

        return this->optionPrice(t, strike, optionType);
    }

    Disposable<Array> AndreasenHugeVolatilityInterpl::getLocalVolSlice(
        Time t, Option::Type optionType) const {

        const Size iu = getExerciseTimeIdx(t);

        const Array& previousNPVs =
            (optionType == Option::Call)? calibrationResults_[iu].callNPVs
                                        : calibrationResults_[iu].putNPVs;

        const boost::shared_ptr<AndreasenHugeCostFunction> costFunction
            = calibrationResults_[iu].costFunction;

        const Time dt = (iu == 0) ? t : t-expiryTimes_[iu-1];
        const Array& sig = calibrationResults_[iu].sigmas;

        const Array cAtJ = costFunction->solveFor(dt, sig, previousNPVs);

        const Array dCdT =
            costFunction->solveFor(dt, sig,
                    costFunction->apply(
                        costFunction->solveFor(dt, sig, previousNPVs)));

        const Array d2CdK2 = costFunction->d2CdK2(cAtJ);

        Array localVol = Sqrt(2*dCdT/d2CdK2);

        for (Size i=1; i < localVol.size()-1; ++i)
            if (!boost::math::isfinite(localVol[i]) || localVol[i] < 0.0)
                localVol[i] = 0.25;

        return localVol;
    }

    Volatility AndreasenHugeVolatilityInterpl::localVol(Time t, Real strike)
    const {
        TimeValueCacheType::const_iterator f = localVolCache_.find(t);

        if (f != localVolCache_.end())
            return getCacheValue(strike, f);

        calculate();

        const boost::shared_ptr<Array> localVol(
            boost::make_shared<Array>(gridPoints_.size()));

        switch (calibrationType_) {
          case CallPut: {
            const Array putLocalVol = getLocalVolSlice(t, Option::Put);
            const Array callLocalVol = getLocalVolSlice(t, Option::Call);

            for (Size i=0, n=localVol->size(); i < n; ++i)
                (*localVol)[i] =
                    (gridPoints_[i] > 0.0)? callLocalVol[i] : putLocalVol[i];
          }
          break;
          case Put:
            (*localVol) = getLocalVolSlice(t, Option::Put);
          break;
          case Call:
            (*localVol) = getLocalVolSlice(t, Option::Call);
          break;
          default:
            QL_FAIL("unknown calibration type");
        }

        const Real fwd = spot_->value()*qTS_->discount(t)/rTS_->discount(t);

        localVolCache_[t] = boost::make_tuple<
            Real,
            boost::shared_ptr<Array>,
            boost::shared_ptr<Interpolation> >(
                fwd, localVol,
                boost::make_shared<LinearInterpolation>(
                    gridPoints_.begin()+1, gridPoints_.end()-1,
                    localVol->begin()+1));

        return this->localVol(t, strike);
    }
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 9623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_CONSTANT_CONSTANTS_FACT_9_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_CONSTANTS_FACT_9_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>
#include <boost/simd/constant/register.hpp>
#include <boost/simd/constant/hierarchy.hpp>
#include <boost/config.hpp>

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable: 4310) // truncation of constant
#endif

namespace boost { namespace simd
{
  namespace tag
  {
   /*!
     @brief Fact_9 generic tag

     Represents the Fact_9 constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    BOOST_SIMD_CONSTANT_REGISTER( Fact_9,double
                                , 362880, 0x48b13000,0x4116260000000000ll
                                )
  }
  namespace ext
  {
   template<class Site, class... Ts>
   BOOST_FORCEINLINE generic_dispatcher<tag::Fact_9, Site> dispatching_Fact_9(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
   {
     return generic_dispatcher<tag::Fact_9, Site>();
   }
   template<class... Args>
   struct impl_Fact_9;
  }
  /*!
    Generates 9! that is 362880,

    @par Semantic:

    @code
    T r = Fact_9<T>();
    @endcode

    is similar to:

    @code
    T r = T(362880);
    @endcode
  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Fact_9, Fact_9)
} }

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

#include <boost/simd/constant/common.hpp>

#endif


/* ============================================================================
 * Copyright (c) 2009-2016 BlueQuartz Software, LLC
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.
 *
 * Redistributions in binary form must reproduce the above copyright notice, this
 * list of conditions and the following disclaimer in the documentation and/or
 * other materials provided with the distribution.
 *
 * Neither the name of BlueQuartz Software, the US Air Force, nor the names of its
 * contributors may be used to endorse or promote products derived from this software
 * without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
 * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * The code contained herein was partially funded by the following contracts:
 *    United States Air Force Prime Contract FA8650-07-D-5800
 *    United States Air Force Prime Contract FA8650-10-D-5210
 *    United States Prime Contract Navy N00173-07-C-2068
 *
 * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */

#include <limits>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Eigen>

#include "SIMPLib/SIMPLib.h"
#include "SIMPLib/Math/MatrixMath.h"
#include "SIMPLib/Math/QuaternionMath.hpp"
#include "SIMPLib/Testing/SIMPLTestFileLocations.h"
#include "SIMPLib/Testing/UnitTestSupport.hpp"

#define DREAM3D_PASSIVE 1.0
#define DREAM3D_ACTIVE -1.0

class QuaternionMathTest
{
public:
  QuaternionMathTest() = default;
  virtual ~QuaternionMathTest() = default;

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  void RemoveTestFiles()
  {
#if REMOVE_TEST_FILES
// QFile::remove();
#endif
  }

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  void TestMatrixMath()
  {
    typedef Eigen::Map<Eigen::Vector3f> VectorMapType;

    {
      float dir[3] = {1.0f, 2.0f, 3.0f};

      VectorMapType array(const_cast<float*>(dir));
      array.normalize();

      array = array * -1.0f;
    }

    {
      float dir[3] = {1.0f, 2.0f, 3.0f};
      MatrixMath::Normalize3x1(dir);
      MatrixMath::Multiply3x1withConstant(dir, -1.0f);
    }
  }

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  void TestQuat_t()
  {
    using QuaternionMathF = QuaternionMath<float>;
    using QuatF = QuaternionMathF::Quaternion;
    QuatF p = QuaternionMathF::New(1.0f, 0.0f, 0.0f, 1.0f);
    QuatF q = QuaternionMathF::New(0.0f, 1.0f, 0.0f, 2.0f);
    QuatF out = QuaternionMathF::New(0.0f, 0.0f, 0.0f, 0.0f);
    QuatF out2 = QuaternionMathF::New(10.0f, 20.0f, 30.0f, 40.0f);

    QuaternionMathF::Negate(out2);
    DREAM3D_REQUIRE_EQUAL(out2.x, -10.0)
    DREAM3D_REQUIRE_EQUAL(out2.y, -20.0)
    DREAM3D_REQUIRE_EQUAL(out2.z, -30.0)
    DREAM3D_REQUIRE_EQUAL(out2.w, -40.0)

    QuaternionMathF::Copy(p, out);
    DREAM3D_REQUIRE_EQUAL(p.x, out.x)
    DREAM3D_REQUIRE_EQUAL(p.y, out.y)
    DREAM3D_REQUIRE_EQUAL(p.z, out.z)
    DREAM3D_REQUIRE_EQUAL(p.w, out.w)

    QuaternionMathF::Identity(out);
    DREAM3D_REQUIRE_EQUAL(out.x, 0.0)
    DREAM3D_REQUIRE_EQUAL(out.y, 0.0)
    DREAM3D_REQUIRE_EQUAL(out.z, 0.0)
    DREAM3D_REQUIRE_EQUAL(out.w, 1.0)

    out = QuaternionMathF::New(-10.5f, -1.5f, -30.66f, -40.987f);
    QuaternionMathF::ElementWiseAbs(out);
    DREAM3D_REQUIRE_EQUAL(out.x, 10.5f)
    DREAM3D_REQUIRE_EQUAL(out.y, 1.5f)
    DREAM3D_REQUIRE_EQUAL(out.z, 30.66f)
    DREAM3D_REQUIRE_EQUAL(out.w, 40.987f)

    out = QuaternionMathF::New(10.0f, 20.0f, 30.0f, 40.0f);
    QuaternionMathF::ScalarMultiply(out, -1.0f);
    DREAM3D_REQUIRE_EQUAL(out.x, -10.0)
    DREAM3D_REQUIRE_EQUAL(out.y, -20.0)
    DREAM3D_REQUIRE_EQUAL(out.z, -30.0)
    DREAM3D_REQUIRE_EQUAL(out.w, -40.0)

    QuaternionMathF::ScalarDivide(out, -1.0f);

    QuaternionMathF::ScalarAdd(out, 50.0f);
    DREAM3D_REQUIRE_EQUAL(out.x, 60.0)
    DREAM3D_REQUIRE_EQUAL(out.y, 70.0)
    DREAM3D_REQUIRE_EQUAL(out.z, 80.0)
    DREAM3D_REQUIRE_EQUAL(out.w, 90.0)

    QuaternionMathF::ElementWiseAssign(out, 5.0f);
    DREAM3D_REQUIRE_EQUAL(out.x, 5.0)
    DREAM3D_REQUIRE_EQUAL(out.y, 5.0)
    DREAM3D_REQUIRE_EQUAL(out.z, 5.0)
    DREAM3D_REQUIRE_EQUAL(out.w, 5.0)

    QuaternionMathF::Negate(out);

    QuaternionMathF::Add(p, q, out);
    QuaternionMathF::Subtract(p, q, out);

    // Conjugate Tests where conjugate of a Quaternion is q*
    // (q*)* = q

    p.x = 1.0f;
    p.y = 2.0f;
    p.z = 3.0f;
    p.w = 1.0f;
    QuaternionMathF::Conjugate(p);
    DREAM3D_REQUIRE_EQUAL(p.x, -1.0)
    DREAM3D_REQUIRE_EQUAL(p.y, -2.0)
    DREAM3D_REQUIRE_EQUAL(p.z, -3.0)
    DREAM3D_REQUIRE_EQUAL(p.w, 1.0)
    QuaternionMathF::Conjugate(p);
    DREAM3D_REQUIRE_EQUAL(p.x, 1.0)
    DREAM3D_REQUIRE_EQUAL(p.y, 2.0)
    DREAM3D_REQUIRE_EQUAL(p.z, 3.0)
    DREAM3D_REQUIRE_EQUAL(p.w, 1.0)

    // (pq)* = q*p*
    q.x = 1.0f;
    q.y = 0.0f;
    q.z = 1.0f;
    q.w = 1.0f;
    QuaternionMathF::Multiply(p, q, out);
    QuaternionMathF::Conjugate(out);
    QuaternionMathF::Conjugate(p);
    QuaternionMathF::Conjugate(q);
    QuaternionMathF::Multiply(q, p, out2);
    DREAM3D_REQUIRE_EQUAL(out.x, out2.x)
    DREAM3D_REQUIRE_EQUAL(out.y, out2.y)
    DREAM3D_REQUIRE_EQUAL(out.z, out2.z)
    DREAM3D_REQUIRE_EQUAL(out.w, out2.w)

    //(p+q)* = p*+q*
    p.x = 1.0f;
    p.y = 2.0f;
    p.z = 3.0f;
    p.w = 1.0f;
    q.x = 1.0f;
    q.y = 0.0f;
    q.z = 1.0f;
    q.w = 1.0f;
    QuaternionMathF::Add(p, q, out);
    QuaternionMathF::Conjugate(out);
    QuaternionMathF::Conjugate(p);
    QuaternionMathF::Conjugate(q);
    QuaternionMathF::Add(p, q, out2);
    DREAM3D_REQUIRE_EQUAL(out.x, out2.x)
    DREAM3D_REQUIRE_EQUAL(out.y, out2.y)
    DREAM3D_REQUIRE_EQUAL(out.z, out2.z)
    DREAM3D_REQUIRE_EQUAL(out.w, out2.w)

    // Multiplication Test
    // pq != qp
    p.x = 1.0f;
    p.y = 0.0f;
    p.z = 0.0f;
    p.w = 1.0f;

    q.x = 0.0f;
    q.y = 1.0f;
    q.z = 0.0f;
    q.w = 2.0f;

    QuaternionMathF::Multiply(p, q, out);
    DREAM3D_REQUIRE_EQUAL(out.x, 2.0)
    DREAM3D_REQUIRE_EQUAL(out.y, 1.0)
    DREAM3D_REQUIRE_EQUAL(out.z, 1.0)
    DREAM3D_REQUIRE_EQUAL(out.w, 2.0)

    QuaternionMathF::Multiply(q, p, out);
    DREAM3D_REQUIRE_EQUAL(out.x, 2.0)
    DREAM3D_REQUIRE_EQUAL(out.y, 1.0)
    DREAM3D_REQUIRE_EQUAL(out.z, -1.0)
    DREAM3D_REQUIRE_EQUAL(out.w, 2.0)

    // Norm Test
    // N(q*) = N(q)
    p.x = 1.0f;
    p.y = 0.0f;
    p.z = 0.0f;
    p.w = 1.0f;
    q.x = 0.0f;
    q.y = 1.0f;
    q.z = 0.0f;
    q.w = 2.0f;
    float norm = QuaternionMathF::Norm(p);
    QuaternionMathF::Conjugate(p);
    float cnorm = QuaternionMathF::Norm(p);
    DREAM3D_REQUIRE_EQUAL(norm, cnorm)

    // Length and Unit Quaternion Tests
    p.x = 2.0f;
    p.y = 2.0f;
    p.z = 2.0f;
    p.w = 2.0f;
    float length = QuaternionMathF::Length(p);
    DREAM3D_REQUIRE_EQUAL(length, 4.0);
    QuaternionMathF::UnitQuaternion(p);
    DREAM3D_REQUIRE_EQUAL(p.x, 0.5)
    DREAM3D_REQUIRE_EQUAL(p.y, 0.5)
    DREAM3D_REQUIRE_EQUAL(p.z, 0.5)
    DREAM3D_REQUIRE_EQUAL(p.w, 0.5)

    p.x = 1.0;
    p.y = 2.0;
    p.z = 3.0;
    p.w = 0.0;
    float vec[3] = {0.0f, 0.0f, 0.0f};
    float ovec[3] = {0.0f, 0.0f, 0.0f};

    QuaternionMathF::GetMisorientationVector(p, vec);
    bool pass = SIMPLibMath::closeEnough(vec[0], 3.14159274f, 1.0E-4f);
    DREAM3D_REQUIRE_EQUAL(pass, true);

    pass = SIMPLibMath::closeEnough(vec[1], 6.28318548f, 1.0E-4f);
    DREAM3D_REQUIRE_EQUAL(pass, true)

    pass = SIMPLibMath::closeEnough(vec[2], 9.42477798f, 1.0E-4f);
    DREAM3D_REQUIRE_EQUAL(pass, true)

    // Test point r=[100] rotated 120Deg @ [111] axis
    vec[0] = 1.0;
    vec[1] = 0.0;
    vec[2] = 0.0;

    // The quaternion Representation for 120@[111]
    q.x = 0.5f;
    q.y = 0.5f;
    q.z = 0.5f;
    q.w = 0.5f;

    // Passive Rotation
    QuaternionMathF::MultiplyQuatVec(q, vec, ovec);

    pass = SIMPLibMath::closeEnough(ovec[0], 0.0f, 1.0E-4f);
    DREAM3D_REQUIRE_EQUAL(pass, true);

    pass = SIMPLibMath::closeEnough(ovec[1], 0.0f, 1.0E-4f);
    DREAM3D_REQUIRE_EQUAL(pass, true)

    pass = SIMPLibMath::closeEnough(ovec[2], 1.0f, 1.0E-4f);
    DREAM3D_REQUIRE_EQUAL(pass, true)

    // Active Rotation
    QuaternionMathF::Conjugate(q);
    QuaternionMathF::MultiplyQuatVec(q, vec, ovec);

    pass = SIMPLibMath::closeEnough(ovec[0], 0.0f, 1.0E-4f);
    DREAM3D_REQUIRE_EQUAL(pass, true);

    pass = SIMPLibMath::closeEnough(ovec[1], 1.0f, 1.0E-4f);
    DREAM3D_REQUIRE_EQUAL(pass, true)

    pass = SIMPLibMath::closeEnough(ovec[2], 0.0f, 1.0E-4f);
    DREAM3D_REQUIRE_EQUAL(pass, true)

    // 90@[100]
    q.x = 0.7071067811865475f;
    q.y = 0.0f;
    q.z = 0.0f;
    q.w = 0.7071067811865476f;

    vec[0] = 0.0;
    vec[1] = 1.0;
    vec[2] = 0.0;
    // Passive Rotation
    QuaternionMathF::MultiplyQuatVec(q, vec, ovec);

    pass = SIMPLibMath::closeEnough(ovec[0], 0.0f, 1.0E-4f);
    DREAM3D_REQUIRE_EQUAL(pass, true);

    pass = SIMPLibMath::closeEnough(ovec[1], 0.0f, 1.0E-4f);
    DREAM3D_REQUIRE_EQUAL(pass, true)

    pass = SIMPLibMath::closeEnough(ovec[2], -1.0f, 1.0E-4f);
    DREAM3D_REQUIRE_EQUAL(pass, true)

    // active Rotation
    QuaternionMathF::Conjugate(q);
    QuaternionMathF::MultiplyQuatVec(q, vec, ovec);

    pass = SIMPLibMath::closeEnough(ovec[0], 0.0f, 1.0E-4f);
    DREAM3D_REQUIRE_EQUAL(pass, true);

    pass = SIMPLibMath::closeEnough(ovec[1], 0.0f, 1.0E-4f);
    DREAM3D_REQUIRE_EQUAL(pass, true)

    pass = SIMPLibMath::closeEnough(ovec[2], 1.0f, 1.0E-4f);
    DREAM3D_REQUIRE_EQUAL(pass, true)
  }

  // -----------------------------------------------------------------------------
  //
  // -----------------------------------------------------------------------------
  void operator()()
  {
    std::cout << "#### QuaternionMathTest Starting ####" << std::endl;

    int err = EXIT_SUCCESS;
    DREAM3D_REGISTER_TEST(TestMatrixMath())
    DREAM3D_REGISTER_TEST(TestQuat_t())
    DREAM3D_REGISTER_TEST(RemoveTestFiles())
  }

private:
  QuaternionMathTest(const QuaternionMathTest&); // Copy Constructor Not Implemented
  void operator=(const QuaternionMathTest&);     // Move assignment Not Implemented
};


#include "SafeArithmetic.h"
#include "cute.h"

#include <boost/safe_numerics/safe_integer.hpp>
#include <boost/safe_numerics/exception_policies.hpp>
#include <boost/safe_numerics/automatic.hpp>

// safe numerics do not save us from the wrong operations... try differently first.
// but we can detect overflowing operations

using safe_diff_t = boost::safe_numerics::safe< std::ptrdiff_t, boost::safe_numerics::automatic >;
using safe_size_t = boost::safe_numerics::safe< size_t >;

void thisIsASafeArithmeticTest() {
	safe_size_t sz {0};
	//static_assert(std::is_signed_v<decltype(sz-1LL)>,"should be signed");
	static_assert(std::is_same_v<decltype(sz-1), safe_size_t>,"staying in the domain?");
	ASSERT_THROWS(sz-1,std::exception);
}
void newTestFunction(){
	ASSERTM("show more usages with strong types", false);
}

cute::suite make_suite_SafeArithmetic() {
	cute::suite s { };
	s.push_back(CUTE(thisIsASafeArithmeticTest));
	s.push_back(CUTE(newTestFunction));
	return s;
}


//  Copyright 2015 Peter Dimov.
//
// Distributed under the Boost Software License, Version 1.0.
//
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt

#include <boost/mp11/algorithm.hpp>
#include <boost/mp11/integral.hpp>
#include <boost/core/lightweight_test_trait.hpp>
#include <type_traits>

int main()
{
    using boost::mp11::mp_list;
    using boost::mp11::mp_iota;
    using boost::mp11::mp_iota_c;
    using boost::mp11::mp_int;
    using boost::mp11::mp_size_t;

    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_iota_c<0>, mp_list<>>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_iota_c<1>, mp_list<mp_size_t<0>>>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_iota_c<2>, mp_list<mp_size_t<0>, mp_size_t<1>>>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_iota_c<3>, mp_list<mp_size_t<0>, mp_size_t<1>, mp_size_t<2>>>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_iota_c<4>, mp_list<mp_size_t<0>, mp_size_t<1>, mp_size_t<2>, mp_size_t<3>>>));

    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_iota<mp_size_t<0>>, mp_list<>>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_iota<mp_size_t<1>>, mp_list<mp_size_t<0>>>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_iota<mp_size_t<2>>, mp_list<mp_size_t<0>, mp_size_t<1>>>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_iota<mp_size_t<3>>, mp_list<mp_size_t<0>, mp_size_t<1>, mp_size_t<2>>>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_iota<mp_size_t<4>>, mp_list<mp_size_t<0>, mp_size_t<1>, mp_size_t<2>, mp_size_t<3>>>));

    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_iota<mp_int<0>>, mp_list<>>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_iota<mp_int<1>>, mp_list<mp_int<0>>>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_iota<mp_int<2>>, mp_list<mp_int<0>, mp_int<1>>>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_iota<mp_int<3>>, mp_list<mp_int<0>, mp_int<1>, mp_int<2>>>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_iota<mp_int<4>>, mp_list<mp_int<0>, mp_int<1>, mp_int<2>, mp_int<3>>>));

    return boost::report_errors();
}


#include <ndt_map/pointcloud_utils.h>
#include <ndt_map_builder/ndt_map_builder.h>
#include <Eigen/Eigen>

#include "pcl/point_cloud.h"
#include "pcl/io/pcd_io.h"
#include "pcl/features/feature.h"
#include "pcl/registration/icp.h"
#include "pcl/filters/voxel_grid.h"

namespace perception_oru{

  //ICP wrapper
  // bool NDTMapBuilder::matchICP(pcl::PointCloud<pcl::PointXYZ> &target,  pcl::PointCloud<pcl::PointXYZ> &source,
  //                              Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> &Tout, double &finalscore)
  // {

  //   typename pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);
  //   typename pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);

  //   typename pcl::PointCloud<pcl::PointXYZ>::ConstPtr m (new pcl::PointCloud<pcl::PointXYZ>(source) );
  //   typename pcl::PointCloud<pcl::PointXYZ>::ConstPtr f (new pcl::PointCloud<pcl::PointXYZ>(target) );

  //   typename pcl::VoxelGrid<pcl::PointXYZ> gr1;
  //   typename pcl::VoxelGrid<pcl::PointXYZ> gr2;
  //   gr1.setLeafSize(0.1,0.1,0.1);
  //   gr2.setLeafSize(0.1,0.1,0.1);

  //   gr1.setInputCloud(m);
  //   gr2.setInputCloud(f);

  //   cloud_in->height = 1;
  //   cloud_in->width = cloud_in->points.size();
  //   cloud_out->height = 1;
  //   cloud_out->width = cloud_out->points.size();
  //   cloud_in->is_dense = false;
  //   cloud_out->is_dense = false;

  //   gr1.filter(*cloud_in);
  //   gr2.filter(*cloud_out);

  //   pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;

  //   icp.setMaximumIterations(1000);
  //   std::cout<<"max itr are "<<icp.getMaximumIterations()<<std::endl;
  //   // icp.setInputCloud(cloud_in); deprecated
  //   icp.setInputSource(cloud_in);
  //   icp.setInputTarget(cloud_out);

  //   icp.setRANSACOutlierRejectionThreshold (2);
  //   icp.setMaxCorrespondenceDistance(10);
  //   icp.setTransformationEpsilon(0.00001);
  //   pcl::PointCloud<pcl::PointXYZ> Final;
  //   icp.align(Final);

  //   //finalscore is the sum of square error. we want to minimize it...
  //   finalscore = icp.getFitnessScore();
  //   Tout = (icp.getFinalTransformation()).cast<double>();
  //   return icp.hasConverged();
  // }

  Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> NDTMapBuilder::addScan(pcl::PointCloud<pcl::PointXYZ> scan, int id)
  {
      Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> T, Tout, Tndt;
    //if first scan, add to vertices.
    if(id == -1)   //automatic IDs
      {
        id = vertices.size();
      }

    if(id == 0)
      {
        MapVertex mv;
        mv.scan = scan;
        mv.pose.setIdentity();
        mv.id = id;

        vertices.push_back(mv);
        return Tout;
          //return false;
      }

    //find last added scan
    if(id-1 <0 || id-1 > vertices.size())
      return Tout;
      //return false;
//当前帧与上一帧匹配
    std::cout<<"Matching scans with ids "<<vertices.back().id<<" and "<<id<<std::endl;
    //try to match them, obtain relative pose

    Eigen::Matrix<double,6,6> cov;
    bool succ = false;
    struct timeval tv_start, tv_end;
    double bestscore = INT_MAX;
    double finalscore;
    double time_match = 0;
    Tout.setIdentity();
      T.setIdentity();
      doHistogram=false;
if(doHistogram) {
    perception_oru::NDTMap fixed(&tr);
    fixed.loadPointCloud(vertices.back().scan);
    perception_oru::NDTMap moving(&tr);
    moving.loadPointCloud(scan);

    moving.computeNDTCells();
    fixed.computeNDTCells();

    perception_oru::NDTHistogram fixedH(fixed);
    perception_oru::NDTHistogram movingH(moving);

    movingH.bestFitToHistogram(fixedH, T);

   // std::cout << "[ndt_map_builder] " << "T fin " << T.translation().transpose()
             // << " r " << T.rotation().inverse().eulerAngles(0, 1, 2).transpose() << std::endl;
}
      // lx change 0228
     pcl::PointCloud<pcl::PointXYZ> cloud3;

     //check in between the top 3 histogram positions and the no initial guess solutions
     //such a good place for some threads! TODO
      for(int q=0; q<1; q++)
      //for(int q=0; q<4; q++)
       {
//         if(!doHistogram && q != 3) continue;
//         if(q!=3)
//           {
//             movingH.getTransform(q,T);
//           }
//         else
//           {
//             T.setIdentity();
//           }
//         	std::cout<<"T init "<<T.translation().transpose()<<" r "<<T.rotation().inverse().eulerAngles(0,1,2).transpose()<<std::endl;
//         Tndt.setIdentity();
//         cloud3 = perception_oru::transformPointCloud(T,scan);

         bool ret;
         gettimeofday(&tv_start,NULL);
         //lx test add 0301

         if(isF2F)
           {
             ret = matcherF2F->match(vertices.back().scan,scan,Tndt);
             finalscore = matcherF2F->finalscore;
           }
         else if(isP2F)
           {
             ret = matcherP2F->match(vertices.back().scan,scan,Tndt);
             finalscore = matcherP2F->finalscore;
           }
         // else
         //   {
         //     //icp
         //     ret = this->matchICP(vertices.back().scan,cloud3,Tndt,finalscore);
         //   }

         gettimeofday(&tv_end,NULL);

         if(finalscore < bestscore)
           {
             Tout = Tndt*T;
             bestscore = finalscore;
             if(isF2F)
               {
                 //matcherF2F->covariance(vertices.back().scan,cloud3,Tndt,cov);
               }
             else if(isP2F)
               {
                 //matcherP2F->covariance(vertices.back().scan,cloud3,Tndt,cov);
               }
             else
               {
                 cov.setIdentity();
                 cov = 0.1*cov;
               }
             //std::cout<<"score = "<<bestscore<<"best is "<<q<<std::endl;
           }
         //std::cout<<"T fin "<<Tout.translation().transpose()<<" r "<<Tout.rotation().inverse().eulerAngles(0,1,2).transpose()<<std::endl;
         time_match += (tv_end.tv_sec-tv_start.tv_sec)*1000.+(tv_end.tv_usec-tv_start.tv_usec)/1000.;
       }

    std::cout<<" TIME: (MATCHING ONLY) "<< time_match << std::endl;

    //best fit pose is stored in Tout
    //add vertex with global pose calculated based on pose of previous scan.
    MapVertex vert;
    vert.scan = scan;
    vert.pose = vertices.back().pose*Tout;
    vert.id = id;
    //vert.hist = movingH;
    vert.timeRegistration = time_match;
    //	(tv_end.tv_sec-tv_start.tv_sec)*1000.+(tv_end.tv_usec-tv_start.tv_usec)/1000.;

    //add edge with covariance and relative pose
//    MapEdge edge;
//    edge.idFirst = vertices.back().id;
//    edge.idSecond = id;
//    edge.relative_pose = Tout;
//    edge.covariance = cov;

    //add them
    vertices.push_back(vert);
//    edges.push_back(edge);

    //go through all previous scans and check for similarity
#if 0
    for(int i=0; i<vertices.size()-2; i++)
      {
        //lslgeneric::NDTMap f(&tr);
        //f.loadPointCloud(vertices[i].scan);
        //f.computeNDTCells();
        lslgeneric::NDTHistogram fH = vertices[i].hist;

        //compare fH and movingH
        double sim = movingH.getSimilarity(fH);

        if(sim < 0.22)
          {
            //try to match
            std::cout<<"Trying to match.. "<<id<<" to "<<vertices[i].id<<" at i "<<i<<" scan similarity "<<sim<<std::endl;
            //check score of match, if good --- add new constraint in the edge graph
            /*bestscore = INT_MAX;
              for(int q=0; q<1; q++) {
              if(q!=3) {
              movingH.getTransform(q,T);
              } else {
              T.setIdentity();
              }
              pcl::PointCloud<PointT> cloud3 = lslgeneric::transformPointCloud(T,scan);
              bool ret = matcherF2F->match(vertices[i].scan,cloud3,Tndt);
              finalscore = matcherF2F->finalscore;
              if(finalscore < bestscore) {
              Tout = Tndt*T;
              bestscore = finalscore;
              matcherF2F->covariance(vertices.back().scan,cloud3,Tndt,cov);
              cout<<"score = "<<bestscore<<"best is "<<q<<endl;
              }
              }*/

            movingH.getTransform(0,T);
            pcl::PointCloud<pcl::PointXYZ> cloud3 = lslgeneric::transformPointCloud(T,scan);

            bool ret;
            if(isF2F)
              {
                ret = matcherF2F->match(vertices[i].scan,cloud3,Tndt);
                finalscore = matcherF2F->finalscore;
              }
            else if(isP2F)
              {
                finalscore = 0;
              }
            else
              {
                //icp
                finalscore = 0;
              }

            if(finalscore < -0.8)
              {
                std::cout<<"NEW EDGE!\n";
                //add edge with covariance and relative pose
                edge.idFirst = vertices[i].id;
                edge.idSecond = id;
                edge.relative_pose = Tout;
                edge.covariance = cov;
                edges.insert(edges.begin(),edge);
              }
          }
      }
#endif
    return Tout;
  }

  void NDTMapBuilder::saveG2OlogFile(const char* fname)
  {
    //open file
    FILE *fout = fopen(fname,"w");
    Eigen::Matrix<double,6,6> eye;
    eye.setIdentity();
    double reg = 1e-4;

    //go through verices and dump them -- these are the "initial guess"
    for(int i=0; i<vertices.size(); i++)
      {
        Eigen::Vector3d t = vertices[i].pose.translation();
        Eigen::Quaternion<double> q;
        q = vertices[i].pose.rotation();
        fprintf(fout,"VERTEX_SE3:QUAT %d %lf %lf %lf %lf %lf %lf %lf\n",vertices[i].id,
                t(0),t(1),t(2),q.x(),q.y(),q.z(),q.w());
      }

    //go through edges and dump them
    for(int i=0; i<edges.size(); i++)
      {
        Eigen::Vector3d t = edges[i].relative_pose.translation();
        Eigen::Quaterniond q;
        q = edges[i].relative_pose.rotation();
        fprintf(fout,"EDGE_SE3:QUAT %d %d %lf %lf %lf %lf %lf %lf %lf ",edges[i].idFirst,edges[i].idSecond,
                t(0),t(1),t(2),q.x(),q.y(),q.z(),q.w());

        Eigen::Matrix<double,6,6> information = (edges[i].covariance+reg*eye).inverse();
        for(int p=0; p<6; p++)
          {
            for(int q=p; q<6; q++)
              {
                fprintf(fout,"%lf ",information(p,q));
              }
          }
        fprintf(fout,"\n");
      }
    fclose(fout);
  }

  void NDTMapBuilder::saveDatlogFile(const char* fname){
    //open file
    FILE *fout = fopen(fname,"w");

    //go through verices and dump them -- these are the "initial guess"
    for(int i=0; i<vertices.size(); i++)
      {
        Eigen::Vector3d t = vertices[i].pose.translation();
        Eigen::Quaternion<double> q;
        q = vertices[i].pose.rotation();
        fprintf(fout,"%d %lf %lf %lf %lf %lf %lf %lf\n",vertices[i].id,
                t(0),t(1),t(2),q.x(),q.y(),q.z(),q.w());
      }
    fclose(fout);

    char fn[500];
    snprintf(fn,499,"%s.times",fname);

    fout = fopen(fn,"w");
    fprintf(fout,"Tr = [");
    for( int i=1; i<vertices.size(); i++)
      {
        fprintf(fout,"%lf ",vertices[i].timeRegistration);
      }
    fprintf(fout,"];\n");

    fclose(fout);
  }

  void NDTMapBuilder::printNodePositions()
  {
    //go through verices and dump them
    for(int i=0; i<vertices.size(); i++){
      std::cout<<vertices[i].pose.translation().transpose()<<" "<<vertices[i].pose.rotation().eulerAngles(0,1,2)<<std::endl;
    }
  }
}


//
// Copyright (c) 2016-2018 CNRS
//

#include "pinocchio/multibody/joint/joint-generic.hpp"

#include "pinocchio/multibody/liegroup/liegroup.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

using namespace pinocchio;

template <typename JointModel>
void test_joint_methods(JointModel & jmodel, typename JointModel::JointDataDerived & jdata)
{
  typedef typename LieGroup<JointModel>::type LieGroupType;

  std::cout << "Testing Joint over " << jmodel.shortname() << std::endl;
  Eigen::VectorXd q1(Eigen::VectorXd::Random (jmodel.nq()));
  Eigen::VectorXd q1_dot(Eigen::VectorXd::Random (jmodel.nv()));
  Eigen::VectorXd q2(Eigen::VectorXd::Random (jmodel.nq()));
  pinocchio::Inertia::Matrix6 Ia(pinocchio::Inertia::Random().matrix());
  bool update_I = false;

  q1 = LieGroupType().random();
  q2 = LieGroupType().random();

  jmodel.calc(jdata, q1, q1_dot);
  jmodel.calc_aba(jdata, Ia, update_I);

  pinocchio::JointModel jma(jmodel);
  pinocchio::JointData jda(jdata);

  jma.calc(jda, q1, q1_dot);
  jma.calc_aba(jda, Ia, update_I);

  std::string error_prefix("JointModel on " + jma.shortname());
  BOOST_CHECK_MESSAGE(jmodel.nq() == jma.nq() ,std::string(error_prefix + " - nq "));
  BOOST_CHECK_MESSAGE(jmodel.nv() == jma.nv() ,std::string(error_prefix + " - nv "));

  BOOST_CHECK_MESSAGE(jmodel.idx_q() == jma.idx_q() ,std::string(error_prefix + " - Idx_q "));
  BOOST_CHECK_MESSAGE(jmodel.idx_v() == jma.idx_v() ,std::string(error_prefix + " - Idx_v "));
  BOOST_CHECK_MESSAGE(jmodel.id() == jma.id() ,std::string(error_prefix + " - JointId "));

  BOOST_CHECK_MESSAGE(jda.S().matrix().isApprox(jdata.S.matrix()),std::string(error_prefix + " - ConstraintXd "));
  BOOST_CHECK_MESSAGE( (jda.M()).isApprox((jdata.M)),std::string(error_prefix + " - Joint transforms ")); // ==  or isApprox ?
  BOOST_CHECK_MESSAGE( (jda.v()).isApprox( (pinocchio::Motion(jdata.v))),std::string(error_prefix + " - Joint motions "));
  BOOST_CHECK_MESSAGE((jda.c()) == (jdata.c),std::string(error_prefix + " - Joint bias "));

  BOOST_CHECK_MESSAGE((jda.U()).isApprox(jdata.U),std::string(error_prefix + " - Joint U inertia matrix decomposition "));
  BOOST_CHECK_MESSAGE((jda.Dinv()).isApprox(jdata.Dinv),std::string(error_prefix + " - Joint DInv inertia matrix decomposition "));
  BOOST_CHECK_MESSAGE((jda.UDinv()).isApprox(jdata.UDinv),std::string(error_prefix + " - Joint UDInv inertia matrix decomposition "));

  // Test vxS
  typedef typename JointModel::Constraint_t Constraint_t;
  typedef typename Constraint_t::DenseBase ConstraintDense;

  Motion v(Motion::Random());
  ConstraintDense vxS(v.cross(jdata.S));
  ConstraintDense vxS_ref = v.toActionMatrix() * jdata.S.matrix();

  BOOST_CHECK_MESSAGE(vxS.isApprox(vxS_ref),std::string(error_prefix + "- Joint vxS operation "));

  // Test Y*S
  const Inertia Isparse(Inertia::Random());
  const Inertia::Matrix6 Idense(Isparse.matrix());

  const ConstraintDense IsparseS = Isparse * jdata.S;
  const ConstraintDense IdenseS = Idense * jdata.S;

  BOOST_CHECK_MESSAGE(IdenseS.isApprox(IsparseS),std::string(error_prefix + "- Joint YS operation "));

}

struct TestJoint{

  template <typename T>
  void operator()(const T ) const
  {
    T jmodel;
    jmodel.setIndexes(0,0,0);
    typename T::JointDataDerived jdata = jmodel.createData();

    test_joint_methods(jmodel, jdata);
  }

  void operator()(const pinocchio::JointModelComposite & ) const
  {
    // pinocchio::JointModelComposite jmodel(2);
    // jmodel.addJointModel(pinocchio::JointModelRX());
    // jmodel.addJointModel(pinocchio::JointModelRY());

    pinocchio::JointModelComposite jmodel((pinocchio::JointModelRX()));
    jmodel.addJoint(pinocchio::JointModelRY());
    jmodel.setIndexes(0,0,0);

    pinocchio::JointModelComposite::JointDataDerived jdata = jmodel.createData();

    // TODO: fixme when LieGroups will be implemented for JointModelComposite
//    test_joint_methods(jmodel, jdata);
  }

  void operator()(const pinocchio::JointModelRevoluteUnaligned & ) const
  {
    pinocchio::JointModelRevoluteUnaligned jmodel(1.5, 1., 0.);
    jmodel.setIndexes(0,0,0);
    pinocchio::JointModelRevoluteUnaligned::JointDataDerived jdata = jmodel.createData();

    test_joint_methods(jmodel, jdata);
  }

  void operator()(const pinocchio::JointModelPrismaticUnaligned & ) const
  {
    pinocchio::JointModelPrismaticUnaligned jmodel(1.5, 1., 0.);
    jmodel.setIndexes(0,0,0);
    pinocchio::JointModelPrismaticUnaligned::JointDataDerived jdata = jmodel.createData();

    test_joint_methods(jmodel, jdata);
  }

};

namespace pinocchio
{

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> struct JointTest;
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> struct JointModelTest;
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> struct JointDataTest;

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  struct traits< JointDataTest<Scalar,Options,JointCollectionTpl> >
  { typedef JointTpl<Scalar,Options,JointCollectionTpl> JointDerived; };

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  struct traits< JointModelTest<Scalar,Options,JointCollectionTpl> >
  { typedef JointTpl<Scalar,Options,JointCollectionTpl> JointDerived; };

  template<typename _Scalar, int _Options, template<typename,int> class JointCollectionTpl>
  struct traits< JointTest<_Scalar,_Options,JointCollectionTpl> >
  {
    enum {
      Options = _Options,
      NQ = Eigen::Dynamic, // Dynamic because unknown at compile time
      NV = Eigen::Dynamic
    };
    typedef _Scalar Scalar;

    typedef JointDataTpl<Scalar,Options,JointCollectionTpl> JointDataDerived;
    typedef JointModelTpl<Scalar,Options,JointCollectionTpl> JointModelDerived;
    typedef ConstraintTpl<Eigen::Dynamic,Scalar,Options> Constraint_t;
    typedef SE3Tpl<Scalar,Options> Transformation_t;
    typedef MotionTpl<Scalar,Options>  Motion_t;
    typedef MotionTpl<Scalar,Options>  Bias_t;

    typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> F_t;
    // [ABA]
    typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> U_t;
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> D_t;
    typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> UD_t;

    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> ConfigVector_t;
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> TangentVector_t;
  };

  template<typename _Scalar, int _Options, template<typename,int> class JointCollectionTpl>
  struct JointModelTest
  : JointCollectionTpl<_Scalar,_Options>::JointModelVariant
  , JointModelBase< JointModelTest<_Scalar,_Options,JointCollectionTpl> >
  {
    typedef JointTest<_Scalar,_Options,JointCollectionTpl> JointDerived;
    typedef JointCollectionTpl<_Scalar,_Options> JointCollection;
    typedef typename JointCollection::JointModelVariant VariantBase;
    typedef typename JointCollection::JointModelVariant JointModelVariant;

    SE3_JOINT_TYPEDEF_TEMPLATE;

    JointModelTest(const JointModelVariant & jmodel)
    : VariantBase(jmodel)
    {}

  };

}

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE(test_joint_from_joint_composite)
{
  typedef JointCollectionDefault JointCollection;
  typedef JointCollection::JointModelVariant JointModelVariant;

  JointModelRX jmodel_revolute_x;
  JointModel jmodel_generic(jmodel_revolute_x);
  JointModelVariant jmodel_variant(jmodel_revolute_x);

  JointModelTest<double,0,JointCollectionDefaultTpl> jmodel_test(jmodel_revolute_x);
  std::vector< JointModelTest<double,0,JointCollectionDefaultTpl> > jmodel_test_vector;
  jmodel_test_vector.push_back(JointModelTest<double,0,JointCollectionDefaultTpl>(jmodel_revolute_x));

  std::vector<JointModelVariant> jmodel_variant_vector;
  jmodel_variant_vector.push_back(jmodel_revolute_x);

  std::vector<JointModel> jmodel_generic_vector;
  jmodel_generic_vector.push_back((JointModel)jmodel_revolute_x);
  JointModelComposite jmodel_composite(jmodel_revolute_x);
}

BOOST_AUTO_TEST_CASE ( test_all_joints )
{
  boost::mpl::for_each<JointModelVariant::types>(TestJoint());
}

BOOST_AUTO_TEST_CASE(test_empty_model)
{
  JointModel jmodel;
  std::cout << "nq " << jmodel.nq() << std::endl;
  std::cout << "nv " << jmodel.nv() << std::endl;
  std::cout << "idx_q " << jmodel.idx_q() << std::endl;
  std::cout << "idx_v " << jmodel.idx_v() << std::endl;
  std::cout << "id " << jmodel.id() << std::endl;
  std::cout << "name " << jmodel.shortname() << std::endl;

  BOOST_CHECK(jmodel.idx_q() == -1);
  BOOST_CHECK(jmodel.idx_v() == -1);
}

BOOST_AUTO_TEST_CASE(isEqual)
{
  JointModelRX joint_revolutex;
  JointModelRY joint_revolutey;

  BOOST_CHECK(joint_revolutex != joint_revolutey);

  JointModel jmodelx(joint_revolutex);
  jmodelx.setIndexes(0,0,0);

  JointModel jmodelx_copy = jmodelx;
  BOOST_CHECK(jmodelx_copy == jmodelx.derived());

  JointModel jmodely(joint_revolutey);
  // TDDO: the comparison of two variants is not supported by some old version of BOOST
//  BOOST_CHECK(jmodely.toVariant() != jmodelx.toVariant());
  BOOST_CHECK(jmodely != jmodelx);
}

BOOST_AUTO_TEST_CASE(cast)
{
  JointModelRX joint_revolutex;

  JointModel jmodelx(joint_revolutex);
  jmodelx.setIndexes(0,0,0);

  BOOST_CHECK(jmodelx.cast<double>() == jmodelx);
  BOOST_CHECK(jmodelx.cast<long double>().cast<double>() == jmodelx);
}

BOOST_AUTO_TEST_SUITE_END ()


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

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

  EXPECT_FALSE(logical_and(0,0));
  EXPECT_FALSE(logical_and(0,1));
  EXPECT_FALSE(logical_and(1,0));
  EXPECT_FALSE(logical_and(0.0, 0.0));
  EXPECT_FALSE(logical_and(0.0, 1.0));
  EXPECT_FALSE(logical_and(1, 0.0));
}

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

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


// Copyright (C) 2014 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/pose/sim_transform_partial_rotation.h"

#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <glog/logging.h>
#include <limits>
#include <math.h>

#include "theia/alignment/alignment.h"
#include "theia/sfm/pose/util.h"

namespace theia {

using Eigen::EigenSolver;
using Eigen::Matrix;
using Eigen::Matrix3d;
using Eigen::Quaterniond;
using Eigen::Vector3d;

typedef Eigen::Matrix<double, 5, 5> Matrix5d;
typedef Eigen::Matrix<double, 5, 1> Vector5d;

namespace {

bool SolveQEP(const Matrix5d& M,
              const Matrix5d& C,
              const Matrix5d& K,
              std::vector<double>* eigenvalues,
              std::vector<Vector5d>* eigenvectors) {
  // Solves the quadratic eigenvalue problem:
  //
  //   Q(s)x = 0
  //
  // where:
  //
  //   Q(s) = s^2*M + s*C + K
  //
  // Returns true if the problem could be solved, false otherwise.
  //
  // This is converted to a generalized eigenvalue as described in:
  //   http://en.wikipedia.org/wiki/Quadratic_eigenvalue_problem
  //
  // The generalized eigenvalue problem is in this form:
  //
  // [ C  K ]z = s[ -M  0  ]z
  // [-I  0 ]     [ 0   -I ]
  //
  // With eigenvector z = [ sx ] and s is the eigenvalue.
  //                      [  x ]
  //
  // The eigenvector of the quadratic eigenvalue problem can be extracted from
  // z.
  //
  // Where z is the eigenvector, s is the eigenvalue.
  // This generalized eigenvalue can be converted to a standard eigenvalue
  // problem by multiplying by the inverse of the RHS matrix. The inverse of
  // the RHS matrix is particularly simple:
  //
  // [ -inv(M)  0 ]
  // [ 0       -I ]
  //
  // So the generalized eigenvalue problem reduces to a standard eigenvalue
  // problem on the constraint matrix:
  //
  // [ -inv(M)C  -inv(M)K ]z = sz
  // [ I         0        ]

  const Eigen::FullPivLU<Matrix5d> lu(M);
  if (!lu.isInvertible()) {
    VLOG(2) << "Matrix M is not invertible (rank " << lu.rank() << ")";
    return false;
  }

  // Negate inverse of M.
  const Matrix5d inv_M = -1.0 * lu.inverse();

  // Set up constraint matrix.
  Matrix<double, 10, 10> constraint = Matrix<double, 10, 10>::Zero();
  // Set upper-left to - inv(M) * C.
  constraint.block<5, 5>(0, 0) = inv_M * C;
  // Set upper-right to -inv(M) * K.
  constraint.block<5, 5>(0, 5) = inv_M * K;
  // Set lower-left to identity.
  constraint.block<5, 5>(5, 0) = Matrix5d::Identity();

  // Extract the left eigenvectors and values from the constraint matrix.
  EigenSolver<Matrix<double, 10, 10> > eig_solver(constraint);
  for (int i = 0; i < eig_solver.eigenvalues().size(); i++) {
    // Two solutions that always occur correspond to (s^2 + 1) so skip these
    // solutions.
    if (fabs(eig_solver.eigenvalues()[i].imag()) == 1 ||
        (i > 0 && eig_solver.eigenvalues()[i].real() ==
                      eig_solver.eigenvalues()[i - 1].real())) {
      continue;
    }
    eigenvalues->push_back(eig_solver.eigenvalues()[i].real());
    eigenvectors->push_back(
        Vector5d(eig_solver.eigenvectors().col(i).tail<5>().real()));
  }
  return true;
}

}  // namespace

void SimTransformPartialRotation(const Vector3d& axis,
                                 const Vector3d image_one_ray_directions[5],
                                 const Vector3d image_one_ray_origins[5],
                                 const Vector3d image_two_ray_directions[5],
                                 const Vector3d image_two_ray_origins[5],
                                 std::vector<Quaterniond>* soln_rotations,
                                 std::vector<Vector3d>* soln_translations,
                                 std::vector<double>* soln_scales) {
  CHECK_DOUBLE_EQ(axis.squaredNorm(), 1.0);
  CHECK_NOTNULL(soln_rotations)->clear();
  CHECK_NOTNULL(soln_translations)->clear();

  // The generalized epipolar constraint between two sets of rays in different
  // coordinate systems (two generalized cameras).
  //
  // The rays are converted to plucker coordinates:
  //   L = [ q  ]
  //       [ p  ]
  //
  // Assuming the first coordinate system has zero rotation and translation,
  // the generalized epipolar constraint is:
  //
  //   q2' * [t]x R * q1 + q2' * R * p1 + q1' * R * p2 = 0
  //
  // v' means transpose v, t is unknown translation, [t]x is the matrix
  // cross product form.
  //
  // Re-arranging to isolate t:
  //
  //   -(q2 x (R * q1))' * t + q2' * R * p1 + q1' * R' * p2 = 0
  //
  // In vector form:
  //
  //  [-(q2 x (R * q1))', q2' * R * p1 + q1' * R' * p2 ] * [ t ] = 0
  //                                                       [ 1 ]
  //
  // If we consider that L2 may not have the same scale as L1, then we must
  // additionally reconcile a scale s. Our equation then becomes:
  //
  //  [-(q2 x (R * q1))',  q1' * R' * p2, q2' * R * p1 ] * [ t ] = 0
  //                                                       [ s ]
  //                                                       [ 1 ]
  //
  // Each correspondence gives another constraint and the constraints can
  // be stacked to create a constraint matrix.
  //
  // The rotation matrix R can be parameterized, up to scale factor, as:
  //
  //   R ~ 2 * (v * v' + alpha * [v]x) + (alpha^2 - 1)I
  //
  //   I = Identity matrix.
  //
  // where is v is the known (unit length) axis and alpha is related to the
  // unknown angle of rotation.
  //
  // The epipolar constraint holds if the rotation matrix is scaled, so the
  // rotation matrix parameterization above can be used. Each row of the
  // constraint matrix is thus a quadratic function in alpha, so the constraint
  // can be written as:
  //
  //   [ M * alpha^2 + C * alpha + k ] * [ t ] = 0
  //                                     [ s ]
  //                                     [ 1 ]
  //
  // This is standard quadratic eigenvalue problem (QEP) and is solved using
  // standard methods.

  // Creates the matrices for the QEP problem.
  Matrix5d M;
  Matrix5d C;
  Matrix5d K;

  // Based on the rotation parameterization above,
  //   R = alpha^2 * rot_alpha_sq + alpha * rot_alpha + rot_constant
  const Matrix3d rot_alpha_sq = Matrix3d::Identity();
  const Matrix3d rot_alpha = 2.0 * CrossProductMatrix(axis);
  const Matrix3d rot_constant =
      2 * axis * axis.transpose() - Matrix3d::Identity();

  for (int i = 0; i < 5; ++i) {
    const Vector3d& f1(image_one_ray_directions[i]);
    const Vector3d& f2(image_two_ray_directions[i]);
    const Matrix3d pos1 = CrossProductMatrix(image_one_ray_origins[i]);
    const Matrix3d pos2 = CrossProductMatrix(image_two_ray_origins[i]);

    M.row(i).head(3) = f1.cross(rot_alpha_sq * f2);
    M.row(i)[3] = -f1.transpose() * (rot_alpha_sq * pos2) * f2;
    M.row(i)[4] = f1.transpose() * (pos1 * rot_alpha_sq) * f2;

    C.row(i).head(3) = f1.cross(rot_alpha * f2);
    C.row(i)[3] = -f1.transpose() * (rot_alpha * pos2) * f2;
    C.row(i)[4] = f1.transpose() * (pos1 * rot_alpha) * f2;

    K.row(i).head(3) = f1.cross(rot_constant * f2);
    K.row(i)[3] = -f1.transpose() * (rot_constant * pos2) * f2;
    K.row(i)[4] = f1.transpose() * (pos1 * rot_constant) * f2;
  }

  std::vector<double> eigenvalues;
  std::vector<Vector5d> eigenvectors;
  static const double kWTolerance = 1e-12;
  if (SolveQEP(M, C, K, &eigenvalues, &eigenvectors)) {
    // Extracts the translations and rotations from the eigenvalues and
    // eigenvectors of the QEP problem.
    for (int i = 0; i < eigenvalues.size(); ++i) {
      if (fabs(eigenvectors[i][4]) < kWTolerance ||
          eigenvectors[i][3] / eigenvectors[i][4] < 0) {
        continue;
      }

      Quaterniond quat(eigenvalues[i], axis[0], axis[1], axis[2]);
      quat.normalize();
      soln_rotations->push_back(quat);
      const Vector3d rotated_translation =
          Vector3d(eigenvectors[i][0] / eigenvectors[i][4],
                   eigenvectors[i][1] / eigenvectors[i][4],
                   eigenvectors[i][2] / eigenvectors[i][4]);
      soln_translations->push_back(rotated_translation);
      soln_scales->push_back(eigenvectors[i][3] / eigenvectors[i][4]);
    }
  } else {
    // When there is zero rotation the vector part of the quaternion disappears
    // and it becomes ([0], 1) (where [0] is the zero vector) and the SolveQEP
    // method cannot be used.
    // However from the equations for M, C, and K above we can see that the
    // C and K matrices contain the axis, which is 0 assuming zero rotation,
    // so to solve for the translation we can directly extract the null space
    // of M.
    const Eigen::FullPivLU<Matrix5d> lu(M);
    if (lu.dimensionOfKernel() != 1) {
      LOG(WARNING) << "The input is a degenerate configuration resulting in a "
                      "rank deficient matrix!";
      return;
    }

    const Vector5d kernel = lu.kernel();
    if (fabs(kernel[4]) > kWTolerance) {
      soln_rotations->push_back(Quaterniond::Identity());
      soln_translations->push_back(kernel.head<3>() / kernel[4]);
      soln_scales->push_back(kernel[3] / kernel[4]);
    }
  }
}

}  // namespace theia


/*
 [auto_generated]
 libs/numeric/odeint/examples/bind_member_functions.hpp

 [begin_description]
 tba.
 [end_description]

 Copyright 2012 Karsten Ahnert
 Copyright 2012 Mario Mulansky

 Distributed under the Boost Software License, Version 1.0.
 (See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#include <iostream>

#include <boost/numeric/odeint.hpp>

namespace odeint = boost::numeric::odeint;

typedef boost::array< double , 3 > state_type;

//[ ode_wrapper
template< class Obj , class Mem >
class ode_wrapper
{
    Obj m_obj;
    Mem m_mem;

public:

    ode_wrapper( Obj obj , Mem mem ) : m_obj( obj ) , m_mem( mem ) { }

    template< class State , class Deriv , class Time >
    void operator()( const State &x , Deriv &dxdt , Time t )
    {
        (m_obj.*m_mem)( x , dxdt , t );
    }
};

template< class Obj , class Mem >
ode_wrapper< Obj , Mem > make_ode_wrapper( Obj obj , Mem mem )
{
    return ode_wrapper< Obj , Mem >( obj , mem );
}
//]

template< class Obj , class Mem >
class observer_wrapper
{
    Obj m_obj;
    Mem m_mem;

public:

    observer_wrapper( Obj obj , Mem mem ) : m_obj( obj ) , m_mem( mem ) { }

    template< class State , class Time >
    void operator()( const State &x , Time t )
    {
        (m_obj.*m_mem)( x , t );
    }
};

template< class Obj , class Mem >
observer_wrapper< Obj , Mem > make_observer_wrapper( Obj obj , Mem mem )
{
    return observer_wrapper< Obj , Mem >( obj , mem );
}

//[ bind_member_function
struct lorenz
{
    void ode( const state_type &x , state_type &dxdt , double t ) const
    {
        dxdt[0] = 10.0 * ( x[1] - x[0] );
        dxdt[1] = 28.0 * x[0] - x[1] - x[0] * x[2];
        dxdt[2] = -8.0 / 3.0 * x[2] + x[0] * x[1];
    }
};

int main( int argc , char *argv[] )
{
    using namespace boost::numeric::odeint;
    state_type x = {{ 10.0 , 10.0 , 10.0 }};
    integrate_const( runge_kutta4< state_type >() , make_ode_wrapper( lorenz() , &lorenz::ode ) ,
                     x , 0.0 , 10.0 , 0.01 );
    return 0;
}
//]

/*
struct lorenz
{
    void ode( const state_type &x , state_type &dxdt , double t ) const
    {
        dxdt[0] = 10.0 * ( x[1] - x[0] );
        dxdt[1] = 28.0 * x[0] - x[1] - x[0] * x[2];
        dxdt[2] = -8.0 / 3.0 * x[2] + x[0] * x[1];
    }

    void obs( const state_type &x , double t ) const
    {
        std::cout << t << " " << x[0] << " " << x[1] << " " << x[2] << "\n";
    }
};

int main( int argc , char *argv[] )
{
    using namespace boost::numeric::odeint;

    state_type x = {{ 10.0 , 10.0 , 10.0 }};
    integrate_const( runge_kutta4< state_type >() ,
                     make_ode_wrapper( lorenz() , &lorenz::ode ) ,
                     x , 0.0 , 10.0 , 0.01 ,
                     make_observer_wrapper( lorenz() , &lorenz::obs ) );

    return 0;
}
*/


/*
 * @Description: 关键帧之间的相对位姿，用于闭环检测
 * @Author: Ren Qian
 * @Date: 2020-02-28 19:13:26
 */
#ifndef LIDAR_LOCALIZATION_SENSOR_DATA_LOOP_POSE_HPP_
#define LIDAR_LOCALIZATION_SENSOR_DATA_LOOP_POSE_HPP_

#include <Eigen/Dense>

namespace lidar_localization {
class LoopPose {
  public:
    double time = 0.0;
    unsigned int index0 = 0;
    unsigned int index1 = 0;
    Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();

  public:
    Eigen::Quaternionf GetQuaternion();
};
}

#endif


/**
 * @file contour_comparison.cpp
 * @author WARG
 *
 * @section LICENSE
 *
 *  Copyright (c) 2015, Waterloo Aerial Robotics Group (WARG)
 *  All rights reserved.
 *
 *  This software is licensed under a modified version of the BSD 3 clause license
 *  that should have been included with this software in a file called COPYING.txt
 *  Otherwise it is available at:
 *  https://raw.githubusercontent.com/UWARG/computer-vision/master/COPYING.txt
 */

#include <opencv2/imgproc.hpp>
#include <boost/log/trivial.hpp>
#include <boost/test/unit_test.hpp>
#include <vector>
#include "contour_comparison.h"

using namespace cv;
using namespace std;

double compare_contours(vector<vector<Point> > & contourA, vector<vector<Point> > & contourB) {
    BOOST_LOG_TRIVIAL(trace) << "Comparing contours";
    if (contourA.size() == 0 || contourB.size() == 0) return 0;
    Rect boundsA;
    Rect boundsB;
    for(int i = 0; i < contourA.size(); i++) boundsA |= boundingRect(contourA[i]);
    for(int i = 0; i < contourB.size(); i++) boundsB |= boundingRect(contourB[i]);
    Size largest = Size(boundsA.x + boundsA.width > boundsB.x + boundsB.width
                        ? boundsA.x + boundsA.width : boundsB.x + boundsB.width,
                        boundsA.y + boundsA.height > boundsB.y + boundsB.height
                        ? boundsA.y + boundsA.height : boundsB.y + boundsB.height);
    Mat imgA = Mat::zeros(largest, CV_8U);
    Mat imgB = Mat::zeros(largest, CV_8U);
    BOOST_LOG_TRIVIAL(trace) << "Drawing contours";
    vector<vector<Point> >hullA(contourA.size()), hullB(contourB.size());

    for(int i = 0; i < hullA.size(); i++) {
        convexHull( Mat(contourA[i]), hullA[i], false );
        drawContours(imgA, hullA, i, Scalar(255,255,255), FILLED);
    }
    for(int i = 0; i < hullB.size(); i++) {
        convexHull( Mat(contourB[i]), hullB[i], false );
        drawContours(imgB, hullB, i, Scalar(255,255,255), FILLED);
    }
    BOOST_LOG_TRIVIAL(trace) << "Comparing images";
    Mat diff = imgA & imgB;
    int countA = countNonZero(imgA);
    int countB = countNonZero(imgB);
    double countDiff = countNonZero(diff);
    double base = countA > countB ? countA : countB;
    BOOST_LOG_TRIVIAL(info) << "Size(contourA) = " << countA << ", Size(contourB) = " << countB << " Size(difference) = " << countDiff;
    BOOST_LOG_TRIVIAL(info) << "Result: " << (countDiff/base);
    return countDiff/base;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/arithmetic/include/functions/toint.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/sdk/simd/io.hpp>

NT2_TEST_CASE_TPL ( toint_real,  BOOST_SIMD_SIMD_REAL_TYPES)
{
  using boost::simd::toint;
  using boost::simd::tag::toint_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>      vT;
  typedef typename boost::dispatch::meta::call<toint_(vT)>::type r_t;

  // specific values tests
  NT2_TEST_EQUAL(toint(boost::simd::One<vT>()), boost::simd::One<r_t>());
  NT2_TEST_EQUAL(toint(boost::simd::Zero<vT>()), boost::simd::Zero<r_t>());
  NT2_TEST_EQUAL(toint(boost::simd::Mone<vT>()), boost::simd::Mone<r_t>());
} // end of test for floating_

NT2_TEST_CASE_TPL ( toint_unsigned_int,  BOOST_SIMD_SIMD_UNSIGNED_TYPES)
{
  using boost::simd::toint;
  using boost::simd::tag::toint_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                    vT;
  typedef typename boost::dispatch::meta::call<toint_(vT)>::type r_t;

  // specific values tests
  NT2_TEST_EQUAL(toint(boost::simd::One<vT>()), boost::simd::One<r_t>());
  NT2_TEST_EQUAL(toint(boost::simd::Zero<vT>()), boost::simd::Zero<r_t>());
} // end of test for unsigned_int_

NT2_TEST_CASE_TPL ( toint_signed_int,  BOOST_SIMD_SIMD_INTEGRAL_SIGNED_TYPES)
{
  using boost::simd::toint;
  using boost::simd::tag::toint_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef typename boost::dispatch::meta::call<toint_(vT)>::type r_t;

  // specific values tests
  NT2_TEST_EQUAL(toint(boost::simd::Mone<vT>()), boost::simd::Mone<r_t>());
  NT2_TEST_EQUAL(toint(boost::simd::One<vT>()), boost::simd::One<r_t>());
  NT2_TEST_EQUAL(toint(boost::simd::Zero<vT>()), boost::simd::Zero<r_t>());
} // end of test for signed_int_


#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <geometry_msgs/Transform.h>
#include <sensor_msgs/JointState.h>
#include <gazebo_msgs/LinkStates.h>
#include <Eigen/Dense>

#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH

visualization_msgs::Marker reached_points, robot_trail, points;
geometry_msgs::Point point_buffer;
double d;
void update_line(const gazebo_msgs::LinkStates::ConstPtr &pos)
{
    int L = pos->pose.size();

    point_buffer.x = 0.5*(pos->pose.at(L - 1).position.x + pos->pose.at(L - 2).position.x);
    point_buffer.y = 0.5*(pos->pose.at(L - 1).position.y + pos->pose.at(L - 2).position.y);
    point_buffer.z = 0.5*(pos->pose.at(L - 1).position.z + pos->pose.at(L - 2).position.z);

    if (robot_trail.points.size() < 800)
    {
        robot_trail.points.push_back(point_buffer);
    }
    else
    {
        robot_trail.points.erase(robot_trail.points.begin());
        robot_trail.points.push_back(point_buffer);
    }

}

void update_point(int data)
{

	double th = 0.02;
	if (data > 2)
		th = 0.06;
    for (int i = 0; i < robot_trail.points.size(); i++)
    {
        for (int j = 0; j < points.points.size(); j++)
        {
            d = sqrt(pow(robot_trail.points.at(i).x - points.points.at(j).x, 2) +
                     pow(robot_trail.points.at(i).y - points.points.at(j).y, 2) +
                     pow(robot_trail.points.at(i).z - points.points.at(j).z, 2));
            if (d < th)
            {
                reached_points.points.push_back(points.points.at(j));
                points.points.erase(points.points.begin() + j);
                return;
            }
        }
    }
}

Eigen::Matrix4d dh_matrix_standard(double a, double alpha, double d, double theta)
{
    Eigen::Matrix4d A;
    A(3, 3) = 1.0;
    A(3, 2) = 0.0;
    A(3, 1) = 0.0;
    A(3, 0) = 0.0;

    A(0, 0) = cos(theta);
    A(0, 1) = -sin(theta)*cos(alpha);
    A(0, 2) = sin(theta)*sin(alpha);
    A(0, 3) = a * cos(theta);

    A(1, 0) = sin(theta);
    A(1, 1) = cos(theta)*cos(alpha);
    A(1, 2) = -cos(theta)*sin(alpha);
    A(1, 3) = a * sin(theta);

    A(2, 0) = 0.0;
    A(2, 1) = sin(alpha);
    A(2, 2) = cos(alpha);
    A(2, 3) = d;

    return A;
}

Eigen::Matrix4d youbot_forward_kine(double joint[])
{

    double a[5] = {-0.033, 0.155, 0.135, -0.002, 0.0};
    double alpha[5] = {M_PI_2, 0.0, 0.0, M_PI_2, M_PI};
    double d[5] = {0.145, 0.0, 0.0, 0.0, -0.185};
    double theta[5] = {170*M_PI/180 - M_PI, M_PI_2 - 65*M_PI/180, 146*M_PI/180, -M_PI_2-102.5*M_PI/180, M_PI - 167.5*M_PI/180};

    Eigen::Matrix4d A = Eigen::Matrix4d::Identity(4, 4);

    for (int i = 0; i < 5;i++)
    {
        if (i == 0)
            A = A * dh_matrix_standard(a[i], alpha[i], d[i], theta[i] - joint[i]);
        else
            A = A * dh_matrix_standard(a[i], alpha[i], d[i], theta[i] + joint[i]);
    }

    return A;

}

int main( int argc, char** argv )
{
    ros::init(argc, argv, "trail_node");
    ros::NodeHandle n;
    ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("/visualization_marker", 10);
    ros::Subscriber traj_sub = n.subscribe<gazebo_msgs::LinkStates>("/gazebo/link_states", 10, update_line);
    ros::Rate r(20);

    int checkpoint_data = 1;

    //Define points message
    points.header.frame_id = robot_trail.header.frame_id = reached_points.header.frame_id = "/base_link";
    points.header.stamp = robot_trail.header.stamp = reached_points.header.stamp = ros::Time::now();
    points.ns = robot_trail.ns = reached_points.ns = "points_and_lines";
    points.action = robot_trail.action = reached_points.action = visualization_msgs::Marker::ADD;
    points.pose.orientation.w = robot_trail.pose.orientation.w = reached_points.pose.orientation.w = 1.0;

    points.id = 0;
    robot_trail.id = 1;
    reached_points.id = 2;

    points.type = visualization_msgs::Marker::POINTS;
    reached_points.type = visualization_msgs::Marker::POINTS;
    robot_trail.type = visualization_msgs::Marker::LINE_STRIP;

    // POINTS markers use x and y scale for width/height respectively
    points.scale.x = 0.01;
    points.scale.y = 0.01;

    // Unreached points are red
    points.color.r = 1.0f;
    points.color.a = 1.0;

    // POINTS markers use x and y scale for width/height respectively
    reached_points.scale.x = 0.01;
    reached_points.scale.y = 0.01;

    // Reached points are red
    reached_points.color.g = 1.0f;
    reached_points.color.a = 1.0;

    //Define the width of robot trail
    robot_trail.scale.x = 0.005;

    // Robot trails are white
    robot_trail.color.r = 1.0f;
    robot_trail.color.g = 1.0f;
    robot_trail.color.b = 1.0f;
    robot_trail.color.a = 1.0;

    rosbag::Bag bag;

    bag.open(MY_BAG_PATH, rosbag::bagmode::Read);

    std::vector<std::string> topics;

    if ((checkpoint_data == 1))
    {
        topics.push_back(std::string("joint_data"));
        rosbag::View view(bag, rosbag::TopicQuery(topics));

                foreach(rosbag::MessageInstance const m, view)
                    {
                        sensor_msgs::JointState::ConstPtr s = m.instantiate<sensor_msgs::JointState>();
                        if (s != NULL)
                        {

                            double joint[5];

                            for (int i = 0; i < 5; i++)
	                    	joint[i] = s->position.at(i);

                            Eigen::Matrix4d current_pose = youbot_forward_kine(joint);

                            geometry_msgs::Point p;
                            p.x = current_pose(0, 3);
                            p.y = current_pose(1, 3);
                            p.z = current_pose(2, 3);
                            points.points.push_back(p);

                        }

                    }

    }
    else
    {
        topics.push_back(std::string("target_position"));
        rosbag::View view(bag, rosbag::TopicQuery(topics));

                foreach(rosbag::MessageInstance const m, view)
                    {
                        geometry_msgs::Transform::ConstPtr s = m.instantiate<geometry_msgs::Transform>();
                        if (s != NULL)
                        {
                            geometry_msgs::Point p;
                            p.x = s->translation.x;
                            p.y = s->translation.y;
                            p.z = s->translation.z;
                            points.points.push_back(p);
                        }

                    }
    }

    bag.close();

    while (ros::ok())
    {
        marker_pub.publish(points);
        marker_pub.publish(robot_trail);
        marker_pub.publish(reached_points);

        update_point(checkpoint_data);

        ros::spinOnce();

        r.sleep();
    }
}


//---------------------------------------------------------------------------//
// 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://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#ifndef BOOST_COMPUTE_EXPERIMENTAL_SORT_BY_TRANSFORM_HPP
#define BOOST_COMPUTE_EXPERIMENTAL_SORT_BY_TRANSFORM_HPP

#include <iterator>

#include <boost/compute/algorithm/sort_by_key.hpp>
#include <boost/compute/algorithm/transform.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/detail/iterator_range_size.hpp>
#include <boost/compute/type_traits/result_of.hpp>

namespace boost {
namespace compute {
namespace experimental {

template<class Iterator, class Transform, class Compare>
inline void sort_by_transform(Iterator first,
                              Iterator last,
                              Transform transform,
                              Compare compare,
                              command_queue &queue = system::default_queue())
{
    typedef typename std::iterator_traits<Iterator>::value_type value_type;
    typedef typename boost::compute::result_of<Transform(value_type)>::type key_type;

    size_t n = detail::iterator_range_size(first, last);
    if(n < 2){
        return;
    }

    const context &context = queue.get_context();

    ::boost::compute::vector<key_type> keys(n, context);

    ::boost::compute::transform(
        first,
        last,
        keys.begin(),
        transform,
        queue
    );

    ::boost::compute::sort_by_key(
        keys.begin(),
        keys.end(),
        first,
        compare,
        queue
    );
}

} // end experimental namespace
} // end compute namespace
} // end boost namespace

#endif // BOOST_COMPUTE_EXPERIMENTAL_SORT_BY_TRANSFORM_HPP


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program 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.
//
// This program 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 this program. If not, see <http://www.gnu.org/licenses/>.

#include "graph_filtering.hh"
#include "graph.hh"
#include "graph_properties.hh"

#include <boost/graph/chrobak_payne_drawing.hpp>
#include <boost/graph/planar_canonical_ordering.hpp>

using namespace std;
using namespace boost;
using namespace graph_tool;

struct point_t
{
    size_t x, y;
};

void planar_layout(GraphInterface& gi, boost::any aembed_map, boost::any apos)
{
    run_action<graph_tool::detail::never_directed>()
        (gi,
         [&](auto& g, auto& _embed, auto& _pos)
         {
             typedef typename std::remove_reference<decltype(g)>::type g_t;
             typedef typename graph_traits<g_t>::edge_descriptor edge_t;
             typedef typename graph_traits<g_t>::vertex_descriptor vertex_t;

             auto eindex = get(edge_index, g);
             vector<edge_t> edges;
             for (auto e : edges_range(g))
             {
                 auto ei = eindex[e];
                 if (ei >= edges.size())
                     edges.resize(ei + 1);
                 edges[ei] = e;
             }

             typename vprop_map_t<std::vector<edge_t>>::type::unchecked_t
                 embed(get(vertex_index, g), num_vertices(g));

             parallel_vertex_loop
                 (g,
                  [&](auto& v)
                  {
                      for (auto ei : _embed[v])
                          embed[v].push_back(edges[ei]);
                  });

             vector<vertex_t> ordering;
             planar_canonical_ordering(g, embed, std::back_inserter(ordering));

             assert(ordering.size() >= 3);

             typename vprop_map_t<point_t>::type::unchecked_t
                 pos(get(vertex_index_t(), g), num_vertices(g));

             chrobak_payne_straight_line_drawing(g, embed, ordering.begin(),
                                                 ordering.end(), pos,
                                                 get(vertex_index, g));
             parallel_vertex_loop
                 (g,
                  [&](auto& v)
                  {
                      auto& p = pos[v];
                      typedef typename std::remove_reference<decltype(_pos[v][0])>::type val_t;
                      _pos[v] = {val_t(p.x), val_t(p.y)};
                  });

         },
         vertex_scalar_vector_properties(), vertex_scalar_vector_properties())
        (aembed_map, apos);
}

#include <boost/python.hpp>

void export_planar()
{
    boost::python::def("planar_layout", &planar_layout);
}


// Copyright Louis Dionne 2015
// Distributed under the Boost Software License, Version 1.0.

#include <boost/mpl/apply.hpp>
#include <boost/mpl/back_inserter.hpp>
#include <boost/mpl/begin.hpp>
#include <boost/mpl/deref.hpp>
#include <boost/mpl/empty.hpp>
#include <boost/mpl/end.hpp>
#include <boost/mpl/equal.hpp>
#include <boost/mpl/identity.hpp>
#include <boost/mpl/insert_range.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/iterator_range.hpp>
#include <boost/mpl/joint_view.hpp>
#include <boost/mpl/lambda.hpp>
#include <boost/mpl/less.hpp>
#include <boost/mpl/next.hpp>
#include <boost/mpl/partition.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/push_back.hpp>
#include <boost/mpl/single_view.hpp>
#include <boost/mpl/vector.hpp>
using namespace boost::mpl;

// sample(sorting-then1)
template <typename Sequence, typename Pred>
struct sort_impl;

template <typename Sequence, typename Pred>
struct sort
  : eval_if<empty<Sequence>,
      identity<Sequence>,
      sort_impl<Sequence, Pred>
  >
{ };
// end-sample

// sample(sorting-then2)
template <typename Sequence, typename Pred>
struct sort_impl {
  using pivot = typename begin<Sequence>::type;
  using parts = typename partition<
    iterator_range<typename next<pivot>::type,
                   typename end<Sequence>::type>
    , apply2<typename lambda<Pred>::type, _1,
             typename deref<pivot>::type>
    , back_inserter<vector<>>
    , back_inserter<vector<>>
  >::type;

  using part1 = typename push_back<
    typename sort<typename parts::first, Pred>::type,
    typename deref<pivot>::type
  >::type;

  using part2 = typename sort<typename parts::second, Pred>::type;

  using type = typename insert_range<
    part1, typename end<part1>::type, part2
  >::type;
};
// end-sample

static_assert(equal<
  sort<vector<>, quote2<less>>::type,
  vector<>
>::value, "");

static_assert(equal<
  sort<vector<int_<1>>, quote2<less>>::type,
  vector<int_<1>>
>::value, "");

static_assert(equal<
  sort<vector<int_<2>, int_<1>>, quote2<less>>::type,
  vector<int_<1>, int_<2>>
>::value, "");

static_assert(equal<
  sort<vector<int_<3>, int_<2>, int_<1>>, quote2<less>>::type,
  vector<int_<1>, int_<2>, int_<3>>
>::value, "");

static_assert(equal<
  sort<vector<int_<4>, int_<3>, int_<2>, int_<1>>, quote2<less>>::type,
  vector<int_<1>, int_<2>, int_<3>, int_<4>>
>::value, "");

int main() { }