/******************************
      Author: Joel Veness
        Date: 2011
******************************/

#include "ctw.hpp"

#include <vector>
#include <cassert>
#include <stack>
#include <iostream>
#include <cmath>

// boost includes
#include <boost/utility.hpp>

// enable both options below for better compression performance on text sources.
// disable both for vanilla CTW.

// do we use the zero redundancy estimator instead of the KT estimator?
static const bool UseZeroRedundancy = false;

// do we only perform weighting at byte boundaries in factored mode?
static const bool UseWeightingOnlyAtByteBoundaries = false;

// precompute some common logarithms
static const double log_point_five = std::log(0.5);
static const double log_quarter    = std::log(0.25);

/* create a new context node */
CTNode::CTNode() :
    m_log_prob_est(0.0),
    m_log_prob_weighted(0.0)
{
    m_count[0] = 0;    m_count[1] = 0;
    m_child[0] = NULL; m_child[1] = NULL;
}

/* update the weighted probabilities */
void CTNode::updateWeighted() {

    // computes P_w = log{0.5 * [P_kt + P_w0*P_w1]}
    double log_prob_on  = child(1) ? child(1)->logProbWeighted() : 0.0;
    double log_prob_off = child(0) ? child(0)->logProbWeighted() : 0.0;
    double log_one_plus_exp = log_prob_off + log_prob_on - logProbEstimated();

    // NOTE: no need to compute the log(1+e^x) if x is large, plus it avoids overflows
    if (log_one_plus_exp < 100.0) log_one_plus_exp = std::log(1.0 + std::exp(log_one_plus_exp));

    m_log_prob_weighted = log_point_five + logProbEstimated() + log_one_plus_exp;
}

/* process a new binary symbol */
void CTNode::update(bit_t b, bool skip) {

    // update the KT estimate and counts
    double log_kt_mul = logKTMul(b);
    m_log_prob_est += log_kt_mul;
    m_count[b]++;

    if (isLeaf()) {
        m_log_prob_weighted = logProbEstimated();
    } else {
        if (skip) {
            double log_prob_on  = child(1) ? child(1)->logProbWeighted() : 0.0;
            double log_prob_off = child(0) ? child(0)->logProbWeighted() : 0.0;
            m_log_prob_weighted = log_prob_on + log_prob_off;
        } else {
            updateWeighted();
        }
    }
}

/* is the current node a leaf node? */
bool CTNode::isLeaf() const {

    return child(0) == NULL && child(1) == NULL;
}

/* Krichevski-Trofimov estimated log probability accessor */
weight_t CTNode::logProbEstimated() const {

    if (UseZeroRedundancy) {
        if (m_count[0]+m_count[1] == 0) return 0.0;
        double rval = log_point_five + m_log_prob_est;
        if (m_count[0] == 0) rval = logAdd(log_quarter, rval);
        if (m_count[1] == 0) rval = logAdd(log_quarter, rval);
        return rval;
    }

    return m_log_prob_est;
}

/* logarithmic weighted probability estimate accessor */
weight_t CTNode::logProbWeighted() const {
    return m_log_prob_weighted;
}

/* child corresponding to a particular symbol */
const CTNode *CTNode::child(bit_t b) const {
    return m_child[b];
}

/* the number of times this context been visited */
int CTNode::visits() const {
    return m_count[0] + m_count[1];
}

/* compute the logarithm of the KT-estimator update multiplier */
double CTNode::logKTMul(bit_t b) const {

    static const double alpha = 0.5;
    static const double alpha2 = 2.0 * alpha;

    double kt_mul_numer = double(m_count[b]) + alpha;
    double kt_mul_denom = double(visits()) + alpha2;

    return std::log(kt_mul_numer / kt_mul_denom);
}

/* number of descendents of a node in the context tree */
size_t CTNode::size() const {

    size_t rval = 1;
    rval += child(0) ? child(0)->size() : 0;
    rval += child(1) ? child(1)->size() : 0;
    return rval;
}

/* create (if necessary) all of the nodes in the current context */
void ContextTree::createNodesInCurrentContext(const context_t &context) {

    CTNode **ctn = &m_root;

    for (size_t i = 0; i < context.size(); i++) {
        ctn = &((*ctn)->m_child[context[i]]);
        if (*ctn == NULL) {
            void *p = m_ctnode_pool.malloc();
            assert(p != NULL);  // TODO: make more robust
            *ctn = new (p) CTNode();
        }
    }
}

/* create a context tree of specified maximum depth and size */
ContextTree::ContextTree(history_t &history, size_t depth, int phase/*=-1*/) :
    m_ctnode_pool(sizeof(CTNode)),
    m_root(new (m_ctnode_pool.malloc()) CTNode()),
    m_phase(phase),
    m_depth(depth),
    m_history(history)
{
}

/* delete the context tree */
ContextTree::~ContextTree(void) {
    deleteCT(m_root);
}

/* recursively deletes the nodes in a context tree */
void ContextTree::deleteCT(CTNode *n) {

    if (n == NULL) return;

    if (n->m_child[0] != NULL) deleteCT(n->m_child[0]);
    if (n->m_child[1] != NULL) deleteCT(n->m_child[1]);

    m_ctnode_pool.free(n);
}

/* compute the current binary context */
void ContextTree::getContext(const history_t &h, context_t &context) const {

    context.clear();
    for (size_t i=0; i < m_depth; ++i) {
        context.push_back(h[h.size()-i-1]);
    }
}

/* updates the context tree with a single bit */
void ContextTree::update(bit_t b) {

    // compute the current context
    context_t context;
    context.reserve(m_depth);
    getContext(m_history, context);

    // 1. create new nodes in the context tree (if necessary)
    createNodesInCurrentContext(context);

    // 2. walk down the tree to the relevant leaf, saving the path as we go
    std::stack<CTNode *, std::vector<CTNode *> > path;
    path.push(m_root); // add the empty context
    CTNode *ctn = m_root;
    for (size_t i = 0; i < context.size(); i++) {
        ctn = ctn->m_child[context[i]];
        path.push(ctn);
    }

    // 3. update the probability estimates from the leaf node back up to the root
    int index = static_cast<int>(m_depth);
    for (; !path.empty(); path.pop()) {
        bool skip = UseWeightingOnlyAtByteBoundaries && m_phase > -1 &&
                    (index % 8) != m_phase && index != 0;
        path.top()->update(b, skip);
        index--;
    }

    // 4. update the history
    m_history.push_back(b != 0);
}

/* the probability of seeing a particular symbol next */
double ContextTree::prob(bit_t b) {

    typedef std::pair<CTNode *, CTNode> ctpair_t;

    double before = logBlockProbability();

    // compute the current context
    context_t context;
    getContext(m_history, context);

    // 1. record newly added or modified nodes
    std::vector<CTNode *> created;
    std::vector<ctpair_t> modified;

    CTNode **ctnp = &m_root;
    modified.push_back(ctpair_t(m_root, *m_root));
    for (size_t i = 0; i < context.size(); i++) {
        ctnp = &((*ctnp)->m_child[context[i]]);
        if (*ctnp == NULL) {
            void *p = m_ctnode_pool.malloc();
            assert(p != NULL);  // TODO: make more robust
            *ctnp = new (p) CTNode();
            created.push_back(*ctnp);
        } else {
            modified.push_back(ctpair_t(*ctnp, **ctnp));
        }
    }

    // 2. walk down the tree to the relevant leaf, saving the path as we go
    std::stack<CTNode *, std::vector<CTNode *> > path;
    path.push(m_root); // add the empty context
    CTNode *ctn = m_root;
    for (size_t i = 0; i < context.size(); i++) {
        ctn = ctn->m_child[context[i]];
        path.push(ctn);
    }

    // 3. update the probability estimates from the leaf node back up to the root
    int index = static_cast<int>(m_depth);
    for (; !path.empty(); path.pop()) {
        bool skip = UseWeightingOnlyAtByteBoundaries && m_phase > -1 &&
                    (index % 8) != m_phase && index != 0;
        path.top()->update(b, skip);
        index--;
    }

    double rval = std::exp(logBlockProbability() - before);

    // now revert the changes
    for (size_t i=0; i < created.size(); ++i) m_ctnode_pool.free(created[i]);
    for (size_t i=0; i < modified.size(); ++i) *modified[i].first = modified[i].second;

    return rval;
}

/* the depth of the context tree */
size_t ContextTree::depth() const {

    return m_depth;
}

/* number of nodes in the context tree */
size_t ContextTree::size(void) const {

    return m_root->size();
}

/* recover the memory used by a node */
void ContextTree::reclaimMemory(CTNode *n) {

    m_ctnode_pool.free(n);
}

/* the logarithm of the block probability of the whole sequence */
double ContextTree::logBlockProbability(void) const {

    return m_root->logProbWeighted();
}


#include <vector>
#include <cmath>
#include <boost/python.hpp>
#include <boost/python/numpy.hpp>

namespace bp = boost::python;
namespace np = boost::python::numpy;

class rawor{
    double nbar_ran, Delta_r, r_max, r_min, V_box;
    int N_parts, N_rans, N_shells;
    std::vector<double> rs, w, x;

    void initVectors();

    void swapIfGreater(double &a, double &b);

    double sphereOverlapVolume(double d, double R, double r);

    double crossSectionVolume(double r1, double r2, double r3);

    int getPermutations(double r1, double r2, double r3);

    double sphericalShellVolume(double r);

    double nbarData(unsigned long *DD, double r, double r1);

    double gaussQuadCrossSection(double r1, double r2, double r3);

    double gaussQuadCrossSectionDDR(unsigned long *DD, double r1, double r2, double r3);

    public:
        rawor(int numParticles, int numRandoms, int numShells, double VolBox, double rMax, double rMin = 0);

        void setNumParts(int numParticles);

        void setNumRans(int numRandoms);

        void setNumShells(int numShells);

        void setRMax(double rMax);

        void setRMin(double rMin);

        void setVBox(double VBox);

        int getNumParts();

        int getNumRans();

        int getNumShells();

        double getRMax();

        double getRMin();

        double getVBox();

        np::ndarray getRRR();

        np::ndarray getDRR();

        np::ndarray getDDR(np::ndarray const &dd);
};

void rawor::initVectors() {
    for (int i = 0; i < rawor::N_shells; ++i) {
        rawor::rs.push_back(rawor::r_min + (i + 0.5)*rawor::Delta_r);
    }

    rawor::w = {0.8888888888888888, 0.5555555555555556, 0.5555555555555556};

    rawor::x = {0.0000000000000000, -0.7745966692414834, 0.7745966692414834};
}

void rawor::swapIfGreater(double &a, double &b) {
    if (a > b) {
        double temp = a;
        a = b;
        b = temp;
    }
}

double rawor::sphereOverlapVolume(double d, double R, double r) {
    double V = 0;
    swapIfGreater(r, R);
    if (d < R + r) {
        if (d > R - r) {
            V = (M_PI*(R + r - d)*(R + r - d)*(d*d + 2.0*d*r - 3.0*r*r + 2.0*d*R + 6.0*r*R - 3.0*R*R))/(12.0*d);
        } else {
            V = (4.0*M_PI/3.0)*r*r*r;
        }
    }
    return V;
}

double rawor::crossSectionVolume(double r1, double r2, double r3) {
    double V_oo = sphereOverlapVolume(r1, r3 + 0.5*rawor::Delta_r, r2 + 0.5*rawor::Delta_r);
    double V_oi = sphereOverlapVolume(r1, r3 + 0.5*rawor::Delta_r, r2 - 0.5*rawor::Delta_r);
    double V_io = sphereOverlapVolume(r1, r3 - 0.5*rawor::Delta_r, r2 + 0.5*rawor::Delta_r);
    double V_ii = sphereOverlapVolume(r1, r3 - 0.5*rawor::Delta_r, r2 - 0.5*rawor::Delta_r);

    return V_oo - V_oi - V_io + V_ii;
}

int rawor::getPermutations(double r1, double r2, double r3) {
    int perm = 1;
    if (r1 != r2 && r1 != r3 && r2 != r3) {
        perm = 6;
    } else if ((r1 == r2 && r1 != r3) || (r1 == r3 && r1 != r2) || (r2 == r3 && r2 != r1)) {
        perm = 3;
    }
    return perm;
}

double rawor::sphericalShellVolume(double r) {
    double r_o = r + 0.5*rawor::Delta_r;
    double r_i = r - 0.5*rawor::Delta_r;
    return 4.0*M_PI*(r_o*r_o*r_o - r_i*r_i*r_i)/3.0;
}

double rawor::nbarData(unsigned long *DD, double r, double r1) {
    int bin = r/rawor::Delta_r;
    double nbar = DD[bin]/(rawor::N_parts*sphericalShellVolume(r1));
    int num_bins = rawor::N_shells;
    if (r <= (bin + 0.5)*rawor::Delta_r) {
        if (bin != 0) {
            double n1 = DD[bin]/(rawor::N_parts*sphericalShellVolume(r1));
            double n2 = DD[bin - 1]/(rawor::N_parts*sphericalShellVolume(r1 - rawor::Delta_r));
            double b = n1 - ((n1 - n2)/rawor::Delta_r)*r1;
            nbar = ((n1 - n2)/rawor::Delta_r)*r + b;
        } else {
            double n1 = DD[bin]/(rawor::N_parts*sphericalShellVolume(r1));
            double n2 = DD[bin + 1]/(rawor::N_parts*sphericalShellVolume(r1 + rawor::Delta_r));
            double b = n1 - ((n2 - n1)/rawor::Delta_r)*r1;
            nbar = ((n2 - n1)/rawor::Delta_r)*r + b;
        }
    } else {
        if (bin != num_bins - 1) {
            double n1 = DD[bin]/(rawor::N_parts*sphericalShellVolume(r1));
            double n2 = DD[bin + 1]/(rawor::N_parts*sphericalShellVolume(r1 + rawor::Delta_r));
            double b = n1 - ((n2 - n1)/rawor::Delta_r)*r1;
            nbar = ((n2 - n1)/rawor::Delta_r)*r + b;
        } else {
            double n1 = DD[bin]/(rawor::N_parts*sphericalShellVolume(r1));
            double n2 = DD[bin - 1]/(rawor::N_parts*sphericalShellVolume(r1 - rawor::Delta_r));
            double b = n1 - ((n1 - n2)/rawor::Delta_r)*r1;
            nbar = ((n1 - n2)/rawor::Delta_r)*r + b;
        }
    }
    return nbar;
}

double rawor::gaussQuadCrossSection(double r1, double r2, double r3) {
    double result = 0.0;
    for (int i = 0; i < rawor::w.size(); ++i) {
        double r_1 = r1 + 0.5*rawor::Delta_r*rawor::x[i];
        result += 0.5*rawor::Delta_r*rawor::w[i]*crossSectionVolume(r_1, r2, r3)*r_1*r_1;
    }
    return result;
}

double rawor::gaussQuadCrossSectionDDR(unsigned long *DD, double r1, double r2, double r3) {
    double result = 0.0;
    for (int i = 0; i < rawor::w.size(); ++i) {
        double r_1 = r1 + 0.5*rawor::Delta_r*rawor::x[i];
        double nbar = nbarData(DD, r_1, r1);
        result += 0.5*rawor::Delta_r*rawor::w[i]*crossSectionVolume(r_1, r2, r3)*r_1*r_1*nbar;
    }
    return result;
}

rawor::rawor(int numParticles, int numRandoms, int numShells, double VolBox, double rMax, double rMin) {
    rawor::N_parts = numParticles;
    rawor::N_rans = numRandoms;
    rawor::N_shells = numShells;
    rawor::r_max = rMax;
    rawor::r_min = rMin;
    rawor::V_box = VolBox;
    rawor::Delta_r = (rMax - rMin)/numShells;
    rawor::nbar_ran = numRandoms/VolBox;
    rawor::initVectors();
}

void rawor::setNumParts(int numParticles) {
    rawor::N_parts = numParticles;
}

void rawor::setNumRans(int numRandoms) {
    rawor::N_rans = numRandoms;
    rawor::nbar_ran = numRandoms/rawor::V_box;
}

void rawor::setNumShells(int numShells) {
    rawor::N_shells = numShells;
    rawor::Delta_r = (rawor::r_max - rawor::r_min)/rawor::N_shells;
}

void rawor::setRMax(double rMax) {
    rawor::r_max = rMax;
    rawor::Delta_r = (rawor::r_max - rawor::r_min)/rawor::N_shells;
}

void rawor::setRMin(double rMin) {
    rawor::r_min = rMin;
    rawor::Delta_r = (rawor::r_max - rawor::r_min)/rawor::N_shells;
}

void rawor::setVBox(double VBox) {
    rawor::V_box = VBox;
    rawor::nbar_ran = rawor::N_rans/rawor::V_box;
}

int rawor::getNumParts() {
    return rawor::N_parts;
}

int rawor::getNumRans() {
    return rawor::N_rans;
}

int rawor::getNumShells() {
    return rawor::N_shells;
}

double rawor::getVBox() {
    return rawor::V_box;
}

double rawor::getRMin() {
    return rawor::r_min;
}

double rawor::getRMax() {
    return rawor::r_max;
}

np::ndarray rawor::getRRR() {
    std::vector<int> N;
    for (int i = 0; i < rawor::N_shells; ++i) {
        for (int j = i; j < rawor::N_shells; ++j) {
            for (int k = j; k < rawor::N_shells; ++k) {
                if (rawor::rs[k] <= rawor::rs[i] + rawor::rs[j]) {
                    int index = k + rawor::N_shells*(j + rawor::N_shells*i);
                    double V = rawor::gaussQuadCrossSection(rawor::rs[i], rawor::rs[j], rawor::rs[k]);
                    int n_perm = rawor::getPermutations(rawor::rs[i], rawor::rs[j], rawor::rs[k]);
                    N.push_back(int(4.0*M_PI*n_perm*rawor::nbar_ran*rawor::nbar_ran*V*rawor::N_rans));
                }
            }
        }
    }
    np::dtype dt = np::dtype::get_builtin<int>();
    np::ndarray n = np::zeros(bp::make_tuple(N.size()), dt);
    std::copy(N.begin(), N.end(), reinterpret_cast<int*>(n.get_data()));
    return n;
}

np::ndarray rawor::getDRR() {
    std::vector<int> N;
    for (int i = 0; i < rawor::N_shells; ++i) {
        for (int j = i; j < rawor::N_shells; ++j) {
            for (int k = j; k < rawor::N_shells; ++k) {
                if (rawor::rs[k] <= rawor::rs[i] + rawor::rs[j]) {
                    int index = k + rawor::N_shells*(j + rawor::N_shells*i);
                    double V = rawor::gaussQuadCrossSection(rawor::rs[i], rawor::rs[j], rawor::rs[k]);
                    int n_perm = rawor::getPermutations(rawor::rs[i], rawor::rs[j], rawor::rs[k]);
                    N.push_back(int(4.0*M_PI*n_perm*rawor::nbar_ran*rawor::nbar_ran*V*rawor::N_parts));
                }
            }
        }
    }
    np::dtype dt = np::dtype::get_builtin<int>();
    np::ndarray n = np::zeros(bp::make_tuple(N.size()), dt);
    std::copy(N.begin(), N.end(), reinterpret_cast<int*>(n.get_data()));
    return n;
}

np::ndarray rawor::getDDR(np::ndarray const &dd) {
    unsigned long *DD = reinterpret_cast<unsigned long *>(dd.get_data());
    std::vector<int> N;
    for (int i = 0; i < rawor::N_shells; ++i) {
        double r1 = rawor::rs[i];
        for (int j = i; j < rawor::N_shells; ++j) {
            double r2 = rawor::rs[j];
            for (int k = j; k < rawor::N_shells; ++k) {
                double r3 = rawor::rs[k];
                if (rawor::rs[k] <= rawor::rs[i] + rawor::rs[j]) {
                   int index = k + rawor::N_shells*(j + rawor::N_shells*i);
                   double V = rawor::gaussQuadCrossSectionDDR(DD, r1, r2, r3);
                   double N_temp = 4.0*M_PI*rawor::nbar_ran*V*rawor::N_parts;
                   if (r1 != r2 && r1 != r3 && r2 != r3) {
                       V = rawor::gaussQuadCrossSectionDDR(DD, r2, r3, r1);
                       N_temp += 4.0*M_PI*rawor::nbar_ran*V*rawor::N_parts;
                       V = rawor::gaussQuadCrossSectionDDR(DD, r3, r1, r2);
                       N_temp += 4.0*M_PI*rawor::nbar_ran*V*rawor::N_parts;
                       V = rawor::gaussQuadCrossSectionDDR(DD, r1, r3, r2);
                       N_temp += 4.0*M_PI*rawor::nbar_ran*V*rawor::N_parts;
                       V = rawor::gaussQuadCrossSectionDDR(DD, r2, r1, r3);
                       N_temp += 4.0*M_PI*rawor::nbar_ran*V*rawor::N_parts;
                       V = rawor::gaussQuadCrossSectionDDR(DD, r3, r2, r1);
                       N_temp += 4.0*M_PI*rawor::nbar_ran*V*rawor::N_parts;
                   } else if ((r1 == r2 && r1 != r3) || (r1 == r3 && r1 != r2) || (r2 == r3 && r2 != r1)) {
                       V = rawor::gaussQuadCrossSectionDDR(DD, r2, r3, r1);
                       N_temp += 4.0*M_PI*rawor::nbar_ran*V*rawor::N_parts;
                       V = rawor::gaussQuadCrossSectionDDR(DD, r3, r1, r2);
                       N_temp += 4.0*M_PI*rawor::nbar_ran*V*rawor::N_parts;
                   }
                   N.push_back(int(floor(N_temp + 0.5)));
                }
            }
        }
    }
    np::dtype dt = np::dtype::get_builtin<int>();
    np::ndarray n = np::zeros(bp::make_tuple(N.size()), dt);
    std::copy(N.begin(), N.end(), reinterpret_cast<int*>(n.get_data()));
    return n;
}

BOOST_PYTHON_MODULE(rawor) {
    np::initialize();
    using namespace boost::python;

    class_<rawor>("rawor", init<int, int, int, double, double, double>())
        .def("set_num_parts", &rawor::setNumParts)
        .def("set_num_rans", &rawor::setNumRans)
        .def("set_num_shells", &rawor::setNumShells)
        .def("set_r_max", &rawor::setRMax)
        .def("set_r_min", &rawor::setRMin)
        .def("set_V_box", &rawor::setVBox)
        .def("get_num_parts", &rawor::getNumParts)
        .def("get_num_rans", &rawor::getNumRans)
        .def("get_num_shells", &rawor::getNumShells)
        .def("get_V_box", &rawor::getVBox)
        .def("get_r_min", &rawor::getRMin)
        .def("get_r_max", &rawor::getRMax)
        .def("get_RRR", &rawor::getRRR)
        .def("get_DRR", &rawor::getDRR)
        .def("get_DDR", &rawor::getDDR)
    ;
}


#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/SVD>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <chrono>

using namespace std;
using namespace cv;

void find_feature_matches (
    const Mat& img_1, const Mat& img_2,
    std::vector<KeyPoint>& keypoints_1,
    std::vector<KeyPoint>& keypoints_2,
    std::vector< DMatch >& matches );

// 像素坐标转相机归一化坐标
Point2d pixel2cam ( const Point2d& p, const Mat& K );

void pose_estimation_3d3d (
    const vector<Point3f>& pts1,
    const vector<Point3f>& pts2,
    Mat& R, Mat& t
);

void bundleAdjustment(
    const vector<Point3f>& points_3d,
    const vector<Point3f>& points_2d,
    Mat& R, Mat& t
);

// g2o edge
class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    EdgeProjectXYZRGBDPoseOnly( const Eigen::Vector3d& point ) : _point(point) {}

    virtual void computeError()
    {
        const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*> ( _vertices[0] );
        // measurement is p, point is p'
        _error = _measurement - pose->estimate().map( _point );
    }

    virtual void linearizeOplus()
    {
        g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap *>(_vertices[0]);
        g2o::SE3Quat T(pose->estimate());
        Eigen::Vector3d xyz_trans = T.map(_point);
        double x = xyz_trans[0];
        double y = xyz_trans[1];
        double z = xyz_trans[2];

        _jacobianOplusXi(0,0) = 0;
        _jacobianOplusXi(0,1) = -z;
        _jacobianOplusXi(0,2) = y;
        _jacobianOplusXi(0,3) = -1;
        _jacobianOplusXi(0,4) = 0;
        _jacobianOplusXi(0,5) = 0;

        _jacobianOplusXi(1,0) = z;
        _jacobianOplusXi(1,1) = 0;
        _jacobianOplusXi(1,2) = -x;
        _jacobianOplusXi(1,3) = 0;
        _jacobianOplusXi(1,4) = -1;
        _jacobianOplusXi(1,5) = 0;

        _jacobianOplusXi(2,0) = -y;
        _jacobianOplusXi(2,1) = x;
        _jacobianOplusXi(2,2) = 0;
        _jacobianOplusXi(2,3) = 0;
        _jacobianOplusXi(2,4) = 0;
        _jacobianOplusXi(2,5) = -1;
    }

    bool read ( istream& in ) {}
    bool write ( ostream& out ) const {}
protected:
    Eigen::Vector3d _point;
};

int main ( int argc, char** argv )
{
    if ( argc != 5 )
    {
        cout<<"usage: pose_estimation_3d3d img1 img2 depth1 depth2"<<endl;
        return 1;
    }
    //-- 读取图像
    Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );
    Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );

    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;
    find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );
    cout<<"一共找到了"<<matches.size() <<"组匹配点"<<endl;

    // 建立3D点
    Mat depth1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED );       // 深度图为16位无符号数，单通道图像
    Mat depth2 = imread ( argv[4], CV_LOAD_IMAGE_UNCHANGED );       // 深度图为16位无符号数，单通道图像
    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
    vector<Point3f> pts1, pts2;

    for ( DMatch m:matches )
    {
        ushort d1 = depth1.ptr<unsigned short> ( int ( keypoints_1[m.queryIdx].pt.y ) ) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
        ushort d2 = depth2.ptr<unsigned short> ( int ( keypoints_2[m.trainIdx].pt.y ) ) [ int ( keypoints_2[m.trainIdx].pt.x ) ];
        if ( d1==0 || d2==0 )   // bad depth
            continue;
        Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
        Point2d p2 = pixel2cam ( keypoints_2[m.trainIdx].pt, K );
        float dd1 = float ( d1 ) /1000.0;
        float dd2 = float ( d2 ) /1000.0;
        pts1.push_back ( Point3f ( p1.x*dd1, p1.y*dd1, dd1 ) );
        pts2.push_back ( Point3f ( p2.x*dd2, p2.y*dd2, dd2 ) );
    }

    cout<<"3d-3d pairs: "<<pts1.size() <<endl;
    Mat R, t;
    pose_estimation_3d3d ( pts1, pts2, R, t );
    cout<<"ICP via SVD results: "<<endl;
    cout<<"R = "<<R<<endl;
    cout<<"t = "<<t<<endl;
    cout<<"R_inv = "<<R.t() <<endl;
    cout<<"t_inv = "<<-R.t() *t<<endl;

    cout<<"calling bundle adjustment"<<endl;

    bundleAdjustment( pts1, pts2, R, t );

    // verify p1 = R*p2 + t
    for ( int i=0; i<5; i++ )
    {
        cout<<"p1 = "<<pts1[i]<<endl;
        cout<<"p2 = "<<pts2[i]<<endl;
        cout<<"(R*p2+t) = "<<
            R * (Mat_<double>(3,1)<<pts2[i].x, pts2[i].y, pts2[i].z) + t
            <<endl;
        cout<<endl;
    }
}

void find_feature_matches ( const Mat& img_1, const Mat& img_2,
                            std::vector<KeyPoint>& keypoints_1,
                            std::vector<KeyPoint>& keypoints_2,
                            std::vector< DMatch >& matches )
{
    //-- 初始化
    Mat descriptors_1, descriptors_2;
    // used in OpenCV3
    Ptr<FeatureDetector> detector = ORB::create();
    Ptr<DescriptorExtractor> descriptor = ORB::create();
    // use this if you are in OpenCV2
    // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
    // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
    Ptr<DescriptorMatcher> matcher  = DescriptorMatcher::create("BruteForce-Hamming");
    //-- 第一步:检测 Oriented FAST 角点位置
    detector->detect ( img_1,keypoints_1 );
    detector->detect ( img_2,keypoints_2 );

    //-- 第二步:根据角点位置计算 BRIEF 描述子
    descriptor->compute ( img_1, keypoints_1, descriptors_1 );
    descriptor->compute ( img_2, keypoints_2, descriptors_2 );

    //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    vector<DMatch> match;
   // BFMatcher matcher ( NORM_HAMMING );
    matcher->match ( descriptors_1, descriptors_2, match );

    //-- 第四步:匹配点对筛选
    double min_dist=10000, max_dist=0;

    //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        double dist = match[i].distance;
        if ( dist < min_dist ) min_dist = dist;
        if ( dist > max_dist ) max_dist = dist;
    }

    printf ( "-- Max dist : %f \n", max_dist );
    printf ( "-- Min dist : %f \n", min_dist );

    //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
    for ( int i = 0; i < descriptors_1.rows; i++ )
    {
        if ( match[i].distance <= max ( 2*min_dist, 30.0 ) )
        {
            matches.push_back ( match[i] );
        }
    }
}

Point2d pixel2cam ( const Point2d& p, const Mat& K )
{
    return Point2d
           (
               ( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ),
               ( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 )
           );
}

void pose_estimation_3d3d (
    const vector<Point3f>& pts1,
    const vector<Point3f>& pts2,
    Mat& R, Mat& t
)
{
    Point3f p1, p2;     // center of mass
    int N = pts1.size();
    for ( int i=0; i<N; i++ )
    {
        p1 += pts1[i];
        p2 += pts2[i];
    }
    p1 = Point3f( Vec3f(p1) /  N);
    p2 = Point3f( Vec3f(p2) / N);
    vector<Point3f>     q1 ( N ), q2 ( N ); // remove the center
    for ( int i=0; i<N; i++ )
    {
        q1[i] = pts1[i] - p1;
        q2[i] = pts2[i] - p2;
    }

    // compute q1*q2^T
    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for ( int i=0; i<N; i++ )
    {
        W += Eigen::Vector3d ( q1[i].x, q1[i].y, q1[i].z ) * Eigen::Vector3d ( q2[i].x, q2[i].y, q2[i].z ).transpose();
    }
    cout<<"W="<<W<<endl;

    // SVD on W
    Eigen::JacobiSVD<Eigen::Matrix3d> svd ( W, Eigen::ComputeFullU|Eigen::ComputeFullV );
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();
    cout<<"U="<<U<<endl;
    cout<<"V="<<V<<endl;

    Eigen::Matrix3d R_ = U* ( V.transpose() );
    Eigen::Vector3d t_ = Eigen::Vector3d ( p1.x, p1.y, p1.z ) - R_ * Eigen::Vector3d ( p2.x, p2.y, p2.z );

    // convert to cv::Mat
    R = ( Mat_<double> ( 3,3 ) <<
          R_ ( 0,0 ), R_ ( 0,1 ), R_ ( 0,2 ),
          R_ ( 1,0 ), R_ ( 1,1 ), R_ ( 1,2 ),
          R_ ( 2,0 ), R_ ( 2,1 ), R_ ( 2,2 )
        );
    t = ( Mat_<double> ( 3,1 ) << t_ ( 0,0 ), t_ ( 1,0 ), t_ ( 2,0 ) );
}

void bundleAdjustment (
    const vector< Point3f >& pts1,
    const vector< Point3f >& pts2,
    Mat& R, Mat& t )
{
    // 初始化g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  // pose维度为 6, landmark 维度为 3
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverEigen<Block::PoseMatrixType>(); // 线性方程求解器
    Block* solver_ptr = new Block( linearSolver );      // 矩阵块求解器
    g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm( solver );

    // vertex
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose
    pose->setId(0);
    pose->setEstimate( g2o::SE3Quat(
        Eigen::Matrix3d::Identity(),
        Eigen::Vector3d( 0,0,0 )
    ) );
    optimizer.addVertex( pose );

    // edges
    int index = 1;
    vector<EdgeProjectXYZRGBDPoseOnly*> edges;
    for ( size_t i=0; i<pts1.size(); i++ )
    {
        EdgeProjectXYZRGBDPoseOnly* edge = new EdgeProjectXYZRGBDPoseOnly(
            Eigen::Vector3d(pts2[i].x, pts2[i].y, pts2[i].z) );
        edge->setId( index );
        edge->setVertex( 0, dynamic_cast<g2o::VertexSE3Expmap*> (pose) );
        edge->setMeasurement( Eigen::Vector3d(
            pts1[i].x, pts1[i].y, pts1[i].z) );
        edge->setInformation( Eigen::Matrix3d::Identity()*1e4 );
        optimizer.addEdge(edge);
        index++;
        edges.push_back(edge);
    }

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.setVerbose( true );
    optimizer.initializeOptimization();
    optimizer.optimize(10);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2-t1);
    cout<<"optimization costs time: "<<time_used.count()<<" seconds."<<endl;

    cout<<endl<<"after optimization:"<<endl;
    cout<<"T="<<endl<<Eigen::Isometry3d( pose->estimate() ).matrix()<<endl;

}


/*
	[Vc,Fc] = cut_mesh_mex(V,F);
    V, F: vertices/faces of input mesh (any topology, with/without boundary)
    Vc, Fc: vertices/faces of cut mesh
*/

#include <iostream>
#include <stdlib.h>     /* srand, rand */

#include "mex.h"
#include <Eigen/Core>
#include <Eigen/Sparse>

#include <igl/matlab_format.h>
#include <igl/adjacency_matrix.h>
#include <igl/boundary_loop.h>
#include "polyvector_field_cut_mesh_with_singularities_randomized.h"
#include <igl/cut_mesh.h>
#include <igl/euler_characteristic.h>

using namespace std;

void mexFunction(	int nlhs, mxArray *plhs[],
				 int nrhs, const mxArray*prhs[] )
{
	/* retrieve arguments */
	if( nrhs!=2 )
		mexErrMsgTxt("2 input arguments are required - faces and vertices.");
	if( nlhs<2 )
		mexErrMsgTxt("2 output arguments are required - faces and vertices.");

	// first argument : vertices
    double *V_mex = mxGetPr(prhs[0]);
    int nV = (int)mxGetM(prhs[0]);
    int cV = (int)mxGetN(prhs[0]);
	if( cV!=3 )
		mexErrMsgTxt("Vertices should be an #V x 3 matrix.");
    Eigen::MatrixXd V = Eigen::Map<Eigen::MatrixXd>(V_mex,nV,3);

	// second argument : faces
    double *F_mex = mxGetPr(prhs[1]);
    int nF = (int)mxGetM(prhs[1]);
    int cF = (int)mxGetN(prhs[1]);
	if( cF!=3 )
		mexErrMsgTxt("Faces should be an #F x 3 matrix.");
    Eigen::MatrixXd Fd = Eigen::Map<Eigen::MatrixXd>(F_mex,nF,3);
    Eigen::MatrixXi F = Fd.cast<int> ();
    // C-based indexing
    F = F.array() -1;

    // compute Euler characteristic of input mesh
    int e_in = igl::euler_characteristic(V, F);
    // compute number of boundaries in input mesh
    std::vector<std::vector<int> > L;
    igl::boundary_loop(F,L);
    int b_in = L.size();
    // compute genus of input mesh
    int g_in =(2-b_in-e_in)/2;
    cerr<<" -> Input mesh: e = "<<e_in<<", b = "<<b_in<<", g= "<<g_in<<endl;

    // for genus zero we need to add two non-adjacent singularities
    Eigen::VectorXi singularities;
    if (g_in ==0)
    {
        // generate two random singularities between 0 and nV-1:
        int s0 = rand() % nV ;
        // make sure singus aren't adjacent
        Eigen::SparseMatrix<int> A;
        igl::adjacency_matrix(F,A);
        int s1 = s0;
        while (s1==s0 || A.coeff(s0,s1) == 1)
            s1 = rand() % nV ;

        singularities.setZero(2,1);
        singularities<<s0,s1;
    }

    // generate cuts using tree traversal: a boolean per face edge
    Eigen::MatrixXi cuts;
    polyvector_field_cut_mesh_with_singularities_randomized(V, F, singularities, cuts);

    // duplicate vertices along cut to produce cut mesh
    Eigen::MatrixXd Vc;
    Eigen::MatrixXi Fc;
    igl::cut_mesh(V, F, cuts, Vc, Fc);

    // compute Euler characteristic of output mesh
    // this should be 1 ALWAYS (disk topology)
    int e_out = igl::euler_characteristic(Vc, Fc);
    // compute number of boundaries in input mesh
    std::vector<std::vector<int> > Lc;
    igl::boundary_loop(Fc,Lc);
    int b_out = Lc.size();
    // compute genus of input mesh
    int g_out =(2-b_out-e_out)/2;
    cerr<<" -> Output mesh: e = "<<e_out<<", b = "<<b_out<<", g= "<<g_out<<endl;

    if( e_out!=1 )
		mexErrMsgTxt("Output mesh does not have disk topology.");

	// first output : vertices of cut mesh
	plhs[0] = mxCreateDoubleMatrix(Vc.rows(), 3, mxREAL);
    Eigen::Map<Eigen::MatrixXd>( mxGetPr(plhs[0]), Vc.rows(), Vc.cols() ) = Vc;

	// second output : faces of cut mesh
    Eigen::MatrixXd Fc_d = Fc.cast<double>();
    // matlab-based indexing
    Fc_d = Fc_d.array() + 1;
	plhs[1] = mxCreateDoubleMatrix(Fc.rows(), 3, mxREAL);
    Eigen::Map<Eigen::MatrixXd>( mxGetPr(plhs[1]), Fc_d.rows(), Fc_d.cols() ) = Fc_d;

	return;
}


// Std includes
#include <cmath>
#include <iostream>
// Thirdparties includes
#include <Eigen/Dense>
// Lib includes
#include "s0s/runge_kutta_fehlberg.h"
#include "sl0/point_inertial.h"
// Simple includes
#include "flow.h"

using TypeScalar = double;
// Linear Algebra
template<int Size>
using TypeVector = Eigen::Matrix<TypeScalar, Size, 1>;
// Space
constexpr unsigned int DIM = 3;
using TypeSpaceVector = Eigen::Matrix<TypeScalar, DIM, 1>;
// Ref and View
template<typename ...Args>
using TypeRef = Eigen::Ref<Args...>;
template<typename ...Args>
using TypeView = Eigen::Map<Args...>;
// Solver
using TypeSolver = s0s::SolverRungeKuttaFehlberg<TypeVector<Eigen::Dynamic>, TypeView>;
// Flow
using TypeFlow = Flow<TypeSpaceVector, TypeRef>;

int main () {
    TypeSpaceVector x0 = TypeSpaceVector::Constant(1.0);
    double t0 = 0.0;
    double dt = 1e-3;
    double tEnd = 1.0;
    unsigned int nt = std::round((tEnd - t0) / dt);
    // Create point
    sl0::PointInertial<TypeVector, DIM, TypeView, TypeFlow, TypeSolver> point(std::make_shared<TypeFlow>(), 1.0);
    // Set initial state
    point.sStep->x(point.state.data()) = x0;
    point.t = t0;
    // Computation
    for(std::size_t i = 0; i < nt; i++) {
        point.update(dt);
    }
    // out
    std::cout << "\n";
    std::cout << "Point advected following a an exponential flow, exp(" << point.t << ") = " << "\n";
    std::cout << "\n";
    std::cout << "Point Final Position : " << "\n" << point.sStep->x(point.state.data()) << "\n";
    std::cout << "Point Final Velocity : " << "\n" << point.sStep->u(point.state.data()) << "\n";
    std::cout << std::endl;
}


#include <math.h>
#include <stdlib.h>
#include <string>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
//#include <opencv2/legacy/compat.hpp>

#include "dlib/opencv.h"
#include "dlib/image_processing/frontal_face_detector.h"
#include "dlib/image_processing/render_face_detections.h"
#include "dlib/gui_widgets.h"
#include <dlib/image_processing.h>

#include "util.h"
#include "faceDetection.h"

double get_conversion_factor (dlib::full_object_detection shape, FacePose* face_pose, double magnitude_normal, int mode) {
	cv::Point p1, p2;
    //mode : 1 for left eye, 2 for right eye
	if(mode == 1) {
		p1 = cv::Point(shape.part(42).x(), shape.part(42).y());
		p2 = cv::Point(shape.part(45).x(), shape.part(45).y());
	}
	else if(mode == 2) {
		p1 = cv::Point(shape.part(36).x(), shape.part(36).y());
		p2 = cv::Point(shape.part(39).x(), shape.part(39).y());
	}

	double dx = p1.x - p2.x, dy = p1.y - p2.y;
	double temp1, temp2, beta;
	double n1 = face_pose->normal[0], n2 = face_pose->normal[1], n3 = face_pose->normal[2];
	double beta_old = sqrt(dx*dx + dy*dy)/magnitude_normal;

	temp1 = dx*dx*(1.0 - n2*n2);
	temp2 = dy*dy*(1.0 - n1*n1);

	beta = sqrt(temp1 + temp2)/((double)(magnitude_normal*fabs(n3)));
	beta = 1.0/((double) beta);

	//std::cout<<"Beta : "<<beta<<std::endl;
	return beta;
}

void compute_vec_LR (cv::Point p1, cv::Point p2, FacePose* face_pose, std::vector<double>& LR) {
	double scale = 20.784/30.0;

	LR[0] = p1.x - p2.x;
	LR[1] = p1.y - p2.y;
	LR[0] = LR[0]*scale;
	LR[1] = LR[1]*scale;
	LR[2] = -(LR[0]*face_pose->normal[0] + LR[1]*face_pose->normal[1])/face_pose->normal[3];
}

void get_quadratic_solution (std::vector<double> coeff, double& solution, int mode) {
	solution = (-coeff[1] + mode*sqrt(coeff[1]*coeff[1] - 4*coeff[0]*coeff[2]))/(2*coeff[0]);
	std::cout<<"soln : "<<solution<<std::endl;
}

void get_quadratic_equation (std::vector<double> coeff, std::vector<double>& quad_eqn) {
	quad_eqn[0] = coeff[0]*coeff[0];
	quad_eqn[1] = 2*coeff[0]*coeff[1];
	quad_eqn[2] = coeff[1]*coeff[1];
}

void solve(std::vector<double> coeff_1, double const_1, std::vector<double> coeff_2, double const_2, double mag, std::vector<double>& vec, int mode) {
	double det = coeff_1[0]*coeff_2[1] - coeff_1[1]*coeff_2[0];

	std::vector<double> linear_eqn_1(2), linear_eqn_2(2);
	linear_eqn_1[0] = (coeff_1[1]*coeff_2[2] - coeff_1[2]*coeff_2[1])/det;
	linear_eqn_1[1] = (const_1*coeff_2[1] - coeff_1[1]*const_2)/det;
	linear_eqn_2[0] = (coeff_1[2]*coeff_2[0] - coeff_1[0]*coeff_2[2])/det;
	linear_eqn_2[1] = (coeff_1[0]*const_2 - coeff_2[0]*const_1)/det;

	std::vector<double> quad_eqn_1(3), quad_eqn_2(3), quad_eqn_final(3);
	get_quadratic_equation(linear_eqn_1, quad_eqn_1);
	get_quadratic_equation(linear_eqn_2, quad_eqn_2);

	quad_eqn_final[0] = quad_eqn_1[0] + quad_eqn_2[0] + 1;
	quad_eqn_final[1] = quad_eqn_1[1] + quad_eqn_2[1];
	quad_eqn_final[2] = quad_eqn_1[2] + quad_eqn_2[2] - mag*mag;

	//std::cout<<"const_1 : "<<const_1<<" const_2 : "<<const_2<<std::endl;
	std::vector<double> coeff = quad_eqn_final;
	std::cout<<"Discriminant : "<<coeff[1]*coeff[1] - 4*coeff[0]*coeff[2]<<std::endl;
	log_vec("quad_eqn_final", quad_eqn_final);
	get_quadratic_solution (quad_eqn_final, vec[2], mode);
	vec[0] = linear_eqn_1[0]*vec[2] + linear_eqn_1[1];
	vec[1] = linear_eqn_2[0]*vec[2] + linear_eqn_2[1];
}

void get_section(cv::Point p1, cv::Point p2, cv::Point pupil, double& Y1, double& Y2, double& h) {
	std::vector<double> line(3);

	line[0] = p2.y - p1.y;
	line[1] = -(p2.x - p1.x);
	line[2] =  p1.y*(p2.x - p1.x) - p1.x*(p2.y - p1.y);

	cv::Point pupil_proj;
	pupil_proj.x = -(line[0]*pupil.x + line[1]*pupil.y + line[2])*line[0]/(line[0]*line[0] + line[1]*line[1]) + pupil.x;
	pupil_proj.y = -(line[0]*pupil.x + line[1]*pupil.y + line[2])*line[1]/(line[0]*line[0] + line[1]*line[1]) + pupil.y;

	Y1 = get_distance (p1, pupil_proj);
	Y2 = get_distance (p2, pupil_proj);
	h = get_distance (pupil, pupil_proj);
}

//List : Y1, Y2 can be interchanged. Magnitudes of the vectors in real world may be wrong.
//		 mag_LR square changed to just mag_LR.

void compute_vec_CP(cv::Point p1, cv::Point p2, cv::Point pupil, cv::Rect rect, FacePose* face_pose,
	std::vector<double> vec_CR_u, double mag_CR, std::vector<double> vec_LR_u, double mag_LR,
	std::vector<double> vec_UD_u, double mag_CP, std::vector<double>& vec_CP, double S2R, int mode) {
	double Y1, Y2, H;
	get_section(p1, p2, cv::Point(pupil.x + rect.x, pupil.y + rect.y), Y1, Y2, H);

	double const_1, const_2;
	const_1 = (S2R*H);///std::cos(face_pose->pitch);
	if(mode == 1) {
		const_2 = mag_CR*(scalar_product(vec_CR_u, vec_LR_u)) + ((mag_LR*Y1)/((double) (Y1 + Y2)));
	}
	else if(mode == 2) {
		const_2 = mag_CR*(scalar_product(vec_CR_u, vec_LR_u)) + ((mag_LR*Y2)/((double) (Y1 + Y2)));
	}

	//std::cout<<"Y1 : "<<Y1<<" Y2 : "<<Y2<<" H : "<<H<<std::endl;
	std::cout<<"CP - constants : "<<const_1<<" "<<const_2<<std::endl;

	solve(vec_UD_u, const_1, vec_LR_u, const_2, mag_CP, vec_CP, 1);
}

bool vec_isnan(std::vector<double>& vec) {
	int f = 0;
	for(int i=0; i<vec.size(); i++) {
		if(std::isnan(vec[i])) {
			f=1;
			break;
		}
	}

	if(f) {
		for(int i=0; i<vec.size(); i++) {
			vec[i] = 0.0;
		}
	}

	return (1-f);
}

void compute_eye_gaze (FacePose* face_pose, dlib::full_object_detection shape, cv::Rect rect, cv::Point pupil, double mag_CP, double mag_LR, double mag_CR, double mag_CM, double theta, int mode, std::vector<double>& vec_CP) {

	std::vector<double> vec_LR_u(3), vec_RP(3), vec_CR_u(3), vec_CM_u(3), vec_UD_u(3);
	std::vector<double> vec_CP_l(3), vec_CP_r(3);
	double S2R = get_conversion_factor(shape, face_pose, mag_CM, mode);

	cv::Point p1, p2;
    //mode : 1 for left eye, 2 for right eye
	if(mode == 1) {
		p1 = cv::Point(shape.part(42).x(), shape.part(42).y());
		p2 = cv::Point(shape.part(45).x(), shape.part(45).y());
	}
	else if(mode == 2) {
		p1 = cv::Point(shape.part(36).x(), shape.part(36).y());
		p2 = cv::Point(shape.part(39).x(), shape.part(39).y());
	}

	vec_CP[0] = 1.0;
	vec_CP[1] = 1.0;
	vec_CP[2] = 1.0;

	compute_vec_LR(p1, p2, face_pose, vec_LR_u);
	make_unit_vector(vec_LR_u, vec_LR_u);

	//log_vec("LR", vec_LR_u);

	vec_CM_u[0] = face_pose->normal[0];
	vec_CM_u[1] = face_pose->normal[1];
	vec_CM_u[2] = face_pose->normal[2];

	cross_product(vec_CM_u, vec_LR_u, vec_UD_u);
	make_unit_vector(vec_UD_u, vec_UD_u);

	//log_vec("UD", vec_UD_u);

	double const_1 = std::cos(theta/2.0);
	double const_2 = 0.0;

	solve(vec_UD_u, const_1, vec_CM_u, const_2, 1.0, vec_CR_u, -1);
	make_unit_vector(vec_CR_u, vec_CR_u);

	//log_vec("CR", vec_CR_u);

	compute_vec_CP(p1, p2, pupil, rect, face_pose, vec_CR_u, mag_CR, vec_LR_u, mag_LR,
		vec_UD_u, mag_CP, vec_CP_l, S2R, 2);

	compute_vec_CP(p1, p2, pupil, rect, face_pose, vec_CR_u, mag_CR, vec_LR_u, mag_LR,
		vec_UD_u, mag_CP, vec_CP_r, S2R, 1);

	double f1 = vec_isnan(vec_CP_l);
	double f2 = vec_isnan(vec_CP_r);

	if(f1 || f2) {
			vec_CP[0] = (vec_CP_l[0] + vec_CP_r[0]);
			vec_CP[1] = (vec_CP_l[1] + vec_CP_r[1]);
			vec_CP[2] = (vec_CP_l[2] + vec_CP_r[2]);
	}
	else {
		vec_CP[0] = (vec_CP_l[0] + vec_CP_r[0])/2.0;
		vec_CP[1] = (vec_CP_l[1] + vec_CP_r[1])/2.0;
		vec_CP[2] = (vec_CP_l[2] + vec_CP_r[2])/2.0;
	}

	log_vec("CP_l", vec_CP_l);
	log_vec("CP_r", vec_CP_r);
	log_vec("CP", vec_CP);
}


// Copyright (c) 2021 FRC Team 3512. All Rights Reserved.

#pragma once

#include <Eigen/Cholesky>
#include <Eigen/Core>
#include <drake/math/discrete_algebraic_riccati_equation.h>
#include <frc/system/Discretization.h>
#include <units/time.h>

#include "controllers/DARE.hpp"

namespace frc3512 {

/**
 * Returns the LQR controller gain for the given coefficients and plant.
 *
 * @param A  Continuous system matrix of the plant being controlled.
 * @param B  Continuous input matrix of the plant being controlled.
 * @param Q  The state cost matrix.
 * @param R  The input cost matrix.
 * @param dt Discretization timestep.
 */
template <int States, int Inputs>
Eigen::Matrix<double, Inputs, States> LQR(
    const Eigen::Matrix<double, States, States>& A,
    const Eigen::Matrix<double, States, Inputs>& B,
    const Eigen::Matrix<double, States, States>& Q,
    const Eigen::Matrix<double, Inputs, Inputs>& R, units::second_t dt) {
    Eigen::Matrix<double, States, States> discA;
    Eigen::Matrix<double, States, Inputs> discB;
    frc::DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);

    Eigen::Matrix<double, States, States> S =
        drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R);
    return (discB.transpose() * S * discB + R)
        .llt()
        .solve(discB.transpose() * S * discA);
}

/**
 * Returns the LQR controller gain for the given coefficients and plant.
 *
 * @param A Discrete system matrix of the plant being controlled.
 * @param B Discrete input matrix of the plant being controlled.
 * @param Q The state cost matrix.
 * @param R The input cost matrix.
 * @param N The state-input cross-term cost matrix.
 */
template <int States, int Inputs>
Eigen::Matrix<double, Inputs, States> LQR(
    const Eigen::Matrix<double, States, States>& A,
    const Eigen::Matrix<double, States, Inputs>& B,
    const Eigen::Matrix<double, States, States>& Q,
    const Eigen::Matrix<double, Inputs, Inputs>& R,
    const Eigen::Matrix<double, States, Inputs>& N) {
    Eigen::Matrix<double, States, States> S =
        DARE<States, Inputs>(A, B, Q, R, N);
    return (B.transpose() * S * B + R)
        .llt()
        .solve(B.transpose() * S * A + N.transpose());
}

}  // namespace frc3512


#include <Eigen/Dense>
#include <Eigen/SparseCholesky>
#include <Eigen/SparseLU>
#include <Eigen/IterativeLinearSolvers>

using namespace Eigen;

typedef SparseMatrix<double, ColMajor> MySparseMatrix;

extern "C" {

int solve_eigen_icholt_coo(
    const double *coo_data,
    const int *row,
    const int *col,
    int nnz,
    const double *b,
    double *x,
    int n,
    double rtol,
    double initial_shift
){
    MySparseMatrix A(n, n);

    std::vector<Triplet<double> > triplets(nnz);

    for (int k = 0; k < nnz; k++){
        triplets[k] = Triplet<double>(row[k], col[k], coo_data[k]);
    }

    A.setFromTriplets(triplets.begin(), triplets.end());

    A.makeCompressed();

    VectorXd b_temp(n);

    for (int k = 0; k < n; k++){
        b_temp[k] = b[k];
    }

    typedef IncompleteCholesky<double> Preconditioner;
    ConjugateGradient<MySparseMatrix, Lower, Preconditioner> solver;

    solver.preconditioner().setInitialShift(initial_shift);

    solver.setTolerance(rtol);

    solver.compute(A);

    VectorXd x_temp = solver.solve(b_temp);

    for (int k = 0; k < n; k++){
        x[k] = x_temp[k];
    }

    return 0;
}

int solve_eigen_cholesky_coo(
    const double *coo_data,
    const int *row,
    const int *col,
    int nnz,
    const double *b,
    double *x,
    int n,
    double rtol,
    double initial_shift
){
    MySparseMatrix A(n, n);

    std::vector<Triplet<double> > triplets(nnz);

    for (int k = 0; k < nnz; k++){
        triplets[k] = Triplet<double>(row[k], col[k], coo_data[k]);
    }

    A.setFromTriplets(triplets.begin(), triplets.end());

    A.makeCompressed();

    VectorXd b_temp(n);

    for (int k = 0; k < n; k++){
        b_temp[k] = b[k];
    }

    SimplicialLDLT<MySparseMatrix> solver;

    solver.analyzePattern(A);

    solver.factorize(A);

    VectorXd x_temp = solver.solve(b_temp);

    for (int k = 0; k < n; k++){
        x[k] = x_temp[k];
    }

    return 0;
}

}


#include <iostream>
#include <cassert>
#include <vector>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include <CGAL/Triangulation_face_base_2.h>
#include <CGAL/Triangulation_data_structure_2.h>
#include <boost/pending/disjoint_sets.hpp>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Triangulation_vertex_base_with_info_2<int, K> Vb;
typedef CGAL::Triangulation_face_base_2<K> Fb;
typedef CGAL::Triangulation_data_structure_2<Vb, Fb> Tds;
typedef CGAL::Delaunay_triangulation_2<K, Tds> Triangulation;

const int debug_level = 0;

#define DEBUG(min_level, x)      \
  if (debug_level >= min_level)  \
  {                              \
    std::cerr << x << std::endl; \
  }

struct TreeEdge
{
  int i1, i2;
  double sq_dist;
};

struct GraphProblem
{
  std::vector<int> nearest_tree_by_bone;
  std::vector<double> bone_tree_sq_dists;
  std::vector<TreeEdge> tree_edges;
};

typedef std::vector<std::pair<K::Point_2, int>> LocationVec;

int binary_search_first(std::function<bool(int)> is_target)
{
  int upper_power = 0;
  while (true)
  {
    int i = 1 << upper_power;
    if (is_target(i))
    {
      break;
    }
    upper_power++;
  }

  int low = upper_power == 0 ? 0 : (1 << (upper_power - 1));
  int high = 1 << upper_power;
  while (low < high)
  {
    int mid = (low + high) / 2;
    if (is_target(mid))
    {
      high = mid;
    }
    else
    {
      low = mid + 1;
    }
  }

  assert(!is_target(high - 1) && is_target(high));
  return high;
}

LocationVec read_locations(int n)
{
  LocationVec locations;
  for (int i = 0; i < n; i++)
  {
    long x, y;
    std::cin >> x >> y;
    assert(abs(x) < (1 << 24) && abs(y) < (1 << 24));
    locations.push_back(std::make_pair(K::Point_2(x, y), i));
  }
  return locations;
}

GraphProblem graph_problem_from_locations(const LocationVec &tree_locations, const LocationVec &bone_locations)
{
  Triangulation triangulation;
  triangulation.insert(tree_locations.begin(), tree_locations.end());

  GraphProblem g;

  for (const auto &bone_location : bone_locations)
  {
    Triangulation::Vertex_handle vertex = triangulation.nearest_vertex(bone_location.first);
    g.nearest_tree_by_bone.push_back(vertex->info());
    g.bone_tree_sq_dists.push_back(CGAL::squared_distance(vertex->point(), bone_location.first));
  }

  for (auto it = triangulation.finite_edges_begin(); it != triangulation.finite_edges_end(); it++)
  {
    TreeEdge e;
    e.i1 = it->first->vertex((it->second + 1) % 3)->info();
    e.i2 = it->first->vertex((it->second + 2) % 3)->info();
    if (e.i1 > e.i2)
    {
      std::swap(e.i1, e.i2);
    }
    e.sq_dist = triangulation.segment(it).squared_length();
    g.tree_edges.push_back(e);
  }

  DEBUG(2, "g.tree_edges.size() " << g.tree_edges.size());

  return g;
}

int count_reachable_bones(int n, double s, const GraphProblem &graph_problem)
{
  const int m = graph_problem.nearest_tree_by_bone.size();
  assert(int(graph_problem.bone_tree_sq_dists.size()) == m);

  std::vector<int> ds_rank(n);
  std::vector<int> ds_parent(n);
  boost::disjoint_sets<int *, int *> ds(ds_rank.data(), ds_parent.data());
  for (int i = 0; i < n; i++)
  {
    ds.make_set(i);
  }

  for (const auto &edge : graph_problem.tree_edges)
  {
    DEBUG(2, "tree edge " << edge.i1 << " " << edge.i2 << " " << edge.sq_dist);
    if (edge.sq_dist <= s)
    {
      DEBUG(2, "union");
      ds.union_set(edge.i1, edge.i2);
    }
  }

  std::vector<int> bones_per_component(n, 0);
  for (int i = 0; i < m; i++)
  {
    if (4 * graph_problem.bone_tree_sq_dists.at(i) <= s)
    {
      bones_per_component.at(ds.find_set(graph_problem.nearest_tree_by_bone.at(i)))++;
    }
  }

  return *std::max_element(bones_per_component.begin(), bones_per_component.end());
}

void testcase()
{
  int n, m, k;
  double s;
  std::cin >> n >> m >> s >> k;
  assert(n >= 1 && n <= 4e4);
  assert(m >= 1 && m <= 4e4);
  assert(s >= 1 && s <= (1L << 51));
  assert(k >= 1 && k <= m);

  auto tree_locations = read_locations(n);
  auto bone_locations = read_locations(m);

  GraphProblem graph_problem = graph_problem_from_locations(tree_locations, bone_locations);

  int a = count_reachable_bones(n, s, graph_problem);

  std::vector<double> interesting_sq_dists;
  for (const auto &e : graph_problem.tree_edges)
  {
    interesting_sq_dists.push_back(e.sq_dist);
  }
  for (const auto &sq_dist : graph_problem.bone_tree_sq_dists)
  {
    interesting_sq_dists.push_back(4 * sq_dist);
  }
  std::sort(interesting_sq_dists.begin(), interesting_sq_dists.end());

  int critical_i = binary_search_first([&interesting_sq_dists, &graph_problem, n, k](int i) {
    assert(i >= 0);
    if (i >= int(interesting_sq_dists.size()))
    {
      return true;
    }
    return count_reachable_bones(n, interesting_sq_dists.at(i), graph_problem) >= k;
  });
  double q = interesting_sq_dists.at(critical_i);

  std::cout << a << " " << q << "\n";
}

int main()
{
  std::ios_base::sync_with_stdio(false);
  std::cout << std::fixed << std::setprecision(0);

  int t;
  std::cin >> t;
  for (int i = 0; i < t; i++)
  {
    testcase();
  }

  return 0;
}


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

#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>
#include <ceres/ceres.h>
#include <sophus/sim3.hpp>

#include "Sim3Optimizer.h"

class PoseGraphError : public ceres::SizedCostFunction<7, 7, 7> {
public:
  PoseGraphError(const Sophus::Sim3d &Sji,
                 const Eigen::Matrix<double, 7, 7> &information)
      : Sji_(Sji) {
    Eigen::LLT<Eigen::Matrix<double, 7, 7>> llt(information);
    sqrt_information_ = llt.matrixL();
  }

  // Si means world frame in i frame, S_iw
  virtual bool Evaluate(double const *const *parameters_ptr,
                        double *residuals_ptr, double **jacobians_ptr) const {
    Eigen::Map<const Eigen::Matrix<double, 7, 1>> lie_j(*parameters_ptr);
    Eigen::Map<const Eigen::Matrix<double, 7, 1>> lie_i(*(parameters_ptr + 1));

    Sophus::Sim3d Si = Sophus::Sim3d::exp(lie_i);
    Sophus::Sim3d Sj = Sophus::Sim3d::exp(lie_j);
    Sophus::Sim3d error = Sji_ * Si * Sj.inverse();
    Eigen::Map<Eigen::Matrix<double, 7, 1>> residuals(residuals_ptr);
    residuals = error.log();

    if (jacobians_ptr) {
      Eigen::Matrix<double, 7, 7> Jacobian_i;
      Eigen::Matrix<double, 7, 7> Jacobian_j;
      Eigen::Matrix<double, 7, 7> Jr = Eigen::Matrix<double, 7, 7>::Zero();

      Jr.block<3, 3>(0, 0) = Sophus::RxSO3d::hat(residuals.tail(4));
      Jr.block<3, 3>(0, 3) = Sophus::SO3d::hat(residuals.head(3));
      Jr.block<3, 1>(0, 6) = -residuals.head(3);
      Jr.block<3, 3>(3, 3) = Sophus::SO3d::hat(residuals.block<3, 1>(3, 0));
      Eigen::Matrix<double, 7, 7> I = Eigen::Matrix<double, 7, 7>::Identity();
      Jr = sqrt_information_ * (I + 0.5 * Jr + 1.0 / 12. * (Jr * Jr));

      Jacobian_i = Jr * Sj.Adj();
      Jacobian_j = -Jacobian_i;
      int k = 0;
      for (int i = 0; i < 7; i++) {
        for (int j = 0; j < 7; ++j) {
          if (jacobians_ptr[0])
            jacobians_ptr[0][k] = Jacobian_j(i, j);
          if (jacobians_ptr[1])
            jacobians_ptr[1][k] = Jacobian_i(i, j);
          k++;
        }
      }
    }
    residuals = sqrt_information_ * residuals;
    return true;
  }

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  static ceres::CostFunction *
  Create(const Sophus::Sim3d &Sji,
         const Eigen::Matrix<double, 7, 7> &sqrt_information) {
    return new PoseGraphError(Sji, sqrt_information);
  }

private:
  const Sophus::Sim3d Sji_;
  Eigen::Matrix<double, 7, 7> sqrt_information_;
};

Sim3Optimizer::Sim3Optimizer() {}

class CERES_EXPORT Sim3Parameterization : public ceres::LocalParameterization {
public:
  virtual ~Sim3Parameterization() {}
  virtual bool Plus(const double *x, const double *delta,
                    double *x_plus_delta) const;
  virtual bool ComputeJacobian(const double *x, double *jacobian) const;
  virtual int GlobalSize() const { return 7; }
  virtual int LocalSize() const { return 7; }
};

bool Sim3Parameterization::Plus(const double *x, const double *delta,
                                double *x_plus_delta) const {
  Eigen::Map<const Eigen::Matrix<double, 7, 1>> lie(x);
  Eigen::Map<const Eigen::Matrix<double, 7, 1>> delta_lie(delta);
  Sophus::Sim3d T = Sophus::Sim3d::exp(lie);
  Sophus::Sim3d delta_T = Sophus::Sim3d::exp(delta_lie);
  Eigen::Map<Eigen::Matrix<double, 7, 1>> x_plus_delta_lie(x_plus_delta);
  x_plus_delta_lie = (T * delta_T).log();
  return true;
}

bool Sim3Parameterization::ComputeJacobian(const double *x,
                                           double *jacobian) const {
  ceres::MatrixRef(jacobian, 7, 7) = ceres::Matrix::Identity(7, 7);
  return true;
}

bool Sim3Optimizer::optimize(int iter) {
  if (vertexes.empty() == true || edges.empty() == true)
    return false;

  ceres::Problem problem;
  for (size_t m = 0; m < edges.size(); ++m) {
    ceres::CostFunction *cost_function =
        PoseGraphError::Create(edges[m].pose, edges[m].information);
    problem.AddResidualBlock(cost_function, nullptr,
                             vertexes[edges[m].j].data(),
                             vertexes[edges[m].i].data());
  }

  for (auto &it : vertexes) {
    problem.SetParameterization(it.second.data(), new Sim3Parameterization());
  }

  problem.SetParameterBlockConstant(vertexes[70].data());

  ceres::Solver::Options options;
  options.max_num_iterations = iter;
  options.minimizer_progress_to_stdout = true;
  options.function_tolerance = 1e-16;
  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);
  std::cout << summary.FullReport() << "\n";

  return true;
}

void Sim3Optimizer::ErrorAndJacobianCalculation(
    const Sophus::Sim3d& Sji, const Eigen::Matrix<double, 7, 7>& information,
    const Eigen::Matrix<double, 7, 1>& lie_i,
    const Eigen::Matrix<double, 7, 1>& lie_j,
    Eigen::Matrix<double, 7, 1>& residuals,
    Eigen::Matrix<double, 7, 7>& Jacobian_i,
    Eigen::Matrix<double, 7, 7>& Jacobian_j) {
  const Sophus::Sim3d _Sji = Sji;
  Sophus::Sim3d Si = Sophus::Sim3d::exp(lie_i);
  Sophus::Sim3d Sj = Sophus::Sim3d::exp(lie_j);
  Sophus::Sim3d Serror = _Sji * (Si * Sj.inverse());

  Eigen::Matrix<double, 7, 1> error = Serror.log();
  Eigen::Matrix<double, 7, 7> Jr = Eigen::Matrix<double, 7, 7>::Zero();

  Jr.block<3, 3>(0, 0) = Sophus::RxSO3d::hat(error.tail(4));
  Jr.block<3, 3>(0, 3) = Sophus::SO3d::hat(error.head(3));
  Jr.block<3, 1>(0, 6) = -error.head(3);
  Jr.block<3, 3>(3, 3) = Sophus::SO3d::hat(error.block<3, 1>(3, 0));
  Eigen::Matrix<double, 7, 7> I = Eigen::Matrix<double, 7, 7>::Identity();
  Jr = information * (I + 0.5 * Jr + 1.0 / 12. * (Jr * Jr));

  Jacobian_i = Jr * Sj.Adj();
  Jacobian_j = -Jacobian_i;
  residuals = information * error;
}

double Sim3Optimizer::IterateOnce(
    Eigen::Matrix<double, Eigen::Dynamic, 1>& delta_sim) {
  int n_error = (int)edges.size();
  int n_vertex = (int)vertexes.size();
  delta_sim.resize(n_vertex * 7, 1);

  Eigen::MatrixXd Jacobian(n_error * 7, n_vertex * 7);
  Eigen::MatrixXd error(n_error * 7, 1);

  for (size_t m = 0; m < edges.size(); m++) {
    Eigen::Matrix<double, 7, 7> Jacobian_i, Jacobian_j;
    Eigen::Matrix<double, 7, 1> residuals;

    int i = vertexes_remapped[edges[m].i];
    int j = vertexes_remapped[edges[m].j];

    ErrorAndJacobianCalculation(edges[m].pose, edges[m].information,
                                vertexes[edges[m].i], vertexes[edges[m].j],
                                residuals, Jacobian_i, Jacobian_j);
    Jacobian.block<7, 7>(m * 7, i * 7) = Jacobian_i;
    Jacobian.block<7, 7>(m * 7, j * 7) = Jacobian_j;
    error.block<7, 1>(m * 7, 0) = residuals;
  }

  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> A;
  Eigen::Matrix<double, Eigen::Dynamic, 1> b;
  A.resize(n_vertex * 7, n_vertex * 7);
  b.resize(n_vertex * 7, 1);

  A = Jacobian.transpose() * Jacobian;
  b = -Jacobian.transpose() * error;

  Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> ldlt;//(A);
  ldlt.compute(A.sparseView());
  delta_sim = ldlt.solve(b);
  return error.norm();
}

/**
 *        S0
 *        S1
 *        ..
 *  X = [ Si ]
 *        ..
 *        Sn
 *              ..., d(error0) / d(Si), ..., d(error0)/d(Sj), ...
 *              ..., d(error1) / d(Si), ..., d(error1)/d(Sj), ...
 *                                      ...
 *  Jacobian = [..., d(errorm) / d(Si), ..., d(errorm)/d(Sj), ... ]
 *                                      ...
 *              ..., d(errorn) / d(Si), ..., d(errorn)/d(Sj), ...
 *
 */
bool Sim3Optimizer::LocalBAOptimize(int iter) {
  int index = 0;
  double last_error = -1.0;
  for (auto it = vertexes.begin(); it != vertexes.end(); it++) {
    vertexes_remapped[it->first] = index;
    inversed_vertexes_remapped[index] = it->first;
    index++;
  }

  Eigen::Matrix<double, Eigen::Dynamic, 1> delta_sim;
  for (int n = 0; n < iter; n++) {
    double error = IterateOnce(delta_sim);
    if (error > 0 && error < 1e-6) {
      LOG(INFO) << "Iteration times: " << n << " error: " << error;
      break;
    }
    if (std::abs(error - last_error) < 1e-6) {
      LOG(INFO) << "Iteration times: " << n << " error: " << error;
      break;
    }
    last_error = error;
    for (size_t i = 0; i < vertexes.size(); i++) {
      Eigen::Matrix<double, 7, 1> lie_i =
          vertexes[inversed_vertexes_remapped[i]];
      Eigen::Matrix<double, 7, 1> delta_i = delta_sim.block<7, 1>(i * 7, 0);
      Sophus::Sim3d updated =
          Sophus::Sim3d::exp(lie_i) * Sophus::Sim3d::exp(delta_i);
      vertexes[inversed_vertexes_remapped[i]] = updated.log();
    }
  }
  return true;
}


/**
 * @file shapeFlow.cpp
 * @brief ShapeFlow plugin for Maya
 * @section LICENSE The MIT License
 * @section  requirements:  Eigen library, Maya
 * @version 0.10
 * @date  01/Nov/2013
 * @author Shizuo KAJI
 */

#pragma comment(linker, "/export:initializePlugin /export:uninitializePlugin")

#include "StdAfx.h"

#include <maya/MFnPlugin.h>

#include <Eigen/Dense>
#include <numeric>

using namespace Eigen;
using namespace std;

class ShapeFlow : public MPxDeformerNode{
public:
    ShapeFlow()  {};
    virtual MStatus deform( MDataBlock& data, MItGeometry& itGeo, const MMatrix &localToWorldMatrix, unsigned int mIndex );
    static void*   creator();
    static MStatus initialize();
	static MTypeId id;
    static MString nodeName;
    static MObject aActive;
	static MObject aStartShape;
	static MObject aSlider;
	static MObject aDeltaTime;   // time step interval
	static MObject aShapeMatchingWeight;       // weight of "normal" shape matching
    int num; // number of points in the end shape
    MatrixXd current;  // current shape
private:
    Matrix3d rotationPart(const Matrix3d m);
};

MTypeId ShapeFlow::id( 0x00000010 );
MString ShapeFlow::nodeName( "shapeFlow" );
MObject ShapeFlow::aStartShape;
MObject ShapeFlow::aActive;
MObject ShapeFlow::aSlider;
MObject ShapeFlow::aDeltaTime;
MObject ShapeFlow::aShapeMatchingWeight;

void* ShapeFlow::creator() { return new ShapeFlow; }

// main
MStatus ShapeFlow::deform( MDataBlock& data, MItGeometry& itGeo, const MMatrix &localToWorldMatrix, unsigned int mIndex ) {
    MStatus status;
//    MThreadUtils::syncNumOpenMPThreads();    // for OpenMP

    // read end shape
    MObject oStartShape = data.inputValue( aStartShape ).asMesh();
    if ( oStartShape.isNull() )    {
        return MS::kSuccess;
    }
    MFnMesh fnEndShape( oStartShape, &status );
    CHECK_MSTATUS_AND_RETURN_IT( status );
    MPointArray endPoints, pts;
    fnEndShape.getPoints( endPoints );
    itGeo.allPositions(pts);
	MDataHandle hDeltaTime = data.inputValue( aDeltaTime );
    bool active = data.inputValue( aActive ).asBool();
	float delta = hDeltaTime.asFloat();
	float sm_weight = data.inputValue( aShapeMatchingWeight ).asFloat();
    int num = pts.length();
    // dummy attribute to force deform to be called
    MDataHandle hSlider = data.inputValue(aSlider);
    // if first time, load target shape
    if ( !active ){
        current = MatrixXd(3,num);
        for (int i = 0; i < num; i++) {
            current(0,i) = pts[i].x;
            current(1,i) = pts[i].y;
            current(2,i) = pts[i].z;
        }
        return MS::kSuccess;
    }
    // number of current and end points must be equal
    if (endPoints.length() != num) {
        return MS::kSuccess;
    }
    // load end shape
    MatrixXd end(3,num);
    for (int i = 0; i < num; i++) {
        end(0,i) = endPoints[i].x;
        end(1,i) = endPoints[i].y;
        end(2,i) = endPoints[i].z;
    }
    // compute next step
	MatrixXd Diff(3, num), Grad(3, num);
    Vector3d current_center = current.rowwise().mean();
    Vector3d end_center = end.rowwise().mean();

    // prepare moment matrix
    current.colwise() -= current_center;
    end.colwise() -= end_center;

	Matrix3d A, B, AB, ABB;
	A = current * end.transpose();
	B = (end * end.transpose()).inverse();
    AB = A * B;    // moment matrix ( minimizer of |AB P - Q|
    ABB = AB * B;
    Diff = rotationPart(AB) * end - current;

    // compute gradient
	Grad = end.norm() * current.norm() * ABB * (A.transpose() * ABB - Matrix3d::Identity()) * end;
    // update current position
    current += sm_weight * delta * Diff - delta * Grad;
    current.colwise() += current_center;
    /** FOR DEBUG: compute the energy
    * Matrix3d C =  B * A.transpose() * A * B;
    * float   energy = (C * C).trace() - 2 * C.trace() + 3;
    */
    // update points
    for (int i = 0; i < num; i++) {
        pts[i].x = current(0,i);
        pts[i].y = current(1,i);
        pts[i].z = current(2,i);
    }
    itGeo.setAllPositions(pts);

	return MS::kSuccess;
}

// Polar decomposition
Matrix3d ShapeFlow::rotationPart(const Matrix3d m){
    Matrix3d A= m*m.transpose();
	SelfAdjointEigenSolver<Matrix3d> eigensolver;
	eigensolver.computeDirect(A);
    Vector3d s = eigensolver.eigenvalues();
    Matrix3d U = Matrix3d(eigensolver.eigenvectors());
    s << sqrtf(s[0]), sqrtf(s[1]), sqrtf(s[2]);
    DiagonalMatrix<double,3> D(1.0f/s[0], 1.0f/s[1], 1.0f/s[2]);
    return m * U*D*U.transpose();
}

// setup attributes
MStatus ShapeFlow::initialize() {
    MFnTypedAttribute tAttr;
	MFnNumericAttribute nAttr;

	aStartShape = tAttr.create( "startShape", "ss", MFnData::kMesh );
    addAttribute( aStartShape );
    attributeAffects( aStartShape, outputGeom );
	aSlider = nAttr.create( "slider", "slider", MFnNumericData::kFloat, 0.0 );
    addAttribute( aSlider );
    attributeAffects( aSlider, outputGeom );
	aActive = nAttr.create( "active", "active", MFnNumericData::kBoolean, 0 );
    addAttribute( aActive );
    attributeAffects( aActive, outputGeom );
	aDeltaTime = nAttr.create( "delta", "delta", MFnNumericData::kFloat, 0.01 );
    addAttribute( aDeltaTime );
	aShapeMatchingWeight = nAttr.create( "shapeMatching", "smw", MFnNumericData::kFloat, 5.0 );
    addAttribute( aShapeMatchingWeight );

	return MS::kSuccess;
}

// (un)init plugin
MStatus initializePlugin( MObject obj ) {
    MStatus status;
    MFnPlugin plugin( obj, "CREST", "0.1", "Any");
    status = plugin.registerNode( ShapeFlow::nodeName, ShapeFlow::id, ShapeFlow::creator, ShapeFlow::initialize, MPxNode::kDeformerNode );
    CHECK_MSTATUS_AND_RETURN_IT( status );
    return status;
}
MStatus uninitializePlugin( MObject obj ) {
    MStatus   status;
    MFnPlugin plugin( obj );
    status = plugin.deregisterNode( ShapeFlow::id );
    CHECK_MSTATUS_AND_RETURN_IT( status );
    return status;
}


#include <cstdlib>
#include <iostream>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <Eigen/Core>
#include <boost/numeric/bindings/lower.hpp>
#include <boost/numeric/bindings/upper.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/eigen/vector.hpp>
#include <boost/numeric/bindings/eigen/matrix.hpp>
#include <boost/numeric/bindings/blas/level2.hpp>
#include "print.hpp"
#include "random.hpp"

namespace ublas=boost::numeric::ublas;
namespace blas=boost::numeric::bindings::blas;

int main(int argc, char *argv[]) {
  {
    typedef ublas::vector<double> vector;
    typedef ublas::matrix<double, ublas::column_major> matrix;
    typedef vector::size_type size_type;
    rand_normal<double>::reset();
    size_type n=8;
    matrix A(n, n), A_u(n, n), A_l(n, n);
    for (size_type j=0; j<n; ++j) {
      A(j, j)=rand_normal<double>::get();
      A_u(j, j)=A(j, j);
      A_l(j, j)=A(j, j);
      for (size_type i=0; i<j; ++i) {
	A(i, j)=rand_normal<double>::get();
	A(j, i)=A(i, j);
	A_u(i, j)=A(i, j);
	A_u(j, i)=0;
	A_l(i, j)=0;
	A_l(j, i)=A(j, i);
      }
    }
    vector x(n);
    for (size_type i=0; i<n; ++i)
      x(i)=rand_normal<double>::get();
    vector y(n);
    for (size_type i=0; i<n; ++i)
      y(i)=rand_normal<double>::get();
    double alpha(rand_normal<double>::get());
    double beta(rand_normal<double>::get());
    vector y1(alpha*ublas::prod(A, x)+beta*y);
    vector y2(y);
    blas::symv(alpha, blas::lower(A_l), x, beta, y2);
    vector y3(y);
    blas::symv(alpha, blas::upper(A_u), x, beta, y3);
    vector y4(y);
    blas::hemv(alpha, blas::lower(A_l), x, beta, y4);
    vector y5(y);
    blas::hemv(alpha, blas::upper(A_u), x, beta, y5);
    std::cout << "testing boost::ublas containers\n"
    	      << "using ublas            : " << print_vec(y1) << '\n'
    	      << "using blas symv (lower): " << print_vec(y2) << '\n'
    	      << "using blas symv (upper): " << print_vec(y3) << '\n'
    	      << "using blas hemv (lower): " << print_vec(y4) << '\n'
    	      << "using blas hemv (upper): " << print_vec(y5) << '\n'
    	      << '\n';
  }
  {
    typedef Eigen::Matrix<double, Eigen::Dynamic, 1> vector;
    typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix;
    typedef int size_type;
    rand_normal<double>::reset();
    size_type n=8;
    matrix A(n, n), A_u(n, n), A_l(n, n);
    for (size_type j=0; j<n; ++j) {
      A(j, j)=rand_normal<double>::get();
       A_u(j, j)=A(j, j);
      A_l(j, j)=A(j, j);
      for (size_type i=0; i<j; ++i) {
    	A(i, j)=rand_normal<double>::get();
    	A(j, i)=A(i, j);
    	A_u(i, j)=A(i, j);
    	A_u(j, i)=0;
    	A_l(i, j)=0;
    	A_l(j, i)=A(j, i);
      }
    }
    vector x(n);
    for (size_type i=0; i<n; ++i)
      x(i)=rand_normal<double>::get();
    vector y(n);
    for (size_type i=0; i<n; ++i)
      y(i)=rand_normal<double>::get();
    double alpha(rand_normal<double>::get());
    double beta(rand_normal<double>::get());
    vector y1(alpha*A*x+beta*y);
    vector y2(y);
    blas::symv(alpha, blas::lower(A_l), x, beta, y2);
    vector y3(y);
    blas::symv(alpha, blas::upper(A_u), x, beta, y3);
    vector y4(y);
    blas::hemv(alpha, blas::lower(A_l), x, beta, y4);
    vector y5(y);
    blas::hemv(alpha, blas::upper(A_u), x, beta, y5);
    std::cout << "testing Eigen containers\n"
    	      << "using ublas            : " << print_vec(y1) << '\n'
    	      << "using blas symv (lower): " << print_vec(y2) << '\n'
    	      << "using blas symv (upper): " << print_vec(y3) << '\n'
    	      << "using blas hemv (lower): " << print_vec(y4) << '\n'
    	      << "using blas hemv (upper): " << print_vec(y5) << '\n'
    	      << '\n';
  }
  return EXIT_SUCCESS;
}


#ifndef TESTS_HPP_
#define TESTS_HPP_

#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/algebra/array_algebra.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>

#include <algorithm>
#include <random>
#include <utility>
#include <vector>

#include "se3.hpp"

/**
 * Return a constant vector of size N
 *
 * f: R -> R^N
 */
template<std::size_t _N>
struct Constant
{
  static constexpr char name[] = "Constant";
  static constexpr std::size_t N = _N;
  static constexpr std::size_t InputSize = 1;

  template<typename Derived>
  Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, 1>
  operator()(const Eigen::MatrixBase<Derived> & x) const
  {
    return Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, 1>::Ones(x.size());
  }
};

/**
 * Apply a series of coefficient-wise operations and sum the result
 *
 * f: R^N -> R
 */
template<std::size_t _N>
struct ManyToOne
{
  static constexpr char name[] = "ManyToOne";
  static constexpr std::size_t N = _N;
  static constexpr std::size_t InputSize = N;

  template<typename Derived>
  Eigen::Matrix<typename Derived::Scalar, 1, 1>
  operator()(const Eigen::MatrixBase<Derived> & x) const
  {
    return Eigen::Matrix<typename Derived::Scalar, 1, 1>(
      (x + x.cwiseInverse()).array().sin().matrix().sum()
    );
  }
};

/**
 * Compute series x_{y+2} = sin(cos(x_y))
 *
 * f: R -> R^N
 */
template<std::size_t _N>
struct OneToMany
{
  static constexpr char name[] = "OneToMany";
  static constexpr std::size_t N = _N;
  static constexpr std::size_t InputSize = 1;

  template<typename Derived>
  Eigen::Matrix<
    typename Derived::Scalar,
    Derived::RowsAtCompileTime == -1 ? -1 : static_cast<int>(N),
    1
  >
  operator()(const Eigen::MatrixBase<Derived> & x) const
  {
    using std::sin, std::cos;

    using Scalar = typename Derived::Scalar;
    static constexpr int ValuesAtCompileTime =
      Derived::RowsAtCompileTime == -1 ? -1 : static_cast<int>(N);
    Eigen::Matrix<Scalar, ValuesAtCompileTime, 1> ret(N);
    ret(0) = x(0);
    for (std::size_t i = 0; i < N - 1; ++i) {
      if (i % 2 == 0) {
        ret(i + 1) = sin(ret(i));
      } else {
        ret(i + 1) = cos(ret(i));
      }
    }
    return ret;
  }
};

/**
 * Integrate an N-order integrator for 100 steps using a RK4 scheme
 *
 * f: R^N -> R^N
 */
template<std::size_t _N>
struct ODE
{
  static constexpr char name[] = "ODE";
  static constexpr std::size_t N = _N;
  static constexpr std::size_t InputSize = N;

  ODE()
  {
    // matrix with ones on super-diagonal
    A_.setZero();
    A_.template block(0, 1, N - 1, N - 1).diagonal().setOnes();
  }

  template<typename Derived>
  Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, 1>
  operator()(const Eigen::MatrixBase<Derived> & x) const
  {
    using scalar_t = typename Derived::Scalar;
    using state_t = Eigen::Matrix<typename Derived::Scalar, Derived::RowsAtCompileTime, 1>;

    auto x0 = x.eval();
    const auto Ac = A_.template cast<scalar_t>().eval();

    boost::numeric::odeint::integrate_n_steps(
      boost::numeric::odeint::runge_kutta4<
        state_t, scalar_t, state_t, scalar_t, boost::numeric::odeint::vector_space_algebra
      >{},
      [&Ac](const state_t & x, state_t & dxdt, const scalar_t) {
        dxdt = Ac * x;
      },
      x0, scalar_t{0.}, scalar_t{0.01}, 100
    );

    return x0;
  }

private:
  Eigen::Matrix<double, N, N> A_;

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

/**
 * Three-layer fully connected neural network with one channel and tanh activations
 *
 * Weights are statically allocated
 *
 * f: R^N -> R
 */
template<std::size_t _N>
struct NeuralNet
{
  static constexpr char name[] = "NeuralNet";
  static constexpr std::size_t N = _N;
  static constexpr std::size_t InputSize = N;

  NeuralNet()
  {
    std::minstd_rand gen(101);  // fixed seed
    std::normal_distribution<double> dis(0, 1);
    auto gen_fcn = [&]() {return dis(gen);};

    W1 = Eigen::Matrix<double, n1, n0>::NullaryExpr(gen_fcn);
    W2 = Eigen::Matrix<double, n2, n1>::NullaryExpr(gen_fcn);
    W3 = Eigen::Matrix<double, n3, n2>::NullaryExpr(gen_fcn);
  }

  template<typename Derived>
  Eigen::Matrix<typename Derived::Scalar, 1, 1>
  operator()(const Eigen::MatrixBase<Derived> & x) const
  {
    const auto z1 = (W1.template cast<typename Derived::Scalar>() * x.normalized()).eval();
    const auto a1 = z1.array().tanh().matrix().eval();

    const auto z2 = (W2.template cast<typename Derived::Scalar>() * a1).eval();
    const auto a2 = z2.array().tanh().matrix().eval();

    const auto z3 = (W3.template cast<typename Derived::Scalar>() * a2).eval();
    const auto a3 = z3.array().tanh().matrix().eval();

    return Eigen::Matrix<typename Derived::Scalar, 1, 1>(
      (a3 - Eigen::Matrix<typename Derived::Scalar, n3, 1>::Ones()).squaredNorm()
    );
  }

private:
  static constexpr std::size_t n0 = N;
  static constexpr std::size_t n1 = std::max<int>(1, n0 / 2);
  static constexpr std::size_t n2 = std::max<int>(1, n1 / 2);
  static constexpr std::size_t n3 = std::max<int>(1, n2 / 2);

  Eigen::Matrix<double, n1, n0> W1;
  Eigen::Matrix<double, n2, n1> W2;
  Eigen::Matrix<double, n3, n2> W3;

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

/**
 * Camera reprojection error for N points
 *
 * f: R^6 -> R
 *
 * f(x) = \sum_i ( x_C_i - proj(CM * (P_CW * exp(x)) * x_W_i) ) .^ 2
 *
 * where - x_C_i the i:th 2d pixel point
 *       - x_W_i the i:th 3d world point
 *       - P_CW a nominal camera pose
 *       - x is a tangent space element defining an incremental pose
 */
template<std::size_t _N>
struct ReprojectionError
{
  static constexpr char name[] = "Reprojection";
  static constexpr std::size_t N = _N;
  static constexpr std::size_t InputSize = 6;

  ReprojectionError()
  {
    // nominal pose
    P_CW_nom = SE3<double>{
      Eigen::Quaterniond::Identity(),
      Eigen::Vector3d{0.1, -0.3, 0.2}
    };

    // camera matrix
    CM.setZero();
    CM(0, 0) = 700;  // fx
    CM(1, 1) = 690;  // fy
    CM(0, 2) = 320;  // cx
    CM(1, 2) = 240;  // cy
    CM(2, 2) = 1;

    // generate random data
    std::minstd_rand gen(101);  // fixed seed
    std::normal_distribution<double> dis(0, 1);
    auto gen_fcn = [&]() {return dis(gen);};

    for (std::size_t i = 0; i != N; ++i) {
      pts_world[i] = Eigen::Vector3d{0, 0, 3} + Eigen::Vector3d::NullaryExpr(gen_fcn);
      Eigen::Vector3d proj = CM * (P_CW_nom * pts_world[i]);
      pts_image[i] = proj.template head<2>() / proj(2);
    }
  }

  template<typename Derived>
  Eigen::Matrix<typename Derived::Scalar, 1, 1>
  operator()(const Eigen::MatrixBase<Derived> & x) const
  {
    using Scalar = typename Derived::Scalar;
    using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
    using Quat = Eigen::Quaternion<Scalar>;

    SE3<Scalar> P_CW = SE3<Scalar>::exp(Scalar{0.01} * x) * P_CW_nom.template cast<Scalar>();
    auto CMc = CM.template cast<Scalar>().eval();

    // Transform world points to camera frame, re-project, square
    Eigen::Matrix<Scalar, 1, 1> ret(0);
    for (std::size_t i = 0; i != N; ++i) {
      Vec3 proj = CMc * (P_CW * pts_world[i].template cast<Scalar>().eval());
      ret(0) +=
        (proj.template head<2>() / proj(2) - pts_image[i].template cast<Scalar>()).squaredNorm();
    }
    return ret;
  }

private:
  Eigen::Matrix<double, 3, 3> CM;            // camera matrix
  SE3<double> P_CW_nom{};                    // nominal camera pose
  std::array<Eigen::Vector3d, N> pts_world;  // points in world frame
  std::array<Eigen::Vector2d, N> pts_image;  // points in image plane

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
};

/**
 * Differentiate the end effector position in an N link robotic arm
 *
 * f: R^6 -> R^6
 */
template<std::size_t _N>
struct Manipulator
{
  static constexpr char name[] = "Manipulator";
  static constexpr std::size_t N = _N;
  static constexpr std::size_t InputSize = 6;

  Manipulator()
  {
    // generate random link positions
    std::minstd_rand gen(101);  // fixed seed
    std::uniform_real_distribution<double> dis(-1, 1);
    auto gen_fcn = [&]() {return dis(gen);};

    for (std::size_t i = 0; i != N; ++i) {
      link_pose[i] = SE3<double>{
        Eigen::AngleAxis(M_PI_2 * dis(gen), Eigen::Vector3d::UnitX()) *
        Eigen::AngleAxis(M_PI_2 * dis(gen), Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxis(M_PI_2 * dis(gen), Eigen::Vector3d::UnitZ()),
        Eigen::Vector3d{2 * dis(gen), 2 * dis(gen), 2 * dis(gen)}
      };
    }
  }

  template<typename Derived>
  Eigen::Matrix<typename Derived::Scalar, 3, 1>
  operator()(const Eigen::MatrixBase<Derived> & x) const
  {
    using Scalar = typename Derived::Scalar;
    SE3<Scalar> P = SE3<Scalar>::exp(x);

    for (std::size_t i = 0; i != N; ++i) {
      P *= link_pose[i].template cast<Scalar>();
    }

    return P * Eigen::Matrix<Scalar, 3, 1>::UnitX().eval();
  }

private:
  std::array<SE3<double>, N> link_pose;
};

/**
 * Integrate a system on SE(3) for N steps using the RK4 scheme
 *
 * f: R^6 -> R^6
 */
template<std::size_t _N>
struct SE3ODE
{
  static constexpr char name[] = "SE3ODE";
  static constexpr std::size_t N = _N;
  static constexpr std::size_t InputSize = 6;

  SE3ODE()
  {
    velocity << 0.1, -0.2, 0.3, 0.1, -0.2, 0.3;
    Pfinal = SE3<double>{} * SE3<double>::exp(static_cast<double>(N) * 0.01 * velocity);
  }

  template<typename Derived>
  Eigen::Matrix<typename Derived::Scalar, 6, 1>
  operator()(const Eigen::MatrixBase<Derived> & x) const
  {
    using scalar_t = typename Derived::Scalar;
    using state_t = SE3<scalar_t>;
    using deriv_t = typename state_t::Tangent;

    const auto vel_c = velocity.template cast<scalar_t>().eval();

    // set initial pose
    SE3<scalar_t> P = SE3<scalar_t>::exp(x);

    boost::numeric::odeint::integrate_n_steps(
      boost::numeric::odeint::runge_kutta4<state_t, scalar_t, deriv_t, scalar_t,
      boost::numeric::odeint::vector_space_algebra, lie_operations>{},
      [&vel_c](const state_t & X, deriv_t & dXdt, const scalar_t) {
        dXdt = vel_c;
      },
      P, scalar_t{0.}, scalar_t{0.01}, N
    );

    const SE3<scalar_t> Pfinalinv = Pfinal.inv().template cast<scalar_t>();
    return (Pfinalinv * P).log();
  }

private:
  Eigen::Matrix<double, 6, 1> velocity;
  SE3<double> Pfinal{};

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

#endif  // TESTS_HPP_


#pragma once

#include <boost/multiprecision/cpp_dec_float.hpp>
#include <cudd/cplusplus/cuddObj.hh>
#include <vector>

#include "number_representation.hpp"

namespace abo::error_metrics {

/**
 * @brief Computes bounds on the average relative difference between f and f_hat
 * It is defined as the average of |f(x) - f_hat(x)| / max(1, |f(x)|) for all inputs x
 * As it is not symmetric, it is not a metric in the mathematical sense
 * This function returns a range in which the actual average relative error is guaranteed to lie
 * The computed bounds are always within a factor of 2, meaning that the maximum error
 * returned by this function is at most twice the minimum error
 * @param mgr The BDD object manager
 * @param f The original function
 * @param f_hat The approximated function. Must have the same number of bits as f
 * @param num_rep The number representation for f and f_hat
 * @return {min, max}, the lower and upper bound on the average case relative error
 */
std::pair<boost::multiprecision::cpp_dec_float_100,
            boost::multiprecision::cpp_dec_float_100>
    acre_bounds(const Cudd& mgr, const std::vector<BDD>& f,
                  const std::vector<BDD>& f_hat,
                  const abo::util::NumberRepresentation num_rep
                        = abo::util::NumberRepresentation::BaseTwo);

/**
 * @brief Computes the average relative difference between f and f_hat
 * It is defined as the average of |f(x) - f_hat(x)| / max(1, |f(x)|) for all inputs x
 * As it is not symmetric, it is not a metric in the mathematical sense
 * The computation is performed with ADDs and might be quite slow
 * @param mgr The BDD object manager
 * @param f The original function
 * @param f_hat The approximated function. Must have the same number of bits as f
 * @param num_rep The number representation for f and f_hat
 * @return the average relative difference of the inputs
 */
boost::multiprecision::cpp_dec_float_100
    acre_add(const Cudd& mgr, const std::vector<BDD>& f,
           const std::vector<BDD>& f_hat,
           const abo::util::NumberRepresentation num_rep
                = abo::util::NumberRepresentation::BaseTwo);

/**
 * @brief Computes the average relative difference between f and f_hat
 * It is defined as the average of |f(x) - f_hat(x)| / max(1, |f(x)|) for all inputs x
 * As it is not symmetric, it is not a metric in the mathematical sense
 * The computation is performed with BDDs using a symbolic division and might be quite slow
 * @param mgr The BDD object manager
 * @param f The original function
 * @param f_hat The approximated function. Must have the same number of bits as f
 * @param num_extra_bits The number of additional fixed precision bits to use during the division
 * As the result of each division is not an integer, the result is described as a fixed point number
 * with exactly num_extra_bits bits with a lower significance than one. Roughly correlates the the
 * precision of the result
 * @param num_rep The number representation for f and f_hat
 * @return the average relative difference of the inputs
 */
boost::multiprecision::cpp_dec_float_100 acre_symbolic_division(
    const Cudd& mgr, const std::vector<BDD>& f, const std::vector<BDD>& f_hat,
    unsigned int num_extra_bits = 16,
    const abo::util::NumberRepresentation num_rep
        = abo::util::NumberRepresentation::BaseTwo);
} // namespace abo::error_metrics


#pragma once

#include <cstdlib>
#include <iostream>
#include <sstream>
#include <cassert>
#include <cmath>
#include <limits>
#include <type_traits>
#include <utility>
#include <vector>
#include <unordered_map>
#include <string_view>
#include <atomic>

#include <fmt/core.h>
#include <fmt/ostream.h>

#include <boost/functional/hash.hpp>
//#include <boost/align/aligned_allocator.hpp>

#ifdef __GNUC__
#define unlikely(x) __builtin_expect(!!(x), 0)
#define likely(x) __builtin_expect(!!(x), 1)
#else
#define unlikely(x) x
#define likely(x) x
#endif

namespace util
{

template<class T>
inline T Sqr(const T &x)
{
  return x*x;
}

template<class T>
inline T Cubed(const T &x)
{
  return x*x*x;
}

template<class T>
inline T Heaviside(const T &x)
{
  return x>T{} ? T{1} : T{0};
}

inline double Rcp(double x)
{
  return 1./x;
}

inline float Rcp(float x)
{
  return 1.f/x;
}

inline constexpr double Pow(double x, std::uint32_t e)
{
  // This is the binary exponentiation algorithm
  // https://de.wikipedia.org/wiki/Bin%C3%A4re_Exponentiation
  std::uint32_t bit = 1<< (sizeof(e)*8-1);
  double ret = 1.;
  while (bit)
  {
    ret = ret*ret;
    ret = (e&bit) ? ret*x : ret;
    bit >>= 1;
  }
  return ret;
}

template<class T>
inline constexpr T Modulus(T a, T m, std::enable_if_t<std::is_integral_v<T> && std::is_signed_v<T>>* = nullptr)
{
  //  Example for operator%: -5 % 3 = -2
  //  Should be one though. Can simply add m if result is negative.
  const T tmp = a % m;
  return tmp < 0 ? tmp + m : tmp;
}

template<class T>
inline constexpr T Modulus(T a, T m, std::enable_if_t<std::is_integral_v<T> && std::is_unsigned_v<T>>* = nullptr)
{
  return a % m;
}

template<class T>
inline constexpr T Modulus(T a, T m, std::enable_if_t<std::is_floating_point_v<T>>* = nullptr)
{
  //  Example for operator%: -5 % 3 = -2
  //  Should be one though. Can simply add m if result is negative.
  const T tmp = std::fmod(a, m);
  return tmp < 0 ? tmp + m : tmp;
}

// t = 0: Returns a
// t = 1: Returns b
// otherwise linear inter/extra-polation
template<class T, class U>
inline T Lerp(const T &a, const T &b, U t)
{
  return (std::remove_reference_t<U>(1) - t) * a + t * b;
}

// Note: Will happily take the signbit from zero. So the result for 0 is basically random.
template<class T, typename std::enable_if_t<std::is_floating_point<T>{}, int> = 0>
inline T Sign(const T &x)
{
  return std::copysign(T(1.), x);
}

// Also from PBRT. Used to compute error bounds for floating point arithmetic. See pg. 216.
template<class T>
inline constexpr T Gamma(int n) {
    constexpr T eps_half = std::numeric_limits<T>::epsilon();
    return (n * eps_half) / (1 - n * eps_half);
}

template<class T>
inline bool Quadratic(T a, T b, T c, T &t0, T &t1)
{
  //from PBRT pg. 1080
  const T d = b*b - T(4)*a*c;
  if (d < T(0))
    return false;
  const T sd = std::sqrt(d);
  const T q = b<0 ? -b+sd : -b-sd;
  t0 = q/T(2)/a;
  t1 = T(2)*c/q;
  if (t0 > t1)
    std::swap(t0 ,t1);
  return true;
}

namespace quadratic_internal
{

template<class T>
inline T Errorformula1(T A, T B, T C, T D, T sD, T eA, T eB, T eC)
{
  constexpr T eps = std::numeric_limits<T>::epsilon();
  const T xi = B<T(0) ? T(1) : T(-1);
  const T G = B-xi*sD;
  const T Ainv = T(1)/A;
  const T sDinv = T(1)/sD;
  const T E1 = std::abs(G*Ainv) + T(3)/T(4)*std::abs(sD*Ainv) + std::abs(C*sDinv) + std::abs(B*B*Ainv*sDinv)/T(4);
  const T E2 = eA*std::abs((C*xi*Ainv*sDinv - G*Ainv*Ainv/T(2))) + eB/T(2)*std::abs(Ainv*(B*xi*sDinv-T(1))) + eC*std::abs(sDinv);
  return eps*E1 + E2;
}

template<class T>
inline T Errorformula2(T A, T B, T C, T D, T sD, T eA, T eB, T eC)
{
  constexpr T eps = std::numeric_limits<T>::epsilon();
  const T xi = B<T(0) ? T(1) : T(-1);
  const T G = B - xi*sD;
  const T sDinv = T(1)/sD;
  const T GGsDinv = T(1)/(G*G*sD);
  const T E1 = std::abs(GGsDinv)*(T(4)*std::abs(C*G*sD) + T(3)*std::abs(C*D) + T(4)*std::abs(A*C*C) + std::abs(B*B*C));
  const T E2 = std::abs(GGsDinv)*(eA*T(4)*std::abs(C*C)+ T(2)*eB*std::abs(C*(B*xi - sD)) + eC*std::abs((T(4)*A*C*xi - T(2)*G*sD)));
  return eps*E1 + E2;
}

};

template<class T>
inline bool Quadratic(T a, T b, T c, T ea, T eb, T ec, T &t0, T &t1, T &err0, T &err1)
{
  using namespace quadratic_internal;
  //from PBRT pg. 1080
  const T d = b*b - T(4)*a*c;
  if (d < T(0))
    return false;
  const T sd = std::sqrt(d);
  err0 = Errorformula1(a, b, c, d, sd, ea, eb, ec);
  err1 = Errorformula2(a, b, c, d, sd, ea, eb, ec);
  const T q = b<0 ? -b+sd : -b-sd;
  t0 = q/T(2)/a;
  t1 = T(2)*c/q;
  if (t0 > t1)
  {
    std::swap(t0 ,t1);
    std::swap(err0, err1);
  }
  return true;
}

inline bool startswith(const std::string &a, const std::string &b)
{
  if (a.size() < b.size())
    return false;
  return a.substr(0, b.size()) == b;
}

inline bool endswith(const std::string &a, const std::string &b)
{
  if (a.size() < b.size())
    return false;
  return a.substr(a.size()-b.size(), b.size()) == b;
}

//  There is no hash support for pairs in the STL.
template<class A, class B>
struct pair_hash
{
  std::size_t operator()(const std::pair<A,B> &v) const
  {
    std::size_t seed = boost::hash_value(v.first);
    boost::hash_combine(seed, boost::hash_value(v.second));
    return seed;
  }
};

// Copy & Paste Fu! https://stackoverflow.com/questions/27140778/range-based-for-with-pairiterator-iterator
template <typename I>
struct iter_pair : std::pair<I, I>
{
    using std::pair<I, I>::pair;

    I begin() { return this->first; }
    I end() { return this->second; }
};

#if 1
template<class T, size_t alignment>
class AlignedAllocator
{
  public:
    using value_type = T;
    using propagate_on_container_move_assignment = std::true_type;
    using is_always_equal = std::true_type;

    static constexpr size_t ComputeTrueAlignment()
    {
      // By default we may have some random align a. But if type specify
      // alignas(b) with b>a, I'd get crashes without this little correction.
      if constexpr (std::is_void_v<T>)
        return alignment;
      else
        return std::max(alignment, alignof(T));
    }

    static constexpr size_t true_alignment = ComputeTrueAlignment();
    // Check requirements for posix_memalign.
    static_assert(true_alignment % sizeof(void*) == 0);

    template<class U>
    struct rebind {
      typedef AlignedAllocator<U, alignment> other;
    };

    constexpr AlignedAllocator() noexcept = default;

    template <class U>
    constexpr AlignedAllocator(const AlignedAllocator<U, alignment>&) noexcept
    {
    }

    [[nodiscard]] T* allocate(std::size_t n)
    {
      #ifdef _MSC_VER
            return (T*)_aligned_malloc(n*sizeof(T), true_alignment);
      #else
            void* result = nullptr;
            posix_memalign(&result, true_alignment, n*sizeof(T)); // returns 0 on success. Not using it obviously.
            return (T*)result;
      #endif
    }

    void deallocate(T* p, std::size_t n)
    {
      #ifdef _MSC_VER
        _aligned_free(p);
      #else
        free(p);
      #endif
    }
};

template <class T, class U, size_t a>
bool operator==(const AlignedAllocator<T,a>&, const AlignedAllocator<U,a>&) noexcept
{
  return true;
}

template <class T, class U, size_t a>
bool operator!=(const AlignedAllocator<T, a>&, const AlignedAllocator<U, a>&) noexcept
{
  return false;
}
#else
template<class T, size_t a>
using AlignedAllocator = boost::alignment::aligned_allocator<T,a>;
#endif

// std::vector with 16 byte aligment as required by Eigen's fixed size types.
// This class also comes with range checking in debug mode.
template<class T, class Alloc = AlignedAllocator<T,16>>
class ToyVector : public std::vector<T, Alloc>
{
  using B = std::vector<T, Alloc>;
public:
  using B::B;

  inline typename B::const_reference operator[](typename B::size_type i) const
  {
    assert(i >= 0 && i<B::size());
    return B::operator[](i);
  }

  inline typename B::reference operator[](typename B::size_type i)
  {
    assert(i >= 0 && i<B::size());
    return B::operator[](i);
  }
};

template<class K, class T, class F, class Hash, class Pred>
inline T GetOrInsertFromFactory(std::unordered_map<K, T, Hash, Pred> &m, const K &k, F factory)
{
  auto it = m.find(k);
  if (it == m.end())
  {
    auto& t = m[k] = factory();
    return t;
  }
  else
    return it->second;
}

template<class T>
inline T ASSERT_NOT_NULL(T x, typename std::enable_if<std::is_pointer<T>::value>::type* = 0)
{
  assert(x != nullptr);
  return x;
}

// Adapted from http://the-witness.net/news/2012/11/scopeexit-in-c11/
template <typename F>
struct ScopeExit {
    ScopeExit(F f) : f(f) {}
    ~ScopeExit() { f(); }
    F f;
};

template <typename F>
ScopeExit<F> MakeScopeExit(F f) {
    return ScopeExit<F>(f);
};

#define SCOPE_EXIT(code) \
    auto scope_exit_ ## __LINE__ = util::MakeScopeExit([=](){ code })

inline int RowMajorOffset(int x, int y, int size_x, int size_y)
{
  return x + y*size_x;
}

inline std::pair<int, int> RowMajorPixel(int offset, int size_x, int size_y)
{
  int y = offset / size_x;
  int x = offset - y*size_x;
  return std::make_pair(x,y);
}

template<class T>
struct enable_if_has_size_member
{
    using type = decltype(std::declval<T&>().size());
};

template<class Container>
inline int isize(const Container &c, typename enable_if_has_size_member<Container>::type = 0)
{
    return static_cast<int>(c.size());
}

template<class Container>
inline long lsize(const Container &c, typename enable_if_has_size_member<Container>::type = 0)
{
  return static_cast<long>(c.size());
}

template<class T>
struct enable_if_has_insert_begin_and_end_members
{
  using type = decltype(std::declval<T&>().insert(
    std::declval<T&>().begin(),
    std::declval<T&>().end(),
    std::declval<T&>().begin() // here this makes actually sense.
  ));
};

template<class Container, typename = typename enable_if_has_insert_begin_and_end_members<Container>::type>
inline void Append(Container &a, const Container &b)
{
  a.insert(a.end(), b.begin(), b.end());
}

template<class T, class Alloc>
inline void PushBackToEnsureSize(std::vector<T, Alloc> &v, std::size_t required_size, const T &filler)
{
  assert(required_size >= v.size());
  std::fill_n(std::back_inserter(v), required_size - v.size(), filler);
}

template<class T>
struct has_begin_end
{
  using type1 = decltype(std::declval<T&>().begin());
  using type2 = decltype(std::declval<T&>().end());
  static constexpr bool value = true;
};

template<class Container, class Func, typename = std::enable_if_t<has_begin_end<Container>::value>>
inline auto TransformVector(Container &a, Func &&f)
{
  using TIn = typename Container::reference;
  using TOut = std::invoke_result_t<Func, TIn>;
  using OutContainer = ToyVector<TOut>;
  OutContainer result;
  std::transform(a.begin(), a.end(), std::back_inserter(result), f);
  return result;
}

// Adapted from https://stackoverflow.com/questions/41660062/how-to-construct-an-stdarray-with-index-sequence
// Use variadic templates and a pack of integers to build the output array without
// invoking default c'tors.
namespace detail {
  template<typename T, typename U, typename F, std::size_t... Is>
  constexpr auto transform_array(F& f, const U* input, std::index_sequence<Is...>)
     -> std::array<T, sizeof...(Is)>
  {
    return {{f(input[std::integral_constant<std::size_t, Is>{}])...}};
  }
}

template<class Func, class T, std::size_t n>
inline constexpr auto TransformArray(std::array<T,n> &a, Func &&f)
{
  using TOut = std::invoke_result_t<Func, T>;
  return detail::transform_array<TOut>(f, a.data(), std::make_index_sequence<n>{});
}

// From https://stackoverflow.com/questions/41660062/how-to-construct-an-stdarray-with-index-sequence
// Use variadic templates and a pack of integers to build the output array without
// invoking default c'tors.
namespace detail {
  template<typename T, typename F, std::size_t... Is>
  constexpr auto generate_array(F& f, std::index_sequence<Is...>)
   -> std::array<T, sizeof...(Is)> {
    return {{f(std::integral_constant<std::size_t, Is>{})...}};
  }
}

template<std::size_t N, typename F>
inline constexpr auto GenerateArray(F &&f) {
  using TOut = std::invoke_result_t<F, std::size_t>;
  return detail::generate_array<TOut>(f, std::make_index_sequence<N>{});
}

/*
 * Perform an atomic addition to the float via spin-locking
 * on compare_exchange_weak. Memory ordering is release on write
 * consume on read
 *
 * from https://www.reddit.com/r/cpp/comments/338pcj/atomic_addition_of_floats_using_compare_exchange/
 */
inline float AtomicAdd(std::atomic<float> &f, float d) {
  float old = f.load(std::memory_order_consume);
  float desired = old + d;  while (!f.compare_exchange_weak(old, desired,
    std::memory_order_release, std::memory_order_consume))
  {
    desired = old + d;
  }
  return desired;
}

// From https://arne-mertz.de/2018/05/overload-build-a-variant-visitor-on-the-fly/
template <class ...Fs>
struct Overload : Fs... {
  template <class ...Ts>
  Overload(Ts&& ...ts) : Fs{std::forward<Ts>(ts)}...
  {}

  using Fs::operator()...;
};

template <class ...Ts>
Overload(Ts&&...) -> Overload<std::remove_reference_t<Ts>...>;

// TODO: check if we have an iterator in It
template<class It, class Trafo>
inline std::string Join(const std::string &sep, It begin, It end, Trafo trafo)
{
  if (begin == end)
    return {};
  std::ostringstream os;
  It prev = begin;
  ++begin;
  while (begin != end)
  {
    os << trafo(*prev) << sep;
    prev = begin;
    ++begin;
  }
  os << trafo(*prev);
  return os.str();
}

} // namespace util

using util::Overload;
using util::Sqr;
using util::Cubed;
using util::Heaviside;
using util::Rcp;
using util::Lerp;
using util::Sign;
using util::ToyVector;
using util::isize;
using util::lsize;
using util::AlignedAllocator;
using util::ASSERT_NOT_NULL;