/*
 *  fisher.cpp
 *
 *  Created by Ania M. Kedzierska on 11/11/11.
 *  Copyright 2011 Politecnic University of Catalonia, Center for Genomic Regulation.  This is program can be redistributed, modified or else as given by the terms of the GNU General Public License.
 *
 */

#include <cstdlib>
#include <cmath>

#include "fisher.h"
#include "matrix.h"
#include "state.h"
#include "state_list.h"
#include "alignment.h"

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <stdexcept>

namespace ublas = boost::numeric::ublas;

//Please refer to the Empar paper for the description of the procedure.

//  The free parameters (without stochastic condition) are encoded as a linear index.
// There will be np*nedges + rdf+1, where
// np: free parameters of the transition matrices
// rdf: df of the root.
// We put the parameters in a matric, first the transition parameters, then the root parameters at the end.
// Root parameters are present only for SSM and GMM.

// Converts an edge-row-column to a linear index identifying the parameter for the given model.
long erc2param(Model &Mod, long e, long r, long c) {
  long p = Mod.matrix_structure(r, c);
  return e*Mod.np + p;
}

// Converts the root distribution index into a parameter index for the model.
long root2param(Tree &T, Model &Mod, long r) {
  return T.nedges*Mod.np + Mod.root_structure(r);
}

// STEP 2
// We then encode the free parameters taking into account the stochastic condition.
// i.e. we remove the diagonal entry and first parameter of the root.

// Converts an edge-row-column to a linear index identifying the free parameter.
long erc2freeparam(Tree &T, Model &Mod, long e, long r, long c) {
  long p = Mod.matrix_structure(r, c);
  long a,b;

  // find first appearence of parameter p.
  for (a=0; a < T.nalpha; a++) {
    for(b=0; b < T.nalpha; b++) {
      if (Mod.matrix_structure(a,b) == p) {
        if (b > a) return e*Mod.df + p - a - 1;
        else if (b < a) return e*Mod.df + p - a;
        else return -1;

      }
    }
  }

  return -1;
}

// Converts the root distribution index into a free parameter index.
long root2freeparam(Tree &T, Model &Mod, long r) {
  long p = Mod.root_structure(r);

  if (p == 0) return -1;   // p = 0 is the parameter killed.
  else return T.nedges*Mod.df + p - 1;
}

// Computes the state at the endpoints of an edge.
// T: tree
// e: edge
// sth: state on hidden nodes
// stl: state on leaves

// Output:
// a: the state of the source.
// b: the state of the target.

void edgestate(Tree &T, long e, State &sth, State &stl, long &a, long &b) {
  a = sth.s[T.edges[e].s - T.nleaves];      // state on the source node
  if (T.edges[e].t < T.nleaves){            // if the edge connects to a leaf
    b = stl.s[T.edges[e].t];                // state on the target leaf
  } else {
    b = sth.s[T.edges[e].t - T.nleaves];    // state on the target node
  }
}

void edgestate(Tree &T, long e, std::vector<int> &sth, std::vector<int> &stl, long &a, long &b) {
  a = sth[T.edges[e].s - T.nleaves];      // state on the source node
  if (T.edges[e].t < T.nleaves){          // if the edge connects to a leaf
    b = stl[T.edges[e].t];                // state on the target leaf
  } else {
    b = sth[T.edges[e].t - T.nleaves];    // state on the target node
  }
}

// Returns the root state
long rootstate(State &sth) {
  return sth.s[0];
}

long rootstate(std::vector<int> &sth) {
  return sth[0];
}

// Let k be a parameter index (running over all free parameters on tm + root)
// Every value of k describes a set of states compatible with it (see text).
//   * if k corresponds to a transition matrix on edge e, parameter l, the compatible
//     states are the ones with a state (a,b) on edge e, such that the free parameter
//     on the entry (a,b) in the transition matrix is l.
//   * if k corresponds to a root parameter, then the compatible states are the ones
//     with root state matching that parameter.

// The following functions fill three arrays of probabilities.

// pleaf is a vector of joint probabilities on the leaves
void fill_pleaf(Tree &T, StateList &sl, Parameters &Par, Array1 &pleaf) {
  long i, j, l, u, v;
  double p;

  // Initializations
  pleaf.resize(T.nstleaves);
  for (i=0; i < T.nstleaves; i++) {
    pleaf[i] = 0;
  }

  // Loop over all states
  for (i=0; i < T.nstleaves; i++) {
    for(j=0; j < T.nsthidden; j++) {

      // Compute leaf probabilities
      p = Par.r[rootstate(sl.h[j])];
      for(l=0; l < T.nedges; l++) {
        edgestate(T, l, sl.h[j], sl.l[i], u, v);
        p = p*Par.tm[l][u][v];
      }
      pleaf[i] = pleaf[i] + p;
    }
  }
}

// pcond1 is a matrix of probabilities p(I and k), where "I and k" refers to states with
// leaf state I and compatible with k.
// rows in pcond1 are indexed by leaf states. Columns are indexed by a linear index encoding
// a free parameter.

void fill_pcond1(Tree &T, Model &Mod, StateList &sl, Parameters &Par, Array2 &pcond1) {

  long i,j,l,u,v;
  long k,e,a,b;
  double p;
  long npars = T.nedges*Mod.np + Mod.rdf + 1;

  // Initializations
  pcond1.resize(T.nstleaves);
  for (i=0; i < T.nstleaves; i++) {
    pcond1[i].resize(npars);
    for(k=0; k < npars; k++) {
      pcond1[i][k] = 0;
    }
  }

  // Loop over all states
  for (i=0; i < T.nstleaves; i++) {
    for(j=0; j < T.nsthidden; j++) {

      // Loop over edges
      for(e=0; e < T.nedges; e++) {

        // Compute cond1 probabilities
        p = Par.r[rootstate(sl.h[j])];
        for(l=0; l < T.nedges; l++) {
          if (l == e) continue;
          edgestate(T, l, sl.h[j], sl.l[i], u, v);
          p = p*Par.tm[l][u][v];
        }

        edgestate(T, e, sl.h[j], sl.l[i], a, b);
        k = erc2param(Mod, e, a, b);
        pcond1[i][k] = pcond1[i][k] + p;
      }

      // Take care of root parameters in pcond1
      p = 1;
      for(l=0; l < T.nedges; l++) {
        edgestate(T, l, sl.h[j], sl.l[i], u, v);
        p = p*Par.tm[l][u][v];
      }

      k = root2param(T, Mod, rootstate(sl.h[j]));
      pcond1[i][k] = pcond1[i][k] + p;
    }
  }
}

// pcond2 is a 3-dimensional array of probabilities p(I and k and kp) where "I and k and kp" refers
// to states with leaf state I, and compatible with k and kp.

void fill_pcond2(Tree &T, Model &Mod, StateList &sl, Parameters &Par, Array3 &pcond2) {
  long i, j, l, u, v;
  long k, e, a, b;
  long kp, ep, ap, bp;
  double p;

  long npars = T.nedges*Mod.np + Mod.rdf + 1;

  // Initializations
  pcond2.resize(T.nstleaves);
  for (i=0; i < T.nstleaves; i++) {
    pcond2[i].resize(npars);
    for(k=0; k < npars; k++) {
      pcond2[i][k].resize(npars);
      for(l=0; l < npars; l++) {
        pcond2[i][k][l] = 0;
      }
    }
  }

  // Loop over all states
  for (i=0; i < T.nstleaves; i++) {
    for(j=0; j < T.nsthidden; j++) {

      // Loop over pairs of edges for the edge-edge part of pcond2
      for(e=0; e < T.nedges; e++) {
        for(ep=0; ep < T.nedges; ep++) {

          // when e==ep, the corresponding entry must be zero.
          if (e==ep) continue;

          // Compute cond2 probabilities
          p = Par.r[rootstate(sl.h[j])];
          for(l=0; l < T.nedges; l++) {
            if (l == e || l == ep) continue;
            edgestate(T, l, sl.h[j], sl.l[i], u, v);
            p = p*Par.tm[l][u][v];
          }

          edgestate(T, e, sl.h[j], sl.l[i], a, b);
          k = erc2param(Mod, e, a, b);

          edgestate(T, ep, sl.h[j], sl.l[i], ap, bp);
          kp = erc2param(Mod, ep, ap, bp);

          pcond2[i][k][kp] = pcond2[i][k][kp] + p;
        }
      }

      // Loop a single edge for the edge-root part of pcond2
      for(e=0; e < T.nedges; e++) {
        // Take care of root parameters in pcond1
        p = 1;
        for(l=0; l < T.nedges; l++) {
          if (l == e) continue;
          edgestate(T, l, sl.h[j], sl.l[i], u, v);
          p = p*Par.tm[l][u][v];
        }

        edgestate(T, e, sl.h[j], sl.l[i], a, b);
        k = erc2param(Mod, e, a, b);

        kp = root2param(T, Mod, rootstate(sl.h[j]));

        pcond2[i][k][kp] = pcond2[i][k][kp] + p;
        pcond2[i][kp][k] = pcond2[i][kp][k] + p;
      }

      // Recall: In pcond2 with two root parameters k and kp,
      // we always have p(I and k and kp) = 0.
    }
  }
}

// Puts the parameter values in a vector indexed as in the Fisher matrix for the model.
// (4 parameters per edge on K81).
void get_param_vector(Tree &T, Model &Mod, Parameters &Par, std::vector<double> &param) {
  long e, a, b, k;
  long npar = T.nedges * Mod.np + Mod.rdf + 1;
  param.resize(npar);

  for(e=0; e < T.nedges; e++) {
    for(a=0; a < T.nalpha; a++) {
      for(b=0; b < T.nalpha; b++) {
        k = erc2param(Mod, e, a, b);
        param[k] = Par.tm[e][a][b];
      }
    }
  }

  for(a=0; a < T.nalpha; a++) {
    k = root2param(T, Mod, a);
    param[k] = Par.r[a];
  }
}

// Gets the free parameters for a branch
void get_branch_free_param_vector(Tree &T, Model &Mod, Parameters &Par, long br, std::vector<double> &param) {
  long a, b, k;
  param.resize(Mod.df);

  for(a=0; a < T.nalpha; a++) {
    for(b=0; b < T.nalpha; b++) {
      k = erc2freeparam(T, Mod, 0, a, b);
      if(k < 0) continue;
      param[k] = Par.tm[br][a][b];
    }
  }
}

// Puts the parameter values in a vector indexed as in the Fisher matrix for the free parameters.
// (3 parameters per edge on K81).
void get_free_param_vector(Tree &T, Model &Mod, Parameters &Par, std::vector<double> &param) {
  long e, a, b, k;
  long npar = T.nedges * Mod.df + Mod.rdf;
  param.resize(npar);

  for(e=0; e < T.nedges; e++) {
    for(a=0; a < T.nalpha; a++) {
      for(b=0; b < T.nalpha; b++) {
        k = erc2freeparam(T, Mod, e, a, b);
        if(k < 0) continue;
        param[k] = Par.tm[e][a][b];
      }
    }
  }

  for(a=0; a < T.nalpha; a++) {
    k = root2freeparam(T, Mod, a);
    if(k < 0) continue;
    param[k] = Par.r[a];
  }
}

// Computes the observed Fisher information of all Parameters in Par, on the model with
// hidden nodes and stores them in the matrix Imod ( no approximation, so the matrix is not diagonal).
// The rows and columns of Imod are indexed by a number between 0 and np*nedges + rdf + 1, encoding the free parameters
// of the model. The stochastic condition is not taken into account.

// If data is not NULL, computes observed Fisher instead of expected Fisher.

// The formula for the Fisher info of the model parameters (without stochastic constraints) is:
// I(k, k') = sum_{leaf states I} N p(I and k)*p(I and k') / (p_k*p_k'*p(I))
//                            - p(I and k and k') / (p_k*p_k')   <----- This term only when k!=k'
// Here p_k denotes the value of the parameter k.

// The observed Fisher info, is given by the same formula, but inside the sum must add the factor
// x_I / N*p(I).

void Fisher_information_model(Tree &T, Model &Mod, Parameters &Par, long N, Counts *data, Array2 &Imod) {
  long i, j, k;

  double factor;

  std::vector<double> param;

  Array1 pleaf;
  Array2 pcond1;
  Array3 pcond2;

  StateList sl;

  long npars = Mod.np*T.nedges + Mod.rdf + 1;

  Array2 Ineg;
  Ineg.resize(npars);
  Imod.resize(npars);
  for (i=0; i < npars; i++) {
    Ineg[i].resize(npars);
    Imod[i].resize(npars);
    for(j=0; j < npars; j++) {
      Ineg[i][j] = 0;
      Imod[i][j] = 0;
    }
  }

  if (data != NULL && data->nspecies != T.nleaves) {
    throw std::length_error("ERROR: In Fisher_information_model. Counts don't match the tree.");

  }

  sl = create_state_list(T);

  // Fills stuff
  fill_pleaf(T, sl, Par, pleaf);
  fill_pcond1(T, Mod, sl, Par, pcond1);
  fill_pcond2(T, Mod, sl, Par, pcond2);

  get_param_vector(T, Mod, Par, param);

  // Fills entries of Imod one by one.
  for (i=0; i < npars; i++) {
    for(j=0; j < npars; j++) {
      for(k=0; k < T.nstleaves; k++) {
        if (data == NULL) {    // Expected Fisher
          factor = (double) N;
        } else {               // Observed Fisher
          factor = data->c[k] / pleaf[k];
        }

        Imod[i][j] = Imod[i][j] +
          factor * pcond1[k][i] * pcond1[k][j] / pleaf[k];

        Ineg[i][j] = Ineg[i][j] + factor * pcond2[k][i][j];
      }

      // We add up the negatives apart to minimize numerical errors.
      Imod[i][j] = Imod[i][j] - Ineg[i][j];

    }
  }
}

// Takes The Fisher info matrix corresponding to a model Imod, and outputs a Fisher info matrix
// for the free parameters. This means, taking into account the stochastic condition. What we do
// is replace the diagonal parameter by "1 - sum of other parameters"
// Imod has np*nedges + rdf + 1 rows and columns
// Ifree has df*nedges + rdf rows and columns.

// Here np means number of parameters, while df means degrees of freedom. For
// example, K81 has np = 4 and df = 3. SSM has np = 8 and df = 6. The fact that SSM has two
// stochastic constraints instead of one, means that SSM must be treated separately.

void Fisher_information_free(Tree &T, Model &Mod, Array2 &Imod, Array2 &Ifree) {
  long i, j, e, p, l;

  long l1, l2;

  long nfreepar = Mod.df*T.nedges + Mod.rdf;
  long nmodpar = Mod.np*T.nedges + Mod.rdf + 1;

  Ifree.resize(nfreepar);
  for(i=0; i < nfreepar; i++) {
    Ifree[i].resize(nfreepar);
    for(j=0; j < nfreepar; j++) {
      Ifree[i][j] = 0;
    }
  }

  // Counts the number of times a parameter appears in the the transition matrices and root dist.
  std::vector<double> coeff;
  coeff.resize(nmodpar);
  for(i=0; i < nmodpar; i++) {
    coeff[i] = 0;
  }

  for(e=0; e < T.nedges; e++) {
    for(i=0; i < T.nalpha; i++) {
      for(j=0; j < T.nalpha; j++) {
        p = erc2param(Mod, e, i, j);
        coeff[p] = coeff[p] + 1;  // adds one to the corresponding parameter index.
      }
    }
  }
  for (i=0; i < T.nalpha; i++) {
    p = root2param(T, Mod, i);
    coeff[p] = coeff[p] + 1;
  }

  // translation between free parameters and model parameters.
  std::vector<long> modpar;     // corresponding model parameter index
  std::vector<long> modparkill; // corresponding model parameter which is killed.

  modpar.resize(nfreepar);
  modparkill.resize(nfreepar);

  // Transition matrices
  for(e=0; e < T.nedges; e++) {
    for(i=0; i < T.nalpha; i++) {
      for(j=0; j < T.nalpha; j++) {
        l = erc2freeparam(T, Mod, e, i, j);
        if (l < 0) continue;   // negative l means the parameter is killed.
        modpar[l] = erc2param(Mod, e, i, j);
        modparkill[l] = erc2param(Mod, e, i, i);
      }
    }
  }

  // Root
  if (Mod.rdf > 0) {
    for(i=0; i < T.nalpha; i++) {
      l = root2freeparam(T, Mod, i);
      if (l < 0) continue;   // negative l means the parameter is killed.
      modpar[l] = root2param(T, Mod, i);
      modparkill[l] = root2param(T, Mod, 0);
    }
  }

  double c1, c2;
  for(l1=0; l1 < nfreepar ; l1++) {
    c1 = coeff[modpar[l1]]/coeff[modparkill[l1]];
    for(l2=0; l2 < nfreepar; l2++) {
      c2 = coeff[modpar[l2]]/coeff[modparkill[l2]];

      Ifree[l1][l2] = Imod[modpar[l1]][modpar[l2]]
           - c1 * Imod[modparkill[l1]][modpar[l2]]
           - c2 * Imod[modpar[l1]][modparkill[l2]]
           + c1*c2 * Imod[modparkill[l1]][modparkill[l2]];
    }
  }
}

// Computes the fisher information for the model with hidden nodes, and stores the result in I.
// I is a matrix with df*nedges + rdf rows and cols.
// the index for the rows and columns encodes a free parameter.
// 0: first free param on edge 0
// 1: second free param on edge 0
// ...
// df: first free param on edge 1
// ...

void Fisher_information(Tree &T, Model &Mod, Parameters &Par, long N, Array2 &I) {
  Array2 Imod;

  Fisher_information_model(T, Mod, Par, N, NULL, Imod);
  Fisher_information_free(T, Mod, Imod, I);
}

void Observed_Fisher_information(Tree &T, Model &Mod, Parameters &Par, Counts &data, Array2 &I) {
  Array2 Imod;

  Fisher_information_model(T, Mod, Par, data.N, &data, Imod);
  Fisher_information_free(T, Mod, Imod, I);
}

/* Matrix inversion routine.
    Uses lu_factorize and lu_substitute in uBLAS to invert a matrix */
template<class T>
bool matrix_inverse (const ublas::matrix<T>& input, ublas::matrix<T>& inverse) {
  using namespace boost::numeric::ublas;
  typedef permutation_matrix<std::size_t> pmatrix;
  // create a working copy of the input
  matrix<T> A(input);
  // create a permutation matrix for the LU-factorization
  pmatrix pm(A.size1());
  // perform LU-factorization
  int res = lu_factorize(A,pm);
  if( res != 0 ) return false;
  // create identity matrix of "inverse"
  inverse.assign(ublas::identity_matrix<T>(A.size1()));
  // backsubstitute to get the inverse
  lu_substitute(A, pm, inverse);
  return true;
}

// Computes the full covariance matrix for the MLE.
void full_MLE_covariance_matrix(Tree &T, Model &Mod, Parameters &Par, long N, Array2 &Cov) {
  long i, j;

  long npars = Mod.df*T.nedges + Mod.rdf;
  ublas::matrix<double> CC(npars, npars);
  ublas::matrix<double> II(npars, npars);
  Array2 I;

  Fisher_information(T, Mod, Par, N, I);

  // Need to convert from Matrix to ublas::matrix<double>
  for(i=0; i < npars; i++) {
    for(j=0; j < npars; j++) {
      II(i, j) = I[i][j];
    }
  }

  bool res = matrix_inverse(II, CC);
  if (!res) {
    throw std::out_of_range( "Could not invert the Fisher information matrix." );
  }

  Cov.resize(npars);
  for(i=0; i < npars; i++) {
    Cov[i].resize(npars);
    for(j=0; j < npars; j++) {
      Cov[i][j] = CC(i,j);
    }
  }
}

// Computes the full covariance matrix for the MLE, using observed Fisher info.
void full_MLE_observed_covariance_matrix(Tree &T, Model &Mod, Parameters &Par, Counts &data, Array2 &Cov) {
  long i, j;

  long npars = Mod.df*T.nedges + Mod.rdf;
  ublas::matrix<double> CC(npars, npars);
  ublas::matrix<double> II(npars, npars);
  Array2 I;

  Observed_Fisher_information(T, Mod, Par, data, I);

  // Need to convert from Matrix to ublas::matrix<double>
  for(i=0; i < npars; i++) {
    for(j=0; j < npars; j++) {
      II(i, j) = I[i][j];
    }
  }

  bool res = matrix_inverse(II, CC);
  if (!res) {
    throw std::out_of_range("Could not invert the Fisher information matrix.");
  }

  Cov.resize(npars);
  for(i=0; i < npars; i++) {
    Cov[i].resize(npars);
    for(j=0; j < npars; j++) {
      Cov[i][j] = CC(i,j);
    }
  }
}

// Extracts the covariance matrix for a given branch from the full covariance matrix.
void branch_covariance_matrix(Model &Mod, Array2 &Covfull, long b, Array2 &Covbr) {
  long i,j;

  Covbr.resize(Mod.df);
  for(i=0; i < Mod.df; i++) {
    Covbr[i].resize(Mod.df);
    for(j=0; j< Mod.df; j++) {
      Covbr[i][j] = Covfull[Mod.df*b + i][Mod.df*b + j];
    }
  }
}

// Extracts the inverted covariance matrix for a given branch from the full covariance matrix.
void branch_inverted_covariance_matrix(Model &Mod, Array2 &Covfull, long b, Array2 &Covbri) {
  long i,j;

  ublas::matrix<double> CC(Mod.df, Mod.df);
  ublas::matrix<double> II(Mod.df, Mod.df);

  for(i=0; i < Mod.df; i++) {
    for(j=0; j< Mod.df; j++) {
      CC(i,j) = Covfull[Mod.df*b + i][Mod.df*b + j];
    }
  }

  bool res = matrix_inverse(CC, II);
  if (!res) {
    throw std::out_of_range( "Could not invert the Covariance matrix." );
  }

  Covbri.resize(Mod.df);
  for(i=0; i < Mod.df; i++) {
    Covbri[i].resize(Mod.df);
    for(j=0; j< Mod.df; j++) {
      Covbri[i][j] = II(i,j);
    }
  }
}


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/div.hpp>
#include <boost/hana/functional/reverse_partial.hpp>
namespace hana = boost::hana;

constexpr auto half = hana::reverse_partial(hana::div, 2);
static_assert(half(4) == 2, "");
static_assert(half(8) == 4, "");

int main() { }


/*--------------------------------------------------------------------------

 File Name:         filter.cpp
 Date Created:      2017/08/07
 Date Modified:     2017/09/08

 Author:            Eric Cristofalo
 Contact:           eric.cristofalo@gmail.com

 Description:       ROS node for estimating realtime pose and odometry via EKF from Optitrack and Ouijibot IMU/wheel encoders

 -------------------------------------------------------------------------*/

#include <iostream>
#include <stdio.h>
#include <math.h>
#include <random>

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
// #include <geometry_msgs/Accel.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>

#include <ouijabot/Wheel_Spd.h>

#include <Eigen/Dense>
#include <Eigen/Core>
#include <unsupported/Eigen/MatrixFunctions> // matrix exponential

using namespace std;
using Eigen::MatrixXd;
using Eigen::VectorXd;

class filterClass {

  ros::NodeHandle nh_;

  ros::Subscriber process_imu_sub_;
  ros::Subscriber process_wheel_speed_sub_;

  ros::Subscriber ground_truth_sub_;
  // ros::Subscriber ground_truth_accel_sub_;

  ros::Publisher state_estimate_pub_;
  tf::TransformBroadcaster state_estimate_broadcaster;
  nav_msgs::Odometry stateEstimateMsg;

  // Initialalize Input Variables
  int displayData, useOptitrack;
  double r_w, r_b;
  VectorXd q_cov_diag, r_cov_diag;

  // Initialize Filter Variables
  int initCount;
  double count, initTime;
  bool initPoseBool, predictBool, updateBool, gyroBool, wheelSpdBool;
  ros::Time timeCur, timePrev;
  VectorXd state, state_, state_gt, y, gyro, accel, wheel_spd;
  MatrixXd Sigma, Sigma_, Q, R;
  MatrixXd R_rl;

public:
  filterClass(int in_01, int in_02, double in_03, double in_04, vector<double> in_05, vector<double> in_06, vector<double> in_07) {

    // Subscribe to Ouijibot IMU
    process_imu_sub_ = nh_.subscribe("/ouijabot/imu", 10, &filterClass::imuCallback, this);

    // Subscribe to Ouijibot Wheel Encoders
    process_wheel_speed_sub_ = nh_.subscribe("/ouijabot/wheel_spd", 10, &filterClass::wheelSpeedCallback, this);

    // Publish Odometry Message of State Estimate
    state_estimate_pub_ = nh_.advertise<nav_msgs::Odometry>("/robot/state_estimate", 10);

    // Initialize Variables
    displayData = in_01;
    useOptitrack = in_02;
    r_w = in_03;
    r_b = in_04;
    q_cov_diag = VectorXd::Zero(9);
    for ( int i=0; i<9; i++ ) {
      q_cov_diag(i) = in_05[i];
    }
    r_cov_diag = VectorXd::Zero(7);
    for ( int i=0; i<7; i++ ) {
      r_cov_diag(i) = in_06[i];
    }
    MatrixXd R_temp = MatrixXd::Zero(3,3);
    for ( int i=0; i<3; i++ ) {
      for ( int j=0; j<3; j++) {
        R_temp(i,j) = in_07[i*3+j];
      }
    }
    R_rl = MatrixXd(2,2);
    R_rl = R_temp.block(0,0,2,2);

    // Subscribe to Optitrack Ground Truth Topics
    // Required mocap_interface_odom node to be publishing "ground truth" odometry data (position, derived velocity, and derived acceleration)
    initPoseBool = false;
    if ( useOptitrack ) {
      initPoseBool = true; // Initialize the filter with current Optitrack measurement
      ground_truth_sub_ = nh_.subscribe("/robot/pose", 10, &filterClass::odomCallback, this);
      // ground_truth_accel_sub_ = nh_.subscribe("/robot/accel", 10, &filterClass::odomAccelCallback, this);
    }

    // Initialize Filter Variables
    timeCur = ros::Time::now();
    timePrev = ros::Time::now();
    initCount = 0;
    initTime = 0.0;
    predictBool = false;
    updateBool = false;
    gyroBool = false;
    wheelSpdBool = false;

    // Initialize State Variables
    count = 0;
    state = VectorXd::Zero(9); // update: [x,y,theta,v_x,v_y,omega,a_x,a_y,a_omega]^T
    state_ = VectorXd::Zero(9); // prediction: [x,y,theta,v_x,v_y,omega,a_x,a_y,a_omega]^T
    state_gt = VectorXd::Zero(9); // ground truth: [x,y,theta,v_x,v_y,omega,a_x,a_y,a_omega]^T
    y = VectorXd::Zero(7); // measurement: [w1, w2, w3, w4, w_gyro, a_x, a_y]^T
    gyro = VectorXd::Zero(3); // gyro measurement
    accel = VectorXd::Zero(3); // accelerometer measurement
    wheel_spd = VectorXd::Zero(4); // wheel speed measurement
    Q = q_cov_diag.asDiagonal(); // dynamics covariance matrix
    R = r_cov_diag.asDiagonal(); // measurement covariance matrix
    Sigma = 1E6*MatrixXd::Identity(9,9); // update: estimation error covariance matrix
    Sigma_ = 1E6*MatrixXd::Identity(9,9); // update: estimation error covariance matrix

    // double phiCur, theCur, psiCur;
    // psiCur = M_PI/2.0;
    // theCur = 0.0;
    // phiCur = M_PI;
    // MatrixXd R_phi = MatrixXd::Zero(3,3);
    // MatrixXd R_the = MatrixXd::Zero(3,3);
    // MatrixXd R_psi = MatrixXd::Zero(3,3);
    // R_phi <<  1.0    ,   0.0         ,   0.0         ,
    //           0.0    ,   cos(phiCur) ,   -sin(phiCur),
    //           0.0    ,   sin(phiCur) ,   cos(phiCur) ;
    // R_the <<  cos(theCur)    ,   0.0 ,   sin(theCur) ,
    //           0.0            ,   1.0 ,   0.0         ,
    //           -sin(theCur)   ,   0.0 ,   cos(theCur) ;
    // R_psi <<  cos(psiCur)    ,   -sin(psiCur),   0.0,
    //           sin(psiCur)    ,   cos(psiCur) ,   0.0,
    //           0.0            ,   0.0         ,   1.0;
    // // Quad Orientation
    // MatrixXd R_test = MatrixXd::Zero(3,3);
    // R_test = R_psi*R_the*R_phi;
    // cout << "ROTATION TEST:" << endl << R_test;

  }

  void runFilter()
  {
    // Initialize Detection Timer
    //std::clock_t start = std::clock();

    // Time Interval
    timeCur = ros::Time::now();
    if ( count==0 ) {
      timePrev = ros::Time::now();
    }
    double dt = (timeCur - timePrev).toSec();

    // Prediction Step
    // if ( predictBool ) {
    // if ( predictBool && updateBool ) {
    if ( updateBool ) {

      // Define Property Matrices For Dynamics Matrices
      MatrixXd Rwr = MatrixXd::Zero(2,2); // Active rotation watrix from world frame to local robot frame
      Rwr <<  cos(state(2)), -sin(state(2)),
              sin(state(2)),  cos(state(2));
      MatrixXd Omega = MatrixXd::Zero(2,2);
      Omega << 0.0, -state(5), state(5), 0.0;

      // Construct Nonlinear Continuous-Time Dynamics Matrix
      MatrixXd F_nl = MatrixXd::Zero(9,9);
      F_nl.block(0,3,2,2) = Rwr.transpose();
      F_nl(2,5) = 1.0;
      F_nl.block(3,3,2,2) = Omega.transpose()*dt;
      F_nl.block(3,6,3,3) = MatrixXd::Identity(3,3);
      F_nl.block(6,6,2,2) = Omega.transpose()*dt;
      // Discretize Dynamics Matrix
      F_nl = F_nl.exp();
      F_nl.block(0,3,3,3) = F_nl.block(0,3,3,3)*dt;
      F_nl.block(0,6,3,3) = F_nl.block(0,6,3,3)*dt*dt;
      F_nl.block(3,6,3,3) = F_nl.block(3,6,3,3)*dt;

      // State Prediction
      state_ = F_nl*state;

      // Construct Linearized Continuous-Time Dynamics Matrix
      MatrixXd F = MatrixXd::Zero(9,9);
      F(0,2) = ( -sin(state(2))*state(3) - cos(state(2))*state(4) )*dt;
      F(1,2) = (  cos(state(2))*state(3) - cos(state(2))*state(4) )*dt;
      F.block(0,3,2,2) = Rwr.transpose();
      F_nl(2,5) = 1.0;
      F.block(3,3,2,2) = Omega.transpose()*dt;
      F(3,5) = state(4)*dt;
      F(4,5) = -state(3)*dt;
      F.block(3,6,3,3) = MatrixXd::Identity(3,3);
      F(6,5) = state(7)*dt;
      F(7,5) = -state(6)*dt;
      F.block(6,6,2,2) = Omega.transpose()*dt;
      // Discretize Dynamics Matrix
      F = F.exp();
      F.block(0,3,3,3) = F.block(0,3,3,3)*dt;
      F.block(0,6,3,3) = F.block(0,6,3,3)*dt*dt;
      F.block(3,6,3,3) = F.block(3,6,3,3)*dt;

      // EKF Covariance Prediction in Discrete Time
      // MatrixXd Q_tilde = F*Q*F.transpose()*dt;
      MatrixXd Q_tilde = Q;
      Sigma_ = F*Sigma*F.transpose() + Q_tilde;

      // End EKF Prediction
      // predictBool = false; // TEMPORARY COMMENTED OUT
      timePrev = timeCur;

      if ( displayData ) {
        cout << "Prediction" << endl;
        cout << "Time Interval: " << endl << dt << endl;
        // cout << "Q: " << endl << Q << endl;
        // cout << "Q_tilde: " << endl << Q_tilde << endl;
      }

    }
    else {
      state_ = state;
      Sigma_ = Sigma;
    }

    // Update Step
    if ( updateBool ) {
    // if ( predictBool && updateBool ) {

      // Linear Measurement Matrix
      MatrixXd H = MatrixXd::Zero(7,9);
      double s = sqrt(2.0);
      H(0,3) = -s/(2.0*r_w);  H(0,4) = -s/(2.0*r_w);  H(0,5) = r_b/r_w;
      H(1,3) = -s/(2.0*r_w);  H(1,4) =  s/(2.0*r_w);  H(1,5) = r_b/r_w;
      H(2,3) =  s/(2.0*r_w);  H(2,4) =  s/(2.0*r_w);  H(2,5) = r_b/r_w;
      H(3,3) =  s/(2.0*r_w);  H(3,4) = -s/(2.0*r_w);  H(3,5) = r_b/r_w;
      H.block(4,5,3,3) = MatrixXd::Identity(3,3);

      // Kalman Gain in Discrete Time
      // MatrixXd R_tilde = R/dt;
      MatrixXd R_tilde = R;
      MatrixXd S = (H*Sigma_*H.transpose() + R_tilde);
      MatrixXd KalmanK = Sigma_*H.transpose()*S.inverse();

      // EKF State Update
      state = state_ + KalmanK*(y - H*state_);

      // EKF Covariance Update
      Sigma = Sigma_ - KalmanK*H*Sigma_;
      // cout << "Sigma: " << endl << Sigma(0,0) << endl << Sigma(3,3) << endl << "-----------" << endl;

      // End EKF Update
      updateBool = false;
      // predictBool = false; // TEMPORARY HERE

      if ( displayData ) {
        cout << "Update" << endl;
        // cout << "R: " << endl << R << endl;
        // cout << "R_tilde: " << endl << R_tilde << endl;
      }

    }
    else {
      state = state_;
      Sigma = Sigma_;
    }

    // Convert to Global Reference Frame
    // Somewhat guessing on the transformations for the moment (see negative sign in gyro as well)
    VectorXd state_output = VectorXd::Zero(9);
    state_output.segment(0,2) = -R_rl*state.segment(0,2);
    state_output(2) = state(2);
    state_output.segment(3,2) = -R_rl*state.segment(3,2);
    state_output(5) = state(5);
    state_output.segment(6,2) = -R_rl*state.segment(6,2);
    state_output(8) = state(8);

    // Publish State Estimate Message
    ros::Time pubTime = ros::Time::now();
    // Publish Odometry Transform
    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(state_output(2));
    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.stamp = pubTime;
    odom_trans.header.frame_id = "/odom_frame_est";
    odom_trans.child_frame_id = "/base_link_est";
    odom_trans.transform.translation.x = state_output(0);
    odom_trans.transform.translation.y = state_output(1);
    odom_trans.transform.translation.z = 0.0;
    odom_trans.transform.rotation = odom_quat;
    state_estimate_broadcaster.sendTransform(odom_trans);
    // ros::Duration(0.5).sleep(); // sleep for half a second
    // Set Final Pose
    stateEstimateMsg.header.stamp = pubTime;
    stateEstimateMsg.header.frame_id = "/odom_frame_est";
    stateEstimateMsg.pose.pose.position.x = state_output(0);
    stateEstimateMsg.pose.pose.position.y = state_output(1);
    stateEstimateMsg.pose.pose.position.z = 0.0;
    stateEstimateMsg.pose.pose.orientation = odom_quat;
    // Set Final Pose Covariance
    stateEstimateMsg.pose.covariance[0] = Sigma(0,0);
    stateEstimateMsg.pose.covariance[1] = Sigma(1,1);
    stateEstimateMsg.pose.covariance[2] = Sigma(2,2);
    // Set Final Velocity
    stateEstimateMsg.child_frame_id = "/base_link_est";
    stateEstimateMsg.twist.twist.linear.x = state_output(3);
    stateEstimateMsg.twist.twist.linear.y = state_output(4);
    stateEstimateMsg.twist.twist.linear.z = 0.0;
    stateEstimateMsg.twist.twist.angular.x = 0.0;
    stateEstimateMsg.twist.twist.angular.y = 0.0;
    stateEstimateMsg.twist.twist.angular.z = state_output(5);
        // Set Final Velocity Covariance
    stateEstimateMsg.twist.covariance[0] = Sigma(3,3);
    stateEstimateMsg.twist.covariance[1] = Sigma(4,4);
    stateEstimateMsg.twist.covariance[2] = Sigma(5,5);

    // Publish Odometry Message
    state_estimate_pub_.publish(stateEstimateMsg);

    // Display Data in Terminal
    if (displayData) {
      cout << "Prediction: " << endl << state_ << endl;
      // cout << "Covariance: " << endl << Sigma_(0,0) << endl << Sigma_(1,1) << endl << Sigma_(2,2) << endl << Sigma_(3,3) << endl << Sigma_(4,4) << endl << Sigma_(5,5) << endl;
      cout << "Update: " << endl << state << endl;
      // cout << "Covariance: " << endl << Sigma(0,0) << endl << Sigma(1,1) << endl << Sigma(2,2) << endl << Sigma(3,3) << endl << Sigma(4,4) << endl << Sigma(5,5) << endl;
      // cout << "------------------------------" << endl;
      // cout << "Current Ground Truth: " << endl << state_gt << endl;
      // cout << "Current Estimate: " << endl << state << endl;
      cout << "Current Measurement: " << endl << y << endl;
      cout << "------------------------------" << endl;
    }

    // Update Filter Count
    count++;
  }

  void readMeasurement()
  {
    if ( gyroBool && wheelSpdBool && !initPoseBool ) {
      // Fill Measurement Vector
      y.head(4) = wheel_spd;
      y(4) = gyro(2);
      y.tail(2) = accel.head(2);
      // Reset Measurements
      gyroBool = false;
      wheelSpdBool = false;
      // Run Filter Update
      updateBool = true;
      runFilter();
    }
  }

  void imuCallback(const sensor_msgs::Imu& msg)
  {
    // Extract Data
    // double qx, qy, qz, qw;
    // qx = msg.orientation.x;
    // qy = msg.orientation.y;
    // qz = msg.orientation.z;
    // qw = msg.orientation.w;
    // Accelerometer is positioned with a rotation of pi about the z-axis!
    accel(0) = -msg.linear_acceleration.x;
    accel(1) = -msg.linear_acceleration.y;
    accel(2) = msg.linear_acceleration.z;
    // Filter Accelerometer Noise
    if ( abs(accel(0)) < 0.25 ) accel(0) = 0;
    if ( abs(accel(1)) < 0.25 ) accel(1) = 0;
    if ( abs(accel(2)) < 0.25 ) accel(2) = 0;
    // Convert to gyro measurement to radians/second
    gyro(0) = msg.angular_velocity.x*M_PI/180.0;
    gyro(1) = msg.angular_velocity.y*M_PI/180.0;
    gyro(2) = -msg.angular_velocity.z*M_PI/180.0; // need negative sign here with negatives in final output
    // Process EKF Measurement
    gyroBool = true;
    readMeasurement();
  }

  void wheelSpeedCallback(const ouijabot::Wheel_Spd& msg)
  {
    // Extract Data
    // Convert from linear velocity to angular velocity (radians/second) with wheel radius
    wheel_spd(0) = msg.wheel_spd[0]/r_w;
    wheel_spd(1) = msg.wheel_spd[1]/r_w;
    wheel_spd(2) = msg.wheel_spd[2]/r_w;
    wheel_spd(3) = msg.wheel_spd[3]/r_w;
    // Process EKF Measurement
    wheelSpdBool = true;
    readMeasurement();
  }

  void odomCallback(const geometry_msgs::PoseStamped& msg)
  {
    if ( useOptitrack ) {
      // Extract Position
      VectorXd state_temp = VectorXd::Zero(9);
      state_temp(0) = msg.pose.position.x;  // x
      state_temp(1) = msg.pose.position.y;  // x
      double qx = msg.pose.orientation.x;
      double qy = msg.pose.orientation.y;
      double qz = msg.pose.orientation.z;
      double qw = msg.pose.orientation.w;
      double phiCur, theCur, psiCur;
      tf::Quaternion q(qx, qy, qz, qw);
      tf::Matrix3x3 m(q);
      m.getRPY(phiCur, theCur, psiCur);
      state_temp(2) = psiCur;               // theta
      // // Extract Velocity
      // state_temp(3) = msg.pose.orinetation.linear.x;   // v_x
      // state_temp(4) = msg.pose.orinetation.linear.y;   // v_y
      // state_temp(5) = msg.pose.orinetation.angular.z;  // omega
      // Compute Initial Pose
      if ( initPoseBool ) {
        if ( initCount==0) {
          timePrev = ros::Time::now();
        }
        timeCur = ros::Time::now();
        double dt = (timeCur-timePrev).toSec();
        initTime = initTime + dt;
        if ( initTime<10.0 ) {
          initCount++;
          state = state + state_temp;
        }
        else {
          // Average Poses
          state = state/double(initCount);
          // Convert to Local Robot Frame
          // Somewhat guessing on the transformations for the moment (see negative sign in gyro as well)
          VectorXd state_output = VectorXd::Zero(9,1);
          state.segment(0,2) = -R_rl.transpose()*state.segment(0,2);
          state(2) = state(2);
          // Finish Up
          timePrev = ros::Time::now();
          initPoseBool = false;
          predictBool = true;
          cout << "ouijabot_filter: initial mean pose after " << double(initCount) << " iterations and " << initTime << " seconds: " << endl << state << endl;
        }
      }
      else {
        state_gt.head(6) = state_temp.head(6);
      }
    }
  }

  // void odomAccelCallback(const geometry_msgs::Accel& msg) {
  //   if ( useOptitrack ) {
  //     state_gt(6) = msg.linear.x;
  //     state_gt(7) = msg.linear.y;
  //     state_gt(8) = msg.angular.z;
  //   }
  // }

};

int main(int argc, char **argv)
{
  ros::init(argc, argv, "filter");
  ros::NodeHandle nh("~");

  int display_data_flag;
  nh.param<int>("display_data_flag", display_data_flag, 0);
  int use_optitrack_flag;
  nh.param<int>("use_optitrack_flag", use_optitrack_flag, 0);

  double wheel_radius;
  nh.getParam( "wheel_radius", wheel_radius );
  if (!nh.hasParam("wheel_radius")) {
    wheel_radius = 0.025;
  }
  double body_radius;
  nh.getParam( "body_radius", body_radius );
  if (!nh.hasParam("body_radius")) {
    body_radius = 0.1;
  }

  std::vector<double> q_cov_diag;
  nh.getParam( "q_cov_diag", q_cov_diag );
  if (!nh.hasParam("q_cov_diag")) {
    q_cov_diag = {0.0001, 0.0001, 0.0001, 0.001, 0.001, 0.001, 0.01, 0.01, 0.01};
  }
  std::vector<double> r_cov_diag;
  nh.getParam( "r_cov_diag", r_cov_diag );
  if (!nh.hasParam("r_cov_diag")) {
    r_cov_diag = {0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001};
  }

  std::vector<double> rotation_robot_local;
  nh.getParam( "rotation_robot_local", rotation_robot_local );
  if (!nh.hasParam("rotation_robot_local")) {
    rotation_robot_local = {0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0};
  }

  filterClass filter(
    display_data_flag,
    use_optitrack_flag,
    wheel_radius,
    body_radius,
    q_cov_diag,
    r_cov_diag,
    rotation_robot_local
  );

  ros::Rate r(20); // 20 hz
  while (ros::ok()) {
    ros::spinOnce();
    r.sleep();
  }
  ros::shutdown();

  return 0;
}


/**************************************************************************
 * Copyright (c) 2017-2019 by the mfmg authors                            *
 * All rights reserved.                                                   *
 *                                                                        *
 * This file is part of the mfmg library. mfmg is distributed under a BSD *
 * 3-clause license. For the licensing terms see the LICENSE file in the  *
 * top-level directory                                                    *
 *                                                                        *
 * SPDX-License-Identifier: BSD-3-Clause                                  *
 *************************************************************************/

#ifndef MFMG_DEALII_MATRIX_FREE_SMOOTHER_HPP
#define MFMG_DEALII_MATRIX_FREE_SMOOTHER_HPP

#include <mfmg/common/smoother.hpp>
#include <mfmg/dealii/dealii_matrix_free_operator.hpp>

#include <deal.II/lac/diagonal_matrix.h>
#include <deal.II/lac/precondition.h>

#include <boost/property_tree/ptree.hpp>

#include <memory>

namespace mfmg
{
template <int dim, typename VectorType>
class DealIIMatrixFreeSmoother final : public Smoother<VectorType>
{
public:
  using vector_type = VectorType;
  using operator_type = DealIIMatrixFreeOperator<dim, VectorType>;
  using preconditioner_type = dealii::DiagonalMatrix<VectorType>;
  using chebyshev_preconditioner =
      dealii::PreconditionChebyshev<operator_type, vector_type,
                                    preconditioner_type>;

  DealIIMatrixFreeSmoother(
      std::shared_ptr<Operator<vector_type> const> op,
      std::shared_ptr<boost::property_tree::ptree const> params);

  virtual ~DealIIMatrixFreeSmoother() override = default;

  void apply(vector_type const &b, vector_type &x) const override;

private:
  std::unique_ptr<chebyshev_preconditioner> _smoother;
};
} // namespace mfmg

#endif


/***************************************************************************
/* Javier Juan Albarracin - jajuaal1@ibime.upv.es                         */
/* Universidad Politecnica de Valencia, Spain                             */
/*                                                                        */
/* Copyright (C) 2018 Javier Juan Albarracin                              */
/*                                                                        */
/***************************************************************************
* CImg <-> MATLAB data type conversions                                    *
***************************************************************************/

#ifndef EIGENITK_HPP
#define EIGENITK_HPP

#include <stdexcept>
#include <Eigen/Dense>
#include <ITKUtils.hpp>
#include <itkImage.h>
#include <itkImageRegionConstIteratorWithIndex.h>

using namespace Eigen;

class EigenITK
{
public:
    template<typename ImageType, typename MaskType>
    static Matrix<typename ImageType::PixelType, Dynamic, Dynamic> toEigen(const typename ImageType::Pointer image, const typename MaskType::Pointer mask);
    template <typename ImageType, typename MaskType, typename Derived>
    static typename ImageType::Pointer toITK(const DenseBase<Derived> &data, const typename MaskType::Pointer mask);
    template <typename ImageType, typename MaskType, typename Derived>
    static void toITK(const DenseBase<Derived> &data, const typename MaskType::Pointer mask, typename ImageType::Pointer image);
};

template<typename ImageType, typename MaskType>
Matrix<typename ImageType::PixelType, Dynamic, Dynamic> EigenITK::toEigen(const typename ImageType::Pointer image, const typename MaskType::Pointer mask)
{
    ITKUtils::AssertCompatibleImageAndMaskTypes<ImageType, MaskType>();
    ITKUtils::AssertCompatibleImageAndMaskSizes<ImageType, MaskType>(image, mask);

    const unsigned int ImageDimension = ImageType::ImageDimension;
    const unsigned int MaskDimension = MaskType::ImageDimension;
    const unsigned int ChannelsDimensionIndex = ImageDimension - 1;

    typename ImageType::SizeType imageSize = image->GetLargestPossibleRegion().GetSize();

    int rows = 1;
    int cols = ImageDimension == MaskDimension ? 1 : imageSize[ChannelsDimensionIndex];
    for (int j = 0; j < ChannelsDimensionIndex; ++j)
    {
        rows *= imageSize[j];
    }

    Matrix<typename ImageType::PixelType, Dynamic, Dynamic> data(rows, cols);

    itk::ImageRegionConstIteratorWithIndex<MaskType> iterator(mask, mask->GetLargestPossibleRegion());
    int i = 0;
    iterator.GoToBegin();
    while(!iterator.IsAtEnd())
    {
        if (iterator.Get())
        {
            itk::Index<MaskDimension> maskIndex = iterator.GetIndex();
            itk::Index<ImageDimension> imageIndex = ITKUtils::GetSpatialIndex<MaskDimension, ImageDimension>(maskIndex);

            if (ImageDimension == MaskDimension)
                data(i, 0) = image->GetPixel(imageIndex);
            else
            {
                for (int j = 0; j < imageSize[ChannelsDimensionIndex]; ++j)
                {
                    imageIndex[ChannelsDimensionIndex] = j;
                    data(i, j) = image->GetPixel(imageIndex);

                }
            }
            ++i;
        }
        ++iterator;
    }
    data.conservativeResize(i, data.cols());
    return data;
}

template <typename ImageType, typename MaskType, typename Derived>
typename ImageType::Pointer EigenITK::toITK(const DenseBase<Derived> &data, const typename MaskType::Pointer mask)
{
    ITKUtils::AssertCompatibleImageAndMaskTypes<ImageType, MaskType>();

    typename ImageType::Pointer image = ImageType::New();
    ITKUtils::AllocateRegionFromInput<ImageType, MaskType>(image, mask, data.cols());
    EigenITK::toITK<ImageType, MaskType, Derived>(data, mask, image);

    return image;
}

template <typename ImageType, typename MaskType, typename Derived>
void EigenITK::toITK(const DenseBase<Derived> &data, const typename MaskType::Pointer mask, typename ImageType::Pointer image)
{
    ITKUtils::AssertCompatibleImageAndMaskTypes<ImageType, MaskType>();
    ITKUtils::AssertCompatibleImageAndMaskSizes<ImageType, MaskType>(image, mask);

    const unsigned int ImageDimension = ImageType::ImageDimension;
    const unsigned int MaskDimension = MaskType::ImageDimension;
    const unsigned int ChannelsDimensionIndex = ImageDimension - 1;

    itk::ImageRegionConstIterator<MaskType> it(mask, mask->GetLargestPossibleRegion());
    it.GoToBegin();
    int count = 0;
    while (!it.IsAtEnd())
    {
        if (it.Get())
            ++count;
        ++it;
    }

    if (count != data.rows())
    {
        std::stringstream s;
        s << "Incompatible number of rows and positive elements in the mask" << std::endl;
        throw std::runtime_error(s.str());
    }

    typename ImageType::SizeType imageSize = image->GetLargestPossibleRegion().GetSize();;

    itk::ImageRegionConstIteratorWithIndex<MaskType> iterator(mask, mask->GetLargestPossibleRegion());
    int i = 0;
    iterator.GoToBegin();
    while (!iterator.IsAtEnd())
    {
        itk::Index<MaskDimension> maskIndex = iterator.GetIndex();
        itk::Index<ImageDimension> imageIndex = ITKUtils::GetSpatialIndex<MaskDimension, ImageDimension>(maskIndex);

        if (ImageDimension == MaskDimension)
            image->SetPixel(imageIndex, (typename ImageType::PixelType) iterator.Get() ? data(i, 0) : 0);
        else
        {
            for (int j = 0; j < imageSize[ChannelsDimensionIndex]; j++)
            {
                imageIndex[ChannelsDimensionIndex] = j;
                image->SetPixel(imageIndex, (typename ImageType::PixelType) iterator.Get() ? data(i, j) : 0);
            }
        }
        i += iterator.Get() ? 1 : 0;
        ++iterator;
    }
}

#endif


#pragma once

#include <polyfem/Common.hpp>
#include <polyfem/NLProblem.hpp>
#include <polyfem/MatrixUtils.hpp>
#include <polyfem/State.hpp>

#include <polyfem/Logger.hpp>

#include <cppoptlib/problem.h>
#include <cppoptlib/solver/isolver.h>
#include <cppoptlib/linesearch/armijo.h>
#include <cppoptlib/linesearch/morethuente.h>
#include <Eigen/LU>
#include <iostream>

namespace cppoptlib
{

	template <typename ProblemType>
	class LbfgsSolverL2 : public ISolver<ProblemType, 1>
	{
	public:
		using Superclass = ISolver<ProblemType, 1>;
		using typename Superclass::Scalar;
		using typename Superclass::THessian;
		using typename Superclass::TVector;
		using MatrixType = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;

		enum class LineSearch
		{
			Armijo,
			MoreThuente,
		};

		LineSearch line_search = LineSearch::Armijo;

		void setLineSearch(const std::string &name)
		{
			if (name == "armijo")
			{
				line_search = LineSearch::Armijo;
			}
			else if (name == "more_thuente")
			{
				line_search = LineSearch::MoreThuente;
			}
			else
			{
				throw std::invalid_argument("[SparseNewtonDescentSolver] Unknown line search.");
			}
			polyfem::logger().debug("\tline search {}", name);
		}

		void minimize(ProblemType &objFunc, TVector &x0)
		{
			const size_t m = 10;
			const size_t DIM = x0.rows();
			MatrixType sVector = MatrixType::Zero(DIM, m);
			MatrixType yVector = MatrixType::Zero(DIM, m);
			Eigen::Matrix<Scalar, Eigen::Dynamic, 1> alpha = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>::Zero(m);
			TVector grad(DIM), q(DIM), grad_old(DIM), s(DIM), y(DIM);
			objFunc.gradient(x0, grad);
			TVector x_old = x0;
			TVector x_old2 = x0;

			size_t iter = 0, globIter = 0;
			Scalar H0k = 1;
			this->m_current.reset();
			do
			{
				const Scalar relativeEpsilon = static_cast<Scalar>(0.0001) * std::max(static_cast<Scalar>(1.0), x0.norm());

				if (grad.norm() < relativeEpsilon)
					break;

				//Algorithm 7.4 (L-BFGS two-loop recursion)
				q = grad;
				const int k = std::min(m, iter);

				// for i = k − 1, k − 2, . . . , k − m§
				for (int i = k - 1; i >= 0; i--)
				{
					// alpha_i <- rho_i*s_i^T*q
					const double rho = 1.0 / static_cast<TVector>(sVector.col(i))
												 .dot(static_cast<TVector>(yVector.col(i)));
					alpha(i) = rho * static_cast<TVector>(sVector.col(i)).dot(q);
					// q <- q - alpha_i*y_i
					q = q - alpha(i) * yVector.col(i);
				}
				// r <- H_k^0*q
				q = H0k * q;
				//for i k − m, k − m + 1, . . . , k − 1
				for (int i = 0; i < k; i++)
				{
					// beta <- rho_i * y_i^T * r
					const Scalar rho = 1.0 / static_cast<TVector>(sVector.col(i))
												 .dot(static_cast<TVector>(yVector.col(i)));
					const Scalar beta = rho * static_cast<TVector>(yVector.col(i)).dot(q);
					// r <- r + s_i * ( alpha_i - beta)
					q = q + sVector.col(i) * (alpha(i) - beta);
				}
				// stop with result "H_k*f_f'=q"

				// any issues with the descent direction ?
				Scalar descent = -grad.dot(q);
				Scalar alpha_init = 1.0 / grad.norm();
				if (descent > -0.0001 * relativeEpsilon)
				{
					q = -1 * grad;
					iter = 0;
					alpha_init = 1.0;
				}

				// find steplength
				// const Scalar rate = MoreThuente<ProblemType, 1>::linesearch(x0, -q,  objFunc, alpha_init) ;
				Scalar rate;
				switch (line_search)
				{
				case LineSearch::Armijo:
					rate = Armijo<ProblemType, 1>::linesearch(x0, -q, objFunc);
					break;
				case LineSearch::MoreThuente:
					rate = MoreThuente<ProblemType, 1>::linesearch(x0, -q, objFunc);
					break;
				}
				// update guess
				x0 = x0 - rate * q;

				grad_old = grad;
				objFunc.gradient(x0, grad);

				s = x0 - x_old;
				y = grad - grad_old;

				// update the history
				if (iter < m)
				{
					sVector.col(iter) = s;
					yVector.col(iter) = y;
				}
				else
				{

					sVector.leftCols(m - 1) = sVector.rightCols(m - 1).eval();
					sVector.rightCols(1) = s;
					yVector.leftCols(m - 1) = yVector.rightCols(m - 1).eval();
					yVector.rightCols(1) = y;
				}
				// update the scaling factor
				H0k = y.dot(s) / static_cast<double>(y.dot(y));

				x_old = x0;
				polyfem::logger().debug("\titer: {}, f = {}, ‖g‖_2 = {}", globIter, objFunc.value(x0), grad.norm());

				iter++;
				globIter++;
				++this->m_current.iterations;
				this->m_current.gradNorm = grad.norm(); // template lpNorm<Eigen::Infinity>();
				this->m_status = checkConvergence(this->m_stop, this->m_current);
			} while ((objFunc.callback(this->m_current, x0)) && (this->m_status == Status::Continue));
		}
	};

} // namespace cppoptlib


//==================================================================================================
/*!
  @file

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T.Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_DETAIL_SCALAR_F_LOG_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_DETAIL_SCALAR_F_LOG_HPP_INCLUDED

#ifndef BOOST_SIMD_NO_NANS
#include <boost/simd/function/scalar/is_nan.hpp>
#endif
#ifndef BOOST_SIMD_NO_INFINITIES
#include <boost/simd/constant/inf.hpp>
#endif
#ifndef BOOST_SIMD_NO_DENORMALS
#include <boost/simd/constant/mlog10two2nmb.hpp>
#include <boost/simd/constant/mlog2two2nmb.hpp>
#include <boost/simd/constant/mlogtwo2nmb.hpp>
#include <boost/simd/constant/smallestposval.hpp>
#include <boost/simd/constant/twotonmb.hpp>
#include <boost/simd/function/scalar/abs.hpp>
#endif
#include <boost/simd/arch/common/detail/generic/f_log_kernel.hpp>
#include <boost/simd/arch/common/detail/tags.hpp>
#include <boost/simd/constant/log10_2hi.hpp>
#include <boost/simd/constant/log10_2lo.hpp>
#include <boost/simd/constant/log10_ehi.hpp>
#include <boost/simd/constant/log10_elo.hpp>
#include <boost/simd/constant/log2_em1.hpp>
#include <boost/simd/constant/log_2hi.hpp>
#include <boost/simd/constant/log_2lo.hpp>
#include <boost/simd/constant/mhalf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/scalar/fma.hpp>
#include <boost/simd/function/scalar/is_eqz.hpp>
#include <boost/simd/function/scalar/is_ltz.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>

namespace boost { namespace simd
{
  namespace detail
  {
    namespace bd = boost::dispatch;
    template < class A0,
               class Style ,
               class base_A0 = bd::scalar_of_t<A0>
               >
               struct logarithm{};

    template < class A0 >
    struct logarithm< A0, tag::not_simd_type, float>
    {
      using kernel_t = kernel<A0, tag::not_simd_type, float>;

      static BOOST_FORCEINLINE A0 log(A0 const& a0) BOOST_NOEXCEPT
      {
      #ifndef BOOST_SIMD_NO_INFINITIES
        if (BOOST_UNLIKELY(a0 == Inf<A0>())) return a0;
      #endif
      #ifdef BOOST_SIMD_NO_NANS
        if (BOOST_UNLIKELY(is_ltz(a0))) return Nan<A0>();
      #else
        if (BOOST_UNLIKELY(is_nan(a0)||is_ltz(a0))) return Nan<A0>();
      #endif
        if (BOOST_UNLIKELY(is_eqz(a0))) return Minf<A0>();
        A0 z = a0;
      #ifndef BOOST_SIMD_NO_DENORMALS
        A0 t = Zero<A0>();
        if(BOOST_UNLIKELY(abs(z) < Smallestposval<A0>()))
        {
          z *= Twotonmb<A0>();
          t = Mlogtwo2nmb<A0>();
        }
      #endif
        A0 x, fe, x2, y;
        kernel_t::log(z, fe, x, x2, y);
        y = fma(fe, Log_2lo<A0>(), y);
        y = fma(Mhalf<A0>(), x2, y);
      #ifdef BOOST_SIMD_NO_DENORMALS
        return fma(Log_2hi<A0>(), fe, x+y);
      #else
        return fma(Log_2hi<A0>(), fe, x+y+t);
      #endif
      }

      static BOOST_FORCEINLINE  A0 log2(A0 const& a0) BOOST_NOEXCEPT
      {
#ifndef BOOST_SIMD_NO_INFINITIES
        if (BOOST_UNLIKELY(a0 == Inf<A0>())) return a0;
#endif
#ifdef BOOST_SIMD_NO_NANS
        if (BOOST_UNLIKELY(is_ltz(a0))) return Nan<A0>();
#else
        if (BOOST_UNLIKELY(is_nan(a0)||is_ltz(a0))) return Nan<A0>();
#endif
        if (BOOST_UNLIKELY(is_eqz(a0))) return Minf<A0>();
        A0 z = a0;
#ifndef BOOST_SIMD_NO_DENORMALS
        A0 t = Zero<A0>();
        if (BOOST_UNLIKELY(abs(z) < Smallestposval<A0>()))
        {
          z *= Twotonmb<A0>();
          t = Mlog2two2nmb<A0>();
        }
#endif
        A0 x, fe, x2, y;
        kernel_t::log(z, fe, x, x2, y);
        y = fma(Mhalf<A0>(),x2, y);
        z = fma(x,Log2_em1<A0>(),y*Log2_em1<A0>());
#ifdef BOOST_SIMD_NO_DENORMALS
        return ((z+y)+x)+fe;
#else
        return ((z+y)+x)+fe+t;
#endif
      }

      static BOOST_FORCEINLINE  A0 log10(A0 const& a0) BOOST_NOEXCEPT
      {
#ifndef BOOST_SIMD_NO_INFINITIES
        if (BOOST_UNLIKELY(a0 == Inf<A0>())) return a0;
#endif
#ifdef BOOST_SIMD_NO_NANS
        if (BOOST_UNLIKELY(is_ltz(a0))) return Nan<A0>();
#else
        if (BOOST_UNLIKELY(is_nan(a0)||is_ltz(a0))) return Nan<A0>();
#endif
        if (BOOST_UNLIKELY(is_eqz(a0))) return Minf<A0>();
        A0 z = a0;
#ifndef BOOST_SIMD_NO_DENORMALS
        A0 t = Zero<A0>();
        if (BOOST_UNLIKELY(abs(z) < Smallestposval<A0>()))
        {
          z *= Twotonmb<A0>();
          t = Mlog10two2nmb<A0>();
        }
#endif
        A0 x, fe, x2, y;
        kernel_t::log(z, fe, x, x2, y);

        y = fma(Mhalf<A0>(), x2, y);
        z = (x+y)*Log10_elo<A0>();
        z = fma( y, Log10_ehi<A0>(), z);
        z = fma(Log10_ehi<A0>(), y,  z);
        z = fma(Log10_2hi<A0>(), fe, z);
#ifdef BOOST_SIMD_NO_DENORMALS
        return fma(Log10_2lo<A0>(), fe, z);
#else
        return fma(Log10_2lo<A0>(),fe, z+t);
#endif
      }
    };
  }
} }
#endif


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_LOG2_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_LOG2_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-exponential
    This function object returns the base two logarithm of its argument.

    @par Header <boost/simd/function/log2.hpp>

    @par Decorators

      - std_ for floating entries calls @c std::log2

    @see log10, log, log1p

    @par Example:

      @snippet log2.cpp log2

    @par Possible output:

      @snippet log2.txt log2

  **/
  IEEEValue log2(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/log2.hpp>
#include <boost/simd/function/simd/log2.hpp>

#endif


#include <Configuration.h> // Package Path
#include <avatar_locomanipulation/enable_pinocchio_with_hpp_fcl.h> // Enable HPP FCL
// Multibody
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/multibody/geometry.hpp"
// Parsers
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/srdf.hpp"
// Algorithms
#include "pinocchio/algorithm/geometry.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
// Standard
#include <math.h>
#include <iostream>
#include <iomanip>
#include <vector>
#include <boost/shared_ptr.hpp>

pinocchio::fcl::Quaternion3f makeQuat(double w, double x, double y, double z){
  pinocchio::fcl::Quaternion3f q;
  q.w() = w;
  q.x() = x;
  q.y() = y;
  q.z() = z;
  return q;
}

int main(int argc, char ** argv){

  pinocchio::Model model;
  pinocchio::GeometryModel geomModel;

  pinocchio::Data data(model);
  pinocchio::GeometryData geomData(geomModel);
  pinocchio::fcl::DistanceResult result;

  //boost::shared_ptr <pinocchio::fcl::CollisionGeometry> CollisionGeometryPtr_t;

  // create two boxes
  boost::shared_ptr <pinocchio::fcl::CollisionGeometry> s1 (new hpp::fcl::Box (1, 1, 1));
  boost::shared_ptr <pinocchio::fcl::CollisionGeometry> s2 (new hpp::fcl::Box (1, 1, 1));
  static double pi = M_PI;
  // give the boxes defined transforms
  pinocchio::fcl::Transform3f tf1 (makeQuat (cos (pi/8), 0, 0, sin (pi/8)), pinocchio::fcl::Vec3f (-2, 1, .5));
  pinocchio::fcl::Transform3f tf2 (makeQuat (cos (pi/8), 0, sin(pi/8),0), pinocchio::fcl::Vec3f (2, .5, .5));

  //pinocchio::fcl::CollisionObject;
  pinocchio::fcl::CollisionObject o1 (s1, tf1);
  pinocchio::fcl::CollisionObject o2 (s2, tf2);

  // Enable computation of nearest points
  pinocchio::fcl::DistanceRequest distanceRequest (true, 0, 0, pinocchio::fcl::GST_INDEP);
  pinocchio::fcl::DistanceResult distanceResult;

  pinocchio::fcl::distance (&o1, &o2, distanceRequest, distanceResult);

  const pinocchio::fcl::Vec3f& p1 = distanceResult.nearest_points [0];
  const pinocchio::fcl::Vec3f& p2 = distanceResult.nearest_points [1];

  std::cout << "Applied transformations on two boxes" << std::endl;
  std::cout << " Translation1 = " << tf1.getTranslation() << std::endl
	    << " Rotation1 = " << tf1.getRotation () << std::endl
	    << " Translation2 = " << tf2.getTranslation() << std::endl
	    << " Rotation2 = " << tf2.getRotation () << std::endl;
  std::cout << "Closest points(nearest_points): p1 = " << distanceResult.nearest_points [0]
	    << ", p2 = " << distanceResult.nearest_points [1]
        << ", distance(min_distance) = " << distanceResult.min_distance << std::endl;

}


#pragma once
#include <boost/geometry.hpp>

#include "math.hpp"

namespace gshhg {

using Cartesian =
    boost::geometry::model::point<double, 3, boost::geometry::cs::cartesian>;
using GeodeticDegree = boost::geometry::model::point<
    double, 3, boost::geometry::cs::geographic<boost::geometry::degree>>;
using GeodeticRadian = boost::geometry::model::point<
    double, 3, boost::geometry::cs::geographic<boost::geometry::radian>>;
using Point =
    boost::geometry::model::point<double, 2, boost::geometry::cs::cartesian>;

using Box = boost::geometry::model::box<Point>;
using Polygon = boost::geometry::model::polygon<Point>;

using Spheroid = boost::geometry::srs::spheroid<double>;

using Andoyer = boost::geometry::strategy::distance::andoyer<Spheroid>;
using Haversine = boost::geometry::strategy::distance::haversine<Spheroid>;
using Thomas = boost::geometry::strategy::distance::thomas<Spheroid>;
using Vincenty = boost::geometry::strategy::distance::vincenty<Spheroid>;

GeodeticRadian cartesian_2_geodetic(const Cartesian& point);
Cartesian geodetic_2_cartesian(const GeodeticRadian& point);

inline GeodeticRadian geodetic_2_radian(const GeodeticDegree& point) {
  return GeodeticRadian(radians(point.get<0>()), radians(point.get<1>()),
                        point.get<2>());
}

inline GeodeticDegree geodetic_2_degree(const GeodeticRadian& point) {
  return GeodeticDegree(degrees(point.get<0>()), degrees(point.get<1>()),
                        point.get<2>());
}

}  // namespace gshhg


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_DETAIL_SIMD_F_TRIG_EVALUATION_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_DETAIL_SIMD_F_TRIG_EVALUATION_HPP_INCLUDED

#include <boost/simd/function/horn.hpp>
#include <boost/simd/constant/mhalf.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>

namespace boost { namespace simd { namespace detail
{
  namespace bd =  boost::dispatch;
  namespace bs =  boost::simd;

  template <class A0> struct trig_evaluation < A0,  tag::simd_type, float>
  {
    typedef typename bd::as_integer<A0, signed>::type          iA0;
    typedef typename bd::scalar_of<A0>::type                 stype;

    static BOOST_FORCEINLINE A0 cos_eval(const A0& z)
    {
      const A0 y = bs::horn<A0
        , 0x3d2aaaa5
        , 0xbab60619
        , 0x37ccf5ce > (z);
      return bs::inc(bs::fma(z,bs::Mhalf<A0>(), y*bs::sqr(z)));
    }

    static BOOST_FORCEINLINE A0 sin_eval(const A0& z, const A0& x)
    {
      const A0 y1 =  bs::horn< A0
        , 0xbe2aaaa2
        , 0x3c08839d
        , 0xb94ca1f9 > (z);
      return bs::fma(bs::multiplies(y1,z),x,x);
    }

    static BOOST_FORCEINLINE A0 base_tancot_eval(const A0& z)
    {
      const A0 zz = bs::sqr(z);
      return fma(bs::horn<A0
                , 0x3eaaaa6f
                , 0x3e0896dd
                , 0x3d5ac5c9
                , 0x3cc821b5
                , 0x3b4c779c
                , 0x3c19c53b>(zz), zz*z, z);
    }

    static BOOST_FORCEINLINE A0 tan_eval(const A0& z,  const iA0& n)
    {
      A0 y = base_tancot_eval(z);
      return bs::if_else(bs::is_equal(n, bs::One<iA0>()),y,-bs::rec(y));
    }

    static BOOST_FORCEINLINE A0 cot_eval(const A0& z,  const iA0& n)
    {
      A0 y = base_tancot_eval(z);
      return bs::if_else(bs::is_equal(n, One<iA0>()),bs::rec(y),-y);
    }
  };
} } }

#endif


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

namespace tyco
{
namespace P3
{

template<typename T, typename Cs>
struct row_vector
{
    Eigen::Matrix<T, 1, 4> raw_;
};

template<typename T, typename Cs>
struct column_vector
{
    Eigen::Matrix<T, 4, 1> raw_;
};

template<typename T, typename CsLeft, typename CsRight>
struct homography
{
    Eigen::Matrix<T, 4, 4> raw_;
};

// M^-1
template<typename T, typename CsLeftIn, typename CsRightIn>
homography<T, CsRightIn, CsLeftIn> inverse(const homography<T, CsLeftIn, CsRightIn>& M)
{
    return {M.raw_.inverse()};
}

// M * p
template<typename T, typename CsLeftIn, typename CsRightIn>
column_vector<T, CsLeftIn> operator*(
    const homography<T, CsLeftIn, CsRightIn>& M,
    const column_vector<T, CsRightIn>& p)
{
    return {M.raw_ * p.raw_};
}

// p^T * M
template<typename T, typename CsLeftIn, typename CsRightIn>
row_vector<T, CsRightIn> operator*(
    const row_vector<T, CsLeftIn>& p,
    const homography<T, CsLeftIn, CsRightIn>& M)
{
    return {p.raw_ * M.raw_};
}

// M1 * M2
template<typename T, typename CsLeft, typename CsMiddle, typename CsRight>
const homography<T, CsLeft, CsRight> operator*(
    const homography<T, CsLeft, CsMiddle>& M1,
    const homography<T, CsMiddle, CsRight>& M2)
{
    return {M1.raw_ * M2.raw_};
}

} // namespace P3
} // namespace tyco


//
//  AMGCLSolver.hpp
//  IPC
//
//  Created by Minchen Li on 11/06/19.
//
#pragma once

#ifdef IPC_WITH_AMGCL

#include "LinSysSolver.hpp"

#include <amgcl/backend/builtin.hpp>
#include <amgcl/make_solver.hpp>
#include <amgcl/solver/cg.hpp>
#include <amgcl/solver/lgmres.hpp>
#include <amgcl/amg.hpp>
#include <amgcl/coarsening/smoothed_aggregation.hpp>
#include <amgcl/coarsening/plain_aggregates.hpp>
#include <amgcl/coarsening/aggregation.hpp>
#include <amgcl/coarsening/ruge_stuben.hpp>
#include <amgcl/relaxation/spai0.hpp>
#include <amgcl/relaxation/gauss_seidel.hpp>
#include <amgcl/adapter/crs_tuple.hpp>
#include <amgcl/adapter/block_matrix.hpp>
#include <amgcl/solver/bicgstab.hpp>
#include <amgcl/solver/gmres.hpp>
#include <amgcl/solver/runtime.hpp>
#include <amgcl/profiler.hpp>
#include <amgcl/io/mm.hpp>
#include <amgcl/relaxation/chebyshev.hpp>
#include <amgcl/coarsening/runtime.hpp>
#include <amgcl/relaxation/runtime.hpp>
#include <amgcl/preconditioner/runtime.hpp>
#include <amgcl/value_type/static_matrix.hpp>
#include <amgcl/adapter/reorder.hpp>
#include <amgcl/adapter/eigen.hpp>
#include <amgcl/profiler.hpp>

#include <boost/property_tree/ptree.hpp>

#include <Eigen/Eigen>

#include <vector>
#include <set>

// #define USE_BW_BACKEND // use blockwise backend, must undef USE_BW_AGGREGATION
// #define USE_BW_AGGREGATION // use scalar backend but blockwise aggregation, must undef USE_BW_BACKEND
// if neither of above is defined, will use scalar backend and aggregation

// #define USE_AMG_SOLVER // use a single V-cycle to approximately solve the linear system
// if not defined then will use V-cycle preconditioned CG to solve the linear system

namespace IPC {

template <typename vectorTypeI, typename vectorTypeS>
class AMGCLSolver : public LinSysSolver<vectorTypeI, vectorTypeS> {
    typedef LinSysSolver<vectorTypeI, vectorTypeS> Base;

#ifdef USE_BW_BACKEND
    typedef amgcl::static_matrix<double, DIM, DIM> value_type;
    typedef amgcl::static_matrix<double, DIM, 1> rhs_type;
    typedef amgcl::backend::builtin<value_type> Backend;
#else
    typedef amgcl::backend::builtin<double> Backend;
#endif
    using Solver = amgcl::make_solver<
        amgcl::runtime::preconditioner<Backend>,
        amgcl::runtime::solver::wrapper<Backend>>;

protected:
    Solver* solver;
    boost::property_tree::ptree params;
    std::vector<int> _ia, _ja;
    std::vector<double> _a;

public:
    AMGCLSolver(void);
    ~AMGCLSolver(void);

    LinSysSolverType type() const override { return LinSysSolverType::AMGCL; }

    void set_pattern(const std::vector<std::set<int>>& vNeighbor, const std::set<int>& fixedVert) override;
    void load(const char* filePath, Eigen::VectorXd& rhs) override;

    void load_AMGCL(const char* filePath, Eigen::VectorXd& rhs);
    void write_AMGCL(const char* filePath, const Eigen::VectorXd& rhs) const;

    void copyOffDiag_IJ(void);
    void copyOffDiag_a(void);

    void analyze_pattern(void) override;

    bool factorize(void) override;

    void solve(Eigen::VectorXd& rhs, Eigen::VectorXd& result) override;
};

} // namespace IPC

#endif


#include "../innerproduct_proof_generator.h"
#include "../innerproduct_proof_verifier.h"
#include "../challenge_generator_impl.h"

#include "./lelantus_test_fixture.h"

#include <boost/test/unit_test.hpp>

namespace lelantus {

class InnerProductTests : public LelantusTestingSetup {
public:
    typedef InnerProductProofGenerator ProofGenerator;
    typedef InnerProductProofVerifier ProofVerifier;
    typedef InnerProductProof Proof;

public:
    InnerProductTests() {}

public:
    void Generate(size_t n) {
        gens_g = RandomizeGroupElements(n);
        gens_h = RandomizeGroupElements(n);
        a = RandomizeScalars(n);
        b = RandomizeScalars(n);
        u.randomize();
    }

    Scalar ComputeC() const {
        return Primitives::scalar_dot_product(a.begin(), a.end(), b.begin(), b.end());
    }

    GroupElement ComputePInit() const {
        return ComputeMultiExponent(gens_g, a) + ComputeMultiExponent(gens_h, b);
    }

    GroupElement ComputeP(Scalar const &x) const {
        return ComputePInit() + u * ComputeC() * x;
    }

public:
    std::vector<GroupElement> gens_g;
    std::vector<GroupElement> gens_h;
    std::vector<Scalar> a;
    std::vector<Scalar> b;
    GroupElement u;
};

BOOST_FIXTURE_TEST_SUITE(lelantus_inner_product_tests, InnerProductTests)

BOOST_AUTO_TEST_CASE(prove_verify_one)
{
    size_t n = 1;
    size_t log2_n = 0;

    Generate(n);

    Scalar x;
    x.randomize();
    unique_ptr<ChallengeGenerator> challengeGenerator = std::make_unique<ChallengeGeneratorImpl<CHash256>>(1);

    // generating proofs
    Proof proof;
    ProofGenerator prover(gens_g, gens_h, u, 2);
    prover.generate_proof(a, b, x, challengeGenerator, proof);

    BOOST_CHECK_EQUAL(ComputePInit(), prover.get_P());

    // validate
    BOOST_CHECK_EQUAL(ComputeC(), proof.c_);
    BOOST_CHECK_EQUAL(a.front(), proof.a_);
    BOOST_CHECK_EQUAL(b.front(), proof.b_);
    BOOST_CHECK_EQUAL(log2_n, proof.L_.size());
    BOOST_CHECK_EQUAL(log2_n, proof.R_.size());

    // verify
    challengeGenerator.reset(new ChallengeGeneratorImpl<CHash256>(1));
    BOOST_CHECK(ProofVerifier(gens_g, gens_h, u, ComputePInit(), 2).verify(x, proof, challengeGenerator));
    challengeGenerator.reset(new ChallengeGeneratorImpl<CHash256>(1));
    BOOST_CHECK(ProofVerifier(gens_g, gens_h, u, ComputePInit(), 2).verify_fast(n, x, proof, challengeGenerator));
}

BOOST_AUTO_TEST_CASE(prove_verify)
{
    size_t n = 32;
    size_t log2_n = 5;

    Generate(n);

    Scalar x;
    x.randomize();
    unique_ptr<ChallengeGenerator> challengeGenerator = std::make_unique<ChallengeGeneratorImpl<CHash256>>(1);

    // generating proofs
    Proof proof;
    ProofGenerator prover(gens_g, gens_h, u, 2);
    prover.generate_proof(a, b, x, challengeGenerator, proof);

    BOOST_CHECK_EQUAL(ComputePInit(), prover.get_P());

    // validate
    BOOST_CHECK_EQUAL(ComputeC(), proof.c_);
    BOOST_CHECK_EQUAL(log2_n, proof.L_.size());
    BOOST_CHECK_EQUAL(log2_n, proof.R_.size());

    // verify
    challengeGenerator.reset(new ChallengeGeneratorImpl<CHash256>(1));
    BOOST_CHECK(ProofVerifier(gens_g, gens_h, u, ComputePInit(), 2).verify(x, proof, challengeGenerator));
    challengeGenerator.reset(new ChallengeGeneratorImpl<CHash256>(1));
    BOOST_CHECK(ProofVerifier(gens_g, gens_h, u, ComputePInit(), 2).verify_fast(n, x, proof, challengeGenerator));
}

BOOST_AUTO_TEST_CASE(fake_proof_not_verify)
{
    size_t n = 32;

    // generating needed objects
    Generate(n);

    Scalar x;
    x.randomize();
    unique_ptr<ChallengeGenerator> challengeGenerator = std::make_unique<ChallengeGeneratorImpl<CHash256>>(1);

    // generating genertor
    Proof proof;
    ProofGenerator(gens_g, gens_h, u, 2).generate_proof(a, b, x, challengeGenerator, proof);

    // verify with fake P
    GroupElement fakeP;
    fakeP.randomize();

    challengeGenerator.reset(new ChallengeGeneratorImpl<CHash256>(1));
    BOOST_CHECK(!ProofVerifier(gens_g, gens_h, u, fakeP, 2).verify(x, proof, challengeGenerator));
    challengeGenerator.reset(new ChallengeGeneratorImpl<CHash256>(1));
    BOOST_CHECK(!ProofVerifier(gens_g, gens_h, u, fakeP, 2).verify_fast(n, x, proof, challengeGenerator));

    // verify with fake proof
    auto verify = [&](Scalar const &_x, Proof const &_p) -> void {
        challengeGenerator.reset(new ChallengeGeneratorImpl<CHash256>(1));
        BOOST_CHECK(!ProofVerifier(gens_g, gens_h, u, ComputePInit(), 2).verify(_x, _p, challengeGenerator));
        challengeGenerator.reset(new ChallengeGeneratorImpl<CHash256>(1));
        BOOST_CHECK(!ProofVerifier(gens_g, gens_h, u, ComputePInit(), 2).verify_fast(n, _x, _p, challengeGenerator));
    };

    auto fakeProof = proof;
    fakeProof.a_.randomize();
    verify(x, fakeProof);

    fakeProof = proof;
    fakeProof.b_.randomize();
    verify(x, fakeProof);

    fakeProof = proof;
    fakeProof.c_.randomize();
    verify(x, fakeProof);

    fakeProof = proof;
    fakeProof.L_[0].randomize();
    verify(x, fakeProof);

    fakeProof = proof;
    fakeProof.R_[0].randomize();
    verify(x, fakeProof);
}

BOOST_AUTO_TEST_SUITE_END()

} // namespace lelantus


/*===========================================================================*\

Author: Matthias W. Smith
Email:  mwsmith2@uw.edu
Date:   11/02/14

Detail: This is a new test program for my Fid libraries

\*===========================================================================*/

//--- std includes ----------------------------------------------------------//
#include <iostream>
#include <fstream>
#include <vector>
#include <cmath>
using std::vector;
using std::cout;
using std::endl;

//--- other includes --------------------------------------------------------//
#include <armadillo>

//--- project includes ------------------------------------------------------//
#include "fid.h"
using namespace fid;

int main(int argc, char** argv)
{
  // set precision
  cout.precision(10);
  cout.setf(std::ios::fixed, std:: ios::floatfield);

  // declare variables
  int fid_length = 5000;
  double ti = -1.0;
  double dt = 0.001;
  double ftruth = 23.0;

  vector<double> wf_re;
  vector<double> tm;
  wf_re.reserve(fid_length);
  tm.reserve(fid_length);

  std::ofstream out;
  out.precision(10);

  for (int i = 0; i < fid_length; i++){
    tm.push_back(i * dt + ti);
  }

  for (int i = 0; i < fid_length; ++i) {
    wf_re.push_back(sin(40 * tm[i]));
  }

  auto wf_im = dsp::hilbert(wf_re);
  arma::cx_vec wf(wf_im.size());

  for (int i = 0; i < wf_re.size(); ++i) {
    wf[i] = arma::cx_double(wf_re[i], wf_im[i]);
  }

  auto wf_rc = dsp::rconvolve(wf, 4000);

  out.open("wvd_test_rc_real.txt");
  for (int i = 0; i < wf_rc.n_elem; ++i) {
    out << wf_rc[i].real() << ",";
  }
  out.close();

  out.open("wvd_test_rc_imag.txt");
  for (int i = 0; i < wf_rc.n_elem; ++i) {
    out << wf_rc[i].imag() << ",";
  }
  out.close();

  return 0;
}


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

/*
 * References:
 * Ooura, Takuya, and Masatake Mori. "A robust double exponential formula for Fourier-type integrals." Journal of computational and applied mathematics 112.1-2 (1999): 229-241.
 * http://www.kurims.kyoto-u.ac.jp/~ooura/intde.html
 */
#ifndef BOOST_MATH_QUADRATURE_OOURA_FOURIER_INTEGRALS_HPP
#define BOOST_MATH_QUADRATURE_OOURA_FOURIER_INTEGRALS_HPP
#include <memory>
#include <boost/math/quadrature/detail/ooura_fourier_integrals_detail.hpp>

namespace boost { namespace math { namespace quadrature {

template<class Real>
class ooura_fourier_sin {
public:
    ooura_fourier_sin(const Real relative_error_tolerance = tools::root_epsilon<Real>(), size_t levels = sizeof(Real)) : impl_(std::make_shared<detail::ooura_fourier_sin_detail<Real>>(relative_error_tolerance, levels))
    {}

    template<class F>
    std::pair<Real, Real> integrate(F const & f, Real omega) {
        return impl_->integrate(f, omega);
    }

    // These are just for debugging/unit tests:
    std::vector<std::vector<Real>> const & big_nodes() const {
        return impl_->big_nodes();
    }

    std::vector<std::vector<Real>> const & weights_for_big_nodes() const {
        return impl_->weights_for_big_nodes();
    }

    std::vector<std::vector<Real>> const & little_nodes() const {
        return impl_->little_nodes();
    }

    std::vector<std::vector<Real>> const & weights_for_little_nodes() const {
        return impl_->weights_for_little_nodes();
    }

private:
    std::shared_ptr<detail::ooura_fourier_sin_detail<Real>> impl_;
};

template<class Real>
class ooura_fourier_cos {
public:
    ooura_fourier_cos(const Real relative_error_tolerance = tools::root_epsilon<Real>(), size_t levels = sizeof(Real)) : impl_(std::make_shared<detail::ooura_fourier_cos_detail<Real>>(relative_error_tolerance, levels))
    {}

    template<class F>
    std::pair<Real, Real> integrate(F const & f, Real omega) {
        return impl_->integrate(f, omega);
    }
private:
    std::shared_ptr<detail::ooura_fourier_cos_detail<Real>> impl_;
};

}}}
#endif


#include <boost/simd/meta/is_power_of_2.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/int.hpp>

int main()
{
  using boost::mpl::int_;

  BOOST_MPL_ASSERT    (( boost::simd::meta::is_power_of_2_c< 2  >::type ));
  BOOST_MPL_ASSERT    (( boost::simd::meta::is_power_of_2_c< 32 >::type ));
  BOOST_MPL_ASSERT_NOT(( boost::simd::meta::is_power_of_2_c< 0  >::type ));
  BOOST_MPL_ASSERT_NOT(( boost::simd::meta::is_power_of_2_c< 6  >::type ));
}


#include "domain/finite_element/finite_element_gaussian.hpp"

#include <deal.II/fe/fe_dgq.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_update_flags.h>

namespace bart::domain::finite_element {

template <int dim>
FiniteElementGaussian<dim>::FiniteElementGaussian(DiscretizationType discretization, int polynomial_degree)
    : polynomial_degree_(polynomial_degree) {
  std::string description{"deal.II Gaussian, " + std::to_string(dim) + "D, "};

  const auto update_flags = dealii::update_values | dealii::update_gradients | dealii::update_quadrature_points |
                            dealii::update_JxW_values;
  const auto face_update_flags = dealii::update_values | dealii::update_gradients | dealii::update_quadrature_points |
      dealii::update_JxW_values | dealii::update_normal_vectors;

  finite_element_ = GetFiniteElement(discretization);
  cell_quadrature_ = std::make_shared<dealii::QGauss<dim>>(polynomial_degree + 1);
  face_quadrature_ = std::make_shared<dealii::QGauss<dim - 1>>(polynomial_degree + 1);
  values_ = std::make_shared<dealii::FEValues<dim>>(*finite_element_, *cell_quadrature_, update_flags);
  face_values_ = std::make_shared<dealii::FEFaceValues<dim>>(*finite_element_, *face_quadrature_, face_update_flags);

  if (discretization == DiscretizationType::kDiscontinuousFEM) {
    neighbor_face_values_ =
        std::make_shared<dealii::FEFaceValues<dim>>(*finite_element_, *face_quadrature_, face_update_flags);
    description += "Discontinuous, ";
  } else {
    description += "Continuous, ";
  }
  description += "Q = " + std::to_string(polynomial_degree);
  this->set_description(description, utility::DefaultImplementation(true));
}

template <int dim>
auto FiniteElementGaussian<dim>::GetFiniteElement(DiscretizationType discretization)
-> std::shared_ptr<dealii::FiniteElement<dim, dim>> {
  switch (discretization) {
    case DiscretizationType::kContinuousFEM: {
      return std::make_shared<dealii::FE_Q<dim>>(polynomial_degree_);
    }
    case DiscretizationType::kDiscontinuousFEM: {
      return std::make_shared<dealii::FE_DGQ<dim>>(polynomial_degree_);
    }
    default: {
      AssertThrow(false, dealii::ExcMessage("Cannot build FiniteElementGaussian object with discretization type None"));
      break;
    }
  }
}

template class FiniteElementGaussian<1>;
template class FiniteElementGaussian<2>;
template class FiniteElementGaussian<3>;

} // namespace bart::domain::finite_element


#include <cstdio>
#include <fstream>

#include <CGAL/ImageIO.h>
#include <CGAL/IO/print_wavefront.h>
#include <CGAL/Random.h>
#include <CGAL/Side_of_triangle_mesh.h>

#include <Eigen/Core>

#include <igl/copyleft/cgal/polyhedron_to_mesh.h>
#include <igl/jet.h>
#include <igl/viewer/Viewer.h>

#include "image_level_surface_to_mesh_converter.h"

double volume(const Eigen::MatrixXd &V, const Eigen::MatrixXi &F,
              const Eigen::VectorXi &include) {

  double total_det = 0;

  for (int i = 0; i < include.rows(); ++i) {
    Eigen::Matrix3d p;
    p << V(F(include(i), 0), 0), V(F(include(i), 1), 0), V(F(include(i), 2), 0),
         V(F(include(i), 0), 1), V(F(include(i), 1), 1), V(F(include(i), 2), 1),
         V(F(include(i), 0), 2), V(F(include(i), 1), 2), V(F(include(i), 2), 2);

    total_det += p.determinant();
  }

  return std::abs(total_det / 6.0);
}

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

  using namespace ::std;
  using namespace ::CGAL;
  using namespace ::cvc::skull_atlas;
  typedef Exact_predicates_inexact_constructions_kernel Kernel;
  typedef Polyhedron_3<Kernel> Polyhedron;
  typedef Kernel::Point_3 Point;

  bool debug = false;

  if (argc < 4) {
    fprintf(stderr, "Usage: %s input_image.inr output_mesh.inr isovalue [debug]\n", argv[0]);
    return -1;
  }

  if (argc == 5) {
    debug = true;
  }

  CGAL::Random::State state;
  unsigned int seed = 0;
  CGAL::Random random(seed);
  random.save_state(state);
  default_random.restore_state(state);

  const char* input_image_filename = argv[1];
  const char* output_mesh_filename = argv[2];
  double isovalue = atof(argv[3]);

  if(debug){
    fprintf(stderr, "Input file is %s\n", input_image_filename);
    fprintf(stderr, "Output file is %s\n", output_mesh_filename);
    fprintf(stderr, "Using an isovalue of %lf\n", isovalue);
  }

  Image_3 image;
  if (!image.read(input_image_filename)) {
    fprintf(stderr, "CGAL failed to read image file \"%s\".\n", input_image_filename);
    return -1;
  }

  if(debug) fprintf(stderr, "Finished reading image, now extracting surface...\n");

  Polyhedron mesh = ImageLevelSurfaceToMeshConverter<>().convertToMesh(image, isovalue);

  if(debug) fprintf(stderr, "Created mesh. Now processing...\n");
  mesh.keep_largest_connected_components(1);

  CGAL::Side_of_triangle_mesh<Polyhedron, Kernel> inside(mesh);

  for (int i = 0; i < image.xdim(); ++i) {
    for (int j = 0; j < image.ydim(); ++j) {
      for (int k = 0; k < image.zdim(); ++k) {
        Point p(i * image.vx(), j * image.vy(), k * image.vz());
        CGAL::Bounded_side res = inside(p);

        // If it's outside, set the value to be zero.
        if (!(res == CGAL::ON_BOUNDED_SIDE || res == CGAL::ON_BOUNDARY)) {
          // Method for accessing data, found at the definition of static_evaluate
          // in CGAL/ImageIO.h.
          ((float*)image.data())[ (k * image.ydim() + j) * image.xdim() + i ] = 0.f;
        }
      }
    }
  }

  // If debug is turned on, display the mesh.
  if (debug) {
    Eigen::MatrixXd V;
    Eigen::MatrixXi F;

    // Viewing the mesh after we're done.
    igl::copyleft::cgal::polyhedron_to_mesh(mesh, V,F);
    igl::viewer::Viewer viewer;
    viewer.data.clear();
    viewer.data.set_mesh(V, F);
    viewer.launch();
  }

  if(debug) fprintf(stderr, "Finished. Now writing image to %s\n", argv[2]);
  // Call CGAL's _writeImage function.
  _writeImage(image.image(), argv[2]);

  return 0;
}


#include<bits/stdc++.h>

#include <pcl/pcl_base.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/common/transforms.h>

#include <map_merge_3d/typedefs.h>
#include<map_merge_3d/ransac_ground.h>

#include <Eigen/Geometry>

namespace map_merge_3d
{

GroundPlane getGroundPlane(const PointCloudConstPtr &input)
{
  pcl::SACSegmentation<PointT> seg;
  ModelCoeffPtr coefficients (new pcl::ModelCoefficients);
  PointIndicesPtr ground (new pcl::PointIndices);

  // Set up parameters for our segmentation/ extraction scheme
  seg.setOptimizeCoefficients(true);
  seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE); //only want points perpendicular to a given axis
  seg.setMaxIterations(1000); // this is key (default is 50 and that sucks)
  seg.setMethodType(pcl::SAC_RANSAC);
  seg.setDistanceThreshold (0.2); // keep points within 0.5 m of the plane

  // because we want a specific plane (X-Y Plane)
  Eigen::Vector3f axis = Eigen::Vector3f(0.0,0.0,1.0); //z axis
  seg.setAxis(axis);
  seg.setEpsAngle(30.0f * (M_PI/180.0f)); // plane can be within 30 degrees of X-Y plane
  seg.setProbability(0.95);
  seg.setInputCloud(input);
  seg.segment(*ground, *coefficients);

  // Create the filtering object
  pcl::ExtractIndices<PointT> extract;
  extract.setInputCloud(input);
  extract.setIndices(ground);

  // Extract non-ground returns
  PointCloudPtr points(new PointCloud);
  extract.setNegative(false);
  extract.filter(*points);

  GroundPlane output;
  output.ground = ground;
  output.coefficients = coefficients;
  output.points = points;

  return output;
}

PointCloudPtr removeGround(const PointCloudConstPtr &input)
{

  GroundPlane planeInfo = getGroundPlane(input);

  // to remove points below and within the plane
  PointCloudPtr filtered(new PointCloud);
  for(int i = 0; i < planeInfo.points->size(); i++)
  {
    PointT point = planeInfo.points->points[i];

    if(point.x * planeInfo.coefficients->values[0] + point.y * planeInfo.coefficients->values[1] +
        point.z *  planeInfo.coefficients->values[2] +  planeInfo.coefficients->values[3] > 0)
    {
      filtered->points.push_back(point);
    }
  }

  return filtered;

}

// This helper function finds indices of points that are considered inliers,
// given a plane description and a condition on distance from the plane.
std::vector<size_t> find_inlier_indices(
    const PointCloudConstPtr &input_cloud_ptr,
    const Plane& plane,
    std::function<bool(float)> condition_z_fn)
{
    typedef Eigen::Transform<float, 3, Eigen::Affine, Eigen::DontAlign> Transform3f;

    auto base_point = plane.base_point;
    auto normal = plane.normal;

    // Before rotation of the coordinate frame we need to relocate the point cloud to
    // the position of base_point of the plane.
    Transform3f world_to_ransac_base = Transform3f::Identity();
    world_to_ransac_base.translate(-base_point);
    PointCloudPtr ransac_base_cloud_ptr (new PointCloud);
    pcl::transformPointCloud(*input_cloud_ptr, *ransac_base_cloud_ptr, world_to_ransac_base);

    // We are going to use a quaternion to determine the rotation transform
    // which is required to rotate a coordinate system that plane's normal
    // becomes aligned with Z coordinate axis.
    auto rotate_to_plane_quat = Eigen::Quaternionf::FromTwoVectors(
        normal,
        Eigen::Vector3f::UnitZ()
    ).normalized();

    // Now we can create a rotation transform and align the cloud that
    // the candidate plane matches XY plane.
    Transform3f ransac_base_to_ransac = Transform3f::Identity();
    ransac_base_to_ransac.rotate(rotate_to_plane_quat);
    PointCloudPtr aligned_cloud_ptr (new PointCloud);
    pcl::transformPointCloud(*ransac_base_cloud_ptr, *aligned_cloud_ptr, ransac_base_to_ransac);

    // Once the point cloud is transformed into the plane coordinates,
    // We can apply a simple criterion on Z coordinate to find inliers.
    std::vector<size_t> indices;
    for (size_t i_point = 0; i_point < aligned_cloud_ptr->size(); i_point++)
    {
        const auto &p = (aligned_cloud_ptr->points)[i_point];
        if (condition_z_fn(p.z))
        {
            indices.push_back(i_point);
        }
    }
    return indices;
}

// This function performs plane detection with RANSAC sampling of planes
// that lie on triplets of points randomly sampled from the cloud.
// Among all trials the plane that is picked is the one that has the highest
// number of inliers. Inlier points are then removed as belonging to the ground.
PointCloudPtr remove_ground_ransac(const PointCloudConstPtr &input_cloud_ptr)
{
    // Threshold for rough point dropping by Z coordinate (meters)
    const float rough_filter_thr = 0.5f;
    // How much to decimate the input cloud for RANSAC sampling and inlier counting
    const size_t decimation_rate = 10;

    // Tolerance threshold on the distance of an inlier to the plane (meters)
    const float ransac_tolerance = 0.1f;
    // After the final plane is found this is the threshold below which all
    // points are discarded as belonging to the ground.
    const float remove_ground_threshold = 0.2f;

    // To reduce the number of outliers (non-ground points) we can roughly crop
    // the point cloud by Z coordinate in the range (-rough_filter_thr, rough_filter_thr).
    // Simultaneously we perform decimation of the remaining points since the full
    // point cloud is excessive for RANSAC.
    std::mt19937::result_type decimation_seed = 41;
    std::mt19937 rng_decimation(decimation_seed);
    auto decimation_gen = std::bind(
        std::uniform_int_distribution<size_t>(0, decimation_rate), rng_decimation);

    PointCloudPtr filtered_ptr(new PointCloud);
    for (auto &p : input_cloud_ptr->points)
    {
        if ((p.z > -rough_filter_thr) && (p.z < rough_filter_thr))
        {
            // Use random number generator to avoid introducing patterns
            // (which are possible with structured subsampling
            // like picking each Nth point).
            if (decimation_gen() == 0)
            {
                filtered_ptr->points.push_back(p);
            }
        }
    }

    // We need a random number generator for sampling triplets of points.
    std::mt19937::result_type sampling_seed = 42;
    std::mt19937 sampling_rng(sampling_seed);
    auto random_index_gen = std::bind(
        std::uniform_int_distribution<size_t>(0, filtered_ptr->size()), sampling_rng);

    // Number of RANSAC trials
    const size_t num_iterations = 25;
    // The best plane is determined by a pair of (number of inliers, plane specification)
    typedef std::pair<size_t, Plane> BestPair;
    auto best = std::unique_ptr<BestPair>();
    for (size_t i_iter = 0; i_iter < num_iterations; i_iter++)
    {
        // Sample 3 random points.
        // pa is special in the sense that is becomes an anchor - a base_point of the plane
        Eigen::Vector3f pa = (*filtered_ptr)[random_index_gen()].getVector3fMap();
        Eigen::Vector3f pb = (*filtered_ptr)[random_index_gen()].getVector3fMap();
        Eigen::Vector3f pc = (*filtered_ptr)[random_index_gen()].getVector3fMap();

        // Here we figure out the normal to the plane which can be easily calculated
        // as a normalized cross product.
        auto vb = pb - pa;
        auto vc = pc - pa;
        Eigen::Vector3f normal = vb.cross(vc).normalized();

        // Flip the normal if points down
        if (normal.dot(Eigen::Vector3f::UnitZ()) < 0)
        {
            normal = -normal;
        }

        Plane plane{pa, normal};

        // Call find_inlier_indices to retrieve inlier indices.
        // We will need only the number of inliers.
        auto inlier_indices = find_inlier_indices(filtered_ptr, plane,
            [&ransac_tolerance](float z) -> bool {
                return (z >= -ransac_tolerance) && (z <= ransac_tolerance);
            });

        // If new best plane is found, update the best
        bool found_new_best = false;
        if (best)
        {
            if (inlier_indices.size() > best->first)
            {
                found_new_best = true;
            }
        }
        else
        {
            // For the first trial update anyway
            found_new_best = true;
        }

        if (found_new_best)
        {
            best = std::unique_ptr<BestPair>(new BestPair{inlier_indices.size(), plane});
        }
    }

    // For the best plane filter out all the points that are
    // below the plane + remove_ground_threshold.
    PointCloudPtr cloud_no_ground_ptr (new PointCloud);
    if (best)
    {
        auto inlier_indices = find_inlier_indices(input_cloud_ptr, best->second,
            [&remove_ground_threshold](float z) -> bool {
                return z <= remove_ground_threshold;
            });
        std::unordered_set<size_t> inlier_set(inlier_indices.begin(), inlier_indices.end());
        for (size_t i_point = 0; i_point < input_cloud_ptr->size(); i_point++)
        {
            bool extract_non_ground = true;
            if ((inlier_set.find(i_point) == inlier_set.end()) == extract_non_ground)
            {
                const auto &p = (input_cloud_ptr->points)[i_point];
                cloud_no_ground_ptr->points.push_back(p);
            }
        }
    }
    else
    {
        *cloud_no_ground_ptr = *input_cloud_ptr;
    }

    return cloud_no_ground_ptr;
}

} // namespace map_merge_3d


/*************************************************
 * Copyright (c) 2017 Toru Ito
 * Released under the MIT license
 * http://opensource.org/licenses/mit-license.php
 *************************************************/

#ifndef Delaunay_hpp
#define Delaunay_hpp

#include <stdio.h>
#include <vector>
#include <set>
#include <Eigen/Core>
#include <Eigen/Geometry>

class Delaunay
{
public:
    struct TriangleData
    {
        unsigned long   index1;
        unsigned long   index2;
        unsigned long   index3;
        double          radius;
        double          bounds[4];
        Eigen::Vector2d center;

        TriangleData()
        {
            index1 = index2 = index3 = 0;
            bounds[0] = bounds[1] = bounds[2] = bounds[3] = 0.0;
            radius = 0.0;
            center = Eigen::Vector2d::Zero();
        }

        bool IsInside( Eigen::Vector2d point )
        {
            if( point.x() < bounds[0] )
                return false;

            if( point.x() > bounds[1] )
                return false;

            if( point.y() < bounds[2] )
                return false;

            if( point.y() > bounds[3] )
                return false;

            if( ( point - center ).norm() > radius )
                return false;

            return true;
        }
    };

    struct EdgeData
    {
        unsigned long index1, index2;

        EdgeData()
        {
            index1 = 0;
            index2 = 0;
        };

        EdgeData( unsigned long index1, unsigned long index2 )
        {
            if( index1 < index2 ) {
                this->index1 = index1;
                this->index2 = index2;
            } else {
                this->index1 = index2;
                this->index2 = index1;
            }
        };
    };

public:
    Delaunay();
    ~Delaunay();

    void SetPoint( std::vector< Eigen::Vector2d > *pPointList );
    void GetResult( std::vector< Eigen::Vector2d > *pPointList, std::vector< std::vector< unsigned int > > *pIndexList );

    void Triangulation();

private:
    std::vector< TriangleData >    m_Triangles;
    std::vector< Eigen::Vector2d > m_Points;
    std::vector< unsigned long >   m_InitTrianglePointIndex;

    void CreateInitTriangle();
    void DeleteInitTriangle();

    void AddTriangle( unsigned long index1, unsigned long index2, unsigned long index3 );
};

#endif /* Delaunay_hpp */


#define BOOST_TEST_MODULE MultivariateGaussianModelTestSuite
#define BOOST_TEST_DYN_LINK

#include <boost/test/included/unit_test.hpp>
#include <Eigen/Core>
#include <hops/Model/MultivariateGaussianModel.hpp>

BOOST_AUTO_TEST_SUITE(MultivariateGaussianModel)

    BOOST_AUTO_TEST_CASE(computeNegativeLogLikelihood) {
        double expectedValue = 1.98943678865;
        Eigen::VectorXd mean1(2);
        mean1 << -.8, -.8;
        Eigen::VectorXd mean2(2);
        mean2 << .8, .8;
        Eigen::MatrixXd covariance(2, 2);
        covariance << 0.04, 0, 0, 0.04;

        hops::MultivariateGaussianModel multivariateGaussianModel1(mean1, covariance);
        hops::MultivariateGaussianModel multivariateGaussianModel2(mean2, covariance);

        Eigen::VectorXd evaluationPoint(2);
        evaluationPoint << 0.8, 0.8;

        double actualValue =
                0.5 * (std::exp(-multivariateGaussianModel1.computeNegativeLogLikelihood(evaluationPoint)) +
                       std::exp(-multivariateGaussianModel2.computeNegativeLogLikelihood(evaluationPoint)));

        BOOST_CHECK_CLOSE(actualValue, expectedValue, 1e-2);
    }

    BOOST_AUTO_TEST_CASE(computeLogLikelihoodGradient) {
        Eigen::VectorXd mean(2);
        mean << -.8, -.8;
        Eigen::MatrixXd covariance(2, 2);
        covariance << 0.04, 0, 0, 0.04;

        hops::MultivariateGaussianModel multivariateGaussianModel(mean, covariance);

        Eigen::VectorXd evaluationPoint1(2);
        evaluationPoint1 << 0.8, 0.8;

        Eigen::VectorXd evaluationPoint2 =
                evaluationPoint1 + 1e-5 * multivariateGaussianModel.computeLogLikelihoodGradient(evaluationPoint1);

        // Tests if negative log likelihood decreases in gradient direction, thus checking correct sign of gradient.
        BOOST_CHECK_GT(multivariateGaussianModel.computeNegativeLogLikelihood(evaluationPoint1),
                  multivariateGaussianModel.computeNegativeLogLikelihood(evaluationPoint2));
    }

BOOST_AUTO_TEST_SUITE_END()