/******************************************************************
 * Created by Wei on 3/12/17.
 *
 * This is an implementation of this paper:
 * "Sparse Inverse Covariance Estimation with the Graphical Lasso"
 *
 * All names of parameters comply with the notaions in the paper.
 ******************************************************************/

#include "GraphicalLasso.hpp"

#include <Eigen/Dense>
#include <Eigen/SVD>
#include <iostream>
#include <map>

#ifdef BAZEL
#include "Models/LinearRegression.hpp"
#include "Models/Model.hpp"
#else
#include "../Models/LinearRegression.hpp"
#include "../Models/Model.hpp"
#endif

using namespace Eigen;
using namespace std;

GraphicalLasso::GraphicalLasso(const unordered_map<string, string>& opts) {
    try {
        tolerance = stod(opts.at("tolerance"));
    } catch(std::out_of_range& oor) {
        tolerance = default_tolerance;
    }
    try {
        learningRate = stod(opts.at("learning_rate"));
    } catch(std::out_of_range& oor) {
        learningRate = default_learning_rate;
    }
}

GraphicalLasso::GraphicalLasso() {
    learningRate = default_learning_rate;
    tolerance = default_tolerance;
}

/*void GraphicalLasso::stop() {
    shouldStop = true;
}*/

void GraphicalLasso::setTolerance(float tol) {
    tolerance = tol;
}

void GraphicalLasso::assertReadyToRun() {
    return;    // there is no data that cannot be inferred
}

void GraphicalLasso::setUpRun() {
    mtx.lock();
    isRunning = true;
    progress = 0.0;
    shouldStop = false;
}

void GraphicalLasso::finishRun() {
    isRunning = false;
    progress = 1.0;
    mtx.unlock();
}

void GraphicalLasso::setLearningRate(float lr) {
    learningRate = lr;
}

MatrixXf GraphicalLasso::matrixRaiseToHalf(MatrixXf& matrix) {
    BDCSVD<MatrixXf> svd(matrix, ComputeThinU | ComputeThinV);
    MatrixXf D = svd.singularValues().array().sqrt().matrix().asDiagonal();
    MatrixXf U = svd.matrixU();
    return U * D * U.transpose();
}

void GraphicalLasso::run(shared_ptr<LinearRegression> model) {
    MatrixXf X = model->getX();
    int num_samples = X.rows();
    int num_features = X.cols();
    stdNormalize(X);
    MatrixXf S = X.transpose() * X / num_samples;
    MatrixXf W = S;
    MatrixXf W_inv = Math::getInstance().pseudoInverse(W);
    MatrixXf W_inv_old = W_inv;
    float regularizer = model->getL1_reg();

    for (int epoch = 0; epoch < maxIteration; epoch++) {
        progress = static_cast<float>(epoch) / maxIteration;
        for (int idx = 0; idx < num_features; idx++) {
            // partition matrixes into blocks
            vector<MatrixXf> S_blocks = partitionBlocks(S);
            MatrixXf W_11 = partitionBlocks(W)[0];
            MatrixXf W_inv_11 = partitionBlocks(W_inv)[0];

            // find W_11 ^ (1/2)
            MatrixXf W_11_half = matrixRaiseToHalf(W_11);
            MatrixXf b = Math::getInstance().pseudoInverse(W_11_half) * S_blocks[1];

            // run linear regression with lasso
            MatrixXf beta = fit(model, W_11_half, b);
            MatrixXf W_12 = W_11 * beta;

            // recover W_inv (W_inv corresponds to theta in the paper)
            float S_22 = S_blocks.back()(0, 0);
            float W_22 = S_22 + regularizer;
            float W_inv_22 = 1 / (W_22 - (W_12.transpose() * beta)(0, 0));
            MatrixXf W_inv_12 = -W_inv_22 * beta;

            W_inv = composeBlocks(W_inv_11, W_inv_12, W_inv_22);
            W = composeBlocks(W_11, W_12, W_22);
            S = composeBlocks(S_blocks[0], S_blocks[1], S_blocks[2](0, 0));
        }
        float diff = (W_inv - W_inv_old).norm();
        if (diff < tolerance) {
            break;
        } else {
            W_inv_old = W_inv;
        }
    }
    for (long col = 0; col < W_inv.cols(); col++) {
        for (long row = 0; row < W_inv.rows(); row++) {
            W_inv(row, col) = abs(W_inv(row, col)) < 1e-5 ? 0 : 1;
        }
    }
    model->updateBeta(W_inv);
}

MatrixXf GraphicalLasso::fit(shared_ptr<LinearRegression> model, MatrixXf& X, MatrixXf& Y) {
    model->setL1_reg(model->getL1_reg()*10);
    model->setX(X);
    model->setY(Y);
    model->initBeta();
    float residue = model->cost();
    VectorXf grad;
    VectorXf in;
    long epoch = 0;
    while (!shouldStop && epoch < maxIteration && residue > tolerance && !shouldStop) {
        grad = model->proximal_derivative();
        in = model->getBeta() - learningRate * grad;
        model->updateBeta(model->proximal_operator(in, learningRate));
        residue = model->cost();
        epoch++;
    }
    return model->getBeta();
}

void GraphicalLasso::stdNormalize(MatrixXf& matrix) {
    RowVectorXf mean = matrix.colwise().mean();
    RowVectorXf std = ((matrix.rowwise() - mean).array().square().colwise().sum() /
                       (matrix.rows())).sqrt();
    matrix = (matrix.rowwise() - mean).array().rowwise() / std.array();
}

vector<MatrixXf> GraphicalLasso::partitionBlocks(MatrixXf& matrix) {
    int num_rows = matrix.rows();
    int num_cols = matrix.cols();
    MatrixXf m_11 = matrix.block(0, 0, num_rows - 1, num_cols - 1);
    MatrixXf m_12 = matrix.block(0, num_cols - 1, num_rows - 1, 1);
    MatrixXf m_22 = matrix.block(num_rows - 1, num_cols - 1, 1, 1);
    return {m_11, m_12, m_22};
}

MatrixXf GraphicalLasso::composeBlocks(MatrixXf& m_11,
                                       MatrixXf& m_12,
                                       float m_22) {
    int num_rows = m_11.rows() + 1;
    int num_cols = m_11.cols() + 1;
    MatrixXf result(num_rows, num_cols);
    result.block(1, 1, num_rows - 1, num_cols - 1) = m_11;
    result.block(1, 0, num_rows - 1, 1) = m_12;
    result.block(0, 1, 1, num_cols - 1) = m_12.transpose();
    result(0, 0) = m_22;
    return result;
}


/*

Copyright (c) 2005-2022, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#ifndef FARHADIFARFORCE_HPP_
#define FARHADIFARFORCE_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>
#include "Exception.hpp"

#include "AbstractForce.hpp"
#include "VertexBasedCellPopulation.hpp"

#include <iostream>

/**
 * A force class for use in Vertex-based simulations. This force is based on the
 * Energy function proposed by Farhadifar et al in  Curr. Biol., 2007, 17, 2095-2104.
 */

template<unsigned DIM>
class FarhadifarForce : public AbstractForce<DIM>
{
friend class TestForces;

private:

    friend class boost::serialization::access;
    /**
     * Boost Serialization method for archiving/checkpointing.
     * Archives the object and its member variables.
     *
     * @param archive  The boost archive.
     * @param version  The current version of this class.
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        archive & boost::serialization::base_object<AbstractForce<DIM> >(*this);
        archive & mAreaElasticityParameter;
        archive & mPerimeterContractilityParameter;
        archive & mLineTensionParameter;
        archive & mBoundaryLineTensionParameter;
    }

protected:

    /**
     * The strength of the area term in the model. Corresponds to K_alpha in Farhadifar's paper.
     */
    double mAreaElasticityParameter;

    /**
     * The strength of the perimeter term in the model. Corresponds to Gamma_alpha in Farhadifar's paper.
     */
    double mPerimeterContractilityParameter;

    /**
     * The strength of the line tension term in the model. Lambda_{i,j} in Farhadifar's paper.
     */
    double mLineTensionParameter;

    /**
     * The strength of the line tension at the boundary. This term does correspond to Lambda_{i,j} in Farhadifar's paper.
     */
    double mBoundaryLineTensionParameter;

public:

    /**
     * Constructor.
     */
    FarhadifarForce();

    /**
     * Destructor.
     */
    virtual ~FarhadifarForce();

    /**
     * Overridden AddForceContribution() method.
     *
     * Calculates the force on each node in the vertex-based cell population based on the energy function
     * Farhadifar's model.
     *
     * @param rCellPopulation reference to the cell population
     */
    virtual void AddForceContribution(AbstractCellPopulation<DIM>& rCellPopulation);

    /**
     * Get the line tension parameter for the edge between two given nodes.
     *
     * @param pNodeA one node
     * @param pNodeB the other node
     * @param rVertexCellPopulation reference to the cell population
     *
     * @return the line tension parameter for this edge.
     */
    virtual double GetLineTensionParameter(Node<DIM>* pNodeA, Node<DIM>* pNodeB, VertexBasedCellPopulation<DIM>& rVertexCellPopulation);

    /**
     * @return mAreaElasticityParameter
     */
    double GetAreaElasticityParameter();

    /**
     * @return mPerimeterContractilityParameter
     */
    double GetPerimeterContractilityParameter();

    /**
     * @return mLineTensionParameter
     */
    double GetLineTensionParameter();

    /**
     * @return mBoundaryLineTensionParameter
     */
    double GetBoundaryLineTensionParameter();

    /**
     * Set mAreaElasticityParameter.
     *
     * @param areaElasticityParameter the new value of mAreaElasticityParameter
     */
    void SetAreaElasticityParameter(double areaElasticityParameter);

    /**
     * Set mPerimeterContractilityParameter.
     *
     * @param perimeterContractilityParameter the new value of perimterContractilityParameter
     */
    void SetPerimeterContractilityParameter(double perimeterContractilityParameter);

    /**
     * Set mLineTensionParameter.
     *
     * @param lineTensionParameter the new value of mLineTensionParameter
     */
    void SetLineTensionParameter(double lineTensionParameter);

    /**
     * Set mBoundaryLineTensionParameter.
     *
     * @param boundaryLineTensionParameter the new value of mBoundaryLineTensionParameter
     */
    void SetBoundaryLineTensionParameter(double boundaryLineTensionParameter);

    /**
     * Overridden OutputForceParameters() method.
     *
     * @param rParamsFile the file stream to which the parameters are output
     */
    void OutputForceParameters(out_stream& rParamsFile);
};

#include "SerializationExportWrapper.hpp"
EXPORT_TEMPLATE_CLASS_SAME_DIMS(FarhadifarForce)

#endif /*FARHADIFARFORCE_HPP_*/


// Expand an orientation set expressed as a set of grids into a set of
// explicit orientations.
//
// Written by Charles Karney
// Copyright (c) 2006 Sarnoff Corporation. All rights reserved.
//
// For more information, see
//
//    https://github.com/cffk/orientation
//
// Compile with, e.g.,
//
//    g++ -O2 -o ExpandSet ExpandSet.cpp
//
// Run with
//
//   ./ExpandSet [-e] < grid-file > orientation-file
//
// If -e is specified, the orientations are written as Euler angles,
// otherwise they are written as quaternions.  Format of the grid file:
//
//     Any number of initial comment lines beginning with #
//     A line containing "format grid"
//     A line containing: delta sigma ntot ncell nent maxrad coverage
//     nent lines containing: k l m weight radius mult
//
// Here k >= l >= m >= 0.  delta and sigma are used to define the grid.
// ntot is the total number of orientations, ncell = ntot/24 is the
// number of orientations per cell of the 48-cell.  maxrad is the
// covering radius of the set and radius is the radius of the Voronoi
// cell.  Both are measured in degrees.  coverage is the coverage of the
// set, i.e., how much overlap there is when caps of radius maxrad are
// placed at each point; coverage = 1 means no overlap.
//
// For each triplet, [k l m], generate mult distinct permutations by
// changing the order and the signs of the elements.  Each [k l m] is
// converted to a point in a truncated cube [x y z] =
// [pind(k/2,delta,sigma) pind(l/2,delta,sigma) pind(m/2,delta,sigma)]
// Each [x y z] is converted to a unit quaternion via p = [1 x y z]; q =
// p/|p| to give ncell orientations.  Finally, the 24 rotational cube
// symmetries are applied to the results to yield ntot = orientations.
// The weights are normalized such that sum mult weight = sum mult =
// ncell

// Format of the orientation file
//
//     Any number of initial comment lines beginning with #
//     A line containing "format quaternion" or "format euler"
//     A line containing: ntot maxrad coverage
//     ntot lines containing: q0 q1 q2 q3 weight # for quaternions
//     ntot lines containing: alpha beta gamma weight # for euler.
//
// The weights are normalized such that sum weight = ntot.

#pragma once

#include <Eigen/Dense>
#include <cassert>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <limits>
#include <string>
#include <vector>
// todo: wrap and adapt this code to make numpy array of quats

// Windows doesn't define M_PI in the standard header?
#if !defined(M_PI)
#define M_PI 3.1415926535897932384626433832795028841971694
#endif

namespace rpxdock {
namespace sampling {
namespace orientations {

using namespace std;

// Minimal quaternion class
class Quaternion {
 public:
  double w, x, y, z;
  Quaternion(double ww = 1, double xx = 0, double yy = 0, double zz = 0)
      : w(ww), x(xx), y(yy), z(zz) {}
  void Normalize() {
    double t = w * w + x * x + y * y + z * z;
    assert(t > 0);
    t = 1 / sqrt(t);
    w *= t;
    x *= t;
    y *= t;
    z *= t;
    return;
  }
  void Canonicalize() {
    Normalize();
    // Make first biggest element positive
    double mag = w;
    if (abs(x) > abs(mag)) mag = x;
    if (abs(y) > abs(mag)) mag = y;
    if (abs(z) > abs(mag)) mag = z;
    if (mag < 0) {
      w *= -1;
      x *= -1;
      y *= -1;
      z *= -1;
    }
    return;
  }
  // a.Times(b) returns a * b
  Quaternion Times(const Quaternion &q) const {
    double mw = w * q.w - x * q.x - y * q.y - z * q.z,
           mx = w * q.x + x * q.w + y * q.z - z * q.y,
           my = w * q.y + y * q.w + z * q.x - x * q.z,
           mz = w * q.z + z * q.w + x * q.y - y * q.x;
    return Quaternion(mw, mx, my, mz);
  }
  void Print(ostream &s) const;
  void PrintEuler(ostream &s) const;
};

// Class to hold a set of orientations and weights
class PackSet {
 public:
  Quaternion Orientation(size_t i) const { return m_v[i]; }
  double Weight(size_t i) const { return m_w[i]; }
  size_t Number() const { return m_v.size(); }
  void Add(const Quaternion &q, double w = 1) {
    Quaternion v(q);
    v.Canonicalize();
    m_v.push_back(v);
    m_w.push_back(w);
  }
  void Clear() {
    m_v.clear();
    m_w.clear();
  }
  void Print(ostream &s, bool euler = false, size_t prec = 6) const {
    for (size_t i = 0; i < Number(); ++i) {
      if (euler)
        m_v[i].PrintEuler(s);
      else
        m_v[i].Print(s);
      s << " " << fixed << setprecision(prec) << setw(prec + 2) << m_w[i]
        << endl;
    }
  }

 private:
  vector<Quaternion> m_v;
  vector<double> m_w;
};

// The triple of grid indices
class Triple {
 public:
  int a, b, c;
  Triple(int aa, int bb, int cc) : a(aa), b(bb), c(cc) {}
};

// Generate the permutations and sign changes for a Triple.
class Permute {
 public:
  Permute(Triple x) {
    assert(x.a >= x.b && x.b >= x.c && x.c >= 0);
    m_arr.push_back(x);
    size_t n = 1;
    // Do the sign changes
    if (x.a != 0) {
      for (size_t i = 0; i < n; ++i)
        m_arr.push_back(Triple(-m_arr[i].a, m_arr[i].b, m_arr[i].c));
      n *= 2;
    }
    if (x.b != 0) {
      for (size_t i = 0; i < n; ++i)
        m_arr.push_back(Triple(m_arr[i].a, -m_arr[i].b, m_arr[i].c));
      n *= 2;
    }
    if (x.c != 0) {
      for (size_t i = 0; i < n; ++i)
        m_arr.push_back(Triple(m_arr[i].a, m_arr[i].b, -m_arr[i].c));
      n *= 2;
    }
    if (x.a == x.b && x.b == x.c) return;
    // With at least two distinct indices we can rotate the set thru 3
    // permuations.
    for (size_t i = 0; i < n; ++i) {
      m_arr.push_back(Triple(m_arr[i].b, m_arr[i].c, m_arr[i].a));
      m_arr.push_back(Triple(m_arr[i].c, m_arr[i].a, m_arr[i].b));
    }
    n *= 3;
    if (x.a == x.b || x.b == x.c) return;
    // With three distinct indices we can in addition interchange the
    // first two indices (to yield all 6 permutations of 3 indices).
    for (size_t i = 0; i < n; ++i) {
      m_arr.push_back(Triple(m_arr[i].b, m_arr[i].a, m_arr[i].c));
    }
    n *= 2;
  }
  size_t Number() const { return m_arr.size(); }
  Triple Member(size_t i) const { return m_arr[i]; }

 private:
  vector<Triple> m_arr;
};

// The rotational symmetries of the cube.  (Not normalized, since
// PackSet.Add does this.)
static double CubeSyms[24][4] = {
    {1, 0, 0, 0},
    // 180 deg rotations about 3 axes
    {0, 1, 0, 0},
    {0, 0, 1, 0},
    {0, 0, 0, 1},
    // +/- 120 degree rotations about 4 leading diagonals
    {1, 1, 1, 1},
    {1, 1, 1, -1},
    {1, 1, -1, 1},
    {1, 1, -1, -1},
    {1, -1, 1, 1},
    {1, -1, 1, -1},
    {1, -1, -1, 1},
    {1, -1, -1, -1},
    // +/- 90 degree rotations about 3 axes
    {1, 1, 0, 0},
    {1, -1, 0, 0},
    {1, 0, 1, 0},
    {1, 0, -1, 0},
    {1, 0, 0, 1},
    {1, 0, 0, -1},
    // 180 degree rotations about 6 face diagonals
    {0, 1, 1, 0},
    {0, 1, -1, 0},
    {0, 1, 0, 1},
    {0, 1, 0, -1},
    {0, 0, 1, 1},
    {0, 0, 1, -1},
};

// Convert from index to position.  The sinh scaling tries to compensate
// for the bunching up that occurs when [1 x y z] is projected onto the
// unit sphere.
double pind(double ind, double delta, double sigma) {
  return (sigma == 0) ? ind * delta : sinh(sigma * ind * delta) / sigma;
}

auto read_karney_orientations(std::string file_content) {
  Eigen::MatrixX4d quats;
  Eigen::VectorXd cover;

  std::istringstream in(file_content);

  bool euler = false;
  assert(in.good());
  string line;
  while (in.peek() == '#') {
    getline(in, line);
    // cout << line << endl;
  }
  assert(in.good());
  getline(in, line);
  assert(line == "format grid");
  double delta, sigma, maxrad, coverage;
  size_t ncell, ntot, nent;
  in >> delta >> sigma >> ntot >> ncell >> nent >> maxrad >> coverage;
  // Use extra digit of precision with weights and radii.  This also
  // triggers a memory minimizing expansion.
  const bool fine = delta < 0.05;
  quats.resize(ntot, 4);
  cover.resize(ntot);
  int quats_i = 0;
  PackSet s;
  size_t ncell1 = 0;
  for (size_t n = 0; n < nent; ++n) {
    int k, l, m;
    size_t mult;
    double r, w;
    assert(in.good());
    in >> k >> l >> m >> w >> r >> mult;
    Permute p(Triple(k, l, m));
    assert(mult == p.Number());
    for (size_t i = 0; i < mult; ++i) {
      Triple t = p.Member(i);
      s.Add(Quaternion(1.0, pind(0.5 * t.a, delta, sigma),
                       pind(0.5 * t.b, delta, sigma),
                       pind(0.5 * t.c, delta, sigma)),
            w);
    }
    ncell1 += mult;
    if (fine) {
      // Skip n = 0; that's already included.
      for (size_t n = 1; n < 24; ++n) {
        Quaternion q(CubeSyms[n][0], CubeSyms[n][1], CubeSyms[n][2],
                     CubeSyms[n][3]);
        for (size_t i = 0; i < mult; ++i)
          s.Add(q.Times(s.Orientation(i)), s.Weight(i));
      }
      // s.Print(cout, euler, fine ? 7 : 6);
      for (size_t i = 0; i < s.Number(); ++i) {
        quats(quats_i, 0) = s.Orientation(i).w;
        quats(quats_i, 1) = s.Orientation(i).x;
        quats(quats_i, 2) = s.Orientation(i).y;
        quats(quats_i, 3) = s.Orientation(i).z;
        cover[quats_i++] = s.Weight(i);
      }
      s.Clear();
    }
  }
  assert(in.good());
  assert(ncell1 == ncell);
  if (!fine) {
    size_t nc = s.Number();
    assert(nc == ncell);
    for (size_t n = 1; n < 24; ++n) {
      Quaternion q(CubeSyms[n][0], CubeSyms[n][1], CubeSyms[n][2],
                   CubeSyms[n][3]);
      for (size_t i = 0; i < nc; ++i)
        s.Add(q.Times(s.Orientation(i)), s.Weight(i));
    }
    assert(s.Number() == ntot);
    // s.Print(cout, euler, fine ? 7 : 6);
    for (size_t i = 0; i < s.Number(); ++i) {
      quats(quats_i, 0) = s.Orientation(i).w;
      quats(quats_i, 1) = s.Orientation(i).x;
      quats(quats_i, 2) = s.Orientation(i).y;
      quats(quats_i, 3) = s.Orientation(i).z;
      cover[quats_i++] = s.Weight(i);
    }
    s.Clear();
  }
  return std::make_tuple(quats, cover);
}

void Quaternion::Print(ostream &s) const {
  s << fixed << setprecision(9) << setw(12) << w << " ";
  s << setw(12) << x << " ";
  s << setw(12) << y << " ";
  s << setw(12) << z;
}

void Quaternion::PrintEuler(ostream &s) const {
  // Print out orientation as a set of Euler angles, following the
  // convention given in
  //
  //    http://www.mhl.soton.ac.uk/research/help/Euler/index.html
  //
  // Rotation by Euler angles [a,b,c] is defined as rotation by c about
  // z axis, followed by rotation by b about y axis. followed by
  // rotation by a about z axis (again).
  //
  // Convert to rotation matrix (assume quaternion is already
  // normalized)
  double
      // m00 = 1 - 2*y*y - 2*z*z,
      m01 = 2 * x * y - 2 * z * w,
      m02 = 2 * x * z + 2 * y * w,
      // m10 =     2*x*y + 2*z*w,
      m11 = 1 - 2 * x * x - 2 * z * z, m12 = 2 * y * z - 2 * x * w,
      m20 = 2 * x * z - 2 * y * w, m21 = 2 * y * z + 2 * x * w,
      m22 = 1 - 2 * x * x - 2 * y * y;
  // Taken from Ken Shoemake, "Euler Angle Conversion", Graphics Gems
  // IV, Academic 1994.
  //
  //    http://vered.rose.utoronto.ca/people/david_dir/GEMS/GEMS.html
  double sy = sqrt(m02 * m02 + m12 * m12);
  //  double sy = sqrt(m10*m10 + m20*m20);
  double a, b, c;
  b = atan2(sy, m22);
  if (sy > 16 * numeric_limits<double>::epsilon()) {
    a = atan2(m12, m02);
    c = atan2(m21, -m20);
  } else {
    a = atan2(-m01, m11);
    c = 0;
  }
  s << fixed << setprecision(9) << setw(12) << a << " " << setw(12) << b << " "
    << setw(12) << c;

#if !defined(NDEBUG)
  // Sanity check.  Convert from Euler angles back to a quaternion, q
  Quaternion q =
      Quaternion(cos(a / 2), 0, 0, sin(a / 2))
          .  // a about z
      Times(Quaternion(cos(b / 2), 0, sin(b / 2), 0)
                .                                              // b about y
            Times(Quaternion(cos(c / 2), 0, 0, sin(c / 2))));  // c about z
  // and check that q is parallel to *this.
  double t = abs(q.w * w + q.x * x + q.y * y + q.z * z);
  assert(t > 1 - 16 * numeric_limits<double>::epsilon());
#endif
}

}  // namespace orientations
}  // namespace sampling
}  // namespace rpxdock


#include <iostream>
#include <fstream>
using namespace std;
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Geometry>
#include <boost/format.hpp>  // for formating strings
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>

int main(int argc, char** argv)
{
  vector<cv::Mat> colorImgs, depthImgs;                                          // 彩色图和深度图
  vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;  // 相机位姿

  ifstream fin("./pose.txt");
  if (!fin)
  {
    cerr << "请在有pose.txt的目录下运行此程序" << endl;
    return 1;
  }

  for (int i = 0; i < 5; i++)
  {
    boost::format fmt("./%s/%d.%s");  //图像文件格式
    colorImgs.push_back(cv::imread((fmt % "color" % (i + 1) % "png").str()));
    depthImgs.push_back(cv::imread((fmt % "depth" % (i + 1) % "pgm").str(), -1));  // 使用-1读取原始图像

    double data[7] = { 0 };
    for (auto& d : data)
      fin >> d;
    Eigen::Quaterniond q(data[6], data[3], data[4], data[5]);
    Eigen::Isometry3d T(q);
    T.pretranslate(Eigen::Vector3d(data[0], data[1], data[2]));
    poses.push_back(T);
  }

  // 计算点云并拼接
  // 相机内参
  double cx = 325.5;
  double cy = 253.5;
  double fx = 518.0;
  double fy = 519.0;
  double depthScale = 1000.0;

  cout << "正在将图像转换为点云..." << endl;

  // 定义点云使用的格式：这里用的是XYZRGB
  typedef pcl::PointXYZRGB PointT;
  typedef pcl::PointCloud<PointT> PointCloud;

  // 新建一个点云
  PointCloud::Ptr pointCloud(new PointCloud);
  for (int i = 0; i < 5; i++)
  {
    cout << "转换图像中: " << i + 1 << endl;
    cv::Mat color = colorImgs[i];
    cv::Mat depth = depthImgs[i];
    Eigen::Isometry3d T = poses[i];
    for (int v = 0; v < color.rows; v++)
      for (int u = 0; u < color.cols; u++)
      {
        unsigned int d = depth.ptr<unsigned short>(v)[u];  // 深度值
        if (d == 0)
          continue;  // 为0表示没有测量到
        Eigen::Vector3d point;
        point[2] = double(d) / depthScale;
        point[0] = (u - cx) * point[2] / fx;
        point[1] = (v - cy) * point[2] / fy;
        Eigen::Vector3d pointWorld = T * point;

        PointT p;
        p.x = pointWorld[0];
        p.y = pointWorld[1];
        p.z = pointWorld[2];
        p.b = color.data[v * color.step + u * color.channels()];
        p.g = color.data[v * color.step + u * color.channels() + 1];
        p.r = color.data[v * color.step + u * color.channels() + 2];
        pointCloud->points.push_back(p);
      }
  }

  pointCloud->is_dense = false;
  cout << "点云共有" << pointCloud->size() << "个点." << endl;
  pcl::io::savePCDFileBinary("map.pcd", *pointCloud);
  return 0;
}


#include "FiniteElementFunction.h"
#include "form/RefElement.h"
#include "tensor/EigenMap.h"

#include <Eigen/Core>

#include <cassert>
#include <cstdio>

namespace tndm {

template <std::size_t D>
Managed<Matrix<double>>
FiniteElementFunction<D>::evaluationMatrix(std::vector<std::array<double, D>> const& points) const {
    return refElement_->evaluateBasisAt(points, {1, 0});
}

template <std::size_t D>
Managed<Tensor<double, 3u>> FiniteElementFunction<D>::gradientEvaluationTensor(
    std::vector<std::array<double, D>> const& points) const {
    return refElement_->evaluateGradientAt(points, {2, 0, 1});
}

template <std::size_t D>
TensorBase<Matrix<double>> FiniteElementFunction<D>::mapResultInfo(std::size_t numPoints) const {
    return TensorBase<Matrix<double>>(numPoints, numQuantities());
}

template <std::size_t D>
void FiniteElementFunction<D>::map(std::size_t eleNo, Matrix<double> const& evalMatrix,
                                   Tensor<double, 2u>& result) const {
    assert(eleNo < numElements());
    assert(result.shape(0) == evalMatrix.shape(0));
    assert(result.shape(1) == numQuantities());
    assert(evalMatrix.shape(1) == numBasisFunctions());

    auto mat = data_.subtensor(slice{}, slice{}, eleNo);
    EigenMap(result) = EigenMap(evalMatrix) * EigenMap(mat);
}

template <std::size_t D>
TensorBase<Tensor<double, 3u>>
FiniteElementFunction<D>::gradientResultInfo(std::size_t numPoints) const {
    return TensorBase<Tensor<double, 3u>>(numPoints, numQuantities(), D);
}

template <std::size_t D>
void FiniteElementFunction<D>::gradient(std::size_t eleNo, Tensor<double, 3u> const& evalTensor,
                                        Tensor<double, 3u> const& jInvAtP,
                                        Tensor<double, 3u>& result) const {
    assert(eleNo < numElements());
    assert(result.shape(0) == evalTensor.shape(0));
    assert(result.shape(1) == numQuantities());
    assert(result.shape(2) == D);
    assert(evalTensor.shape(1) == numBasisFunctions());
    assert(evalTensor.shape(2) == D);
    assert(jInvAtP.shape(0) == D);
    assert(jInvAtP.shape(1) == D);
    assert(jInvAtP.shape(2) == result.shape(0));

    auto mat = data_.subtensor(slice{}, slice{}, eleNo);
    for (std::ptrdiff_t j = 0; j < evalTensor.shape(2); ++j) {
        auto d_j = evalTensor.subtensor(slice{}, slice{}, j);
        auto result_j = result.subtensor(slice{}, slice{}, j);
        EigenMap(result_j) = EigenMap(d_j) * EigenMap(mat);
    }

    for (std::ptrdiff_t q = 0; q < result.shape(0); ++q) {
        auto jInvAtP_q = jInvAtP.subtensor(slice{}, slice{}, q);
        auto result_q = result.subtensor(q, slice{}, slice{});
        EigenMap(result_q) = EigenMap(result_q) * EigenMap(jInvAtP_q);
    }
}

template <std::size_t D> std::string FiniteElementFunction<D>::name(std::size_t q) const {
    assert(q < numQuantities());
    if (!names_.empty()) {
        return names_[q];
    }
    char buf[100];
    snprintf(buf, sizeof(buf), "q%lu", q);
    return std::string(buf);
}

template class FiniteElementFunction<1ul>;
template class FiniteElementFunction<2ul>;
template class FiniteElementFunction<3ul>;

} // namespace tndm


/*
 [auto_generated]
 boost/numeric/odeint/stepper/implicit_euler.hpp

 [begin_description]
 Impementation of the implicit Euler method. Works with ublas::vector as state type.
 [end_description]

 Copyright 2010-2012 Mario Mulansky
 Copyright 2010-2012 Karsten Ahnert
 Copyright 2012 Christoph Koke

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

#ifndef BOOST_NUMERIC_ODEINT_STEPPER_IMPLICIT_EULER_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_IMPLICIT_EULER_HPP_INCLUDED

#include <iostream>
#include <utility>

#include <boost/numeric/odeint/external/eigen/eigen_resize.hpp>
#include <boost/numeric/odeint/external/eigen/eigen_algebra.hpp>

#include <boost/numeric/odeint/util/bind.hpp>
#include <boost/numeric/odeint/util/unwrap_reference.hpp>
#include <boost/numeric/odeint/stepper/stepper_categories.hpp>

#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>
#include <boost/numeric/odeint/util/state_wrapper.hpp>

#include<Eigen/IterativeLinearSolvers>
#include <Eigen/PardisoSupport>
#include <Eigen/Sparse>
#include<Eigen/SparseQR>

namespace boost {
namespace numeric {
namespace odeint {

template< class ValueType , class Resizer = initially_resizer >
class implicit_euler
{

public:

    typedef ValueType value_type;
    typedef value_type time_type;
    typedef Eigen::Matrix<value_type, Eigen::Dynamic, 1> state_type;
    typedef state_wrapper< state_type > wrapped_state_type;
    typedef state_type deriv_type;
    typedef state_wrapper< deriv_type > wrapped_deriv_type;
    typedef Eigen::SparseMatrix<value_type> matrix_type;
    typedef state_wrapper< matrix_type > wrapped_matrix_type;
    typedef Resizer resizer_type;
    typedef stepper_tag stepper_category;
    typedef implicit_euler< ValueType , Resizer > stepper_type;

    implicit_euler( value_type epsilon = 1E-6 )
    : m_epsilon( epsilon )
    { }

    template< class System >
    void do_step( System system , state_type &x , time_type t , time_type dt )
    {
        typedef typename odeint::unwrap_reference< System >::type system_type;
        typedef typename odeint::unwrap_reference< typename system_type::first_type >::type deriv_func_type;
        typedef typename odeint::unwrap_reference< typename system_type::second_type >::type jacobi_func_type;
        system_type &sys = system;
        deriv_func_type &deriv_func = sys.first;
        jacobi_func_type &jacobi_func = sys.second;

        m_resizer.adjust_size( x , detail::bind( &stepper_type::template resize_impl<state_type> , detail::ref( *this ) , detail::_1 ) );

        t += dt;

        // apply first Newton step
        deriv_func( x , m_dxdt.m_v , t );

        m_b.m_v = dt * m_dxdt.m_v;

        jacobi_func( x , m_jacobi.m_v  , t );
        matrix_type m_identity(x.size(), x.size());
        m_identity.setIdentity();
        m_jacobi.m_v *= dt;
        m_jacobi.m_v -= m_identity;
        Eigen::PardisoLU<matrix_type> solver(m_jacobi.m_v);

        m_b.m_v = solver.solve(m_b.m_v).eval();

        m_x.m_v = x - m_b.m_v;

        // iterate Newton until some precision is reached
        // ToDo: maybe we should apply only one Newton step -> linear implicit one-step scheme
        while(m_b.m_v.squaredNorm() > m_epsilon )
        {
            deriv_func( m_x.m_v , m_dxdt.m_v , t );
            m_b.m_v = x - m_x.m_v + dt*m_dxdt.m_v;

            // simplified version, only the first Jacobian is used
            //            jacobi( m_x , m_jacobi , t );
            //            m_jacobi *= dt;
            //            m_jacobi -= boost::numeric::ublas::identity_matrix< value_type >( x.size() );

            m_b.m_v = solver.solve(m_b.m_v).eval();
            m_x.m_v -= m_b.m_v;
        }
        x = m_x.m_v;
    }

    template< class StateType >
    void adjust_size( const StateType &x )
    {
        resize_impl( x );
    }

private:

    template< class StateIn >
    bool resize_impl( const StateIn &x )
    {
        bool resized = false;
        resized |= adjust_size_by_resizeability( m_dxdt , x , typename is_resizeable<deriv_type>::type() );
        resized |= adjust_size_by_resizeability( m_x , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_b , x , typename is_resizeable<deriv_type>::type() );
        resized |= adjust_size_by_resizeability( m_jacobi , x , typename is_resizeable<matrix_type>::type() );
        return resized;
    }

private:

    value_type m_epsilon;
    resizer_type m_resizer;
    wrapped_deriv_type m_dxdt;
    wrapped_state_type m_x;
    wrapped_deriv_type m_b;
    wrapped_matrix_type m_jacobi;
};

} // odeint
} // numeric
} // boost

#endif // BOOST_NUMERIC_ODEINT_STEPPER_IMPLICIT_EULER_HPP_INCLUDED


///////////////////////////////////////////////////////////////////////////////
// Copyright 2010 Manuel Peinado Gallego                                     //
// Distributed under the Boost Software License, Version 1.0. (See           //
// accompanying file LICENSE_1_0.txt or copy at                              //
// http://www.boost.org/LICENSE_1_0.txt)                                     //
///////////////////////////////////////////////////////////////////////////////
#include <ctime>
#include <algorithm>
#include <string>
#include <vector>
#include <iterator>
#include <cstdlib>
#include <boost/bind.hpp>
#include <libs/assign/v2/speed/tools.h>

// http://code.google.com/p/truffle/source/browse/trunk/include/mpg/TimeIt.h
// http://code.google.com/p/truffle/source/browse/trunk/include/mpg/Random.h

// http://www.eternallyconfuzzled.com/arts/jsw_art_rand.aspx
inline double uniform_deviate ( int seed )
{
    return seed * ( 1.0 / ( RAND_MAX + 1.0 ) );
}
inline int rand(int M, int N) // Range [M..N)
{
    return int(M + uniform_deviate(std::rand()) * (N - M));
}
char rand_letter()
{
    return char(rand('a', 'z' + 1));
}

std::string rand_str(int len)
{
    std::string result;
    std::generate_n(std::back_inserter(result), len, &rand_letter);
    return result;
}

std::vector<int>
rand_vec(int max_n)
{
    std::vector<int> result(
        (std::size_t)mpg::rand(1, max_n)
    );
    std::generate(
        result.begin(),
        result.end(),
        boost::bind(
            &mpg::rand,
            0,
            20
        )
    );
    return result;
}

namespace mpg
{
    namespace detail
    {
        double clock_diff_to_sec(long clock_diff)
        {
            return double(clock_diff) / CLOCKS_PER_SEC;
        }

        template<class Proc>
        double time_it_impl(Proc proc, int N) // returns time in microseconds
        {
            std::clock_t const start = std::clock();
            for(int i = 0; i < N; ++i)
                proc();
            std::clock_t const end = std::clock();
            if(clock_diff_to_sec(end - start) < .2)
                return time_it_impl(proc, N * 5);
            return clock_diff_to_sec(end - start) * (1e6 / N);
        }

        template<class Proc, class Result>
        double time_it_impl(Proc proc, Result & result, int N) // returns time in microseconds
        {
            std::clock_t const start = std::clock();
            for(int i = 0; i < N; ++i)
                result = proc();
            std::clock_t const end = std::clock();
            if(clock_diff_to_sec(end - start) < .2)
                return time_it_impl(proc, result, N * 5);
            return clock_diff_to_sec(end - start) * (1e6 / N);
        }
    }

    template<class Proc>
    double time_it(Proc proc) // returns time in microseconds
    {
        return detail::time_it_impl(proc, 1);
    }

    template<class Proc, class Result>
    double time_it(Proc proc, Result & result) // returns time in microseconds
    {
        return detail::time_it_impl(proc, result, 1);
    }
}

namespace mpg
{
    inline double rand_dbl()
    {
        return double(::rand()) / RAND_MAX;
    }

    inline double rand_dbl(double M, double N)
    {
        return M + rand_dbl() * (N - M);
    }

    // http://www.eternallyconfuzzled.com/arts/jsw_art_rand.aspx
    inline int rand(int M, int N) // Range (M..N)
    {
        return int(M + std::rand() * ( 1.0 / ( RAND_MAX + 1.0 )) * (N - M));
    }

    inline char rand_letter()
    {
        return char(rand('a', 'z' + 1));
    }

    inline std::string rand_str(int len)
    {
        std::string result;
        result.reserve(len);
        for(int i = 0; i < len; ++i)
            result.push_back(rand_letter());
        return result;
    }
}


/**
   \file gsl_simplex.hpp
   \brief a simple wrapper for the GSL_SIMPLEX method
   \author Junhua Gu
 */

#ifndef GSL_SIMPLEX_METHOD
#define GSL_SIMPLEX_METHOD
#define OPT_HEADER
#include <core/optimizer.hpp>
//#include <blitz/array.h>
#include <vector>
#include <limits>
#include <cassert>
#include <cmath>
#include <algorithm>
#include <gsl/gsl_multimin.h>
#include <iostream>

namespace opt_utilities
{

  /**
     \brief object function of the gsl simplex function
   */
  template <typename rT,typename pT>
  double gsl_func_adapter(const gsl_vector* v,void* params)
  {
    pT temp;
    temp.resize(v->size);
    for(size_t i=0;i<get_size(temp);++i)
      {
	set_element(temp,i,gsl_vector_get(v,i));
      }
    return ((func_obj<rT,pT>*)params)->eval(temp);
  }

  /**
     \brief wrapper for the gsl simplex optimization method
     \tparam return type of the object function
     \tparam param type of the object function
  */
  template <typename rT,typename pT>
  class gsl_simplex
    :public opt_method<rT,pT>
  {
  public:
    typedef pT array1d_type;
    typedef rT T;
  private:
    func_obj<rT,pT>* p_fo;
    optimizer<rT,pT>* p_optimizer;

    //typedef blitz::Array<rT,2> array2d_type;

  private:
    array1d_type start_point;
    array1d_type end_point;

  private:
    rT threshold;
  private:
    rT func(const pT& x)
    {
      assert(p_fo!=0);
      return p_fo->eval(x);
    }

    const char* do_get_type_name()const
    {
      return "gsl simplex";
    }
  public:
    gsl_simplex()
      :threshold(1e-4)
    {}

    virtual ~gsl_simplex()
    {
    };

    gsl_simplex(const gsl_simplex<rT,pT>& rhs)
      :p_fo(rhs.p_fo),p_optimizer(rhs.p_optimizer),
       start_point(rhs.start_point),
       end_point(rhs.end_point),
       threshold(rhs.threshold)
    {
    }

    gsl_simplex<rT,pT>& operator=(const gsl_simplex<rT,pT>& rhs)
    {
      threshold=rhs.threshold;
      p_fo=rhs.p_fo;
      p_optimizer=rhs.p_optimizer;
      opt_eq(start_point,rhs.start_point);
      opt_eq(end_point,rhs.end_point);
    }

    opt_method<rT,pT>* do_clone()const
    {
      return new gsl_simplex<rT,pT>(*this);
    }

    void do_set_start_point(const array1d_type& p)
    {
      start_point.resize(get_size(p));
      opt_eq(start_point,p);

    }

    array1d_type do_get_start_point()const
    {
      return start_point;
    }

    void do_set_precision(rT t)
    {
      threshold=t;
    }

    rT do_get_precision()const
    {
      return threshold;
    }

    void do_set_optimizer(optimizer<rT,pT>& o)
    {
      p_optimizer=&o;
      p_fo=p_optimizer->ptr_func_obj();
    }

    pT do_optimize()
    {
      const gsl_multimin_fminimizer_type *T =
	gsl_multimin_fminimizer_nmsimplex;
      gsl_multimin_fminimizer *s = NULL;
      gsl_vector *ss, *x;
      gsl_multimin_function minex_func;

      size_t iter = 0;
      int status;
      double size;

      /* Starting point */
      x = gsl_vector_alloc (get_size(start_point));
      //      gsl_vector_set (x, 0, 5.0);
      //gsl_vector_set (x, 1, 7.0);
      for(size_t i=0;i!=get_size(start_point);++i)
	{
	  gsl_vector_set(x,i,get_element(start_point,i));
	}

      /* Set initial step sizes to 1 */
      ss = gsl_vector_alloc (get_size(start_point));
      gsl_vector_set_all (ss, 1.0);

      //foo f;
      /* Initialize method and iterate */
      minex_func.n = get_size(start_point);
      minex_func.f = &gsl_func_adapter<double,std::vector<double> >;
      minex_func.params = (void *)p_fo;

      s = gsl_multimin_fminimizer_alloc (T, get_size(start_point));
      gsl_multimin_fminimizer_set (s, &minex_func, x, ss);

      do
	{
	  iter++;
	  status = gsl_multimin_fminimizer_iterate(s);

	  if (status)
	    {
	      break;
	    }
	  //std::cerr<<"threshold="<<threshold<<std::endl;
	  size = gsl_multimin_fminimizer_size (s);
	  status = gsl_multimin_test_size (size, threshold);

	  if (status == GSL_SUCCESS)
	    {
	      //printf ("converged to minimum at\n");
	    }

	  //printf ("%5d %10.3e %10.3ef f() = %7.3f size = %.3f\n",
	  //iter,
	  //gsl_vector_get (s->x, 0),
	  //gsl_vector_get (s->x, 1),
	  //  s->fval, size);
	}
      while (status == GSL_CONTINUE);

      /*
	foo f;
	gsl_vector_set (x, 0, 0.0);
	gsl_vector_set (x, 1, 0.0);
	cout<<"fdsa ";
	cout<<gsl_func_adapter<double,vector<double> >(x,(void*)&f)<<endl;;

      */

      end_point.resize(get_size(start_point));
      for(size_t i=0;i<get_size(start_point);++i)
	{
	  set_element(end_point,i,gsl_vector_get(s->x,i));
	}

      gsl_vector_free(x);
      gsl_vector_free(ss);
      gsl_multimin_fminimizer_free (s);

      return end_point;
    }
  };

}

#endif
//EOF


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE ForwardBackward
#include <boost/test/unit_test.hpp>
#include <HSMM.hpp>
#include <ForwardBackward.hpp>
#include <memory>
#include <exception>
#include <set>

#define EPSILON 1e-6

using namespace arma;
using namespace hsmm;
using namespace std;

mat transition = {{0.0, 0.1, 0.4, 0.5},
                  {0.3, 0.0, 0.6, 0.1},
                  {0.2, 0.2, 0.0, 0.6},
                  {0.4, 0.4, 0.2, 0.0}};

mat duration = {{0.0, 0.1, 0.4, 0.5},
                {0.3, 0.0, 0.6, 0.1},
                {0.2, 0.2, 0.0, 0.6},
                {0.4, 0.4, 0.2, 0.0}};

int min_duration = 4;

vec pi = {0.25, 0.25, 0.25, 0.25};

int ndurations = duration.n_cols;

int nstates = duration.n_rows;

// Emission parameters.
vec means = {0, 5, 10, 15};
vec std_devs =  {0.5, 1.0, 0.1, 2.0};

BOOST_AUTO_TEST_CASE( ForwardBackwardWithLabels ) {
    shared_ptr<AbstractEmission> ptr_emission(new DummyGaussianEmission(
            means, std_devs));
    HSMM dhsmm(ptr_emission, transition, pi, duration, min_duration);
    ivec hiddenStates, hiddenDurations;
    int nSampledSegments = 50;
    field<mat> samples = dhsmm.sampleSegments(nSampledSegments, hiddenStates,
            hiddenDurations);
    int nobs = samples.n_elem;

    // Output parameters of the Forward-Backward algorithm.
    mat alpha(nstates, nobs, fill::zeros);
    mat beta(nstates, nobs, fill::zeros);
    mat alpha_s(nstates, nobs, fill::zeros);
    mat beta_s(nstates, nobs, fill::zeros);
    vec beta_s_0(nstates, fill::zeros);
    cube eta(nstates, ndurations, nobs, fill::zeros);
    cube zeta(nstates, nstates, nobs - 1, fill::zeros);

    cube logpdf = dhsmm.computeEmissionsLogLikelihood(samples);
    Labels full_labels;
    Labels sparse_labels;
    set<int> sparse_segment_ids = {10, 30, 40, 45};

    // Required for generating the runs of unobserved segments.
    vector<pair<int, int>> seq_unobserved_segments;
    int starting_unobserved_segment_idx = 0;

    int current_idx = 0;
    double expected_llikelihood_fulllabels = 0;
    for(int i = 0; i < nSampledSegments; i++) {
        int hs = hiddenStates(i);
        int d = hiddenDurations(i);
        expected_llikelihood_fulllabels += logpdf(hs, current_idx,
                d - min_duration) + log(duration(hs, d - min_duration));
        if (i == 0)
            expected_llikelihood_fulllabels += log(pi(hs));
        else
            expected_llikelihood_fulllabels += log(transition(hiddenStates(
                            i - 1), hs));
        int starting_idx = current_idx;
        current_idx += d;
        int ending_idx = current_idx - 1;
        full_labels.setLabel(ending_idx, d, hs);
        if (sparse_segment_ids.find(i) != sparse_segment_ids.end()) {
            sparse_labels.setLabel(ending_idx, d, hs);
            seq_unobserved_segments.push_back(make_pair(
                    starting_unobserved_segment_idx, starting_idx - 1));
            starting_unobserved_segment_idx = ending_idx + 1;
        }
    }
    seq_unobserved_segments.push_back(make_pair(starting_unobserved_segment_idx,
                current_idx - 1));

    // Testing with full labels.
    logsFB(log(transition), log(pi), log(duration), logpdf, full_labels,
            alpha, beta, alpha_s, beta_s, beta_s_0, eta, zeta, min_duration,
            nobs);
    eta = exp(eta);
    zeta = exp(zeta);
    double llikelihood = logsumexp(alpha.col(nobs - 1));
	BOOST_CHECK(fabs(llikelihood - expected_llikelihood_fulllabels) < EPSILON);
    current_idx = 0;
    for(int i = 0; i < nSampledSegments - 1; i++) {
        int hs = hiddenStates(i);
        int next_hs = hiddenStates(i + 1);
        int d = hiddenDurations(i);
        current_idx += d;

        // Checking that the observed transitions have probability one and the
        // rest have zero probability.
        for(int j = 0; j < nstates; j++)
            for(int k = 0; k < nstates; k++)
                if (j == hs && k == next_hs)
                    BOOST_CHECK(fabs(zeta(j, k, current_idx - 1) - 1.0) < EPSILON);
                else
                    BOOST_CHECK(fabs(zeta(j, k, current_idx - 1) - 0.0) < EPSILON);
    }

    // Testing with sparse labels.
    logsFB(log(transition), log(pi), log(duration), logpdf, sparse_labels,
            alpha, beta, alpha_s, beta_s, beta_s_0, eta, zeta, min_duration,
            nobs);
    eta = exp(eta);
    zeta = exp(zeta);
    for(auto p : seq_unobserved_segments) {
        double sum_starting = 0;
        double sum_ending = 0;
        int start_seg = p.first;
        int end_seg = p.second;
        for(int i = 0; i < nstates; i++)
            for(int d = 0; d < ndurations; d++) {
                sum_starting += eta(i, d, start_seg + min_duration + d - 1);
                sum_ending += eta(i, d, end_seg);
            }

        // There must be a segment which explains the starting part of an
        // unobserved run.
        BOOST_CHECK(fabs(sum_starting - 1.0) < EPSILON);

        // The same applies for the ending part.
        BOOST_CHECK(fabs(sum_ending - 1.0) < EPSILON);
    }

    current_idx = 0;
    for(int i = 0; i < nSampledSegments; i++) {
        int hs = hiddenStates(i);
        int d = hiddenDurations(i);
        int starting_idx = current_idx;
        current_idx += d;
        int ending_idx = current_idx - 1;
        if (sparse_segment_ids.find(i) != sparse_segment_ids.end()) {

            // Checking that the provided labels apper as ones in eta.
            BOOST_CHECK(fabs(eta(hs, d - min_duration, ending_idx) - 1) <
                    EPSILON);
            for(int j = 0; j < nstates; j++) {

                double sum_eta_last = 0;
                double sum_eta_first = 0;
                for(int k = 0; k < ndurations; k++) {
                    sum_eta_last += eta(j, k, starting_idx - 1);
                    sum_eta_first += eta(j, k, ending_idx + 1);
                }

                // Checking that the expected transition from an unobserved
                // state to an observed one (or viceversa) is equal to the
                // expected value of being in the unobserved state.
                BOOST_CHECK(fabs(sum_eta_last - zeta(j, hs, starting_idx - 1))
                        < EPSILON);
                BOOST_CHECK(fabs(sum_eta_first - zeta(hs, j, ending_idx + 1))
                        < EPSILON);

                // Checking that unfeasible transitions have 0 posterior mass.
                for(int k = 0; k < nstates; k++)
                    if (k != hs)
                        BOOST_CHECK(zeta(j, k, starting_idx - 1) < EPSILON &&
                                zeta(k, j, ending_idx + 1) < EPSILON);

            }
        }
    }

    // Checking that the expected number of segments in the observation
    // sequence is equal to the actual value. This is expected because
    // the actual parameters are used for inference.
    BOOST_CHECK(fabs(accu(eta) - nSampledSegments) < EPSILON);
}

BOOST_AUTO_TEST_CASE( LogSumExp ) {
    int n = 10;
    vec ones_v = ones<vec>(n);
    double sum = exp(logsumexp(log(ones_v)));
    BOOST_CHECK(fabs(sum - n) < EPSILON);
    int nzeros = 3;
    ones_v.subvec(0, nzeros - 1).fill(0.0);
    sum = exp(logsumexp(log(ones_v)));
    BOOST_CHECK(fabs(sum - (n-nzeros)) < EPSILON);
}


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// See LICENSE file in the project root for full license information.
//

#include "RefFakeQuantizationFloat32Workload.hpp"

#include "RefWorkloadUtils.hpp"

#include "Profiling.hpp"

#include <boost/numeric/conversion/cast.hpp>

namespace armnn
{

void FakeQuantization(const float* inputData, float* outputData, uint32_t numElements, float min, float max)
{
    float scale = (max - min) / 255.f;
    int32_t offset = boost::numeric_cast<int32_t>((-min * 255.f) / (max - min));

    for (uint32_t i = 0; i < numElements; i++)
    {
        outputData[i] = static_cast<float>(armnn::Quantize<uint8_t>(inputData[i], scale, offset));
    }

}

void RefFakeQuantizationFloat32Workload::Execute() const
{
    ARMNN_SCOPED_PROFILING_EVENT(Compute::CpuRef, "RefFakeQuantizationFloat32Workload_Execute");

    const TensorInfo& inputInfo = GetTensorInfo(m_Data.m_Inputs[0]);

    const float* inputData = GetInputTensorDataFloat(0, m_Data);
    float* outputData = GetOutputTensorDataFloat(0, m_Data);
    FakeQuantization(inputData, outputData, inputInfo.GetNumElements(),
                     m_Data.m_Parameters.m_Min,
                     m_Data.m_Parameters.m_Max);
}

} //namespace armnn


#include <iostream>
#include <vector>
#include <functional>
#include <cassert>
#include <cmath>
#include <chrono>
#include <NTL/matrix.h>
#include <NTL/ZZ_pX.h> // includes ZZ_p.h and ZZ.h as well

using std::vector;
using namespace std::chrono;
using NTL::Mat;
using NTL::ZZ_p;
using NTL::ZZ_pX;
using NTL::ZZ;

// TODO: add return type to final algorithm

// Reference: https://www.dropbox.com/home/urop2019/literature?preview=Bostan%2C+Gaudry%2C+Schost+-+Linear+recurrences+with+polynomial+coefficients+and+application+to+integer+factorization+and+Cartier-Manin+operator.pdf
// Page 1781 (Lemma 1)
void invert_all(vector<ZZ_p> &out, vector<ZZ_p> &a);
// Page 1781 (Lemma 2)
void find_delta(vector<ZZ_p> &out, ZZ &p);
void find_delta(vector<ZZ_p> &out, ZZ &p, ZZ_p &a);
// Page 1784-5 (Theorem 5)
void shift_values(vector<ZZ_p> &out, vector<ZZ_p> &values, ZZ_p &a, ZZ_p &b, ZZ &p);
void shift_values(vector<Mat<ZZ_p>> &out, vector<Mat<ZZ_p>> &values, ZZ_p &a, ZZ_p &b, ZZ &p);
// Page 1786-8 (Section 4)
void multieval_prod(vector<Mat<ZZ_p>> &out, std::function<void (Mat<ZZ_p>&, ZZ_p&)> A, ZZ &p);
void multieval_prod(vector<Mat<ZZ_p>> &out, ZZ_p &k, std::function<void (Mat<ZZ_p>&, ZZ_p&)> A, ZZ &p);
// Page 1792 (Equation (5))
void matrix_factorial(Mat<ZZ_p> &out, long n, std::function<void (Mat<ZZ_p>&, ZZ_p&)> A, ZZ &p);
// Following functions for testing:
// matrix definition
void A(Mat<ZZ_p> &out, ZZ_p &x);
// print contents of a vector
template <typename T>
void print(vector<T> vec);

/*
 * Evaluate M(x), returned in out
 */
void A(Mat<ZZ_p> &out, ZZ_p &x){
    ZZ_p zero;
    zero.init(x.modulus());
    zero = 0;
    ZZ_p one;
    one.init(x.modulus());
    one = 1;
    /*out.SetDims(2, 2);
    out.put(0, 0, x+1);
    out.put(0, 1, zero);
    out.put(1, 0, one);
    out.put(1, 1, one);
    */
    out.SetDims(1, 1);
    out.put(0, 0, x);

}

int main(){
    std::function<void (Mat<ZZ_p>&, ZZ_p&)> A_func = A;

    Mat<ZZ_p> answer;
    for(long i = 1073741824; i <= 1073741824; i*=2){
        ZZ p;
        NextPrime(p, ZZ(i));
        long p1;
        conv(p1, p-1);
        mul(p, p, p);
        uint64_t start = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();
        matrix_factorial(answer, p1, A_func, p);
        uint64_t time = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count() - start;

        std::cout << "final answer: " << answer << std::endl;
        //std::cout << time << ", ";
    }
    std::cout << std::endl;

}

template<typename T>
void print(vector<T> vec){
    for(long i = 0; i < vec.size(); i++){
        std::cout << vec[i];
        if(i < vec.size()-1){
            std::cout << ", ";
        }
    }
    std::cout << std::endl;
}

/*
 * Steps:
 * [DONE] Create function that take inputs a0, a1, ..., an-1 and outputs 1/a0, ..., 1/an-1
 * [DONE] Create function that takes input d and outputs 1/prod(0-j), 1/prod(1-j), ..., 1/prod(d-j)
 * [DONE] Create function that takes input a, d and outputs prod(a+0-j), prod(a+1-j), ..., prod(a+d-j)
 * [DONE] Create function that take inputs M(a), M(a+b), ..., M(a+kb) and outputs M(a+i), M(a+b+i), ..., M(a+kb+i)
 * [DONE] Create function to calculate M_k(0), M_k(k), ..., M_k(k^2)
 * [DONE] Create main function to calculate M(1)...M(k^2) = M_k(0)M_k(k)...M_k(k^2-k)
 */

/*
 * Takes a list of elements a and returns their inversions mod p
 */
void invert_all(vector<ZZ_p> &out, vector<ZZ_p> &a){
    assert(out.size() == a.size());

    long n = a.size();
    vector<ZZ_p> inverses(n);

    vector<ZZ_p> accum_prod(n); // a0, a0a1, ..., a0a1...an-1

    accum_prod[0] = a[0];
    for(long i = 1; i < n; i++){
        mul(accum_prod[i], accum_prod[i-1], a[i]);
    }

    inv(accum_prod[n-1], accum_prod[n-1]);
    for(long i = n-1; i > 0; i--){
        mul(inverses[i], accum_prod[i], accum_prod[i-1]); // 1/ai = 1/(a1...ai) * a1...a(i-1)
        mul(accum_prod[i-1], accum_prod[i], a[i]); // a1...a(i-1) -> 1/(a1...a(i-1))
    }
    inverses[0] = accum_prod[0];

    out = inverses;
}

/*
 * d = out.size()-1;
 * Returns 1/delta(i, d) = 1/prod(i-j) where the product goes from j=0 to d skipping i.
 */
void find_delta(vector<ZZ_p> &out, ZZ &p){
    long d = out.size() - 1;
    vector<ZZ_p> ints(d);
    for(long i = 0; i < ints.size(); i++){
        ints[i].init(p);
        ints[i] = i+1;
    }
    vector<ZZ_p> inv_ints(d);
    invert_all(inv_ints, ints);

    vector<ZZ_p> deltas(d+1);
    deltas[0] = inv_ints[0]; // = 1
    for(long i = 1; i < inv_ints.size(); i++){
        mul(deltas[0], deltas[0], inv_ints[i]);
    } // deltas[0] = 1/d!

    if(d%2 == 1){
        deltas[0] = -deltas[0]; // deltas[0] = 1/((-1)^d*d!)
    }

    for(long i = 1; i < deltas.size(); i++){
        mul(deltas[i], deltas[i-1], i-1-d);
        mul(deltas[i], deltas[i], inv_ints[i-1]);
    }

    out = deltas;
}

/*
 * d = out.size()-1;
 * Returns Delta(a, i, d) = prod(a+i-j) where the product goes from j=0 to d.
 */
void find_delta(vector<ZZ_p> &out, ZZ_p &a, ZZ &p){
    long d = out.size() - 1;

    vector<ZZ_p> ints(d);
    for(long i = 0; i < ints.size(); i++){
        ints[i].init(p);
        ints[i] = a-d+i;
    }
    vector<ZZ_p> inv_ints(d);
    invert_all(inv_ints, ints);

    vector<ZZ_p> Deltas(d+1);
    Deltas[0] = a;
    for(long i = 0; i < inv_ints.size(); i++){
        mul(Deltas[0], Deltas[0], ints[i]);
    } // Deltas[0] = (a-d)...(a-1)(a)
    for(long i = 1; i < Deltas.size(); i++){
        mul(Deltas[i], Deltas[i-1], a+i);
        mul(Deltas[i], Deltas[i], inv_ints[i-1]);
    }

    out = Deltas;
}

/*
 * Takes in the values of polynomials F(r), F(r+b), ..., F(r+db) and outputs F(r+a), F(r+b+a), ... F(r+db+a)
 * Requires deg(F) <= d to work mathematically, since it will interpolate based on the given evaluations of F.
 */
void shift_values(vector<ZZ_p> &out, vector<ZZ_p> &values, ZZ_p &a, ZZ_p &b, ZZ &p){
    assert(out.size() == values.size());

    long d = values.size() - 1;
    vector<ZZ_p> P(d+1);
    find_delta(P, p);

    ZZ_pX Px;
    for(long i = 0; i < values.size(); i++){
        mul(P[i], P[i], values[i]);
        SetCoeff(Px, i, P[i]);
    }

    ZZ_p shift;
    shift.init(p);
    inv(shift, b);
    mul(shift, shift, a); // shift = a/b

    vector<ZZ_p> S(2*d+1);
    for(long i = 0; i < S.size(); i++){
        S[i] = shift + i - d;
    }
    invert_all(S, S);

    ZZ_pX Sx;
    for(long i = 0; i < S.size(); i++){
        SetCoeff(Sx, i, S[i]);
    }

    ZZ_pX PS;
    mul(PS, Px, Sx);

    find_delta(out, shift, p);
    for(long i = 0; i < out.size(); i++){
        mul(out[i], out[i], coeff(PS, i+d));
    }
}

/*
 * Takes in the values of matrices of polynomials M(r), M(r+b), ..., M(r+db) and outputs M(r+a), M(r+b+a), ... M(r+db+a)
 * Requires the maximal degree of any element of M(x) to be <= d to successfully interpolate
 */
void shift_values(vector<Mat<ZZ_p>> &out, vector<Mat<ZZ_p>> &values, ZZ_p &a, ZZ_p &b, ZZ &p){
    assert(out.size() == values.size());
    assert(values[0].NumRows() == values[0].NumCols());
    for(long i = 0; i < out.size(); i++){
        assert(values[i].NumRows() == values[0].NumRows());
        assert(values[i].NumCols() == values[0].NumCols());
        out[i].SetDims(values[0].NumRows(), values[0].NumCols());
    }

    for(long row = 0; row < values[0].NumRows(); row++){
        for(long col = 0; col < values[0].NumCols(); col++){
            vector<ZZ_p> vals(values.size());
            for(long i = 0; i < values.size(); i++){
                vals[i] = values[i].get(row, col);
            }
            vector<ZZ_p> shifted_vals(vals.size());
            shift_values(shifted_vals, vals, a, b, p);
            for(long i = 0; i < shifted_vals.size(); i++){
                out[i].put(row, col, shifted_vals[i]);
            }
        }
    }
}

/*
 * k := out.size()-1
 * Calculates M_k(x) at 0, k, ..., k^2 recursively
 * M_k(x) = M(x+1)M(x+2)...M(x+k)
 * To calculate M_m(x) at 0, k, ..., mk, use the value M_m/2(x) at 0, k, ..., (m/2)k
 */
void multieval_prod(vector<Mat<ZZ_p>> &out, std::function<void (Mat<ZZ_p>&, ZZ_p&)> A, ZZ &p){
    ZZ_p kp;
    kp.init(p);
    kp = out.size()-1;
    multieval_prod(out, kp, A, p);
}
/*
 * m := out.size()-1
 */
void multieval_prod(vector<Mat<ZZ_p>> &out, ZZ_p &k, std::function<void (Mat<ZZ_p>&, ZZ_p&)> A, ZZ &p){
    long m = out.size()-1;

    if(m == 1){
        ZZ_p one;
        one.init(p);
        one = 1;
        ZZ_p kone = k+1;
        A(out[0], one);
        A(out[1], kone);
        return;
    }

    vector<Mat<ZZ_p>> lower_layer(m/2+1);
    multieval_prod(lower_layer, k, A, p);

    vector<Mat<ZZ_p>> ll_extend(lower_layer.size());
    ZZ_p m21k = k*(m/2+1);
    shift_values(ll_extend, lower_layer, m21k, k, p);

    vector<Mat<ZZ_p>> ll_total;
    ll_total.reserve(lower_layer.size() + ll_extend.size());
    ll_total.insert(ll_total.end(), lower_layer.begin(), lower_layer.end());
    ll_total.insert(ll_total.end(), ll_extend.begin(), ll_extend.end());

    vector<Mat<ZZ_p>> ll_shift(ll_total.size());
    ZZ_p m2;
    m2.init(p);
    m2 = m/2;
    shift_values(ll_shift, ll_total, m2, k, p);

    for(long i = 0; i < out.size(); i++){
        mul(out[i], ll_shift[i], ll_total[i]);
    }
    if(m % 2 == 1){
        for(long i = 0; i < out.size(); i++){
            ZZ_p ikm = k*i + m;
            Mat<ZZ_p> extra;
            A(extra, ikm);
            mul(out[i], extra, out[i]);
        }
    }

}

/*
 * Calculate M(1)M(2)...M(n) mod p
 */
void matrix_factorial(Mat<ZZ_p> &out, long n, std::function<void (Mat<ZZ_p>&, ZZ_p&)> A, ZZ &p){
    long rtn = sqrt(n);

    vector<Mat<ZZ_p>> seg_prods(rtn+1);
    multieval_prod(seg_prods, A, p);

    if(n < rtn*rtn + rtn){
        for(long i = seg_prods.size()-2; i > 0; i--){
            mul(seg_prods[i-1], seg_prods[i], seg_prods[i-1]);
        }
        for(long i = rtn*rtn+1; i <= n; i++){
            Mat<ZZ_p> extra;
            ZZ_p x;
            x.init(p);
            x = i;
            A(extra, x);
            mul(seg_prods[0], extra, seg_prods[0]);
        }
    }
    else{
        for(long i = seg_prods.size()-1; i > 0; i--){
            mul(seg_prods[i-1], seg_prods[i], seg_prods[i-1]);
        }
        for(long i = rtn*rtn+rtn+1; i <= n; i++){
            Mat<ZZ_p> extra;
            ZZ_p x;
            x.init(p);
            x = i;
            A(extra, x);
            mul(seg_prods[0], extra, seg_prods[0]);
        }
    }
    out = seg_prods[0];
}


#include <boost/test/unit_test.hpp>
#include <k52/dsp/transform/inverse_fourier_transform.h>
#include <k52/dsp/transform/fourier_transform.h>

#include  <k52/common/constants.h>

#include <cmath>
#include <list>

#include "../boost_test_tools_extensions.h"

using k52::common::Constants;
using k52::dsp::FourierTransform;
using k52::dsp::IFourierTransform;
using k52::dsp::InverseFourierTransform;

struct InverseFourierTransformTestFixture
{
    InverseFourierTransformTestFixture() :
            ift(IFourierTransform::shared_ptr(new FourierTransform()))
    {}

    InverseFourierTransform ift;
    FourierTransform ft;
};

BOOST_FIXTURE_TEST_SUITE(inverse_fourier_transform_tests, InverseFourierTransformTestFixture);

void test_inverse_fourier_transform(
        const FourierTransform& ft,
        const InverseFourierTransform& ift,
        const std::vector< std::complex <double > >& samples)
{
    //Test
    std::vector< std::complex <double > > result = ift.Transform(ft.Transform(samples));

    //Check
    BOOST_REQUIRE_EQUAL(samples.size(), result.size());

    for (size_t n = 0; n < samples.size(); ++n)
    {
        CheckComplexEqual(samples[n], result[n]);
    }
}

BOOST_AUTO_TEST_CASE(zero)
{
    //Prepare
    std::vector< std::complex <double > > zeros(10);

    test_inverse_fourier_transform(ft, ift, zeros);
}

BOOST_AUTO_TEST_CASE(simple_impulse)
{
    //Prepare
    std::vector< std::complex <double > > impulse_samples(4);
    impulse_samples[0] = 1;

    test_inverse_fourier_transform(ft, ift, impulse_samples);
}

BOOST_AUTO_TEST_CASE(impulse)
{
    //Prepare
    std::vector< std::complex <double > > impulse_samples(11);
    impulse_samples[2] = 1;

    test_inverse_fourier_transform(ft, ift, impulse_samples);
}

BOOST_AUTO_TEST_CASE(ladder)
{
    std::vector< std::complex <double > > ladder_samples(9);
    for (size_t n = 0; n < ladder_samples.size(); ++n)
    {
        ladder_samples[n] = n;
    }
    test_inverse_fourier_transform(ft, ift, ladder_samples);
}

BOOST_AUTO_TEST_CASE(complex)
{
    std::vector< std::complex <double > > complex_samples(14);
    for (size_t n = 0; n < complex_samples.size(); ++n)
    {
        complex_samples[n] = (double)n * Constants::ImaginaryUnit;
    }
    test_inverse_fourier_transform(ft, ift, complex_samples);
}

BOOST_AUTO_TEST_SUITE_END();


/**
 * @file   work.cpp
 * @author ALIKAWA Hidehisa <alleyhide@gmail.com>
 * @date   2018/09/01

 *
 * @brief  for the experimental tests of gweyl
 *
 * Released under the MIT license
 */
#include <iostream>
#include <boost/assign.hpp>

#include "gweyl.hpp"

using namespace gweyl;

struct RepresentationTheory {
    IrreducibleRepresentation rep;
    std::vector<matrix> PositiveSingleRootVectorAction;
    std::vector<matrix> NegativeSingleRootVectorAction;
    bool calculated{false};
};

std::vector<RepresentationTheory> g_RepresentationTheory_A1;

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

    std::cout << "Hello gweyl!" << std::endl;

    try {
        // L(pi_1)
        NumberVector nv1(1);
        nv1(0) = 1;
        VectorRootSpace v1(Type::A, nv1, Coordinate::fundamental);
        IrreducibleRepresentation rep1(v1);

        int m = rep1.dimension();
        std::cout << "A1 pi_1 dimension " << std::to_string(m) << std::endl;

        RepresentationTheory repth1;
        repth1.rep = rep1;

        matrix E1p(m,m);
        E1p(0,1) = 1;
        repth1.PositiveSingleRootVectorAction.push_back(E1p);

        matrix E1m(m,m);
        E1p(1,0) = 1;
        repth1.NegativeSingleRootVectorAction.push_back(E1m);

        repth1.calculated = true;

        g_RepresentationTheory_A1.push_back(repth1);

    }catch (std::exception &e){
        std::cout << "Error\n what(): ";
        std::cout << e.what();
        std::cout << std::endl;
        return -1;
    }

    return 0;
}


#include <boost/config.hpp>
#include <boost/version.hpp>

#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
#include <iostream>
#include <fstream>

#include <CGAL/IO/WKT.h>

#include <CGAL/Simple_cartesian.h>
#include <CGAL/Exact_predicates_exact_constructions_kernel.h>

#include <vector>
#include <deque>

//typedef CGAL::Simple_cartesian<CGAL::Gmpq> Kernel;

typedef CGAL::Exact_predicates_exact_constructions_kernel Kernel;

int main(int argc, char* argv[])
{
  typedef CGAL::Polygon_with_holes_2<Kernel> Polygon;
  typedef std::deque<Polygon> MultiPolygon;

  {
    std::ifstream is((argc>1)?argv[1]:"data/polygons.wkt");
    std::list<Polygon> polys;
    do
      {
        Polygon p;
        CGAL::read_polygon_WKT(is, p);
        if(!p.outer_boundary().is_empty())
          polys.push_back(p);
      }while(is.good() && !is.eof());
    for(Polygon p : polys)
      std::cout<<p<<std::endl;
  }

  {
    std::ifstream  is((argc>2)?argv[2]:"data/multipolygon.wkt");
    MultiPolygon mp;
    CGAL::read_multi_polygon_WKT(is, mp);
    for(Polygon p : mp)
      std::cout<<p<<std::endl;
  }
  return 0;
}
#else
int main()
{
  return 0;
}
#endif


#include <boost/random/triangle_distribution.hpp>


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2020 Baidyanath Kundu, Haldia, India.

// Use, modification and distribution is 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)

//[to_wkt
//` Shows the usage of to_wkt

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>

int main()
{
    namespace geom = boost::geometry;
    typedef geom::model::d2::point_xy<double> point_type;

    point_type point = geom::make<point_type>(3, 2);
    geom::model::polygon<point_type> polygon;
    geom::append(geom::exterior_ring(polygon), geom::make<point_type>(0, 0));
    geom::append(geom::exterior_ring(polygon), geom::make<point_type>(0, 4));
    geom::append(geom::exterior_ring(polygon), geom::make<point_type>(4, 4));
    geom::append(geom::exterior_ring(polygon), geom::make<point_type>(4, 0));
    geom::append(geom::exterior_ring(polygon), geom::make<point_type>(0, 0));

    std::cout << boost::geometry::to_wkt(point) << std::endl;
    std::cout << boost::geometry::to_wkt(polygon) << std::endl;

    point_type point_frac = geom::make<point_type>(3.141592654, 27.18281828);
    geom::model::polygon<point_type> polygon_frac;
    geom::append(geom::exterior_ring(polygon_frac), geom::make<point_type>(0.00000, 0.00000));
    geom::append(geom::exterior_ring(polygon_frac), geom::make<point_type>(0.00000, 4.00001));
    geom::append(geom::exterior_ring(polygon_frac), geom::make<point_type>(4.00001, 4.00001));
    geom::append(geom::exterior_ring(polygon_frac), geom::make<point_type>(4.00001, 0.00000));
    geom::append(geom::exterior_ring(polygon_frac), geom::make<point_type>(0.00000, 0.00000));

    std::cout << boost::geometry::to_wkt(point_frac, 3) << std::endl;
    std::cout << boost::geometry::to_wkt(polygon_frac, 3) << std::endl;

    return 0;
}

//]

//[to_wkt_output
/*`
Output:
[pre
POINT(3 2)
POLYGON((0 0,0 4,4 4,4 0,0 0))
POINT(3.14 27.2)
POLYGON((0 0,0 4,4 4,4 0,0 0))
]

*/
//]


// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2018 www.open3d.org
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
// ----------------------------------------------------------------------------

#include "TransformationEstimation.h"

#include <Eigen/Geometry>
#include <Core/Geometry/PointCloud.h>
#include <Core/Utility/Eigen.h>

namespace open3d{

double TransformationEstimationPointToPoint::ComputeRMSE(
        const PointCloud &source, const PointCloud &target,
        const CorrespondenceSet &corres) const
{
    if (corres.empty()) return 0.0;
    double err = 0.0;
    for (const auto &c : corres) {
        err += (source.points_[c[0]] - target.points_[c[1]]).squaredNorm();
    }
    return std::sqrt(err / (double)corres.size());
}

Eigen::Matrix4d TransformationEstimationPointToPoint::ComputeTransformation(
        const PointCloud &source, const PointCloud &target,
        const CorrespondenceSet &corres) const
{
    if (corres.empty()) return Eigen::Matrix4d::Identity();
    Eigen::MatrixXd source_mat(3, corres.size());
    Eigen::MatrixXd target_mat(3, corres.size());
    for (size_t i = 0; i < corres.size(); i++) {
        source_mat.block<3, 1>(0, i) = source.points_[corres[i][0]];
        target_mat.block<3, 1>(0, i) = target.points_[corres[i][1]];
    }
    return Eigen::umeyama(source_mat, target_mat, with_scaling_);
}

double TransformationEstimationPointToPlane::ComputeRMSE(
        const PointCloud &source, const PointCloud &target,
        const CorrespondenceSet &corres) const
{
    if (corres.empty() || target.HasNormals() == false) return 0.0;
    double err = 0.0, r;
    for (const auto &c : corres) {
        r = (source.points_[c[0]] - target.points_[c[1]]).dot(
                target.normals_[c[1]]);
        err += r * r;
    }
    return std::sqrt(err / (double)corres.size());
}

Eigen::Matrix4d TransformationEstimationPointToPlane::ComputeTransformation(
        const PointCloud &source, const PointCloud &target,
        const CorrespondenceSet &corres) const
{
    if (corres.empty() || target.HasNormals() == false)
        return Eigen::Matrix4d::Identity();

    auto compute_jacobian_and_residual = [&]
            (int i, Eigen::Vector6d &J_r, double &r) {
        const Eigen::Vector3d &vs = source.points_[corres[i][0]];
        const Eigen::Vector3d &vt = target.points_[corres[i][1]];
        const Eigen::Vector3d &nt = target.normals_[corres[i][1]];
        r = (vs - vt).dot(nt);
        J_r.block<3, 1>(0, 0) = vs.cross(nt);
        J_r.block<3, 1>(3, 0) = nt;
    };

    Eigen::Matrix6d JTJ;
    Eigen::Vector6d JTr;
    std::tie(JTJ, JTr) = ComputeJTJandJTr<Eigen::Matrix6d, Eigen::Vector6d>(
            compute_jacobian_and_residual, (int)corres.size());

    bool is_success;
    Eigen::Matrix4d extrinsic;
    std::tie(is_success, extrinsic) =
            SolveJacobianSystemAndObtainExtrinsicMatrix(JTJ, JTr);

    return is_success ? extrinsic : Eigen::Matrix4d::Identity();
}

}    // namespace open3d


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

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

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/SVD>
#include <glog/logging.h>
#include <vector>

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

namespace theia {
using Eigen::Map;
using Eigen::Matrix;
using Eigen::Matrix3d;
using Eigen::Vector2d;
using Eigen::Vector3d;

namespace {

inline Matrix<double, 2, 9> CreateActionConstraint(const Vector2d& img1_point,
                                                   const Vector2d& img2_point) {
  Matrix<double, 2, 9> constraint;
  constraint << Eigen::RowVector3d::Zero(), -img1_point.transpose(), -1.0,
      img1_point.transpose() * img2_point.y(), img2_point.y(),
      img1_point.transpose(), 1.0, Eigen::RowVector3d::Zero(),
      -img1_point.transpose() * img2_point.x(), -img2_point.x();

  return constraint;
}

}  // namespace

// Normalized DLT method to compute the homography H that maps image points in
// image_1 to image_2 via x' = Hx (where x is in image 1 and x' is in image
// 2). The DLT algorithm implemented is from Algorithm 4.2 in Hartley and
// Zisserman (page 109).
bool FourPointHomography(const std::vector<Vector2d>& image_1_points,
                         const std::vector<Vector2d>& image_2_points,
                         Matrix3d* homography) {
  CHECK_GE(image_1_points.size(), 4);
  CHECK_EQ(image_1_points.size(), image_2_points.size());

  // Normalize the image points.
  std::vector<Vector2d> norm_image_1_points, norm_image_2_points;
  Matrix3d norm_image_1_mat, norm_image_2_mat;
  NormalizeImagePoints(image_1_points, &norm_image_1_points, &norm_image_1_mat);
  NormalizeImagePoints(image_2_points, &norm_image_2_points, &norm_image_2_mat);

  // Create the constraint matrix based on x' = Hx (Eq. 4.1 in Hartley and
  // Zisserman).
  Matrix<double, Eigen::Dynamic, 9> action_matrix(
      2 * image_1_points.size(), 9);
  for (int i = 0; i < image_1_points.size(); i++) {
    action_matrix.block<2, 9>(2 * i, 0) =
        CreateActionConstraint(norm_image_1_points[i], norm_image_2_points[i]);
  }

  const Matrix<double, 9, 1> null_vector =
      (action_matrix.transpose() * action_matrix).jacobiSvd(Eigen::ComputeFullV)
          .matrixV().rightCols<1>();

  *homography = norm_image_2_mat.inverse() *
                Eigen::Map<const Matrix3d>(null_vector.data()).transpose() *
                norm_image_1_mat;
  return true;
}

}  // namespace theia


#include "ros/ros.h"
#include "geometry_msgs/PoseStamped.h"
#include "nav_msgs/Path.h"
#include "geometry_msgs/Point32.h"
#include "sensor_msgs/PointCloud.h"
#include <opencv2/opencv.hpp>
#include <Eigen/Dense>
#include <chrono>
#include <iostream>
#include <vector>
using namespace std;

/**
 *  该文件是没有删除注释的最终版 camera-line-tracking
 *
 *  @author: panguoping
 *  @version: v1.0
 *
 *
 *
 * */

/** 参数定义 **/
const float fx = 760.4862674784594; // 相机内参
const float fy = 761.4971958529285;
const float cx = 631.6715834996345;
const float cy = 329.3054436037627;

const float y = 129.5585;   // 固定相机离地面高度，单位mm，通过计算得到，不一定准

float offset;
float thetaX;
float thetaY;
float scale;
int imgWidth;  // 图片大小
int imgHeight;
float up_rate;
float down_rate;
int sizeThres;

ros::Publisher pathpub;
ros::Publisher leftpoint;
ros::Publisher rightpoint;

/** 将图片中的 点uv 转化为 现实坐标中的 点xy **/
cv::Point3f getXYZ(cv::Point2f& point){
    cv::Point3f point3d;
    point3d.x = (fy * y/(point.y-cy))/scale;
    point3d.y = (-(point.x-cx)*point3d.x/fx);
    point3d.z = 0.0;
    // std::cout<<point3d<<std::endl;
    return point3d;
}

/** 将二维边线转化为三维边线 **/
void transTo3D(vector<cv::Point2f>& uv,vector<cv::Point3f>& posearray){
    for(auto p:uv){
        cv::Point3f pose = getXYZ(p);
        posearray.push_back(pose);
    }
}

/** 方法一：画出中间线 **/
void pubMiddleline(vector<cv::Point2f>& uvLeft,vector<cv::Point2f>& uvRight){
    vector<cv::Point3f> xyLeft,xyRight;
    sensor_msgs::PointCloud path;
    transTo3D(uvLeft,xyLeft);
    transTo3D(uvRight,xyRight);
//    std::cout<<xyLeft<<std::endl;
//    std::cout<<xyRight<<std::endl;

    // 选择最长size，将头部多出点去掉
    ROS_INFO("int two middle");
    bool lbigthanr = xyLeft.size() > xyRight.size() ? true:false;
    int ir = 0;
    int il = 0;
    if(lbigthanr){
        path.points.resize(xyLeft.size());
        while(il<xyLeft.size()&&abs(xyLeft[il].x-xyRight[ir].x)>thetaX){
            ++il;
        }
    } else{
        path.points.resize(xyRight.size());
        while(ir<xyRight.size()&&abs(xyLeft[il].x-uvRight[ir].x)>thetaX){
            ++ir;
        }
    }
    // 生成左右边线和中线
    int k = 0;
    sensor_msgs::PointCloud pcLeft;
    pcLeft.header.stamp = ros::Time::now();
    pcLeft.header.frame_id = "camera";
    pcLeft.points.resize(uvLeft.size());

    sensor_msgs::PointCloud pcRight;
    pcRight.header.stamp = ros::Time::now();
    pcRight.header.frame_id = "camera";
    pcRight.points.resize(uvRight.size());
    int i = 0;
    //size需要强制转换
    for(i,k,ir,il; il<int(xyLeft.size()-5)&&ir<int(uvRight.size()-5);++ir,++il){
        pcLeft.points[i].x = xyLeft[i].x;
        pcLeft.points[i].y = xyLeft[i].y;
        pcLeft.points[i].z = 0.0;
        pcRight.points[i].x = xyRight[i].x;
        pcRight.points[i].y = xyRight[i].y;
        pcRight.points[i].z = 0.0;
        ++i;
        if(abs(xyLeft[il].x-uvRight[ir].x)>thetaX   // 左右边线纵向距离不超过thetaX,否则舍弃
//           && abs(xyLeft[il+1].y-xyLeft[il].y)>thetaY
//           && abs(xyRight[ir+1].y-xyRight[ir].y)>thetaY
           )
            continue;
        ROS_INFO("yes");

        path.points[k].x = (xyLeft[il].x+xyRight[ir].x)/2;
        path.points[k].y = (xyLeft[il].y+xyRight[ir].y)/2;
        path.points[k].z = 0.0;
        ++k;

    }

    // 发布信息
    ROS_INFO("pathpose:%d",path.points.size());
    path.header.stamp = ros::Time::now();
    path.header.frame_id = "camera";
    pathpub.publish(path);
    leftpoint.publish(pcLeft);
    rightpoint.publish(pcRight);
    ROS_INFO("publish successfully");

}

/** 方法二：不舍弃纵向距离点，直接+、-偏移量移到中央位置**/
void pubMiddlelinePro(vector<cv::Point2f>& uvLeft,vector<cv::Point2f>& uvRight){
    // 1.将数据转化为三维坐标系下
    vector<cv::Point3f> xyLeft,xyRight;
    ROS_INFO("int two middle");
    transTo3D(uvLeft,xyLeft);
    transTo3D(uvRight,xyRight);

    // 2.建立左右点云

    sensor_msgs::PointCloud pcLeft;
    pcLeft.header.stamp = ros::Time::now();
    pcLeft.header.frame_id = "camera";
    pcLeft.points.resize(uvLeft.size());

    sensor_msgs::PointCloud pcRight;
    pcRight.header.stamp = ros::Time::now();
    pcRight.header.frame_id = "camera";
    pcRight.points.resize(uvRight.size());

    int size = uvLeft.size()>=uvRight.size() ? int(uvLeft.size()):int(uvRight.size());
    sensor_msgs::PointCloud path;
    path.points.resize(uvLeft.size()+uvRight.size());

    // 3.左中右往下走
    int r = 0;
    int l = 0;
    int m = 0;
    int pline = imgHeight * 2 / 3 - 10;
    for(int i = pline;i<imgHeight;++i){
        // ROS_INFO("%d",i);
        // float z = ((fy * y)/(i - cy))/scale;
        ROS_INFO("lz:%f rz:%f",xyLeft[l].x,xyRight[r].x);
        if(l<xyLeft.size()&&r<xyRight.size()&&xyLeft[l].x==xyRight[r].x){   //当两者相等时
            ROS_INFO("Equal!");
            path.points[m].x = xyRight[r].x;
            path.points[m].y = (xyLeft[l].y+xyRight[r].y)/2;
            path.points[m].z = 0.0;
            pcLeft.points[l].x = xyLeft[l].x;
            pcLeft.points[l].y = xyLeft[l].y;
            pcLeft.points[l].z = 0.0;
            pcRight.points[r].x = xyRight[r].x;
            pcRight.points[r].y = xyRight[r].y;
            pcRight.points[r].z = 0.0;
            ++l;
            ++r;
            ++m;
        }
        else if(xyRight[r].x > xyLeft[l].x){    //若右距离大于左
            ROS_INFO("r bigthan l");
            if(r<xyRight.size()){
                path.points[m].x = xyRight[r].x;
                path.points[m].y = xyRight[r].y - offset;
                path.points[m].z = 0.0;
                pcRight.points[r].x = xyRight[r].x;
                pcRight.points[r].y = xyRight[r].y;
                pcRight.points[r].z = 0.0;
                ++r;
                ++m;
            }
        }
        else if(xyLeft[l].x>xyRight[r].x){  //若左距离大于右
                ROS_INFO("l bigthan r");
                if(l<xyLeft.size()){
                path.points[m].x = xyLeft[l].x;
                path.points[m].y = xyLeft[l].y + offset;
                path.points[m].z = 0.0;
                pcLeft.points[l].x = xyLeft[l].x;
                pcLeft.points[l].y = xyLeft[l].y;
                pcLeft.points[l].z = 0.0;
                ++l;
                ++m;
            }
        }

    }

    ROS_INFO("pathpose:%d",path.points.size());
    path.header.stamp = ros::Time::now();
    path.header.frame_id = "camera";
    pathpub.publish(path);
    leftpoint.publish(pcLeft);
    rightpoint.publish(pcRight);
    ROS_INFO("publish successfully");

}

/** 配合方法一、二：处理单边情况 **/
void pubMiddleline(vector<cv::Point2f>& uv){
    vector<cv::Point3f> xy;
    sensor_msgs::PointCloud path;
    transTo3D(uv,xy);
    path.points.resize(xy.size());
    int i = 0;
    for(auto p:xy){
        if(p.y>0){
            path.points[i].x = p.x;
            path.points[i].y = p.y-offset;
            path.points[i].z = 0.0;
            ++i;
        }
        else if(p.y<0){
            path.points[i].x = p.x;
            path.points[i].y = p.y+offset;
            path.points[i].z = 0.0;
            ++i;
        }

    }
    ROS_INFO("pathpose:%d",path.points.size());
    path.header.stamp = ros::Time::now();
    path.header.frame_id = "camera";
    pathpub.publish(path);
    ROS_INFO("publish successfully");
}

/** 方法四：混合处理不分单双边线 **/
void pubMiddlelineCombine(vector<cv::Point2f>& uvLeft,vector<cv::Point2f>& uvRight){

    // 1.将数据转化为三维坐标系下
    vector<cv::Point3f> xyLeft,xyRight;
    ROS_INFO("int two middle");
    transTo3D(uvLeft,xyLeft);
    transTo3D(uvRight,xyRight);

    // 2.参数定义
    int r = 0;
    int l = 0;
    float max_distance = 0;
    sensor_msgs::PointCloud pc;
    pc.header.stamp = ros::Time::now();
    pc.header.frame_id = "camera";

    float lastparallel = 0; // 记录上一点的横向坐标
    bool init = 0;

    // 计算中线，只要有一边线的点都用完了就退出循环
    while (l<xyLeft.size() && r<xyRight.size())
    {
        // ROS_INFO("l: %d, r:%d",l,r);
        // 1.初始化
        if(!init){
            lastparallel =( xyLeft[l].y + xyRight[r].y )/2;
            init = 1;
            continue;
        }
        // 2.两边线纵向距离相等时
        if(xyLeft[l].x == xyRight[r].x){
            if(xyLeft[l].x > max_distance)
                max_distance = xyLeft[l].x;
            float tmp = ( xyLeft[l].y + xyRight[r].y )/2;
            if(abs(tmp - lastparallel)>0.1){// 与上一点横坐标的偏差
                l++;
                r++;
                continue;
            }
            geometry_msgs::Point32 p;
            p.x = xyLeft[l].x;
            p.y = tmp;
            p.z = 0.0;
            pc.points.push_back(p);
            l++;
            r++;
            lastparallel = tmp;
        }
        // 3.左边线纵向距离大于右边线
        else if(xyLeft[l].x > xyRight[r].x){
            if(xyLeft[l].x > max_distance)
                max_distance = xyLeft[l].x;
            float tmp = xyLeft[l].y - offset;
            // cout<<"LEFT:"<<tmp<<endl;

            if(abs(tmp - lastparallel)>0.1){
                l++;
                continue;
            }
            geometry_msgs::Point32 p;
            p.x = xyLeft[l].x;
            p.y = tmp;
            p.z = 0.0;
            pc.points.push_back(p);
            l++;
            lastparallel = tmp;
        }
        // 4.右边线纵向距离大于左边线
        else{
            if(xyRight[l].x > max_distance)
                max_distance = xyRight[l].x;
            // cout<<"RIGHT:"<<xyRight[r].y<<endl;
            float tmp = xyRight[r].y + offset;
            // cout<<"RIGHT:"<<tmp<<endl;
            if(abs(tmp - lastparallel)>0.1){
                r++;
                continue;
            }
            geometry_msgs::Point32 p;
            p.x = xyRight[r].x;
            p.y = tmp;
            p.z = 0.0;
            pc.points.push_back(p);
            r++;
            lastparallel = tmp;
        }

    }
    // ROS_INFO("OUT1");
    // 将另一边剩下的轨迹点加入
    // while (l<xyLeft.size())
    // {
    //     if(xyLeft[l].x > max_distance)
    //         max_distance = xyLeft[l].x;
    //     float tmp = xyLeft[l].y - offset;

    //     geometry_msgs::Point32 p;
    //     p.x = xyLeft[l].x;
    //     p.y = tmp;
    //     p.z = 0.0;
    //     pc.points.push_back(p);
    //     l++;
    // }

    // while (r<xyRight.size())
    // {
    //     if(xyRight[r].x > max_distance)
    //         max_distance = xyRight[r].x;
    //     // cout<<"RIGHT BEFORT:"<<xyRight[r].y<<endl;
    //     float tmp = xyRight[r].y + offset;
    //     // cout<<"RIGHT AFTER:"<<tmp<<endl;
    //     geometry_msgs::Point32 p;
    //     p.x = xyRight[r].x;
    //     p.y = tmp;
    //     p.z = 0.0;
    //     pc.points.push_back(p);
    //     r++;
    // }
    // ROS_INFO("OUT2");
    ROS_INFO("MAX DISTANCE %f",max_distance);

    pathpub.publish(pc);
}

/** 转换到lab色域 **/
void binaryFromLab(cv::Mat & frame,cv::Mat & binary){
    cv::Mat lab;
    cv::cvtColor(frame,lab,CV_RGB2Lab);
    cv::Mat lab_1;
    vector<cv::Mat> channels;
    cv::split(lab,channels);
    lab_1 = channels.at(1);
    double max;
    double min;
    cv::minMaxLoc(lab_1,&min,&max);
    float thres = max - min;
    for(int i = 0;i<frame.rows;++i){
        uchar* datalab = lab_1.ptr<uchar>(i);
        uchar* databinary = binary.ptr<uchar>(i);
        for(int j = 0;j<frame.cols;++j){
            if((datalab[j] - min)/thres<0.2){
                databinary[j] = 255;
            }
            else{
                databinary[j] = 0;
            }
        }
    }

}

/** cmyk色域 **/
void binaryFromCMYK(cv::Mat & frame,cv::Mat & binary){
    for(int i = 0;i<frame.rows;++i){
        uchar* dataframe = frame.ptr<uchar>(i);
        uchar* datagray = binary.ptr<uchar>(i);
        for(int j = 0;j<frame.cols;++j){
            uchar c = 255 - dataframe[j*3+2];
            uchar m = 255 - dataframe[j*3+2];
            uchar y = 255 - dataframe[j*3+2];
            uchar K = min(min(c,m),y);
            if(K == 255){
                datagray[j] = 0;
                continue;
            }
            uchar C = (uchar)((c - K)*255.0 / (255 - K));
            uchar M = (uchar)((m - K)*255.0 / (255 - K));
            uchar Y = (uchar)((y - K)*255.0 / (255 - K));
            if(Y>150){
                datagray[j] = 255;
            }
            else{
                datagray[j] = 0;
            }
        }
    }
}

int main(int argc ,char **argv){
    ros::init(argc,argv,"linktracking");
    ros::NodeHandle nh("~");
    pathpub = nh.advertise<sensor_msgs::PointCloud>("path",1);
    leftpoint = nh.advertise<sensor_msgs::PointCloud>("leftpath",1);
    rightpoint = nh.advertise<sensor_msgs::PointCloud>("rightpath",1);
    string video;
    nh.param<string>("video",video,"../dataset/test.mp4");
    nh.param<float>("offset",offset,0.45);
    nh.param<float>("thetaX",thetaX,0.2);
    nh.param<float>("thetaY",thetaY,0.1);
    nh.param<float>("scale",scale,1000.0);
    nh.param<int>("imgWidth",imgWidth,1280);
    nh.param<int>("imgHeight",imgHeight,720);
    nh.param<float>("up_rate",up_rate,0.5);
    nh.param<float>("down_rate",down_rate,0.9);
    nh.param<int>("size_thres",sizeThres,10);

    ROS_INFO("offset : %f",offset);
    ROS_INFO("thetaX : %f",thetaY);
    ROS_INFO("thetaY : %f",thetaX);
    ROS_INFO("scale : %f",scale);
    ROS_INFO("imgWidth : %d",imgWidth);
    ROS_INFO("imgHeight : %d",imgHeight);

    /** 1.获得视频数据 **/
    cv::VideoCapture cap = cv::VideoCapture(video);
    cap.set(CV_CAP_PROP_FRAME_WIDTH, imgWidth);
    cap.set(CV_CAP_PROP_FRAME_HEIGHT, imgHeight);
    cap.set(cv::CAP_PROP_FPS,30);

    ROS_INFO("%s",video.c_str());
    cv::Mat frame;
    cv::Mat gray;
    cv::Mat binary;

    if(!cap.isOpened()){
        ROS_INFO("Don't load the Image!");
        return 1;
    }

    while (ros::ok()&&cap.isOpened()) {
        /** 2.获得帧并二值化 **/
        cap.read(frame);
        if(!frame.data){
            ROS_INFO("Don't get this the frame");
            break;
        }

        // method 1:转到lab色域
        // cv::Mat binary(frame.rows,frame.cols, CV_8UC1);
        // binaryFromLab(frame,binary);
        // cv::dilate(binary,binary,11);

        // method 2:自适应二值化
        cv::cvtColor(frame, gray, CV_RGB2GRAY);
        cv::threshold(gray, binary, 150, 255, CV_THRESH_BINARY);
        cv::erode(binary, binary, cv::Mat(), cv::Point(-1, -1), 2);
        cv::dilate(binary,binary,11);

        // cv::Sobel(gray,binary,-1,0,1);
        //cv::erode(binary, binary, cv::Mat(), cv::Point(-1, -1), 2);

        // method 3:适应性二值化
        // cv::adaptiveThreshold(frame,binary,255,CV_ADAPTIVE_THRESH_MEAN_C,CV_THRESH_BINARY,11,2);
        // cv::erode(binary, binary, cv::Mat(), cv::Point(-1, -1), 2);
        // cv::imshow("show", binary);
        // cv::waitKey(0);

        /** 3.获得两边轨迹点 **/
        int vline = binary.cols / 2;
        int pline = binary.rows * 2 / 3 - 10;
        vector<cv::Point2f> uvRight;
        vector<cv::Point2f> uvLeft;
        // right side
        for (int i = int(imgHeight*up_rate); i < int(imgHeight*down_rate)-1; ++i) {
            uchar *data = binary.ptr<uchar>(i);
            for (int j = vline; j < binary.cols - 1; ++j) {
                if (data[j] == 0 && data[j + 1] == 255) {
                    data[j] = 150;
                    data[j+1] = 150;
                    data[j+2] = 150;
                    data[j+3] = 150;
                    data[j+4] = 150;
                    data[j+5] = 150;
                    data[j+6] = 150;
                    data[j+7] = 150;
                    data[j+8] = 150;
                    data[j+9] = 150;
                    data[j+10] = 150;
                    uvRight.push_back(cv::Point2f(j, i));
                    break;
                }
            }
        }
        // left side
        for (int i = int(imgHeight*up_rate); i < int(imgHeight*down_rate)-1; ++i) {
            uchar *data = binary.ptr<uchar>(i);
            for (int j = vline; j > 0; --j) {
                if (data[j] == 0 && data[j - 1] == 255) {
                    data[j] = 150;
                    data[j-1] = 150;
                    data[j-2] = 150;
                    data[j-3] = 150;
                    data[j-4] = 150;
                    data[j-5] = 150;
                    data[j-6] = 150;
                    data[j-7] = 150;
                    data[j-8] = 150;
                    data[j-9] = 150;
                    data[j-10] = 150;
                    uvLeft.push_back(cv::Point2f(j, i));
                    break;
                }
            }
        }
        // for (int i = pline; i < imgHeight; ++i) {
        //     uchar *data = binary.ptr<uchar>(i);
        //     for (int j = 0; j<imgWidth; ++j) {
        //         if (data[j] == 0 && data[j + 1] == 255) {
        //             data[j] = 150;
        //             data[j+1] = 150;
        //             data[j+2] = 150;
        //             data[j+3] = 150;
        //             data[j+4] = 150;
        //             data[j+5] = 150;
        //             data[j+6] = 150;
        //             data[j+7] = 150;
        //             data[j+8] = 150;
        //             data[j+9] = 150;
        //             data[j+10] = 150;
        //             uvLeft.push_back(cv::Point2f(j, i));
        //             break;
        //         }
        //     }
        // }
        cv::imshow("frame",binary);
        cv::waitKey(0);

        /** 4.绘制两边线 **/
        ROS_INFO("right.size = %d,left.size = %d",uvRight.size(),uvLeft.size());

        // if(uvLeft.size()>0&&uvRight.size()>0){
        //     pubMiddlelinePro(uvLeft,uvRight);
        // }
        // else if(uvLeft.size()>0){
        //     pubMiddleline(uvLeft);
        // }
        // else if(uvRight.size()>0){
        //     pubMiddleline(uvRight);
        // }
        if(uvRight.size()>sizeThres && uvLeft.size()>sizeThres)
            pubMiddlelineCombine(uvLeft,uvRight);
        else if(uvRight.size()>=uvLeft.size()&&uvRight.size()>sizeThres)
            pubMiddleline(uvRight);
        else if(uvLeft.size()>uvRight.size()&&uvLeft.size()>sizeThres)
            pubMiddleline(uvLeft);

    }
    cv::destroyAllWindows();
    cap.release();
    return 0;
}