// Copyright (c) 2017 Jeffrey Ichnowski
// All rights reserved.
//
// BSD 3 Clause
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. 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.
// 3. Neither the name of the copyright holder 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.
#pragma once
#ifndef UNC_ROBOTICS_KDTREE_SPACES_HPP
#define UNC_ROBOTICS_KDTREE_SPACES_HPP

#include <Eigen/Dense>
#include <tuple>
#include <functional>
#include <utility>
#include <type_traits>
#include <ratio>
#include "_tuple.hpp"

namespace unc {
namespace robotics {
namespace kdtree {

namespace detail {
template <typename _Scalar, int _dimensions>
class L2SpaceBase {
public:
    typedef _Scalar Distance;
    typedef Eigen::Matrix<_Scalar, _dimensions, 1> State;

    template <typename _Derived>
    bool isValid(const Eigen::MatrixBase<_Derived>& q) const {
        return q.rows() == _dimensions && q.cols() == 1 && q.allFinite();
    }

    template <typename _DerivedA, typename _DerivedB>
    constexpr Distance distance(
        const Eigen::MatrixBase<_DerivedA>& a,
        const Eigen::MatrixBase<_DerivedB>& b) const
    {
        return (a - b).norm();
    }

    template <typename _DerivedA, typename _DerivedB>
    constexpr State interpolate(
        const Eigen::MatrixBase<_DerivedA>& from,
        const Eigen::MatrixBase<_DerivedB>& to,
        Distance t) const
    {
        return from + (to - from) * t;
    }
};
}

template <typename _Scalar, int _dimensions>
class L2Space : public detail::L2SpaceBase<_Scalar, _dimensions> {
public:
    L2Space(unsigned dimensions = _dimensions) {
        assert(dimensions == _dimensions);
    }

    constexpr unsigned dimensions() const { return _dimensions; }

    template <typename _Derived>
    bool isValid(const Eigen::MatrixBase<_Derived>& q) const {
        return q.rows() == _dimensions && detail::L2SpaceBase<_Scalar, _dimensions>::isValid(q);
    }
};

template <typename _Scalar>
class L2Space<_Scalar, Eigen::Dynamic> : public detail::L2SpaceBase<_Scalar, Eigen::Dynamic> {
    unsigned dimensions_;

public:
    L2Space(unsigned dimensions)
        : dimensions_(dimensions)
    {
    }

    constexpr unsigned dimensions() const {
        return dimensions_;
    }

    template <typename _Derived>
    bool isValid(const Eigen::MatrixBase<_Derived>& q) const {
        return q.rows() == dimensions_ && detail::L2SpaceBase<_Scalar, Eigen::Dynamic>::isValid(q);
    }
};

template <typename _Scalar, int _dimensions>
class BoundedL2Space : public L2Space<_Scalar, _dimensions> {
    Eigen::Array<_Scalar, _dimensions, 2> bounds_;

    typedef typename Eigen::Array<_Scalar, _dimensions, 2>::Index Index;

    void checkBounds() {
        assert((bounds_.col(0) < bounds_.col(1)).all());
        assert((bounds_.col(1) - bounds_.col(0)).allFinite());
    }

public:
    using typename L2Space<_Scalar, _dimensions>::State;

    template <typename _Derived>
    BoundedL2Space(const Eigen::DenseBase<_Derived>& bounds)
        : L2Space<_Scalar, _dimensions>(bounds.rows()),
          bounds_(bounds)
    {
        checkBounds();
    }

    template <typename _DerivedMin, typename _DerivedMax>
    BoundedL2Space(
        const Eigen::DenseBase<_DerivedMin>& min,
        const Eigen::DenseBase<_DerivedMax>& max)
        : L2Space<_Scalar, _dimensions>(min.rows())
    {
        bounds_.col(0) = min;
        bounds_.col(1) = max;
    }

    template <typename _Derived>
    bool isValid(const Eigen::MatrixBase<_Derived>& q) const {
        return L2Space<_Scalar, _dimensions>::isValid(q)
            && (bounds_.col(0) <= q).all()
            && (bounds_.col(1) >= q).all();
    }

    const Eigen::Array<_Scalar, _dimensions, 2>& bounds() const {
        return bounds_;
    }

    _Scalar bounds(Index dim, Index j) const {
        return bounds_(dim, j);
    }
};

template <typename _Scalar>
class SO3Space {
public:
    typedef _Scalar Distance;
    typedef Eigen::Quaternion<_Scalar> State;

    constexpr unsigned dimensions() const { return 3; }

    template <typename _Derived>
    bool isValid(const Eigen::QuaternionBase<_Derived>& q) const {
        return std::abs(1 - q.coeffs().squaredNorm()) <= 1e-5;
    }

    template <typename _DerivedA, typename _DerivedB>
    inline Distance distance(
        const Eigen::QuaternionBase<_DerivedA>& a,
        const Eigen::QuaternionBase<_DerivedB>& b) const
    {
        Distance dot = std::abs(a.coeffs().matrix().dot(b.coeffs().matrix()));
        return dot < 0 ? M_PI_2 : dot > 1 ? 0 : std::acos(dot);
    }

    template <typename _DerivedA, typename _DerivedB>
    constexpr State interpolate(
        const Eigen::QuaternionBase<_DerivedA>& from,
        const Eigen::QuaternionBase<_DerivedB>& to,
        Distance t) const
    {
        Distance dq = from.coeffs().matrix().dot(to.coeffs().matrix());
        if (std::abs(dq) >= 1)
            return from;

        Distance theta = std::acos(std::abs(dq));
        Distance d = 1 / std::sin(theta);
        Distance s0 = std::sin((1 - t) * theta);
        Distance s1 = std::sin(t * theta);

        if (dq < 0)
            s1 = -s1;

        return State(d * (from.coeffs() * s0 + to.coeffs() * s1));
    }
};

template <typename _Scalar>
class SO3AltSpace {
public:
    typedef _Scalar Distance;
    typedef Eigen::Quaternion<_Scalar> State;

    constexpr unsigned dimensions() const { return 3; }

    template <typename _Derived>
    bool isValid(const Eigen::QuaternionBase<_Derived>& q) const {
        return std::abs(1 - q.coeffs().squaredNorm()) <= 1e-5;
    }

    template <typename _DerivedA, typename _DerivedB>
    inline Distance distance(
        const Eigen::QuaternionBase<_DerivedA>& a,
        const Eigen::QuaternionBase<_DerivedB>& b) const
    {
        Distance dot = std::abs(a.coeffs().matrix().dot(b.coeffs().matrix()));
        return dot < 0 ? M_PI_2 : dot > 1 ? 0 : std::acos(dot);
    }
};

template <typename _Scalar>
class SO3RLSpace {
public:
    typedef _Scalar Distance;
    typedef Eigen::Quaternion<_Scalar> State;

    constexpr unsigned dimensions() const { return 4; }

    template <typename _Derived>
    bool isValid(const Eigen::QuaternionBase<_Derived>& q) const {
        return std::abs(1 - q.coeffs().squaredNorm()) <= 1e-5;
    }

    template <typename _DerivedA, typename _DerivedB>
    inline Distance distance(
        const Eigen::QuaternionBase<_DerivedA>& a,
        const Eigen::QuaternionBase<_DerivedB>& b) const
    {
        Distance dot = std::abs(a.coeffs().matrix().dot(b.coeffs().matrix()));
        return dot < 0 ? M_PI_2 : dot > 1 ? 0 : std::acos(dot);
    }
};

template <typename _Space, typename _Ratio = std::ratio<1>>
class RatioWeightedSpace : public _Space {
public:
    typedef _Ratio Ratio;

    static constexpr std::intmax_t num = Ratio::num;
    static constexpr std::intmax_t den = Ratio::den;

    // inherit constructor
    using _Space::_Space;

    // RatioWeightedSpace() {}

    RatioWeightedSpace(const _Space& space)
        : _Space(space)
    {
    }

    RatioWeightedSpace(_Space&& space)
        : _Space(std::forward<_Space>(space))
    {
    }

    template <typename _A, typename _B>
    inline typename _Space::Distance distance(const _A& a, const _B& b) const {
        return _Space::distance(a, b) * num / den;
    }
};

template <std::intmax_t num, std::intmax_t den = 1, typename _Space>
auto makeRatioWeightedSpace(_Space&& space) {
    return RatioWeightedSpace<_Space, std::ratio<num, den>>(std::forward<_Space>(space));
}

template <typename _Space>
class WeightedSpace : public _Space {
public:
    using typename _Space::Distance;

private:
    Distance weight_;

public:
    WeightedSpace(Distance weight, const _Space& space = _Space())
        : _Space(space), weight_(weight)
    {
    }

    WeightedSpace(Distance weight, _Space&& space)
        : _Space(std::forward<_Space>(space)), weight_(weight)
    {
    }

    template <typename ... _Args>
    WeightedSpace(Distance weight, _Args&& ... args)
        : _Space(std::forward<_Args>(args)...),
          weight_(weight)
    {
    }

    constexpr Distance weight() const {
        return weight_;
    }

    template <typename _A, typename _B>
    constexpr Distance distance(const _A& a, const _B& b) const {
        return _Space::distance(a, b) * weight_;
    }
};

template <typename ... _Spaces>
class CompoundSpace {
    typedef std::tuple<_Spaces...> Spaces;

    Spaces spaces_;

    static_assert(sizeof...(_Spaces) > 1, "compound space must have two or more subspaces");

public:
    typedef std::tuple<typename _Spaces::State...> State;
    typedef typename detail::SumResultType<typename _Spaces::Distance...>::type Distance;

    explicit CompoundSpace() {
    }

    explicit CompoundSpace(const _Spaces& ... args)
        : spaces_(args...)
    {
    }

    template <typename ... _Args>
    explicit CompoundSpace(_Args&& ... args)
        : spaces_(std::forward<_Args>(args)...)
    {
    }

    template <std::size_t I>
    constexpr typename std::tuple_element<I, Spaces>::type& get() {
        return std::get<I>(spaces_);
    }

    template <std::size_t I>
    constexpr typename std::tuple_element<I, Spaces>::type const& get() const {
        return std::get<I>(spaces_);
    }

    inline constexpr unsigned dimensions() const {
        using namespace detail;
        return sum(map([](const auto& space) { return space.dimensions(); }, spaces_));
    }

    template <typename _State>
    bool isValid(_State&& q) const {
        using namespace detail;
        // TODO: return reduce(std::logical_and<bool>(), zip([](auto&& subs, auto&& subq) { return subs.isValid(subq); }, spaces_, q));
        assert(false);
        return false;
    }

    template <typename _StateA, typename _StateB>
    inline Distance distance(_StateA&& a, _StateB&& b) const {
        using namespace detail;
        return sum(zip([](auto&& subs, auto&& suba, auto&& subb) {
                    return subs.distance(suba, subb);
                }, spaces_, a, b));
    }

private:
    template <typename _StateA, typename _StateB, std::size_t ... I>
    constexpr State interpolate(
        const _StateA& from,
        const _StateB& to,
        Distance t,
        std::index_sequence<I...>) const
    {
        return State(std::get<I>(spaces_).interpolate(std::get<I>(from), std::get<I>(to), t)...);
    }

public:
    template <typename _StateA, typename _StateB>
    constexpr State interpolate(
        const _StateA& from,
        const _StateB& to,
        Distance t) const
    {
        return interpolate(from, to, t, std::make_index_sequence<sizeof...(_Spaces)>{});
    }

};

template <typename ... _Spaces>
constexpr auto makeCompoundSpace(_Spaces&&... args) {
    return CompoundSpace<typename std::decay<_Spaces>::type...>(std::forward<_Spaces>(args)...);
}

template <typename _Space>
constexpr auto makeWeightedSpace(
    typename _Space::Distance weight,
    _Space&& space = _Space())
{
    return WeightedSpace<_Space>(weight, std::forward<_Space>(space));
}

template <typename _Scalar, std::intmax_t _qWeight = 1, std::intmax_t _tWeight = 1>
using SE3Space = CompoundSpace<
    RatioWeightedSpace<SO3Space<_Scalar>, std::ratio<_qWeight>>,
    RatioWeightedSpace<L2Space<_Scalar, 3>, std::ratio<_tWeight>>>;

template <typename _Scalar, std::intmax_t _qWeight = 1, std::intmax_t _tWeight = 1>
using BoundedSE3Space = CompoundSpace<
    RatioWeightedSpace<SO3Space<_Scalar>, std::ratio<_qWeight>>,
    RatioWeightedSpace<BoundedL2Space<_Scalar, 3>, std::ratio<_tWeight>>>;

}
}
}

namespace std {

template <std::size_t I, class ... _Spaces>
class tuple_element<I, unc::robotics::kdtree::CompoundSpace<_Spaces...>> {
public:
    typedef typename std::tuple_element<I, std::tuple<_Spaces...>>::type type;
};

template <std::size_t I, class ... _Spaces>
constexpr typename std::tuple_element<I, std::tuple<_Spaces...>>::type&
get(unc::robotics::kdtree::CompoundSpace<_Spaces...>& space) {
    return space.template get<I>();
}

template <std::size_t I, class ... _Spaces>
constexpr typename std::tuple_element<I, std::tuple<_Spaces...>>::type const&
get(const unc::robotics::kdtree::CompoundSpace<_Spaces...>& space) {
    return space.template get<I>();
}

}

#endif // UNC_ROBOTICS_KDTREE_SPACES_HPP


#ifndef RANDOM_GENERATOR_H_
#define RANDOM_GENERATOR_H_

#include <memory>
#include <time.h>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/uniform_int.hpp>

// https://www.boost.org/doc/libs/1_59_0/libs/random/example/random_demo.cpp
// https://www.boost.org/doc/libs/1_43_0/doc/html/boost_random/tutorial.html
class RandomGenerator {
  private:
    static RandomGenerator* instance;
//    shared_ptr<boost::variate_generator<boost::mt19937, boost::normal_distribution<>>> pGenerator_;
    std::shared_ptr<boost::variate_generator<boost::mt19937, boost::uniform_real<>>> pGenerator_;
    std::shared_ptr<boost::variate_generator<boost::mt19937, boost::uniform_int<>>> pIntGenerator_;

    RandomGenerator();

  public:
    static RandomGenerator* getInstance();
    double getDouble();
    double getMaxDouble();
    int getInt();
    int getMaxInt();
};

#endif // RANDOM_GENERATOR_H_


//          Copyright Gavin Band 2008 - 2012.
// 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)

#ifndef QCTOOL_RELATEDNESS_COMPONENT_LAPACK_EIGEN_SOLVER
#define QCTOOL_RELATEDNESS_COMPONENT_LAPACK_EIGEN_SOLVER

#include <Eigen/Core>

namespace lapack
{
	void compute_eigendecomposition( Eigen::MatrixXd const& matrix, Eigen::VectorXd* eigenvalues, Eigen::MatrixXd* eigenvectors ) ;

	void compute_partial_eigendecomposition(
		Eigen::MatrixXd const& matrix,
		Eigen::VectorXd* eigenvalues,
		Eigen::MatrixXd* eigenvectors,
		double minimum_eigenvalue,
		double maximum_eigenvalue = std::numeric_limits< double >::max()
	) ;

	void compute_partial_eigendecomposition(
		Eigen::MatrixXd const& matrix,
		Eigen::VectorXd* eigenvalues,
		Eigen::MatrixXd* eigenvectors,
		int number_of_eigenvalues
	) ;
}

#endif


#include "clstm.h"
#include <assert.h>
#include <iostream>
#include <vector>
#include <memory>
#include <math.h>
#include <Eigen/Dense>
#include <string>
#include <sstream>
#include <fstream>
#include <iostream>

#include "multidim.h"
#include "pymulti.h"
#include "extras.h"

using std_string = std::string;
#define string std_string
using std::vector;
using std::shared_ptr;
using std::make_shared;
using std::unique_ptr;
using std::to_string;
using std::make_pair;
using std::cout;
using std::stoi;
using namespace Eigen;
using namespace ocropus;
using namespace pymulti;

struct Image2Image : public ITrainable {
  mdarray<float> input, d_input, output, d_output;
  int idepth = -1;
  int odepth = -1;
  Network net;
  void setLearningRate(Float lr, Float momentum) {
    net->setLearningRate(lr, momentum);
  }
};

inline int clip(int x, int hi) {
  if (x > hi) return hi;
  if (x < 0) return 0;
  return x;
}

void batches_of_image_0(Sequence &batches, mdarray<float> &image, int depth,
                        int border = 0) {
  int d0 = image.dim(0), d1 = image.dim(1), d2 = image.dim(2);
  assert(depth == image.dim(2));
  int batch_depth = (2 * border + 1) * d2;
  batches.resize(d0);
  for (int i = 0; i < d0; i++) {
    batches[i].resize(batch_depth, d1);
    for (int j = 0; j < d1; j++) {
      int l = 0;
      for (int r = -border; r <= border; r++) {
        for (int k = 0; k < d2; k++)
          batches[i](l++, j) = image(clip(i + r, d0), j, k);
      }
      assert(l == batch_depth);
    }
  }
}

void image_of_batches_0(mdarray<float> &image, Sequence &batches, int depth,
                        int border = 0) {
  int d0 = batches.size();
  int d1 = COLS(batches[0]);
  int d2 = depth;
  assert((2 * border + 1) * depth == ROWS(batches[0]));
  image.resize(d0, d1, d2);
  for (int i = 0; i < d0; i++) {
    for (int j = 0; j < d1; j++) {
      int l = 0;
      for (int r = -border; r <= border; r++) {
        for (int k = 0; k < d2; k++)
          if (i + r >= 0 && i + r < d0) image(i + r, j, k) = batches[i](l++, j);
      }
      assert(l == ROWS(batches[i]));
    }
  }
}

struct Vstrips : public Image2Image {
  int border = 0;
  void initialize() {
    this->odepth = iattr("noutput");
    this->idepth = iattr("ninput");
    net.reset(make_LSTM1());
    net->init(odepth, (2 * border + 1) * idepth);
  }
  void forward() {
    assert(input.rank() == 3);
    assert(input.dim(2) == idepth);
    batches_of_image_0(net->inputs, input, idepth, border);
    // print("inputs", net->inputs.size(), ROWS(net->inputs[0]),
    // COLS(net->inputs[0]), "/", net->ninput());
    net->forward();
    image_of_batches_0(output, net->outputs, odepth);
  }
  void backward() {
    assert(d_output.rank() == 3);
    assert(output.dim(2) == odepth);
    batches_of_image_0(net->d_outputs, d_output, odepth);
    net->backward();
    image_of_batches_0(d_input, net->d_inputs, idepth, border);
  }
  void update() { net->update(); }
};

int main1(int argc, char **argv) {
  try {
    mdarray<float> input, output;
    Sequence seq;
    read_png(input, argv[1]);
    print("input", input.dim(0), input.dim(1), input.dim(2));
    print("irange", input.min(), input.max());
    batches_of_image_0(seq, input, 3);
    print("seq", seq.size(), ROWS(seq[0]), COLS(seq[0]));
    image_of_batches_0(output, seq, 3);
    write_png(argv[2], input);
  } catch (const char *msg) {
    print("ERROR:", msg);
  }
}

int main2(int argc, char **argv) {
  try {
    shared_ptr<Vstrips> net;
    net.reset(new Vstrips());
    net->border = 0;
    net->init(3, 3);
    read_png(net->input, argv[1]);
    net->forward();
    write_png(argv[2], net->input);
  } catch (const char *msg) {
    print("ERROR:", msg);
  }
}

int main3(int argc, char **argv) {
  double lrate = getrenv("lrate", 1e-3);
  double momentum = getrenv("momentum", 0.9);
  vector<string> files;
  string line;
  std::ifstream stream(argv[1]);
  while (getline(stream, line)) files.push_back(line);
  try {
    shared_ptr<Vstrips> net;
    net.reset(new Vstrips());
    net->border = 0;
    net->init(3, 3);
    net->setLearningRate(lrate, momentum);
    for (int trial = 0; trial < 1000000; trial++) {
      mdarray<float> image, out;
      string file = files[trial % files.size()];
      print(trial, file);
      read_png(image, file.c_str());
      print("image", image.dim(0), image.dim(1), image.dim(2));
      net->input = image;
      print("input", net->input.dim(0), net->input.dim(1), net->input.dim(2));
      print("irange", net->input.min(), net->input.max());
      net->forward();
      print("output", net->output.dim(0), net->output.dim(1),
            net->output.dim(2));
      print("orange", net->output.min(), net->output.max());
      out = net->output;
      out.clip(0.0, 1.0);
      write_png("temp.png", out);
      net->d_output = image;
      net->d_output -= net->output;
      net->backward();
      net->update();
    }
  } catch (const char *msg) {
    print("ERROR:", msg);
  }
}

double sqr(double x) { return x * x; }
double norm(Sequence &seq) {
  double err = 0.0;
  int dim = ROWS(seq[0]), bs = COLS(seq[0]);
  for (int t = 0; t < seq.size(); t++)
    for (int i = 0; i < dim; i++)
      for (int j = 0; j < bs; j++) err += sqr(seq[t](i, j));
  return err;
}
void threshold(Sequence &seq, double threshold) {
  int dim = ROWS(seq[0]);
  int bs = COLS(seq[0]);
  for (int t = 0; t < seq.size(); t++)
    for (int i = 0; i < dim; i++)
      for (int j = 0; j < bs; j++) seq[t](i, j) = (seq[t](i, j) > threshold);
}

int main4(int argc, char **argv) {
  double lrate = getrenv("lrate", 1e-4);
  double momentum = getrenv("momentum", 0.9);
  try {
    Network net;
    net.reset(make_LSTM1());
    net->init(2, 10, 2);
    net->setLearningRate(lrate, momentum);
    for (int trial = 0; trial < 1000000; trial++) {
      Sequence seq;
      seq.resize(20);
      for (int t = 0; t < seq.size(); t++) seq[t] = Mat::Random(2, 1);
      threshold(seq, 0.5);
      set_inputs(net.get(), seq);
      net->forward();
      set_targets(net.get(), seq);
      if (trial % 100 == 0) print(trial, norm(net->d_outputs));
      net->backward();
      net->update();
      if (trial % 10000 == 0) {
        Sequence &out = net->outputs;
        for (int t = 0; t < seq.size(); t++)
          print(t, ":", seq[t](0, 0), seq[t](1, 0), " / ", out[t](0, 0),
                out[t](1, 0));
      }
    }
  } catch (const char *msg) {
    print("ERROR:", msg);
  }
}

int main5(int argc, char **argv) {
  double lrate = getrenv("lrate", 1e-3);
  double momentum = getrenv("momentum", 0.9);
  try {
    Network lstm;
    lstm.reset(make_LSTM1());
    lstm->init(2, 2, 2);
    lstm->setLearningRate(lrate, momentum);
    shared_ptr<Vstrips> net;
    net.reset(new Vstrips());
    net->border = 0;
    net->net = lstm;
    net->idepth = 2;
    net->odepth = 2;
    for (int trial = 0; trial < 1000000; trial++) {
      mdarray<float> input, output;
      Sequence seq;
      seq.resize(20);
      for (int t = 0; t < seq.size(); t++)
        seq[t] = Mat::Random(2, 1).cwiseAbs();
      threshold(seq, 0.5);
      input.resize((int)seq.size(), (int)COLS(seq[0]), (int)ROWS(seq[0]));
      for (int t = 0; t < input.dim(0); t++)
        for (int i = 0; i < input.dim(1); i++)
          for (int j = 0; j < input.dim(2); j++) input(t, i, j) = seq[t](j, i);
      net->input = input;
      net->forward();
      output = net->output;
      net->d_output = input;
      net->d_output -= output;
      if (trial % 10000 == 0) {
        print("in", input.min(), input.max(), "out", output.min(), output.max(),
              "err", net->d_output.norm());
        for (int t = 0; t < input.dim(0); t++)
          print(t, ":", input(t, 0, 0), input(t, 0, 1), " / ", output(t, 0, 0),
                output(t, 0, 1));
      }
      net->backward();
      net->update();
    }
  } catch (const char *msg) {
    print("ERROR:", msg);
  }
}

#ifndef MAIN
#define MAIN main4
#endif

int main(int argc, char **argv) { return MAIN(argc, argv); }


/** @file
 *****************************************************************************

 Implementation of interfaces for the class ZerocashParams.

 See ZerocashParams.h .

 *****************************************************************************
 * @author     This file is part of libzerocash, developed by the Zerocash
 *             project and contributors (see AUTHORS).
 * @copyright  MIT license (see LICENSE file)
 *****************************************************************************/

#include <fstream>
#include <boost/format.hpp>

#include "Zerocash.h"
#include "ZerocashParams.h"

static void throw_missing_param_file_exception(std::string paramtype, std::string path) {
    /* paramtype should be either "proving" or "verifying". */
    const char* tmpl = ("Could not open %s key file: %s\n"
                        "Please refer to user documentation for installing this file.");
    throw std::runtime_error((boost::format(tmpl) % paramtype % path).str());
}

namespace libzerocash {

int ZerocashParams::getTreeDepth()
{
    return treeDepth;
}

zerocash_pour_keypair<ZerocashParams::zerocash_pp> ZerocashParams::GenerateNewKeyPair(const unsigned int tree_depth)
{
    libzerocash::ZerocashParams::zerocash_pp::init_public_params();
    libzerocash::zerocash_pour_keypair<libzerocash::ZerocashParams::zerocash_pp> kp_v1 =
        libzerocash::zerocash_pour_ppzksnark_generator<libzerocash::ZerocashParams::zerocash_pp>(
            libzerocash::ZerocashParams::numPourInputs,
            libzerocash::ZerocashParams::numPourOutputs,
            tree_depth
        );
    return kp_v1;
}

void ZerocashParams::SaveProvingKeyToFile(const zerocash_pour_proving_key<ZerocashParams::zerocash_pp>* p_pk_1, std::string path)
{
    std::stringstream ssProving;
    ssProving << p_pk_1->r1cs_pk;
    std::ofstream pkFilePtr;
    pkFilePtr.open(path, std::ios::binary);
    ssProving.rdbuf()->pubseekpos(0, std::ios_base::out);
    pkFilePtr << ssProving.rdbuf();
    pkFilePtr.flush();
    pkFilePtr.close();
}

void ZerocashParams::SaveVerificationKeyToFile(const zerocash_pour_verification_key<ZerocashParams::zerocash_pp>* p_vk_1, std::string path)
{
    std::stringstream ssVerification;
    ssVerification << p_vk_1->r1cs_vk;
    std::ofstream vkFilePtr;
    vkFilePtr.open(path, std::ios::binary);
    ssVerification.rdbuf()->pubseekpos(0, std::ios_base::out);
    vkFilePtr << ssVerification.rdbuf();
    vkFilePtr.flush();
    vkFilePtr.close();
}

zerocash_pour_proving_key<ZerocashParams::zerocash_pp> ZerocashParams::LoadProvingKeyFromFile(std::string path, const unsigned int tree_depth)
{
    std::stringstream ssProving;
    std::ifstream fileProving(path, std::ios::binary);

    if(!fileProving.is_open()) {
        throw_missing_param_file_exception("proving", path);
    }

    ssProving << fileProving.rdbuf();
    fileProving.close();

    ssProving.rdbuf()->pubseekpos(0, std::ios_base::in);

    r1cs_ppzksnark_proving_key<ZerocashParams::zerocash_pp> pk_temp;
    ssProving >> pk_temp;

    return zerocash_pour_proving_key<ZerocashParams::zerocash_pp>(
        libzerocash::ZerocashParams::numPourInputs,
        libzerocash::ZerocashParams::numPourOutputs,
        tree_depth,
        std::move(pk_temp)
    );
}

zerocash_pour_verification_key<ZerocashParams::zerocash_pp> ZerocashParams::LoadVerificationKeyFromFile(std::string path, const unsigned int tree_depth)
{
    std::stringstream ssVerification;
    std::ifstream fileVerification(path, std::ios::binary);

    if(!fileVerification.is_open()) {
        throw_missing_param_file_exception("verification", path);
    }

    ssVerification << fileVerification.rdbuf();
    fileVerification.close();

    ssVerification.rdbuf()->pubseekpos(0, std::ios_base::in);

    r1cs_ppzksnark_verification_key<ZerocashParams::zerocash_pp> vk_temp;
    ssVerification >> vk_temp;

    return zerocash_pour_verification_key<ZerocashParams::zerocash_pp>(
        libzerocash::ZerocashParams::numPourInputs,
        libzerocash::ZerocashParams::numPourOutputs,
        std::move(vk_temp)
    );
}

ZerocashParams::ZerocashParams(
    const unsigned int tree_depth,
    zerocash_pour_keypair<ZerocashParams::zerocash_pp> *keypair
) :
    treeDepth(tree_depth)
{
    params_pk_v1 = new zerocash_pour_proving_key<ZerocashParams::zerocash_pp>(keypair->pk);
    params_vk_v1 = new zerocash_pour_verification_key<ZerocashParams::zerocash_pp>(keypair->vk);
}

ZerocashParams::ZerocashParams(
    const unsigned int tree_depth,
    std::string proving_key_path,
    zerocash_pour_verification_key<ZerocashParams::zerocash_pp>* p_vk_1
) :
    treeDepth(tree_depth), provingKeyPath(proving_key_path)
{
    params_vk_v1 = new zerocash_pour_verification_key<ZerocashParams::zerocash_pp>(*p_vk_1);
    params_pk_v1 = NULL;
}

ZerocashParams::ZerocashParams(
    const unsigned int tree_depth,
    zerocash_pour_proving_key<ZerocashParams::zerocash_pp>* p_pk_1,
    zerocash_pour_verification_key<ZerocashParams::zerocash_pp>* p_vk_1
) :
    treeDepth(tree_depth)
{
    assert(p_pk_1 != NULL || p_vk_1 != NULL);

    if (p_pk_1 == NULL) {
        params_pk_v1 = NULL;
    } else {
        params_pk_v1 = new zerocash_pour_proving_key<ZerocashParams::zerocash_pp>(*p_pk_1);
    }

    if (p_vk_1 == NULL) {
        params_vk_v1 = NULL;
    } else {
        params_vk_v1 = new zerocash_pour_verification_key<ZerocashParams::zerocash_pp>(*p_vk_1);
    }
}

ZerocashParams::~ZerocashParams()
{
    if (params_pk_v1 != NULL) {
        delete params_pk_v1;
    }
    if (params_vk_v1 != NULL) {
        delete params_vk_v1;
    }
}

const zerocash_pour_proving_key<ZerocashParams::zerocash_pp>& ZerocashParams::getProvingKey()
{
    if (params_pk_v1 != NULL) {
        return *params_pk_v1;
    } else {
        throw std::runtime_error("Pour proving key not set.");
    }
}

const zerocash_pour_verification_key<ZerocashParams::zerocash_pp>& ZerocashParams::getVerificationKey()
{
    if (params_vk_v1 != NULL) {
        return *params_vk_v1;
    } else {
        throw std::runtime_error("Pour verification key not set.");
    }
}

} /* namespace libzerocash */


#ifndef CZI_EXPRESSION_MATRIX2_SIGNATURE_GRAPH_HPP
#define CZI_EXPRESSION_MATRIX2_SIGNATURE_GRAPH_HPP

// In the signature graph, each vertex represents
// the set of all cells with a given signature.

#include "BitSet.hpp"
#include "Ids.hpp"

#include <boost/graph/adjacency_list.hpp>

#include "array.hpp"
#include "iosfwd.hpp"
#include "map.hpp"
#include "utility.hpp"

namespace ChanZuckerberg {
    namespace ExpressionMatrix2 {

        class SignatureGraph;
        class SignatureGraphEdge;
        class SignatureGraphVertex;

        // The base class for class SignatureGraph.
        typedef boost::adjacency_list<
            boost::vecS,
            boost::vecS,
            boost::undirectedS,
            SignatureGraphVertex,
            SignatureGraphEdge> SignatureGraphBaseClass;
    }
}

class ChanZuckerberg::ExpressionMatrix2::SignatureGraphVertex {
public:

    // The signature common to all cells of this vertex.
    BitSetPointer signature;

    // The cells with this signature.
    vector<CellId> localCellIds;    // Local to cell set used to create the signature graph.
    vector<CellId> globalCellIds;
    CellId cellCount() const
    {
        return CellId(localCellIds.size());
    }

    // The position of this vertex in the 2-D graph layout.
    array<double, 2> position;

    // The colors to be used to render this vertex.
    // If more than onem, the vertex is rendered as a pie chart.
    // Each color comes with a weight that determines
    // its fraction of the pie chart.
    vector< pair<string, double> > colors;
};

class ChanZuckerberg::ExpressionMatrix2::SignatureGraphEdge {
public:
};

class ChanZuckerberg::ExpressionMatrix2::SignatureGraph :
    public SignatureGraphBaseClass {
public:

    // The vertex map gives the vertex with a given signature.
    map<BitSetPointer, vertex_descriptor> vertexMap;

    // Given the vertices, create the edges.
    void createEdges(size_t lshBitCount);

    // Write out the signature graph in Graphviz format.
    void writeGraphviz(const string& fileName) const;
    void writeGraphviz(ostream&) const;

    // Parameters to control svg output.
    class SvgParameters {
    public:
        // Parameters that can be set arbitrarily.
        bool hideEdges = false;
        double svgSizePixels = 500;
        double xShift = 0.;
        double yShift = 0.;
        double zoomFactor = 1.;
        double vertexSizeFactor = 1.;
        double edgeThicknessFactor = 1.;
        // Parameters derived from the above values.
        // These are filled in by writeSvg.
        double xCenter;
        double yCenter;
        double halfViewBoxSize;
        double pixelSize;
    };

    // Write out the signature graph in SVG format.
    void writeSvg(
        const string& fileName,
        SvgParameters&);
    void writeSvg(
        ostream& s,
        SvgParameters&);

private:
    class Writer {
    public:
        Writer(const SignatureGraph&);
        void operator()(ostream&) const;
        void operator()(ostream&, vertex_descriptor) const;
        void operator()(ostream&, edge_descriptor) const;
        const SignatureGraph& graph;
    };

    // Use Graphviz to compute the graph layout and store it in the vertex positions.
    void computeLayout();
    bool layoutWasComputed = false;
};

#endif


// Author of MMS_SLAM: Wang Han
// Email wh200720041@gmail.com
// Homepage https://wanghan.pro

//c++ lib
#include <cmath>
#include <vector>
#include <mutex>
#include <queue>
#include <thread>
#include <chrono>

//ros lib
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>

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

//pcl lib
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
#include <pcl_ros/impl/transforms.hpp>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/filters/crop_box.h>
#include <pcl/filters/voxel_grid.h>
//cv_bridge
#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>

//opencv lib
#include <opencv2/imgproc.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui.hpp>

std::mutex mutex_lock;
std::queue<sensor_msgs::ImageConstPtr> colorImageBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> pointCloudBuf;
double map_resolution = 0.4;

ros::Publisher pubStaticPointCloud;
ros::Publisher pubDynamicPointCloud;
ros::Publisher pubVisualPointCloud;
ros::Publisher pubimage;
void ColorImageHandler(const sensor_msgs::ImageConstPtr &colorImageMsg)
{
    mutex_lock.lock();
    colorImageBuf.push(colorImageMsg);
    mutex_lock.unlock();
     //ROS_INFO("im in");
}
void velodyneHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{
    mutex_lock.lock();
    pointCloudBuf.push(laserCloudMsg);
    mutex_lock.unlock();
    //ROS_INFO("pc in");
}

void draBoundingBox(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& pc_in, double min_x, double min_y, double min_z, double max_x, double max_y, double max_z, int r, int g, int b){
    for(double i=min_x;i<max_x;i=i+0.02){
        pcl::PointXYZRGB point_temp1;
        point_temp1.x = i;
        point_temp1.y = min_y;
        point_temp1.z = min_z;
        point_temp1.r = r;
        point_temp1.g = g;
        point_temp1.b = b;
        pc_in->push_back(point_temp1);

        pcl::PointXYZRGB point_temp2;
        point_temp2.x = i;
        point_temp2.y = min_y;
        point_temp2.z = max_z;
        point_temp2.r = r;
        point_temp2.g = g;
        point_temp2.b = b;
        pc_in->push_back(point_temp2);

        pcl::PointXYZRGB point_temp3;
        point_temp3.x = i;
        point_temp3.y = max_y;
        point_temp3.z = min_z;
        point_temp3.r = r;
        point_temp3.g = g;
        point_temp3.b = b;
        pc_in->push_back(point_temp3);

        pcl::PointXYZRGB point_temp4;
        point_temp4.x = i;
        point_temp4.y = max_y;
        point_temp4.z = max_z;
        point_temp4.r = r;
        point_temp4.g = g;
        point_temp4.b = b;
        pc_in->push_back(point_temp4);
    }

    for(double i=min_y;i<max_y;i=i+0.02){
        pcl::PointXYZRGB point_temp1;
        point_temp1.x = min_x;
        point_temp1.y = i;
        point_temp1.z = min_z;
        point_temp1.r = r;
        point_temp1.g = g;
        point_temp1.b = b;
        pc_in->push_back(point_temp1);

        pcl::PointXYZRGB point_temp2;
        point_temp2.x = min_x;
        point_temp2.y = i;
        point_temp2.z = max_z;
        point_temp2.r = r;
        point_temp2.g = g;
        point_temp2.b = b;
        pc_in->push_back(point_temp2);

        pcl::PointXYZRGB point_temp3;
        point_temp3.x = max_x;
        point_temp3.y = i;
        point_temp3.z = min_z;
        point_temp3.r = r;
        point_temp3.g = g;
        point_temp3.b = b;
        pc_in->push_back(point_temp3);

        pcl::PointXYZRGB point_temp4;
        point_temp4.x = max_x;
        point_temp4.y = i;
        point_temp4.z = max_z;
        point_temp4.r = r;
        point_temp4.g = g;
        point_temp4.b = b;
        pc_in->push_back(point_temp4);
    }

    for(double i=min_z;i<max_z;i=i+0.02){
        pcl::PointXYZRGB point_temp1;
        point_temp1.x = min_x;
        point_temp1.y = min_y;
        point_temp1.z = i;
        point_temp1.r = r;
        point_temp1.g = g;
        point_temp1.b = b;
        pc_in->push_back(point_temp1);

        pcl::PointXYZRGB point_temp2;
        point_temp2.x = min_x;
        point_temp2.y = max_y;
        point_temp2.z = i;
        point_temp2.r = r;
        point_temp2.g = g;
        point_temp2.b = b;
        pc_in->push_back(point_temp2);

        pcl::PointXYZRGB point_temp3;
        point_temp3.x = max_x;
        point_temp3.y = min_y;
        point_temp3.z = i;
        point_temp3.r = r;
        point_temp3.g = g;
        point_temp3.b = b;
        pc_in->push_back(point_temp3);

        pcl::PointXYZRGB point_temp4;
        point_temp4.x = max_x;
        point_temp4.y = max_y;
        point_temp4.z = i;
        point_temp4.r = r;
        point_temp4.g = g;
        point_temp4.b = b;
        pc_in->push_back(point_temp4);
    }

}

double total_time =0;
int total_frame=0;
void image2pc(){
        // std::chrono::milliseconds dura(3000);
        // std::this_thread::sleep_for(dura);
    while(1){
        if(!pointCloudBuf.empty() && !colorImageBuf.empty()){

            //read data
            mutex_lock.lock();
            //if(total_frame<100){

            if(pointCloudBuf.front()->header.stamp.toSec() - 0.02 > colorImageBuf.front()->header.stamp.toSec()){
                ROS_INFO("time stamp unaligned error and colorimage discarded, pc:%f, image:%f--> image2pc_process",pointCloudBuf.front()->header.stamp.toSec(), colorImageBuf.front()->header.stamp.toSec());
                colorImageBuf.pop();
                mutex_lock.unlock();

                continue;
            }
            if(colorImageBuf.front()->header.stamp.toSec() - 0.02 > pointCloudBuf.front()->header.stamp.toSec()){
                ROS_INFO("time stamp unaligned error and depthimage discarded, pc:%f, image:%f--> image2pc_process",pointCloudBuf.front()->header.stamp.toSec(), colorImageBuf.front()->header.stamp.toSec());
                pointCloudBuf.pop();
                mutex_lock.unlock();
                continue;
            }

            //ROS_INFO("time stamp unaligned error and colorimage discarded, pc:%f, image:%f--> image2pc_process",pointCloudBuf.front()->header.stamp.toSec(), colorImageBuf.front()->header.stamp.toSec());
            //if time aligned
            cv_bridge::CvImagePtr color_image_ptr = cv_bridge::toCvCopy(*colorImageBuf.front(), sensor_msgs::image_encodings::MONO8);
            pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud_in(new pcl::PointCloud<pcl::PointXYZRGB>());
            pcl::fromROSMsg(*pointCloudBuf.front(), *pointcloud_in);
            ros::Time pointcloud_time = (colorImageBuf.front())->header.stamp;
            colorImageBuf.pop();
            pointCloudBuf.pop();
            mutex_lock.unlock();

            std::chrono::time_point<std::chrono::system_clock> start, end;
            start = std::chrono::system_clock::now();
            total_frame++;

            // if( (total_frame>20&&total_frame<40)||(total_frame>127&&total_frame<145) || (total_frame>217&&total_frame<230) || (total_frame>280&&total_frame<315) || (total_frame>394)){//50
            //     continue;
            // }
            //ROS_INFO("total frame %d",total_frame);
            pcl::PointCloud<pcl::PointXYZRGB>::Ptr static_pc(new pcl::PointCloud<pcl::PointXYZRGB>());
            pcl::PointCloud<pcl::PointXYZRGB>::Ptr dynamic_pc(new pcl::PointCloud<pcl::PointXYZRGB>());
            pcl::PointCloud<pcl::PointXYZRGB>::Ptr static_obj_pc(new pcl::PointCloud<pcl::PointXYZRGB>());
            pcl::PointCloud<pcl::PointXYZRGB>::Ptr bounding_box_pc(new pcl::PointCloud<pcl::PointXYZRGB>());
            //fuse color image and depth image

            Eigen::Isometry3d pose_trans = Eigen::Isometry3d::Identity();
            pose_trans.translation() = Eigen::Vector3d(0.001, 0.014, -0.007);
            Eigen::Quaterniond quaternion_temp(1.00,-0.012, -0.001, -0.003);
            pose_trans.linear() = quaternion_temp.toRotationMatrix();;
            pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_pc(new pcl::PointCloud<pcl::PointXYZRGB>());
            pcl::transformPointCloud(*pointcloud_in, *transformed_pc, pose_trans.cast<float>());
            //image dilation

            //0: Rect - 1: Cross - 2: Ellipse
            int morph_elem = 1;
            //max 21
            int morph_size = 3;
            cv::Mat element = cv::getStructuringElement(morph_elem, cv::Size( 2*morph_size + 1, 2*morph_size+1 ), cv::Point( morph_size, morph_size ) );
            cv::Mat dilated_image;

            //ROS_INFO("dimension %d * %d",color_image_ptr->image.cols,color_image_ptr->image.rows);
            if(color_image_ptr->image.cols!=1280 || color_image_ptr->image.rows!=720){
                ROS_WARN("input mask dimension error,%d,%d",color_image_ptr->image.rows,color_image_ptr->image.cols);
                continue;
            }
            cv::dilate(color_image_ptr->image, dilated_image, element,cv::Point(-1,-1), 2, cv::BORDER_REPLICATE);
            // count_temp=0;
            for(int i=0;i<1280;i++){
                for(int j=0;j<720;j++){
                    if (dilated_image.at<unsigned char>(j,i)!=0)
                        dilated_image.at<unsigned char>(j,i) *= 100;
                }
            }
            //ROS_INFO("total points dilated %d * %d",dilated_image.rows,dilated_image.cols);

            std::vector<int> static_pc_id;
            std::vector<int> dynamic_pc_id;
            bool is_dynamic_final[pointcloud_in->points.size()];
            bool is_visual_final[pointcloud_in->points.size()];
            for(int i=0;i<pointcloud_in->points.size();i++){
                is_dynamic_final[i]=false;
                is_visual_final[i]=false;
                pcl::PointXYZRGB point_temp;
                point_temp.x = pointcloud_in->points[i].x;
                point_temp.y = pointcloud_in->points[i].y;
                point_temp.z = pointcloud_in->points[i].z;
                point_temp.r = pointcloud_in->points[i].r;
                point_temp.g = pointcloud_in->points[i].g;
                point_temp.b = pointcloud_in->points[i].b;

                int pixel_x = 910.0393676757812 * transformed_pc->points[i].x/transformed_pc->points[i].z + 647.5104370117188;
                int pixel_y = 910.4710693359375 * transformed_pc->points[i].y/transformed_pc->points[i].z + 363.0339050292969;
                //ROS_INFO("width = %d, height=%d",color_image_ptr->image.cols,color_image_ptr->image.rows);
                if(pixel_x< 0 || pixel_x>=color_image_ptr->image.cols){
                    ROS_WARN("unaligned points");
                    continue;
                }
                if(pixel_y< 0 || pixel_y>=color_image_ptr->image.rows){
                    ROS_WARN("unaligned points");
                    continue;
                }

                    // static_pc->push_back(point_temp);
                    // static_pc_id.push_back(i);

                if (dilated_image.at<unsigned char>(pixel_y,pixel_x)==100)
                {
                    //is_dynamic_final[i]=true;
                    is_visual_final[i] = true;
                    dynamic_pc->push_back(point_temp);
                    dynamic_pc_id.push_back(i);
                }else if (dilated_image.at<unsigned char>(pixel_y,pixel_x)!=0){
                    is_visual_final[i] = true;
                    static_obj_pc->push_back(point_temp);
                    static_pc->push_back(point_temp);
                    static_pc_id.push_back(i);
                }else{
                    static_pc->push_back(point_temp);
                    static_pc_id.push_back(i);
                }

            }

            if(pointcloud_in->points.size() - dynamic_pc->points.size()<100){
                ROS_WARN("not enough static points%d/%d", dynamic_pc->points.size(),pointcloud_in->points.size());
                continue;
            }

            //search for dynamic shape
            if(dynamic_pc->points.size()>0){

                pcl::PointCloud<pcl::PointXYZRGB>::Ptr dynamic_pc_downsized(new pcl::PointCloud<pcl::PointXYZRGB>());
                pcl::VoxelGrid<pcl::PointXYZRGB> downSizeFilter;
                downSizeFilter.setInputCloud (dynamic_pc);
                downSizeFilter.setLeafSize (map_resolution, map_resolution, map_resolution);
                downSizeFilter.filter (*dynamic_pc_downsized);

                // pcl::PointCloud<pcl::PointXYZRGB>::Ptr full_pc_downsized(new pcl::PointCloud<pcl::PointXYZRGB>());
                // downSizeFilter.setInputCloud (pointcloud_in);
                // downSizeFilter.filter (*pointcloud_in_downsized);

                //step 1 dynamic point cloud segmentation
                pcl::search::KdTree<pcl::PointXYZRGB>::Ptr kdtreeDynamicMap (new pcl::search::KdTree<pcl::PointXYZRGB>);
                //pcl::KdTree<pcl::PointXYZRGB>::Ptr kdtreeDynamicMap(new pcl::KdTree<pcl::PointXYZRGB>);
                kdtreeDynamicMap->setInputCloud (dynamic_pc_downsized);
                std::vector<pcl::PointIndices> cluster_indices;
                pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
                ec.setClusterTolerance (0.1);
                ec.setMinClusterSize(20);
                ec.setMaxClusterSize(100000);
                ec.setSearchMethod(kdtreeDynamicMap);
                ec.setInputCloud(dynamic_pc_downsized);
                ec.extract(cluster_indices);

                for (int cluster_index=0;cluster_index<cluster_indices.size();cluster_index++)
                {
                    int segment_size = cluster_indices[cluster_index].indices.size();
                    //search for the area density
                    //ROS_INFO("segment_size: %d", segment_size);
                    if(segment_size>2000){
                        double max_x = -1000.0;
                        double max_y = -1000.0;
                        double max_z = -1000.0;
                        double min_x = 1000.0;
                        double min_y = 1000.0;
                        double min_z = 1000.0;
                        for(int i=0;i<segment_size;i++){
                            if(dynamic_pc_downsized->points[cluster_indices[cluster_index].indices[i]].x > max_x) max_x = dynamic_pc_downsized->points[cluster_indices[cluster_index].indices[i]].x;
                            if(dynamic_pc_downsized->points[cluster_indices[cluster_index].indices[i]].y > max_y) max_y = dynamic_pc_downsized->points[cluster_indices[cluster_index].indices[i]].y;
                            if(dynamic_pc_downsized->points[cluster_indices[cluster_index].indices[i]].z > max_z) max_z = dynamic_pc_downsized->points[cluster_indices[cluster_index].indices[i]].z;
                            if(dynamic_pc_downsized->points[cluster_indices[cluster_index].indices[i]].x < min_x) min_x = dynamic_pc_downsized->points[cluster_indices[cluster_index].indices[i]].x;
                            if(dynamic_pc_downsized->points[cluster_indices[cluster_index].indices[i]].y < min_y) min_y = dynamic_pc_downsized->points[cluster_indices[cluster_index].indices[i]].y;
                            if(dynamic_pc_downsized->points[cluster_indices[cluster_index].indices[i]].z < min_z) min_z = dynamic_pc_downsized->points[cluster_indices[cluster_index].indices[i]].z;
                        }
                        //calculate intensity
                        pcl::CropBox<pcl::PointXYZRGB> cropBoxFilter;
                        double expansion_size = 0.2;
                        cropBoxFilter.setMin(Eigen::Vector4f(min_x-expansion_size, min_y-expansion_size, min_z-3*expansion_size, 1.0));
                        cropBoxFilter.setMax(Eigen::Vector4f(max_x+expansion_size, max_y+expansion_size, max_z+3*expansion_size, 1.0));
                        cropBoxFilter.setNegative(true);
                        cropBoxFilter.setInputCloud(static_pc);
                        //cropBoxFilter.filter(*static_pc);

                        //draw bounding box
                        draBoundingBox(bounding_box_pc,min_x,min_y,min_z,max_x,max_y,max_z,255,0,0);

                    }
                }

                //step 2 dynamic point cloud search
                //setp2.1 define the main obkects

                pcl::PointCloud<pcl::PointXYZRGB>::Ptr main_dynamic_pc(new pcl::PointCloud<pcl::PointXYZRGB>());
                for (int cluster_index=0;cluster_index<cluster_indices.size();cluster_index++)
                {
                    int segment_size = cluster_indices[cluster_index].indices.size();
                    //search for the area density
                    // ROS_INFO("segment_size: %d", segment_size);
                    if(segment_size>2000){
                        for(int i=0;i<segment_size;i++){
                            main_dynamic_pc->push_back(dynamic_pc_downsized->points[cluster_indices[cluster_index].indices[i]]);
                        }
                    }
                }

                if(main_dynamic_pc->points.size()>10){
                    //ROS_WARN("wtf");
                    pcl::KdTreeFLANN<pcl::PointXYZRGB>::Ptr kdtreeStaticMap = pcl::KdTreeFLANN<pcl::PointXYZRGB>::Ptr(new pcl::KdTreeFLANN<pcl::PointXYZRGB>());
                    kdtreeStaticMap->setInputCloud(static_pc);

                    pcl::KdTreeFLANN<pcl::PointXYZRGB>::Ptr kdtreeDynamicMap = pcl::KdTreeFLANN<pcl::PointXYZRGB>::Ptr(new pcl::KdTreeFLANN<pcl::PointXYZRGB>());
                    kdtreeDynamicMap->setInputCloud(dynamic_pc);

                    //have main object
                    pcl::KdTreeFLANN<pcl::PointXYZRGB>::Ptr dynamicMap = pcl::KdTreeFLANN<pcl::PointXYZRGB>::Ptr(new pcl::KdTreeFLANN<pcl::PointXYZRGB>());
                    dynamicMap->setInputCloud(main_dynamic_pc);

                    // step 2.2 seach each point cloud and check for neaby points if they are close
                    for (int cluster_index=0;cluster_index<cluster_indices.size();cluster_index++){
                        int segment_size = cluster_indices[cluster_index].indices.size();
                         //search min distance
                        bool is_dynamic_segments = false;
                        double x_center =0.0;
                        double y_center =0.0;
                        double z_center =0.0;
                        double min_distance_squared = 0.16;
                        for (int i = 0; i < segment_size; ++i){
                            x_center += dynamic_pc_downsized->points[cluster_indices[cluster_index].indices[i]].x;
                            y_center += dynamic_pc_downsized->points[cluster_indices[cluster_index].indices[i]].y;
                            z_center += dynamic_pc_downsized->points[cluster_indices[cluster_index].indices[i]].z;

                        }
                        pcl::PointXYZRGB point_temp;
                        point_temp.x = x_center / segment_size;
                        point_temp.y = y_center / segment_size;
                        point_temp.z = z_center / segment_size;
                        std::vector<int> pointIdxRadiusSearch;
                        std::vector<float> pointRadiusSquaredDistance;
                        if (dynamicMap->nearestKSearch(point_temp, 1, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 ){
                            if(pointRadiusSquaredDistance[0]>min_distance_squared){
                                continue;
                            }
                        }

                        //point cloud dilation
                        //std::vector<int> segment_dilated;
                        int original_pc_size = 0;
                        // bool is_dynamic_map_dynamic[dynamic_pc->points.size()];
                        // for(int i=0;i<dynamic_pc->points.size();i++){
                        //     is_dynamic[i]=false;
                        // }

                        //calculate original point cloud
                        for (int i = 0; i < segment_size; ++i){
                            std::vector<int> pointIdxRadiusSearch_temp;
                            std::vector<float> pointRadiusSquaredDistance_temp;
                            if (kdtreeDynamicMap->radiusSearch(dynamic_pc_downsized->points[cluster_indices[cluster_index].indices[i]], map_resolution*2, pointIdxRadiusSearch_temp, pointRadiusSquaredDistance_temp) > 0 ){
                                for (int i = 0; i < pointIdxRadiusSearch_temp.size(); ++i){
                                    if(is_dynamic_final[dynamic_pc_id[pointIdxRadiusSearch_temp[i]]]==false){
                                        is_dynamic_final[dynamic_pc_id[pointIdxRadiusSearch_temp[i]]]=true;
                                        original_pc_size++;
                                    }
                                }
                            }

                        }

                        //init segment_dilated
                        bool is_static_map_dynamic[static_pc->points.size()];
                        for(int i=0;i<static_pc->points.size();i++){
                            is_static_map_dynamic[i]=false;
                        }
                        std::vector<int> segment_dilated;
                        for (int i = 0; i < segment_size; ++i){
                            std::vector<int> pointIdxRadiusSearch_temp;
                            std::vector<float> pointRadiusSquaredDistance_temp;
                            if (kdtreeStaticMap->radiusSearch(dynamic_pc_downsized->points[cluster_indices[cluster_index].indices[i]], map_resolution*3, pointIdxRadiusSearch_temp, pointRadiusSquaredDistance_temp) > 0 ){
                                for (int i = 0; i < pointIdxRadiusSearch_temp.size(); ++i){
                                    if(is_static_map_dynamic[pointIdxRadiusSearch_temp[i]]==false){
                                        is_static_map_dynamic[pointIdxRadiusSearch_temp[i]] = true;
                                        segment_dilated.push_back(pointIdxRadiusSearch_temp[i]);
                                    }
                                }
                            }

                        }

                        //dilate original point cloud
                        //iterative ssearch
                        int points_dilated_count=0;
                        bool is_dilation_success = true;
                        while(points_dilated_count < segment_dilated.size()){
                            std::vector<int> pointIdxRadiusSearch_temp;
                            std::vector<float> pointRadiusSquaredDistance_temp;
                            if (kdtreeStaticMap->radiusSearch(static_pc->points[segment_dilated[points_dilated_count]], map_resolution*3, pointIdxRadiusSearch_temp, pointRadiusSquaredDistance_temp) > 0 ){
                                for (int i = 0; i < pointIdxRadiusSearch_temp.size(); ++i){
                                    if(is_static_map_dynamic[pointIdxRadiusSearch_temp[i]]==false){
                                        is_static_map_dynamic[pointIdxRadiusSearch_temp[i]]=true;
                                        segment_dilated.push_back(pointIdxRadiusSearch_temp[i]);
                                    }
                                }
                            }
                            points_dilated_count++;
                            if(points_dilated_count>1800 + ((int)(original_pc_size/2))) {
                                is_dilation_success = false;
                                ROS_WARN("expansion failes%d/%d",original_pc_size,segment_dilated.size());
                                break;
                            }
                        }

                        //add points
                        if(is_dilation_success == true){
                            //add static map dunamic points
                            for (int i = 0; i < static_pc->points.size(); ++i){
                                if(is_static_map_dynamic[i]==true){
                                    is_dynamic_final[static_pc_id[i]]=true;
                                }

                            }

                        }

                    }

                }

            }

            pcl::PointCloud<pcl::PointXYZRGB>::Ptr static_pc_final(new pcl::PointCloud<pcl::PointXYZRGB>());
            pcl::PointCloud<pcl::PointXYZRGB>::Ptr dynamic_pc_final(new pcl::PointCloud<pcl::PointXYZRGB>());
            pcl::PointCloud<pcl::PointXYZRGB>::Ptr visual_pc_final(new pcl::PointCloud<pcl::PointXYZRGB>());
            for(int i=0;i<pointcloud_in->points.size();i++){
                if(is_visual_final[i] == true)
                    dynamic_pc_final->push_back(pointcloud_in->points[i]);
                else
                    static_pc_final->push_back(pointcloud_in->points[i]);
                /*
                if(is_dynamic_final[i]==true && is_visual_final[i] == true)
                    dynamic_pc_final->push_back(pointcloud_in->points[i]);
                else if(is_dynamic_final[i]==true && is_visual_final[i] == false){
                    visual_pc_final->push_back(pointcloud_in->points[i]);
                    dynamic_pc_final->push_back(pointcloud_in->points[i]);
                    static_pc_final->push_back(pointcloud_in->points[i]);
                }else
                    static_pc_final->push_back(pointcloud_in->points[i]);
                */
            }

            end = std::chrono::system_clock::now();
            std::chrono::duration<float> elapsed_seconds = end - start;
            if(total_frame%50==0)
                ROS_INFO("average point cloud filtering time %f ", elapsed_seconds.count() * 1000 / total_frame);

            // publish point cloud
            sensor_msgs::PointCloud2 static_pc_msg;
            pcl::toROSMsg(*static_pc_final, static_pc_msg);
            //pcl::toROSMsg(*static_pc+*dynamic_pc, static_pc_msg);
            static_pc_msg.header.stamp = pointcloud_time;
            static_pc_msg.header.frame_id = "/camera_depth_optical_frame";
            pubStaticPointCloud.publish(static_pc_msg);

            //dynamic pc
            sensor_msgs::PointCloud2 dynamic_pc_msg;
            pcl::toROSMsg(*dynamic_pc_final /* + *bounding_box_pc*/, dynamic_pc_msg);
            dynamic_pc_msg.header.stamp = pointcloud_time;
            dynamic_pc_msg.header.frame_id = "/camera_depth_optical_frame";
            pubDynamicPointCloud.publish(dynamic_pc_msg);

            //dynamic pc
            sensor_msgs::PointCloud2 visual_pc_msg;
            pcl::toROSMsg(*visual_pc_final /* + *bounding_box_pc*/, visual_pc_msg);
            visual_pc_msg.header.stamp = pointcloud_time;
            visual_pc_msg.header.frame_id = "/camera_depth_optical_frame";
            pubVisualPointCloud.publish(visual_pc_msg);

            cv_bridge::CvImage out_msg;
            out_msg.header.frame_id  = "/camera_depth_optical_frame";
            out_msg.header.stamp  = pointcloud_time;
            out_msg.encoding = sensor_msgs::image_encodings::MONO8;
            out_msg.image    = dilated_image;
            pubimage.publish(out_msg.toImageMsg());

        }
        //sleep 2 ms every time
        std::chrono::milliseconds dura(2);
        std::this_thread::sleep_for(dura);
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "main");
    ros::NodeHandle nh;
    image_transport::ImageTransport it(nh);
    //Eigen::Matrix<double,double>

    nh.getParam("/map_resolution", map_resolution);
    map_resolution = 0.01;
    ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/camera/depth/color/points", 100, velodyneHandler);
    image_transport::Subscriber colorImageSub = it.subscribe("/solo_node/mask", 100, ColorImageHandler);

    pubStaticPointCloud = nh.advertise<sensor_msgs::PointCloud2>("filtered_points_static", 100);
    pubDynamicPointCloud = nh.advertise<sensor_msgs::PointCloud2>("filtered_points_dynamic", 100);
    pubVisualPointCloud = nh.advertise<sensor_msgs::PointCloud2>("velodyne_points_visual", 100);
    pubimage = nh.advertise<sensor_msgs::Image>("dilated_image", 100);
    std::thread image2pc_process{image2pc};

    ros::spin();

    return 0;
}


#define BOOST_TEST_MODULE ranked_queries

#include "succinct/test_common.hpp"
#include <boost/test/floating_point_comparison.hpp>

#include "ds2i_config.hpp"
#include "index_types.hpp"
#include "queries.hpp"

namespace ds2i { namespace test {

    struct index_initialization {

        typedef single_index index_type;

        index_initialization()
            : collection(DS2I_SOURCE_DIR "/test/test_data/test_collection")
            , document_sizes(DS2I_SOURCE_DIR "/test/test_data/test_collection.sizes")
            , wdata(document_sizes.begin()->begin(), collection.num_docs(), collection)
        {
            index_type::builder builder(collection.num_docs(), params);
            for (auto const& plist: collection) {
                uint64_t freqs_sum = std::accumulate(plist.freqs.begin(),
                                                     plist.freqs.end(), uint64_t(0));
                builder.add_posting_list(plist.docs.size(), plist.docs.begin(),
                                         plist.freqs.begin(), freqs_sum);
            }
            builder.build(index);

            term_id_vec q;
            std::ifstream qfile("test_data/queries");
            while (read_query(q, qfile)) queries.push_back(q);
        }

        global_parameters params;
        binary_freq_collection collection;
        binary_collection document_sizes;
        index_type index;
        std::vector<term_id_vec> queries;
        wand_data<> wdata;

        template <typename QueryOp>
        void test_against_or(QueryOp& op_q) const
        {
            ranked_or_query or_q(wdata, 10);

            for (auto const& q: queries) {
                or_q(index, q);
                op_q(index, q);
                BOOST_REQUIRE_EQUAL(or_q.topk().size(), op_q.topk().size());
                for (size_t i = 0; i < or_q.topk().size(); ++i) {
                    BOOST_REQUIRE_CLOSE(or_q.topk()[i], op_q.topk()[i], 0.1); // tolerance is % relative
                }
            }
        }

    };

}}

BOOST_FIXTURE_TEST_CASE(wand,
                        ds2i::test::index_initialization)
{
    ds2i::wand_query wand_q(wdata, 10);
    test_against_or(wand_q);
}

BOOST_FIXTURE_TEST_CASE(maxscore,
                        ds2i::test::index_initialization)
{
    ds2i::maxscore_query maxscore_q(wdata, 10);
    test_against_or(maxscore_q);
}


#include <stan/math/prim/mat.hpp>
#include <stan/math/prim/scal.hpp>
#include <test/unit/math/prim/prob/vector_rng_test_helper.hpp>
#include <test/unit/math/prim/prob/VectorIntRNGTestRig.hpp>
#include <gtest/gtest.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/math/distributions.hpp>
#include <limits>
#include <vector>
#include <string>

class NegativeBinomialTestRig : public VectorIntRNGTestRig {
 public:
  NegativeBinomialTestRig()
      : VectorIntRNGTestRig(10000, 10, {0, 1, 2, 3, 4, 5, 6}, {0.1, 1.7, 3.99},
                            {1, 2, 3}, {-2.1, -0.5, 0.0}, {-3, -1, 0},
                            {0.1, 1.1, 4.99}, {1, 2, 3}, {-3.0, -2.0, 0.0},
                            {-3, -1, 0}) {}

  template <typename T1, typename T2, typename T3, typename T_rng>
  auto generate_samples(const T1& alpha, const T2& beta, const T3&,
                        T_rng& rng) const {
    return stan::math::neg_binomial_rng(alpha, beta, rng);
  }

  template <typename T1>
  double pmf(int y, T1 alpha, double beta, double) const {
    return std::exp(stan::math::neg_binomial_lpmf(y, alpha, beta));
  }
};

TEST(ProbDistributionsNegativeBinomial, errorCheck) {
  check_dist_throws_all_types(NegativeBinomialTestRig());
}

TEST(ProbDistributionsNegativeBinomial, distributionCheck) {
  check_counts_real_real(NegativeBinomialTestRig());
}

TEST(ProbDistributionsNegBinomial, error_check) {
  boost::random::mt19937 rng;
  EXPECT_NO_THROW(stan::math::neg_binomial_rng(6, 2, rng));
  EXPECT_NO_THROW(stan::math::neg_binomial_rng(0.5, 1, rng));
  EXPECT_NO_THROW(stan::math::neg_binomial_rng(1e9, 1, rng));

  EXPECT_THROW(stan::math::neg_binomial_rng(0, -2, rng), std::domain_error);
  EXPECT_THROW(stan::math::neg_binomial_rng(6, -2, rng), std::domain_error);
  EXPECT_THROW(stan::math::neg_binomial_rng(-6, -0.1, rng), std::domain_error);
  EXPECT_THROW(
      stan::math::neg_binomial_rng(stan::math::positive_infinity(), 2, rng),
      std::domain_error);
  EXPECT_THROW(
      stan::math::neg_binomial_rng(stan::math::positive_infinity(), 6, rng),
      std::domain_error);
  EXPECT_THROW(
      stan::math::neg_binomial_rng(2, stan::math::positive_infinity(), rng),
      std::domain_error);

  std::string error_msg;
  error_msg
      = "neg_binomial_rng: Random number that "
        "came from gamma distribution is";
  try {
    stan::math::neg_binomial_rng(1e10, 1, rng);
    FAIL() << "neg_binomial_rng should have thrown" << std::endl;
  } catch (const std::exception& e) {
    if (std::string(e.what()).find(error_msg) == std::string::npos)
      FAIL() << "Error message is different than expected" << std::endl
             << "EXPECTED: " << error_msg << std::endl
             << "FOUND: " << e.what() << std::endl;
    SUCCEED();
  }
}

void expected_bin_sizes(double* expect, const int K, const int N,
                        const double alpha, const double beta) {
  double p = 0;
  for (int i = 0; i < K; i++) {
    expect[i] = N * std::exp(stan::math::neg_binomial_log(i, alpha, beta));
    p += std::exp(stan::math::neg_binomial_log(i, alpha, beta));
  }
  expect[K - 1] = N * (1.0 - p);
}

TEST(ProbDistributionsNegBinomial, chiSquareGoodnessFitTest) {
  boost::random::mt19937 rng;
  double p = 0.6;
  double alpha = 5;
  double beta = p / (1 - p);
  int N = 1000;
  int K = stan::math::round(2 * std::pow(N, (1 - p)));
  boost::math::chi_squared mydist(K - 1);

  int loc[K - 1];
  for (int i = 1; i < K; i++)
    loc[i - 1] = i - 1;

  int count = 0;
  double bin[K];
  double expect[K];

  for (int i = 0; i < K; i++)
    bin[i] = 0;
  expected_bin_sizes(expect, K, N, alpha, beta);

  while (count < N) {
    int a = stan::math::neg_binomial_rng(alpha, beta, rng);
    int i = 0;
    while (i < K - 1 && a > loc[i])
      ++i;
    ++bin[i];
    count++;
  }

  double chi = 0;

  for (int j = 0; j < K; j++)
    chi += ((bin[j] - expect[j]) * (bin[j] - expect[j]) / expect[j]);

  EXPECT_LT(chi, boost::math::quantile(boost::math::complement(mydist, 1e-6)));
}

TEST(ProbDistributionsNegBinomial, chiSquareGoodnessFitTest2) {
  boost::random::mt19937 rng;
  double p = 0.8;
  double alpha = 2.4;
  double beta = p / (1 - p);
  int N = 1000;
  int K = stan::math::round(2 * std::pow(N, (1 - p)));
  boost::math::chi_squared mydist(K - 1);

  int loc[K - 1];
  for (int i = 1; i < K; i++)
    loc[i - 1] = i - 1;

  int count = 0;
  double bin[K];
  double expect[K];

  for (int i = 0; i < K; i++)
    bin[i] = 0;
  expected_bin_sizes(expect, K, N, alpha, beta);

  while (count < N) {
    int a = stan::math::neg_binomial_rng(alpha, beta, rng);
    int i = 0;
    while (i < K - 1 && a > loc[i])
      ++i;
    ++bin[i];
    count++;
  }

  double chi = 0;

  for (int j = 0; j < K; j++)
    chi += ((bin[j] - expect[j]) * (bin[j] - expect[j]) / expect[j]);

  EXPECT_LT(chi, boost::math::quantile(boost::math::complement(mydist, 1e-6)));
}

TEST(ProbDistributionsNegBinomial, chiSquareGoodnessFitTest3) {
  boost::random::mt19937 rng;
  double p = 0.2;
  double alpha = 0.4;
  double beta = p / (1 - p);
  int N = 1000;
  int K = stan::math::round(2 * std::pow(N, (1 - p)));
  boost::math::chi_squared mydist(K - 1);

  int loc[K - 1];
  for (int i = 1; i < K; i++)
    loc[i - 1] = i - 1;

  int count = 0;
  double bin[K];
  double expect[K];

  for (int i = 0; i < K; i++)
    bin[i] = 0;
  expected_bin_sizes(expect, K, N, alpha, beta);

  while (count < N) {
    int a = stan::math::neg_binomial_rng(alpha, beta, rng);
    int i = 0;
    while (i < K - 1 && a > loc[i])
      ++i;
    ++bin[i];
    count++;
  }

  double chi = 0;

  for (int j = 0; j < K; j++)
    chi += ((bin[j] - expect[j]) * (bin[j] - expect[j]) / expect[j]);

  EXPECT_LT(chi, boost::math::quantile(boost::math::complement(mydist, 1e-6)));
}


#include <iostream>

#include "Design/Components/Component.hpp"
#include "Util/Software_in_the_loop_driver.hpp"

#include <standard_atmosphere.hpp>
#include <Eigen/Core>

// Entry point to 6dof simulation. Prases input argument and drives sim
int main(int argc, char **argv) {
    using namespace std;

    // Parse input arguments
    if (argc == 0) {
        cout << "Print Help Menu" << endl;
        cout << "USAGE:" << endl;
        cout << "-ork [FILE]  : \".ork\" file which contains the model of the rocket" << endl;
        cout << "-tc [FILE]   : \".csv\" file which contains the thrust curve of the motor" << endl;
        cout << "-env [FILE]  : \".csv\" file which contains the [lat lon alt]" << endl;
        cout << "-wind [FILE] : \".csv\" file which contains precomputed wind speed values" << endl;
    }
    for (int i = 0; i < argc; i++) {
        cout << argv[i] << endl;
    }

    double temperature, pressure, density;
    std::tie(temperature, pressure, density) = rvt::standard_atmosphere(10);
    cout << temperature << " " << pressure << " " << density << endl;

    Eigen::Vector3d vec = Eigen::Vector3d::Zero();
    cout << "Vec = " << vec << endl;

    // Load open rocket from example

    // Run Simulation
    //drive_sim();

    // Save data

    return 0;
}


#include "LaserLocalization.h"
#include "common/feature_utils.h"
#include "common/math_utils.h"
#include "common/nanoflann_pcl.h"
#include "common/ros_utils.h"
#include "common/transform_utils.h"

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

namespace lidar_slam {

LaserLocalization::LaserLocalization() {}

LaserLocalization::~LaserLocalization() {}

bool LaserLocalization::init(ros::NodeHandle &node,
                             ros::NodeHandle &privateNode) {

  map_frame = "map";
  _isLidarPoseReset = false;
  _lidarPoseReset = Eigen::Isometry3f::Identity();
  if (setup(node, privateNode)) {
    _subInitialPose = node.subscribe(
        "initialpose2", 2, &LaserLocalization::initialPoseHandler2, this);
    _subInitialPose2 = node.subscribe(
        "initialpose", 2, &LaserLocalization::initialPoseHandler, this);

    spin_thread = std::thread(&LaserLocalization::spin, this);

    return true;
  }
  return false;
}

void LaserLocalization::initialPoseHandler2(
    const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg) {
  handleInitialPoseMessage(*msg);
}

void LaserLocalization::initialPoseHandler(
    const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg) {
  std::string global_frame_id_ = "/map";
  if (msg->header.frame_id == "") {
    ROS_WARN("Received initial pose with empty frame_id.  You should always "
             "supply a frame_id.");
    return;
  }

  Eigen::Quaternionf Qml;
  Qml.x() = msg->pose.pose.orientation.x;
  Qml.y() = msg->pose.pose.orientation.y;
  Qml.z() = msg->pose.pose.orientation.z * -1.0;
  Qml.w() = msg->pose.pose.orientation.w * -1.0;

  Eigen::Vector3f Vml;
  Vml(0) = msg->pose.pose.position.x;
  Vml(1) = msg->pose.pose.position.y;
  Vml(2) = msg->pose.pose.position.z;

  Eigen::Isometry3f Tml = Eigen::Isometry3f::Identity();
  Tml.rotate(Qml);
  Tml.pretranslate(Vml);

  mtx_transform.lock();
  Eigen::Quaternionf quat(Tml.rotation());
  imu_que.reset(Vml, quat);
  ROS_INFO_STREAM("init:\n" << Tml.matrix());
  _isLidarPoseReset = true;
  _lidarPoseReset = Tml;
  mtx_transform.unlock();
  _initialized = true;
}

void LaserLocalization::handleInitialPoseMessage(
    const geometry_msgs::PoseWithCovarianceStamped &msg) {
  std::string global_frame_id_ = "/map";
  if (msg.header.frame_id == "") {
    ROS_WARN("Received initial pose with empty frame_id.  You should always "
             "supply a frame_id.");
    return;
  }

  Eigen::Quaternionf Qml;
  Qml.x() = msg.pose.pose.orientation.x;
  Qml.y() = msg.pose.pose.orientation.y;
  Qml.z() = msg.pose.pose.orientation.z;
  Qml.w() = msg.pose.pose.orientation.w;

  Eigen::Vector3f Vml;
  Vml(0) = msg.pose.pose.position.x;
  Vml(1) = msg.pose.pose.position.y;
  Vml(2) = msg.pose.pose.position.z;

  Eigen::Isometry3f Tml = Eigen::Isometry3f::Identity();
  Tml.rotate(Qml);
  Tml.pretranslate(Vml);

  mtx_transform.lock();
  Eigen::Quaternionf quat(Tml.rotation());
  imu_que.reset(Vml, quat);
  ROS_INFO_STREAM("init:\n" << Tml.matrix());
  _isLidarPoseReset = true;
  _lidarPoseReset = Tml;
  mtx_transform.unlock();
  _initialized = true;
}

void LaserLocalization::spin() {
  ros::Rate rate(500);
  bool status = ros::ok();

  while (status) {
    ros::spinOnce();
    process();
    status = ros::ok();
    rate.sleep();
  }
}

void LaserLocalization::optimizeTransform() {

  if(_dynamicMode)
    _dynamic_feature_map.scanMatchScan(_laserCloudCornerStackDS, _laserCloudSurfStackDS,
                            _lidarMappedNew);
  else
    _feature_map->scanMatchScan(_laserCloudCornerStackDS, _laserCloudSurfStackDS,
                            _lidarMappedNew);
 /*
  _scan_match.scanMatchScan(_laserCloudCornerFromMap, _laserCloudSurfFromMap,
                            _laserCloudCornerStackDS, _laserCloudSurfStackDS,
                            _lidarMappedNew);

*/
}

void LaserLocalization::transformUpdate() {
  mtx_transform.lock();
  if (_isLidarPoseReset) {
    _lidarMappedNew = _lidarPoseReset;
    _isLidarPoseReset = false;
  }
  Eigen::Vector3f pos_last = _lidarMappedLast.translation();
  _lidarMappedLast = _lidarMappedNew;
   _lidarOdomLast = _lidarOdomLastMerged;

  mtx_transform.unlock();

  //imu fusion
  Eigen::Isometry3f lidarPose = _lidarMappedNew;
  Eigen::Isometry3f pre_pos, cor_pos;
  imu_que.predict(_timeLaserOdometryMerged, pre_pos);
  if(!_timeLaserOdometryLast.is_zero()){
    _velocity = (lidarPose.translation() - _lidarPoseLast.translation())/(_timeLaserOdometryMerged-_timeLaserOdometryLast).toSec();
    if(_velocity.norm()>30)
      _velocity = Eigen::Vector3f::Zero();
      Eigen::Vector3f velocity = _velocity;
    imu_que.correct(lidarPose, cor_pos, velocity);
  }
  _lidarPoseLast = lidarPose;
  _timeLaserOdometryLast = _timeLaserOdometryMerged;
}

void LaserLocalization::process() {
  if (!hasNewData() || !_initialized) {
    return;
  }

  reset();

  _inputFrameCount++;
  if (_inputFrameSkip > 0) {
    if (_inputFrameCount % (_inputFrameSkip + 1) != 1)
      return;
  }

  transformMerge();
  prepareFeatureFrame();
  prepareFeatureSurround();
  optimizeTransform();
  transformUpdate();
  // featureMapUpdate();
  publishResult();
}

} // end namespace lidar_slam


// Copyright 2018-2019 Henry Schreiner and Hans Dembinski
//
// Distributed under the 3-Clause BSD License.  See accompanying
// file LICENSE or https://github.com/scikit-hep/boost-histogram for details.

#pragma once

#include <bh_python/pybind11.hpp>

#include <bh_python/accumulators/mean.hpp>
#include <bh_python/accumulators/weighted_mean.hpp>
#include <bh_python/accumulators/weighted_sum.hpp>

#include <boost/histogram/accumulators/count.hpp>
#include <boost/histogram/storage_adaptor.hpp>
#include <boost/histogram/unlimited_storage.hpp>

#include <algorithm>
#include <cstdint>
#include <type_traits>

namespace storage {

// Names match Python names
using int64         = bh::dense_storage<int64_t>;
using atomic_int64  = bh::dense_storage<bh::accumulators::count<int64_t, true>>;
using double_       = bh::dense_storage<double>;
using unlimited     = bh::unlimited_storage<>;
using weight        = bh::dense_storage<accumulators::weighted_sum<double>>;
using mean          = bh::dense_storage<accumulators::mean<double>>;
using weighted_mean = bh::dense_storage<accumulators::weighted_mean<double>>;

// Allow repr to show python name
template <class S>
inline const char* name() {
    return "unknown";
}

template <>
inline const char* name<int64>() {
    return "int64";
}

template <>
inline const char* name<atomic_int64>() {
    return "atomic_int64";
}

template <>
inline const char* name<double_>() {
    return "double";
}

template <>
inline const char* name<unlimited>() {
    return "unlimited";
}

template <>
inline const char* name<weight>() {
    return "weight";
}

template <>
inline const char* name<mean>() {
    return "mean";
}

template <>
inline const char* name<weighted_mean>() {
    return "weighted_mean";
}

} // namespace storage

// It is very important that storages with accumulators have specialized serialization.
// No specializations needed for dense_storage<ArithmeticType> or unlimited_storage.

template <class Archive>
void save(Archive& ar, const storage::atomic_int64& s, unsigned /* version */) {
    // We cannot view the memory as a numpy array, because the internal layout of
    // std::atomic is undefined. So no reinterpret_casts are allowed.
    py::array_t<std::int64_t> a(static_cast<py::ssize_t>(s.size()));

    auto in_ptr  = s.begin();
    auto out_ptr = a.mutable_data();
    for(; in_ptr != s.end(); ++in_ptr, ++out_ptr)
        *out_ptr = in_ptr->value();
    ar << a;
}

template <class Archive>
void load(Archive& ar, storage::atomic_int64& s, unsigned /* version */) {
    // data is stored as flat numpy array
    py::array_t<std::int64_t> a;
    ar >> a;
    s.resize(static_cast<std::size_t>(a.size()));
    // sadly we cannot move the memory from the numpy array into the vector
    std::copy(a.data(), a.data() + a.size(), s.data());
}

template <class Archive>
void save(Archive& ar,
          const bh::dense_storage<accumulators::weighted_sum<double>>& s,
          unsigned /* version */) {
    using T = accumulators::weighted_sum<double>;
    static_assert(std::is_standard_layout<T>::value
                      && std::is_trivially_copyable<T>::value
                      && sizeof(T) == 2 * sizeof(double),
                  "weighted_sum cannot be fast serialized");
    // view storage buffer as flat numpy array
    py::array_t<double> a(static_cast<py::ssize_t>(s.size()) * 2,
                          reinterpret_cast<const double*>(s.data()));
    ar << a;
}

template <class Archive>
void load(Archive& ar,
          bh::dense_storage<accumulators::weighted_sum<double>>& s,
          unsigned /* version */) {
    // data is stored as flat numpy array
    py::array_t<double> a;
    ar >> a;
    s.resize(static_cast<std::size_t>(a.size() / 2));
    // sadly we cannot move the memory from the numpy array into the vector
    std::copy(a.data(), a.data() + a.size(), reinterpret_cast<double*>(s.data()));
}

template <class Archive>
void save(Archive& ar,
          const bh::dense_storage<accumulators::mean<double>>& s,
          unsigned /* version */) {
    using T = accumulators::mean<double>;
    static_assert(std::is_standard_layout<T>::value
                      && std::is_trivially_copyable<T>::value
                      && sizeof(T) == 3 * sizeof(double),
                  "mean cannot be fast serialized");
    // view storage buffer as flat numpy array
    py::array_t<double> a(static_cast<py::ssize_t>(s.size()) * 3,
                          reinterpret_cast<const double*>(s.data()));
    ar << a;
}

template <class Archive>
void load(Archive& ar,
          bh::dense_storage<accumulators::mean<double>>& s,
          unsigned /* version */) {
    // data is stored as flat numpy array
    py::array_t<double> a;
    ar >> a;
    s.resize(static_cast<std::size_t>(a.size() / 3));
    // sadly we cannot move the memory from the numpy array into the vector
    std::copy(a.data(), a.data() + a.size(), reinterpret_cast<double*>(s.data()));
}

template <class Archive>
void save(Archive& ar,
          const bh::dense_storage<accumulators::weighted_mean<double>>& s,
          unsigned /* version */) {
    using T = accumulators::weighted_mean<double>;
    static_assert(std::is_standard_layout<T>::value
                      && std::is_trivially_copyable<T>::value
                      && sizeof(T) == 4 * sizeof(double),
                  "weighted_mean cannot be fast serialized");
    // view storage buffer as flat numpy array
    py::array_t<double> a(static_cast<py::ssize_t>(s.size()) * 4,
                          reinterpret_cast<const double*>(s.data()));
    ar << a;
}

template <class Archive>
void load(Archive& ar,
          bh::dense_storage<accumulators::weighted_mean<double>>& s,
          unsigned /* version */) {
    // data is stored as flat numpy array
    py::array_t<double> a;
    ar >> a;
    s.resize(static_cast<std::size_t>(a.size() / 4));
    // sadly we cannot move the memory from the numpy array into the vector
    std::copy(a.data(), a.data() + a.size(), reinterpret_cast<double*>(s.data()));
}

namespace pybind11 {
namespace detail {
/// Allow a Python int to implicitly convert to an atomic int in C++
template <>
struct type_caster<storage::atomic_int64::value_type> {
    PYBIND11_TYPE_CASTER(storage::atomic_int64::value_type, _("atomic_int64"));

    bool load(handle src, bool) {
        auto ptr = PyNumber_Long(src.ptr());
        if(!ptr)
            return false;
        value = PyLong_AsLongLong(ptr);
        Py_DECREF(ptr);
        return !PyErr_Occurred();
    }

    static handle cast(storage::atomic_int64::value_type src,
                       return_value_policy /* policy */,
                       handle /* parent */) {
        return PyLong_FromLongLong(src.value());
    }
};
} // namespace detail
} // namespace pybind11


#ifndef BOOST_TEST_DYN_LINK
#define BOOST_TEST_DYN_LINK
#endif

#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>

#include "../simulation/world.hpp"
#include "../simulation/pheromoneMap.hpp"
#include "../simulation/point.hpp"

namespace PheromoneMapTest
{

const float g_epsilon = 0.0001;

struct Fixture
{
    const unsigned size_x = 10;
    const unsigned size_y = 10;
    const float decay_coef = 1.0;
    const float test_strength = 10.0;
    const float radius_one = 1.0;
    const float radius_five = 5.0;
    World world;
    PheromoneMap pheromone_map;

    Fixture() :
        world(), pheromone_map(&world, PheromoneMap::Type::ToFood, size_x,
            size_y, decay_coef)
    {}
};

BOOST_FIXTURE_TEST_CASE(test_getInvalidPoint_shouldThrow, Fixture)
{
    BOOST_CHECK_THROW(pheromone_map.getStrengthAtPosition(Point(-5, -5)),
        std::exception);
    BOOST_CHECK_THROW(pheromone_map.getStrengthAtPosition(
        Point(size_x+1, size_y+1)), std::exception);
}

BOOST_FIXTURE_TEST_CASE(test_resetMap_shouldBeZeroEverywhere, Fixture)
{
    pheromone_map.createBlob(Point(5, 5), radius_one, test_strength);
    BOOST_CHECK_CLOSE(pheromone_map.getStrengthAtPosition(Point(5, 5)),
        test_strength, g_epsilon);

    pheromone_map.reset();
    BOOST_CHECK_CLOSE(pheromone_map.getStrengthAtPosition(Point(5, 5)),
        0.0, g_epsilon);
}

BOOST_FIXTURE_TEST_CASE(test_create1SquareBlob_shouldBeOnly1SquareWide, Fixture)
{
    pheromone_map.createBlob(Point(5, 5), radius_one, test_strength);
    BOOST_CHECK_CLOSE(pheromone_map.getStrengthAtPosition(Point(5, 5)),
        test_strength, g_epsilon);
    BOOST_CHECK_CLOSE(pheromone_map.getStrengthAtPosition(Point(6, 5)),
        0.0, g_epsilon);
}

BOOST_FIXTURE_TEST_CASE(test_createBlobExceedBoundary_shouldClipNoExceptions,
    Fixture)
{
    BOOST_CHECK_NO_THROW(pheromone_map.createBlob(Point(9, 9), radius_one,
        test_strength));
}

BOOST_FIXTURE_TEST_CASE(test_createBlob_shouldDropOffWithDistance, Fixture)
{
    pheromone_map.createBlob(Point(9, 9), radius_five, test_strength);
    BOOST_CHECK_CLOSE(pheromone_map.getStrengthAtPosition(Point(9, 9)),
        test_strength, g_epsilon);
    BOOST_CHECK_LE(pheromone_map.getStrengthAtPosition(Point(6, 6)),
        test_strength);
}

BOOST_FIXTURE_TEST_CASE(test_createBlobCentreOutsideMap_shouldThrow, Fixture)
{
    BOOST_CHECK_THROW(pheromone_map.createBlob(Point(999, 999), 5,
        5), std::runtime_error);
}

BOOST_FIXTURE_TEST_CASE(test_decay_shouldDropWithTime, Fixture)
{
    pheromone_map.createBlob(Point(5, 5), radius_five, test_strength);
    BOOST_CHECK_CLOSE(pheromone_map.getStrengthAtPosition(Point(5, 5)),
        test_strength, g_epsilon);
    pheromone_map.step(1);
    pheromone_map.step(1);
    pheromone_map.step(1);
    pheromone_map.step(1);
    BOOST_CHECK_LE(pheromone_map.getStrengthAtPosition(Point(5, 5)),
        test_strength);
}

BOOST_FIXTURE_TEST_CASE(test_getStrongestAtArea_shouldReturnOrThrowIfNoneFound,
    Fixture)
{
    const float radius = 5.0;
    const Point blob_centre = Point(5, 5);
    pheromone_map.createBlob(blob_centre, radius, test_strength);
    const Point in_range(3, 3);
    const float r = 3.0;
    BOOST_CHECK_EQUAL(pheromone_map.getStrongestAtArea(in_range, r),
        blob_centre);
    const Point out_of_range(99, 99);
    BOOST_CHECK_THROW(pheromone_map.getStrongestAtArea(out_of_range, r),
        std::runtime_error);
}

BOOST_FIXTURE_TEST_CASE(test_createThenGetMap, Fixture)
{
    auto m = pheromone_map.getMap();
    BOOST_CHECK_EQUAL(m.size(), 10);
    BOOST_CHECK_EQUAL(m[0].size(), 10);
}

BOOST_FIXTURE_TEST_CASE(test_getType, Fixture)
{
    BOOST_CHECK(pheromone_map.getType() == PheromoneMap::Type::ToFood);
}

} // namespace PheromoneMapTest