//
//  dt_util.cpp
//  Classifer_RF
//
//  Created by jimmy on 2017-02-16.
//  Copyright (c) 2017 Nowhere Planet. All rights reserved.
//

#include "dt_util.hpp"
#include <Eigen/QR>
#include <iostream>
#include <map>

using std::cout;
using std::endl;
using std::map;

namespace dt {
    template< class T>
    vector<T> randomDimension(const T dim, const T num)
    {
        assert(dim > 0);
        assert(num > 0);
        assert(num <= dim);

        vector<T> dims;
        for (T i = 0; i<dim; i++) {
            dims.push_back(i);
        }
        std::random_shuffle(dims.begin(), dims.end());
        vector<T> random_dim(dims.begin(), dims.begin() + num);
        assert(random_dim.size() > 0 && random_dim.size() <= dims.size());

        return random_dim;
    }

    template< class vectorType>
    void meanStd(const vector<vectorType> & labels, vectorType & mean, vectorType & sigma)
    {
        assert(labels.size() > 0);

        mean = vectorType::Zero(labels[0].size());

        for (int i = 0; i<labels.size(); i++) {
            mean += labels[i];
        }
        mean /= labels.size();

        sigma = vectorType::Zero(labels[0].size());
        if (labels.size() == 1) {
            return;
        }
        for (int i = 0; i<labels.size(); i++) {
            vectorType dif = labels[i] - mean;
            for (int j = 0; j<sigma.size(); j++) {
                sigma[j] += dif[j] * dif[j];
            }
        }
        for (int j = 0; j<sigma.size(); j++) {
            sigma[j] = sqrt(fabs(sigma[j])/labels.size());
        }
    }

    template<class vectorType, class intType>
    void meanStd(const vector<vectorType> & data, const vector<intType> & indices,
                 vectorType & mean, vectorType & sigma)
    {
        assert(data.size() > 0);
        assert(indices.size() > 0);

        assert(indices.size() > 0);

        mean = vectorType::Zero(data[0].size());

        for (int i = 0; i<indices.size(); i++) {
            int index = indices[i];
            assert(index >= 0 && index < data.size());
            mean += data[index];
        }
        mean /= indices.size();

        sigma = vectorType::Zero(data[0].size());
        if (indices.size() == 1) {
            return;
        }
        for (int i = 0; i<indices.size(); i++) {
            vectorType dif = data[indices[i]] - mean;
            for (int j = 0; j<sigma.size(); j++) {
                sigma[j] += dif[j] * dif[j];
            }
        }
        for (int j = 0; j<sigma.size(); j++) {
            sigma[j] = sqrt(fabs(sigma[j])/indices.size());
        }
    }

    template <class intType>
    vector<intType> balanceSamples(const vector<intType> & example_indices, const vector<intType> & labels, const int category_num)
    {
        assert(example_indices.size() <= labels.size());

        // step 1: count example numbers in each category
        vector<intType> count(category_num, 0);
        for (int i = 0; i<example_indices.size(); i++) {
            intType idx = example_indices[i];
            count[labels[idx]]++;
        }
        intType min_count = *std::min_element(count.begin(), count.end());
        assert(min_count >= 0);

        // step 2: select the first min_count example in each category
        count = vector<int>(category_num, 0);
        vector<intType> balanced_indices;
        for (int i = 0; i<example_indices.size(); i++) {
            intType idx = example_indices[i];
            intType cur_label = labels[idx];

            // skip this category
            if (count[cur_label] >= min_count) {
                continue;
            }
            else {
                balanced_indices.push_back(idx);
                count[cur_label]++;
            }
        }
        return balanced_indices;
    }

    template<class VectorType, class IntType>
    double sumOfVariance(const vector<VectorType> & labels, const vector<IntType> & indices)
    {
        if (indices.size() <= 0) {
            return 0.0;
        }
        assert(indices.size() > 0);

        VectorType mean = VectorType::Zero(labels[0].size());

        for (int i = 0; i<indices.size(); i++) {
            IntType index = indices[i];
            assert(index >= 0 && index < labels.size());
            mean += labels[index];
        }
        mean /= indices.size();

        double var = 0.0;
        for (int i = 0; i<indices.size(); i++) {
            IntType index = indices[i];
            assert(index >= 0 && index < labels.size());
            VectorType dif = labels[index] - mean;
            for (int j = 0; j<dif.size(); j++) {
                var += dif[j] * dif[j];
            }
        }
        return var;
    }

    template<class intType>
    intType mostCommon(const vector<intType> & data)
    {
        assert(data.size() > 0);
        int max_count = 0;
        int most_common = 0;
        std::map<intType, int> m;
        for (auto vi = data.begin(); vi != data.end(); vi++) {
            m[*vi]++;
            if (m[*vi] > max_count) {
                max_count = m[*vi];
                most_common = *vi;
            }
        }
        return most_common;
    }

    template <class T>
    void meanMedianError(const vector<T> & errors,
                                 T & mean,
                                 T & median)
    {
        assert(errors.size() > 0);
        const int dim = (int)errors[0].size();
        mean = T::Zero(dim);
        median = T::Zero(dim);

        vector<vector<double> > each_dim_data(dim);
        for (int i = 0; i<errors.size(); i++) {
            T err = errors[i].cwiseAbs();
            mean += err;
            for (int j = 0; j<err.size(); j++) {
                each_dim_data[j].push_back(err[j]);
            }
        }
        mean /= errors.size();

        for (int i = 0; i<each_dim_data.size(); i++) {
            std::sort(each_dim_data[i].begin(), each_dim_data[i].end());
            median[i] = each_dim_data[i][each_dim_data[i].size()/2];
        }
    }

    template vector<int> randomDimension(int dim, int num);

    template void meanStd(const vector<Eigen::VectorXd> & labels, Eigen::VectorXd & mean, Eigen::VectorXd & sigma);
    template void meanStd(const vector<Eigen::Vector3d> & labels, Eigen::Vector3d & mean, Eigen::Vector3d & sigma);

    template void meanStd(const vector<Eigen::VectorXf> & labels, const vector<int> & indices,
                          Eigen::VectorXf & mean, Eigen::VectorXf & sigma);

    template
    vector<int> balanceSamples(const vector<int> & example_indices, const vector<int> & labels, const int category_num);

    template
    double sumOfVariance(const vector<Eigen::VectorXf> & labels, const vector<int> & indices);

    template
    int mostCommon(const vector<int> & data);

    template
    void meanMedianError(const vector<Eigen::VectorXf> & errors, Eigen::VectorXf & mean, Eigen::VectorXf & median);

    template
    void meanMedianError(const vector<Eigen::VectorXd> & errors, Eigen::VectorXd & mean, Eigen::VectorXd & median);
}

vector<unsigned int> DTUtil::randomDimensions(const int dimension, const int candidate_dimension)
{
    assert(dimension > 0);
    assert(candidate_dimension > 0);
    assert(candidate_dimension <= dimension);

    vector<unsigned int> dims;
    for (unsigned int i = 0; i<dimension; i++) {
        dims.push_back(i);
    }
    std::random_shuffle(dims.begin(), dims.end());
    vector<unsigned int> random_dim(dims.begin(), dims.begin() + candidate_dimension);
    assert(random_dim.size() > 0 && random_dim.size() <= dims.size());

    return random_dim;
}

template <class T>
double DTUtil::spatialVariance(const vector<T> & labels, const vector<unsigned int> & indices)
{
    if (indices.size() <= 0) {
        return 0.0;
    }
    assert(indices.size() > 0);

    T mean = T::Zero(labels[0].size());

    for (int i = 0; i<indices.size(); i++) {
        int index = indices[i];
        assert(index >= 0 && index < labels.size());
        mean += labels[index];
    }
    mean /= indices.size();

    double var = 0.0;
    for (int i = 0; i<indices.size(); i++) {
        int index = indices[i];
        assert(index >= 0 && index < labels.size());
        T dif = labels[index] - mean;
        for (int j = 0; j<dif.size(); j++) {
            var += dif[j] * dif[j];
        }
    }
    return var;
}

template<class T>
double DTUtil::fullVariance(const vector<T>& labels, const vector<unsigned int> & indices)
{
    assert(indices.size() > 1);
    typedef typename T::Scalar Scalar;
    typedef typename Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixType;

    double loss = 0.0;

    const int length = (int)labels[0].size();
    MatrixType sampled_data(indices.size(), length);
    for (unsigned i = 0; i<indices.size(); i++) {
        unsigned int index = indices[i];
        assert(index >= 0 && index < labels.size());
        sampled_data.row(i) = labels[index];
    }

    MatrixType centered = sampled_data.rowwise() - sampled_data.colwise().mean();
    MatrixType cov = (centered.adjoint() * centered) / sampled_data.rows();

    Eigen::ColPivHouseholderQR<MatrixType> qr(cov);
    if (qr.rank() == length) {
        loss = qr.logAbsDeterminant();
    }
    else {
        loss = qr.logAbsDeterminant();
        // avoid underflow
        if (std::isnan(loss) || std::isinf(loss)) {
            loss = log(0.0000001);
        }
        //printf("Warning: full variance underflow, use a small number log(0.0000001) instead. Sample number %ld\n", indices.size());
        //cout<<"covariance matrix \n"<<cov<<endl;
        //printf("logAbsDeterminant vs log(0.0000001): %lf, %lf\n", qr.logAbsDeterminant(), log(0.0000001));
    }

    return loss;
}

template <class MatrixType>
double DTUtil::sumOfVariance(const vector<MatrixType> & labels, const int row_index, const vector<unsigned int> & indices)
{
    typedef typename MatrixType::Scalar Scalar;
    typedef typename Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ScalarVector;

    if (indices.size() <= 0) {
        return 0.0;
    }
    assert(indices.size() > 0);

    ScalarVector mean = ScalarVector::Zero(labels[0].row(0).size());

    for (int i = 0; i<indices.size(); i++) {
        int index = indices[i];
        assert(index >= 0 && index < labels.size());
        mean += labels[index].row(row_index);
    }
    mean /= indices.size();

    double var = 0.0;
    for (int i = 0; i<indices.size(); i++) {
        int index = indices[i];
        assert(index >= 0 && index < labels.size());
        ScalarVector dif = labels[index].row(row_index) - mean;
        for (int j = 0; j<dif.size(); j++) {
            var += dif[j] * dif[j];
        }
    }
    return var;
}

template<class Type1, class Type2>
double DTUtil::spatialVariance(const vector<Type1> & labels, const vector<unsigned int> & indices, const vector<Type2> & wt)
{
    if (indices.size() <= 0) {
        return 0.0;
    }
    assert(indices.size() > 0);
    assert(wt.size() == labels.front().size());

    Type1 mean = Type1::Zero(labels[0].size());

    for (int i = 0; i<indices.size(); i++) {
        int index = indices[i];
        assert(index >= 0 && index < labels.size());
        mean += labels[index];
    }
    mean /= indices.size();

    double var = 0.0;
    for (int i = 0; i<indices.size(); i++) {
        int index = indices[i];
        assert(index >= 0 && index < labels.size());
        Type1 dif = labels[index] - mean;
        for (int j = 0; j<dif.size(); j++) {
            var += dif[j] * dif[j] * fabs(double(wt[j]));
        }
    }
    return var;
}

template <class T>
void DTUtil::meanStddev(const vector<T> & labels, const vector<unsigned int> & indices, T & mean, T & sigma)
{
    assert(indices.size() > 0);

    mean = T::Zero(labels[0].size());

    for (int i = 0; i<indices.size(); i++) {
        int index = indices[i];
        assert(index >= 0 && index < labels.size());
        mean += labels[index];
    }
    mean /= indices.size();

    sigma = T::Zero(labels[0].size());
    if (indices.size() == 1) {
        return;
    }
    for (int i = 0; i<indices.size(); i++) {
        T dif = labels[indices[i]] - mean;
        for (int j = 0; j<sigma.size(); j++) {
            sigma[j] += dif[j] * dif[j];
        }
    }
    for (int j = 0; j<sigma.size(); j++) {
        sigma[j] = sqrt(fabs(sigma[j])/indices.size());
    }
}

template <class vectorT, class indexT>
vectorT DTUtil::mean(const vector<vectorT> & data, const vector<indexT> & indices)
{
    assert(indices.size() > 0);

    vectorT m = vectorT::Zero(data[0].size());

    for (int i = 0; i<indices.size(); i++) {
        int index = indices[i];
        assert(index >= 0 && index < data.size());
        m += data[index];
    }
    m /= indices.size();

    return m;
}

template <class T>
T DTUtil::mean(const vector<T> & data)
{
    assert(data.size() > 0);

    T m = T::Zero(data[0].size());

    for (int i = 0; i<data.size(); i++) {
        m += data[i];
    }
    m /= data.size();
    return m;
}

template <class matrixType, class vectorType>
void DTUtil::rowMeanStddev(const vector<matrixType> & labels, const vector<unsigned int> & indices, const int row_index, vectorType & mean, vectorType & sigma)
{
    assert(indices.size() > 0);

    mean = vectorType::Zero(labels[0].row(0).size());

    for (int i = 0; i<indices.size(); i++) {
        int index = indices[i];
        assert(index >= 0 && index < labels.size());
        mean += labels[index].row(row_index);
    }
    mean /= indices.size();

    sigma = vectorType::Zero(labels[0].row(0).size());
    if (indices.size() == 1) {
        return;
    }
    for (int i = 0; i<indices.size(); i++) {
        vectorType dif = labels[indices[i]].row(row_index) - mean;
        for (int j = 0; j<sigma.size(); j++) {
            sigma[j] += dif[j] * dif[j];
        }
    }
    for (int j = 0; j<sigma.size(); j++) {
        sigma[j] = sqrt(fabs(sigma[j])/indices.size());
    }
}

template<class vectorT>
void DTUtil::quartileError(const vector<vectorT> & errors, vectorT& q1, vectorT& q2, vectorT& q3)
{
    assert(errors.size() > 0);
    const int dim = (int)errors[0].size();

    q1 = vectorT::Zero(dim);
    q2 = vectorT::Zero(dim);
    q3 = vectorT::Zero(dim);

    vector<vector<double> > each_dim_data(dim);
    for (int i = 0; i<errors.size(); i++) {
        vectorT err = errors[i].cwiseAbs();
        for (int j = 0; j<err.size(); j++) {
            each_dim_data[j].push_back(err[j]);
        }
    }

    for (int i = 0; i<each_dim_data.size(); i++) {
        std::sort(each_dim_data[i].begin(), each_dim_data[i].end());
        q1[i] = each_dim_data[i][each_dim_data[i].size()/4];
        q2[i] = each_dim_data[i][each_dim_data[i].size()/2];
        q3[i] = each_dim_data[i][each_dim_data[i].size()/4*3];
    }
}

template <class MatrixType>
void DTUtil::matrixMeanError(const vector<MatrixType> & errors, MatrixType & mean)
{
    assert(errors.size() > 0);

    const int cols  = (int)errors[0].cols();
    const int rows = (int)errors[0].rows();
    mean  = MatrixType::Zero(rows, cols);

    for (int i = 0; i<errors.size(); i++) {
        mean += errors[i];
    }
    mean /= errors.size();
}

double DTUtil::crossEntropy(const Eigen::VectorXd & prob)
{
    double entropy = 0.0;
    for (int i = 0; i<prob.size(); i++) {
        double p = prob[i];
        if (p == 0.0) {
            continue;
        }
        assert(p > 0 && p <= 1);
        entropy += - p * std::log(p);
    }
    return entropy;
}

double DTUtil::crossEntropy(const Eigen::VectorXf& prob)
{
    double entropy = 0.0;
    for (int i = 0; i<prob.size(); i++) {
        double p = prob[i];
        if (p == 0.0) {
            continue;
        }
        assert(p > 0 && p <= 1);
        entropy += - p * std::log(p);
    }
    return entropy;
}

double DTUtil::balanceLoss(const int leftNodeSize, const int rightNodeSize)
{
    double dif = leftNodeSize - rightNodeSize;
    double num = leftNodeSize + rightNodeSize;
    double loss = fabs(dif)/num;
    assert(loss >= 0);
    return loss;
}

bool
DTUtil::isSameLabel(const vector<unsigned int> & labels, const vector<unsigned int> & indices)
{
    assert(indices.size() >= 1);
    unsigned label = labels[indices[0]];
    for (int i = 1; i<indices.size(); i++) {
        if (label != labels[indices[i]]) {
            return false;
        }
    }
    return true;
}

bool DTUtil::isSameLabel(const vector<int>& labels, const vector<int>& indices)
{
    assert(indices.size() >= 1);
    int label = labels[indices[0]];
    for (int i = 1; i<indices.size(); i++) {
        if (label != labels[indices[i]]) {
            return false;
        }
    }
    return true;
}

int DTUtil::minLabelNumber(const vector<unsigned int> & labels, const vector<unsigned int> & indices,
                           const int num_category)
{
    vector<int> num(num_category, 0);
    for (int i = 0; i<indices.size(); i++) {
        int label = labels[indices[i]];
        num[label]++;
    }
    return *std::min_element(num.begin(), num.end());
}

int DTUtil::minLabelNumber(const vector<VectorXi> & labels,
                           const vector<unsigned int> & indices,
                           const int time_step,
                           const int num_category)
{
    vector<int> num(num_category, 0);
    for (int i = 0; i<indices.size(); i++) {
        int label = labels[indices[i]][time_step];
        num[label]++;
    }

    return *std::min_element(num.begin(), num.end());
}

template <class integerType>
Eigen::MatrixXd DTUtil::confusionMatrix(const vector<integerType> & preds,
                                        const vector<integerType> & labels,
                                        const int category_num,
                                        bool normalize)
{
    assert(preds.size() == labels.size());
    assert(category_num > 0);

    Eigen::MatrixXd confusion = Eigen::MatrixXd::Zero(category_num, category_num);
    for (int i = 0; i<preds.size(); i++) {
        confusion(labels[i], preds[i]) += 1.0;
    }
    if (normalize) {
        confusion = 1.0 / preds.size() * confusion;
    }
    return confusion;
}

Eigen::VectorXd DTUtil::accuracyFromConfusionMatrix(const Eigen::MatrixXd & conf)
{
    assert(conf.rows() == conf.cols());

    Eigen::VectorXd acc = Eigen::VectorXd(conf.rows() + 1, 1);
    Eigen::VectorXd row_sum = conf.rowwise().sum();
    double all_sum = conf.sum();
    double trace = conf.trace();
    for (int r = 0; r<conf.rows(); r++) {
        acc[r] = conf(r ,r)/row_sum[r];
    }
    acc[conf.rows()] = trace/all_sum;
    return acc;
}

Eigen::VectorXd DTUtil::precisionFromConfusionMatrix(const Eigen::MatrixXd & conf)
{
    assert(conf.rows() == conf.cols());

    Eigen::VectorXd precision = Eigen::VectorXd(conf.rows() + 1, 1);
    Eigen::VectorXd row_sum = conf.rowwise().sum();
    double all_sum = conf.sum();
    double trace = conf.trace();
    for (int r = 0; r<conf.rows(); r++) {
        precision[r] = conf(r ,r)/row_sum[r];
    }
    precision[conf.rows()] = trace/all_sum;
    return precision;
}

template double
DTUtil::spatialVariance(const vector<Eigen::VectorXf> & labels, const vector<unsigned int> & indices);

template double
DTUtil::fullVariance(const vector<Eigen::VectorXf>& labels, const vector<unsigned int> & indices);

template double
DTUtil::sumOfVariance(const vector<Eigen::MatrixXf> & labels, const int row_index, const vector<unsigned int> & indices);

template double
DTUtil::spatialVariance(const vector<Eigen::VectorXf> & labels, const vector<unsigned int> & indices, const vector<int> & wt);

template void
DTUtil::meanStddev(const vector<Eigen::VectorXf> & labels, const vector<unsigned int> & indices, Eigen::VectorXf & mean, Eigen::VectorXf & sigma);

template Eigen::VectorXf
DTUtil::mean(const vector<Eigen::VectorXf> & data, const vector<unsigned int> & indices);

template Eigen::VectorXf
DTUtil::mean(const vector<Eigen::VectorXf> & data, const vector<int> & indices);

template Eigen::VectorXf
DTUtil::mean(const vector<Eigen::VectorXf> & data);

template void
DTUtil::rowMeanStddev(const vector<Eigen::MatrixXf> & labels, const vector<unsigned int> & indices,
                      const int row_index, Eigen::VectorXf & mean, Eigen::VectorXf & sigma);

template
void DTUtil::quartileError(const vector<Eigen::VectorXf> & errors, Eigen::VectorXf& q1, Eigen::VectorXf& q2, Eigen::VectorXf& q3);

template void
DTUtil::matrixMeanError(const vector<Eigen::MatrixXf> & errors, Eigen::MatrixXf & mean);

template Eigen::MatrixXd
DTUtil::confusionMatrix(const vector<unsigned int> & preds,
                        const vector<unsigned int> & labels,
                        const int category_num,
                        bool normalize);

template Eigen::MatrixXd
DTUtil::confusionMatrix(const vector<int> & preds,
                        const vector<int> & labels,
                        const int category_num,
                        bool normalize);


#include <GL/freeglut.h>
#include <vector>
#include <Eigen/Eigen>
#include <iostream>
#include <iomanip>

#include "structures.h"
#include "transformations.h"
#include "relative_pose_tait_bryan_wc_jacobian.h"

const unsigned int window_width = 1920;
const unsigned int window_height = 1080;
int mouse_old_x, mouse_old_y;
int mouse_buttons = 0;
float rotate_x = 0.0, rotate_y = 0.0;
float translate_z = -10.0;
float translate_x, translate_y = 0.0;

bool initGL(int *argc, char **argv);
void display();
void keyboard(unsigned char key, int x, int y);
void mouse(int button, int state, int x, int y);
void motion(int x, int y);
void reshape(int w, int h);
void printHelp();

bool is_loop_closure_added = false;

std::vector<Eigen::Affine3d> m_poses;
std::vector<Eigen::Affine3d> m_poses_desired;
std::vector<std::pair<int, int>> odo_edges;

void calculate_cov(std::vector<Eigen::Affine3d> m_poses,
				   std::vector<Eigen::Affine3d> m_poses_desired,
				   std::vector<std::pair<int, int>> odo_edges,
				   Eigen::MatrixXd & cov_b);

void draw_ellipse(const Eigen::Matrix3d& covar, Eigen::Vector3d& mean, Eigen::Vector3f color, float nstd  = 3)
{
    Eigen::LLT<Eigen::Matrix<double,3,3> > cholSolver(covar);
    Eigen::Matrix3d transform = cholSolver.matrixL();

    const double pi = 3.141592;
    const double di = 0.02;
    const double dj = 0.04;
    const double du = di*2*pi;
    const double dv = dj*pi;
    glColor3f(color.x(), color.y(),color.z());

    for (double i = 0; i < 1.0; i+=di)  //horizonal
    {
        for (double j = 0; j < 1.0; j+=dj)  //vertical
        {
            double u = i*2*pi;      //0     to  2pi
            double v = (j-0.5)*pi;  //-pi/2 to pi/2

            const Eigen::Vector3d pp0( cos(v)* cos(u),cos(v) * sin(u),sin(v));
            const Eigen::Vector3d pp1(cos(v) * cos(u + du) ,cos(v) * sin(u + du) ,sin(v));
            const Eigen::Vector3d pp2(cos(v + dv)* cos(u + du) ,cos(v + dv)* sin(u + du) ,sin(v + dv));
            const Eigen::Vector3d pp3( cos(v + dv)* cos(u),cos(v + dv)* sin(u),sin(v + dv));
            Eigen::Vector3d tp0 = transform * (nstd*pp0) + mean;
            Eigen::Vector3d tp1 = transform * (nstd*pp1) + mean;
            Eigen::Vector3d tp2 = transform * (nstd*pp2) + mean;
            Eigen::Vector3d tp3 = transform * (nstd*pp3) + mean;

            glBegin(GL_LINE_LOOP);
            glVertex3dv(tp0.data());
            glVertex3dv(tp1.data());
            glVertex3dv(tp2.data());
            glVertex3dv(tp3.data());
            glEnd();
        }
    }
}

void draw_ellipse2D(const Eigen::Matrix3d& covar, Eigen::Vector3d& mean, Eigen::Vector3f color, float nstd  = 3)
{
    Eigen::LLT<Eigen::Matrix<double,3,3> > cholSolver(covar);
    Eigen::Matrix3d transform = cholSolver.matrixL();

    const double pi = 3.141592;
    const double di = 0.02;
    const double dj = 0.04;
    const double du = di*2*pi;
    const double dv = dj*pi;
    glColor3f(color.x(), color.y(),color.z());

    for (double i = 0; i < 1.0; i+=di) { //horizonal
		double u = i*2*pi;      //0     to  2pi
		const Eigen::Vector3d pp0( cos(u), sin (u),0);
		const Eigen::Vector3d pp1( cos(u+du), sin(u+du),0);
		Eigen::Vector3d tp0 = transform * (nstd*pp0) + mean;
		Eigen::Vector3d tp1 = transform * (nstd*pp1) + mean;
		glBegin(GL_LINE_LOOP);
		glVertex3dv(tp0.data());
		glVertex3dv(tp1.data());
		glEnd();
	}
}

int main(int argc, char *argv[]){
	TaitBryanPose p0;
	p0.px = 0;
	p0.py = 0;
	p0.pz = 0;
	p0.om = 0;
	p0.fi = 0;
	p0.ka = 0;
	Eigen::Affine3d m = affine_matrix_from_pose_tait_bryan(p0);
	m_poses.push_back(m);

	TaitBryanPose p_rel_forward_x;
	p_rel_forward_x.px = 1.0;
	p_rel_forward_x.py = 0.0;
	p_rel_forward_x.pz = 0.0;
	p_rel_forward_x.om = 0.0;
	p_rel_forward_x.fi = 0.0;
	p_rel_forward_x.ka = 0.0;

	TaitBryanPose p_rel_rotate;
	p_rel_rotate.px = 0.0;
	p_rel_rotate.py = 0.0;
	p_rel_rotate.pz = 0.0;
	p_rel_rotate.om = 0.0;
	p_rel_rotate.fi = 0.0;
	p_rel_rotate.ka = 90 * M_PI/180.0;

	Eigen::Affine3d m_rel;

	for(int c = 0; c < 4; c++){
		for(size_t i = 0 ; i < 3; i++){
			m_rel = affine_matrix_from_pose_tait_bryan(p_rel_forward_x);
			m = m * m_rel;
			m_poses.push_back(m);
		}
		m_rel = affine_matrix_from_pose_tait_bryan(p_rel_rotate);
		m = m * m_rel;
	}
	m_poses.pop_back();
	m_poses_desired = m_poses;
	m_poses.clear();
	p0.px = 0;
	p0.py = 0;
	p0.pz = 0;
	p0.om = 0;
	p0.fi = 0;
	p0.ka = 30 * M_PI/180;
	m = affine_matrix_from_pose_tait_bryan(p0);
	m_poses.push_back(m);

	p_rel_forward_x.px = 1.1;
	p_rel_forward_x.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.1;
	p_rel_forward_x.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
	p_rel_forward_x.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
	p_rel_forward_x.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
	p_rel_forward_x.ka = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;

	p_rel_rotate.px = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
	p_rel_rotate.py = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
	p_rel_rotate.pz = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
	p_rel_rotate.om = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
	p_rel_rotate.fi = ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
	p_rel_rotate.ka = 80 * M_PI/180.0;

	for(int c = 0; c < 4; c++){
		for(size_t i = 0 ; i < 3; i++){
			m_rel = affine_matrix_from_pose_tait_bryan(p_rel_forward_x);
			m = m * m_rel;
			m_poses.push_back(m);
			p_rel_forward_x.px += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.1;
			p_rel_forward_x.py += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.1;
			p_rel_forward_x.pz += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			p_rel_forward_x.om += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			p_rel_forward_x.fi += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
			p_rel_forward_x.ka += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
		}
		m_rel = affine_matrix_from_pose_tait_bryan(p_rel_rotate);
		m = m * m_rel;
		p_rel_rotate.px += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
		p_rel_rotate.py += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
		p_rel_rotate.pz += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
		p_rel_rotate.om += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
		p_rel_rotate.fi += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
		p_rel_rotate.ka += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.000001;
	}
	m_poses.pop_back();
	for(size_t i = 1; i < m_poses.size(); i++){
		odo_edges.emplace_back(i-1,i);
	}
	if (false == initGL(&argc, argv)) {
		return 4;
	}
	printHelp();
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMouseFunc(mouse);
	glutMotionFunc(motion);
	glutMainLoop();
	return 0;
}

bool initGL(int *argc, char **argv) {
	glutInit(argc, argv);
	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);
	glutInitWindowSize(window_width, window_height);
	glutCreateWindow("relative_pose_covariances");
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutMotionFunc(motion);

	// default initialization
	glClearColor(1.0, 1.0, 1.0, 1.0);
	glEnable(GL_DEPTH_TEST);

	// viewport
	glViewport(0, 0, window_width, window_height);

	// projection
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) window_width / (GLfloat) window_height, 0.01,
			10000.0);
	glutReshapeFunc(reshape);
	return true;
}

void display() {
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glTranslatef(translate_x, translate_y, translate_z);
	glRotatef(rotate_x, 1.0, 0.0, 0.0);
	glRotatef(rotate_y, 0.0, 0.0, 1.0);

	/*glBegin(GL_LINES);
	glColor3f(1.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(1.0f, 0.0f, 0.0f);

	glColor3f(0.0f, 1.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 1.0f, 0.0f);

	glColor3f(0.0f, 0.0f, 1.0f);
	glVertex3f(0.0f, 0.0f, 0.0f);
	glVertex3f(0.0f, 0.0f, 1.0f);
	glEnd();*/

	glColor3f(1,0,0);
	glBegin(GL_LINES);
	for(size_t i = 0; i < odo_edges.size(); i++){
		glVertex3f(m_poses[odo_edges[i].first](0,3), m_poses[odo_edges[i].first](1,3), m_poses[odo_edges[i].first](2,3) );
		glVertex3f(m_poses[odo_edges[i].second](0,3), m_poses[odo_edges[i].second](1,3), m_poses[odo_edges[i].second](2,3) );
	}
	glEnd();

	Eigen::MatrixXd cov_b(m_poses.size() * 2, m_poses.size() * 2);
	cov_b = Eigen::MatrixXd::Zero(m_poses.size() * 2, m_poses.size() * 2);
	calculate_cov( m_poses,	m_poses_desired, odo_edges, cov_b);

	for(size_t i = 0; i < m_poses.size(); i++){
		Eigen::Vector3d mean(m_poses[i](0,3), m_poses[i](1,3), m_poses[i](2,3));
		Eigen::Matrix3d cov;
		int r = i * 6;
		int c = i * 6;
		cov(0,0) = cov_b(r+0,c+0);
		cov(0,1) = cov_b(r+0,c+1);
		cov(0,2) = cov_b(r+0,c+2);
		cov(1,0) = cov_b(r+1,c+0);
		cov(1,1) = cov_b(r+1,c+1);
		cov(1,2) = cov_b(r+1,c+2);
		cov(2,0) = cov_b(r+2,c+0);
		cov(2,1) = cov_b(r+2,c+1);
		cov(2,2) = cov_b(r+2,c+2);
		draw_ellipse2D(cov, mean, Eigen::Vector3f(1,0,0),1);
		draw_ellipse2D(cov, mean, Eigen::Vector3f(0,1,0),2);
		draw_ellipse2D(cov, mean, Eigen::Vector3f(0,0,1),3);
	}
	glutSwapBuffers();
}

void keyboard(unsigned char key, int /*x*/, int /*y*/) {
	switch (key) {
		case (27): {
			glutDestroyWindow(glutGetWindow());
			return;
		}
		case 'e':{
			odo_edges.emplace_back(1, m_poses.size()-2);
			is_loop_closure_added = true;
			break;
		}
		case 'n':{
			for(size_t i = 0 ; i < m_poses.size(); i++){
				TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(m_poses[i]);
				pose.px += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.1;
				pose.py += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.1;
				pose.pz += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.001;
				pose.om += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.0001;
				pose.fi += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.0001;
				pose.ka += ((float(rand()%1000000))/1000000.0f - 0.5) * 0.0001;
				m_poses[i] = affine_matrix_from_pose_tait_bryan(pose);
			}
			break;
		}
		case 't':{
			std::vector<Eigen::Triplet<double>> tripletListA;
			std::vector<Eigen::Triplet<double>> tripletListP;
			std::vector<Eigen::Triplet<double>> tripletListB;

			std::vector<TaitBryanPose> poses;
			std::vector<TaitBryanPose> poses_desired;

			for(size_t i = 0 ; i < m_poses.size(); i++){
				poses.push_back(pose_tait_bryan_from_affine_matrix(m_poses[i]));
			}
			for(size_t i = 0 ; i < m_poses_desired.size(); i++){
				poses_desired.push_back(pose_tait_bryan_from_affine_matrix(m_poses_desired[i]));
			}

			for(size_t i = 0 ; i < odo_edges.size(); i++){
				Eigen::Matrix<double, 6, 1> relative_pose_measurement_odo;
				relative_pose_tait_bryan_wc_case1(relative_pose_measurement_odo,
						poses_desired[odo_edges[i].first].px,
						poses_desired[odo_edges[i].first].py,
						poses_desired[odo_edges[i].first].pz,
						poses_desired[odo_edges[i].first].om,
						poses_desired[odo_edges[i].first].fi,
						poses_desired[odo_edges[i].first].ka,
						poses_desired[odo_edges[i].second].px,
						poses_desired[odo_edges[i].second].py,
						poses_desired[odo_edges[i].second].pz,
						poses_desired[odo_edges[i].second].om,
						poses_desired[odo_edges[i].second].fi,
						poses_desired[odo_edges[i].second].ka);

				Eigen::Matrix<double, 6, 1> delta;
				relative_pose_obs_eq_tait_bryan_wc_case1(
						delta,
						poses[odo_edges[i].first].px,
						poses[odo_edges[i].first].py,
						poses[odo_edges[i].first].pz,
						poses[odo_edges[i].first].om,
						poses[odo_edges[i].first].fi,
						poses[odo_edges[i].first].ka,
						poses[odo_edges[i].second].px,
						poses[odo_edges[i].second].py,
						poses[odo_edges[i].second].pz,
						poses[odo_edges[i].second].om,
						poses[odo_edges[i].second].fi,
						poses[odo_edges[i].second].ka,
						relative_pose_measurement_odo(0,0),
						relative_pose_measurement_odo(1,0),
						relative_pose_measurement_odo(2,0),
						relative_pose_measurement_odo(3,0),
						relative_pose_measurement_odo(4,0),
						relative_pose_measurement_odo(5,0));

				Eigen::Matrix<double, 6, 12, Eigen::RowMajor> jacobian;
				relative_pose_obs_eq_tait_bryan_wc_case1_jacobian(jacobian,
						poses[odo_edges[i].first].px,
						poses[odo_edges[i].first].py,
						poses[odo_edges[i].first].pz,
						poses[odo_edges[i].first].om,
						poses[odo_edges[i].first].fi,
						poses[odo_edges[i].first].ka,
						poses[odo_edges[i].second].px,
						poses[odo_edges[i].second].py,
						poses[odo_edges[i].second].pz,
						poses[odo_edges[i].second].om,
						poses[odo_edges[i].second].fi,
						poses[odo_edges[i].second].ka);

				int ir = tripletListB.size();

				int ic_1 = odo_edges[i].first * 6;
				int ic_2 = odo_edges[i].second * 6;

				for(size_t row = 0 ; row < 6; row ++){
					tripletListA.emplace_back(ir + row, ic_1    , -jacobian(row,0));
					tripletListA.emplace_back(ir + row, ic_1 + 1, -jacobian(row,1));
					tripletListA.emplace_back(ir + row, ic_1 + 2, -jacobian(row,2));
					tripletListA.emplace_back(ir + row, ic_1 + 3, -jacobian(row,3));
					tripletListA.emplace_back(ir + row, ic_1 + 4, -jacobian(row,4));
					tripletListA.emplace_back(ir + row, ic_1 + 5, -jacobian(row,5));

					tripletListA.emplace_back(ir + row, ic_2    , -jacobian(row,6));
					tripletListA.emplace_back(ir + row, ic_2 + 1, -jacobian(row,7));
					tripletListA.emplace_back(ir + row, ic_2 + 2, -jacobian(row,8));
					tripletListA.emplace_back(ir + row, ic_2 + 3, -jacobian(row,9));
					tripletListA.emplace_back(ir + row, ic_2 + 4, -jacobian(row,10));
					tripletListA.emplace_back(ir + row, ic_2 + 5, -jacobian(row,11));
				}

				tripletListB.emplace_back(ir,     0, delta(0,0));
				tripletListB.emplace_back(ir + 1, 0, delta(1,0));
				tripletListB.emplace_back(ir + 2, 0, delta(2,0));

				float angle_diff = delta(3,0);
				if(fabs(angle_diff) > M_PI){
					angle_diff -= 2.0*M_PI;
				}
				if(fabs(angle_diff)< -M_PI){
					angle_diff += 2.0*M_PI;
				}
				tripletListB.emplace_back(ir + 3, 0, angle_diff);

				angle_diff = delta(4,0);
				if(fabs(angle_diff) > M_PI){
					angle_diff -= 2.0*M_PI;
				}
				if(fabs(angle_diff)< -M_PI){
					angle_diff += 2.0*M_PI;
				}
				tripletListB.emplace_back(ir + 4, 0, angle_diff);

				angle_diff = delta(5,0);
				if(fabs(angle_diff) > M_PI){
					angle_diff -= 2.0*M_PI;
				}
				if(fabs(angle_diff)< -M_PI){
					angle_diff += 2.0*M_PI;
				}
				tripletListB.emplace_back(ir + 5, 0, angle_diff);

				tripletListP.emplace_back(ir ,    ir,     1);
				tripletListP.emplace_back(ir + 1, ir + 1, 1);
				tripletListP.emplace_back(ir + 2, ir + 2, 1);
				tripletListP.emplace_back(ir + 3, ir + 3, 1);
				tripletListP.emplace_back(ir + 4, ir + 4, 1);
				tripletListP.emplace_back(ir + 5, ir + 5, 1);
			}

			int ir = tripletListB.size();
			tripletListA.emplace_back(ir     , 0, 1);
			tripletListA.emplace_back(ir + 1 , 1, 1);
			tripletListA.emplace_back(ir + 2 , 2, 1);
			tripletListA.emplace_back(ir + 3 , 3, 1);
			tripletListA.emplace_back(ir + 4 , 4, 1);
			tripletListA.emplace_back(ir + 5 , 5, 1);

			tripletListP.emplace_back(ir     , ir,     1);
			tripletListP.emplace_back(ir + 1 , ir + 1, 1);
			tripletListP.emplace_back(ir + 2 , ir + 2, 1);
			tripletListP.emplace_back(ir + 3 , ir + 3, 1);
			tripletListP.emplace_back(ir + 4 , ir + 4, 1);
			tripletListP.emplace_back(ir + 5 , ir + 5, 1);

			tripletListB.emplace_back(ir     , 0, 0);
			tripletListB.emplace_back(ir + 1 , 0, 0);
			tripletListB.emplace_back(ir + 2 , 0, 0);
			tripletListB.emplace_back(ir + 3 , 0, 0);
			tripletListB.emplace_back(ir + 4 , 0, 0);
			tripletListB.emplace_back(ir + 5 , 0, 0);

			Eigen::SparseMatrix<double> matA(tripletListB.size(), m_poses.size() * 6);
			Eigen::SparseMatrix<double> matP(tripletListB.size(), tripletListB.size());
			Eigen::SparseMatrix<double> matB(tripletListB.size(), 1);

			matA.setFromTriplets(tripletListA.begin(), tripletListA.end());
			matP.setFromTriplets(tripletListP.begin(), tripletListP.end());
			matB.setFromTriplets(tripletListB.begin(), tripletListB.end());

			Eigen::SparseMatrix<double> AtPA(m_poses.size() * 6 , m_poses.size() * 6);
			Eigen::SparseMatrix<double> AtPB(m_poses.size() * 6 , 1);

			{
				Eigen::SparseMatrix<double> AtP = matA.transpose() * matP;
				AtPA = (AtP) * matA;
				AtPB = (AtP) * matB;
			}

			tripletListA.clear();
			tripletListP.clear();
			tripletListB.clear();

			std::cout << "AtPA.size: " << AtPA.size() << std::endl;
			std::cout << "AtPB.size: " << AtPB.size() << std::endl;

			std::cout << "start solving AtPA=AtPB" << std::endl;
			Eigen::SimplicialCholesky<Eigen::SparseMatrix<double>> solver(AtPA);

			std::cout << "x = solver.solve(AtPB)" << std::endl;
			Eigen::SparseMatrix<double> x = solver.solve(AtPB);

			std::vector<double> h_x;

			for (int k=0; k<x.outerSize(); ++k){
				for (Eigen::SparseMatrix<double>::InnerIterator it(x,k); it; ++it){
					h_x.push_back(it.value());
				}
			}
			std::cout << "h_x.size(): " << h_x.size() << std::endl;
			std::cout << "AtPA=AtPB SOLVED" << std::endl;

			for(size_t i = 0 ; i < h_x.size(); i++){
				std::cout << h_x[i] << std::endl;
			}

			if(h_x.size() == 6 * m_poses.size()){
				int counter = 0;

				for(size_t i = 0; i < m_poses.size(); i++){
					TaitBryanPose pose = pose_tait_bryan_from_affine_matrix(m_poses[i]);
					pose.px += h_x[counter++];
					pose.py += h_x[counter++];
					pose.pz += h_x[counter++];
					pose.om += h_x[counter++];
					pose.fi += h_x[counter++];
					pose.ka += h_x[counter++];
					m_poses[i] = affine_matrix_from_pose_tait_bryan(pose);
				}
				std::cout << "optimizing with tait bryan finished" << std::endl;
			}else{
				std::cout << "optimizing with tait bryan FAILED" << std::endl;
			}
			break;
		}
	}
	printHelp();
	glutPostRedisplay();
}

void mouse(int button, int state, int x, int y) {
	if (state == GLUT_DOWN) {
		mouse_buttons |= 1 << button;
	} else if (state == GLUT_UP) {
		mouse_buttons = 0;
	}
	mouse_old_x = x;
	mouse_old_y = y;
}

void motion(int x, int y) {
	float dx, dy;
	dx = (float) (x - mouse_old_x);
	dy = (float) (y - mouse_old_y);

	if (mouse_buttons & 1) {
		rotate_x += dy * 0.2f;
		rotate_y += dx * 0.2f;

	} else if (mouse_buttons & 4) {
		translate_z += dy * 0.05f;
	} else if (mouse_buttons & 3) {
		translate_x += dx * 0.05f;
		translate_y -= dy * 0.05f;
	}

	mouse_old_x = x;
	mouse_old_y = y;

	glutPostRedisplay();
}

void reshape(int w, int h) {
	glViewport(0, 0, (GLsizei) w, (GLsizei) h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60.0, (GLfloat) w / (GLfloat) h, 0.01, 10000.0);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}

void printHelp() {
	std::cout << "-------help-------" << std::endl;
	std::cout << "n: add noise to poses" << std::endl;
	std::cout << "t: optimize (Tait-Bryan)" << std::endl;
	std::cout << "e: add edge (loop closure)" << std::endl;
}

void calculate_cov(std::vector<Eigen::Affine3d> m_poses,
				   std::vector<Eigen::Affine3d> m_poses_desired,
				   std::vector<std::pair<int, int>> odo_edges,
				   Eigen::MatrixXd & cov_b)
{
	Eigen::MatrixXd d2sum_dbeta2(m_poses.size() * 6, m_poses.size() * 6);
	d2sum_dbeta2 = Eigen::MatrixXd::Zero(m_poses.size() * 6, m_poses.size() * 6);

	std::vector<TaitBryanPose> poses;
	std::vector<TaitBryanPose> poses_desired;

	for(size_t i = 0 ; i < m_poses.size(); i++){
		poses.push_back(pose_tait_bryan_from_affine_matrix(m_poses[i]));
	}
	for(size_t i = 0 ; i < m_poses_desired.size(); i++){
		poses_desired.push_back(pose_tait_bryan_from_affine_matrix(m_poses_desired[i]));
	}

	for (size_t i = 0; i < odo_edges.size(); i++ ){
		Eigen::Matrix<double, 6, 1> relative_pose_measurement_odo;
		relative_pose_tait_bryan_wc_case1(relative_pose_measurement_odo,
			poses_desired[odo_edges[i].first].px,
			poses_desired[odo_edges[i].first].py,
			poses_desired[odo_edges[i].first].pz,
			poses_desired[odo_edges[i].first].om,
			poses_desired[odo_edges[i].first].fi,
			poses_desired[odo_edges[i].first].ka,
			poses_desired[odo_edges[i].second].px,
			poses_desired[odo_edges[i].second].py,
			poses_desired[odo_edges[i].second].pz,
			poses_desired[odo_edges[i].second].om,
			poses_desired[odo_edges[i].second].fi,
			poses_desired[odo_edges[i].second].ka);

		Eigen::Matrix<double, 12, 12, Eigen::RowMajor> d2sum_dbeta2i;
		relative_pose_obs_eq_tait_bryan_wc_case1_d2sum_dbeta2(
				d2sum_dbeta2i,
				poses[odo_edges[i].first].px,
				poses[odo_edges[i].first].py,
				poses[odo_edges[i].first].pz,
				poses[odo_edges[i].first].om,
				poses[odo_edges[i].first].fi,
				poses[odo_edges[i].first].ka,
				poses[odo_edges[i].second].px,
				poses[odo_edges[i].second].py,
				poses[odo_edges[i].second].pz,
				poses[odo_edges[i].second].om,
				poses[odo_edges[i].second].fi,
				poses[odo_edges[i].second].ka,
				relative_pose_measurement_odo(0,0),
				relative_pose_measurement_odo(1,0),
				relative_pose_measurement_odo(2,0),
				relative_pose_measurement_odo(3,0),
				relative_pose_measurement_odo(4,0),
				relative_pose_measurement_odo(5,0));

		int raw = odo_edges[i].first * 6;
		int cal = odo_edges[i].first * 6;
		for(size_t r = 0; r < 6; r++){
			for(size_t c = 0; c < 6; c++){
				d2sum_dbeta2(raw + r, cal + c) =
						d2sum_dbeta2(raw + r, cal + c) + d2sum_dbeta2i(r,c);
			}
		}
		raw = odo_edges[i].second * 6;
		cal = odo_edges[i].second * 6;
		for(size_t r = 0; r < 6; r++){
			for(size_t c = 0; c < 6; c++){
				d2sum_dbeta2(raw + r, cal + c) =
						d2sum_dbeta2(raw + r, cal + c) + d2sum_dbeta2i(r+6,c+6);
			}
		}

		raw = odo_edges[i].first * 6;
		cal = odo_edges[i].second * 6;
		for(size_t r = 0; r < 6; r++){
			for(size_t c = 0; c < 6; c++){
				d2sum_dbeta2(raw + r, cal + c) =
						d2sum_dbeta2(raw + r, cal + c) + d2sum_dbeta2i(r,c+6);
			}
		}
		raw = odo_edges[i].second * 6;
		cal = odo_edges[i].first * 6;
		for(size_t r = 0; r < 6; r++){
			for(size_t c = 0; c < 6; c++){
				d2sum_dbeta2(raw + r, cal + c) =
						d2sum_dbeta2(raw + r, cal + c) + d2sum_dbeta2i(r + 6,c);
			}
		}
	}

	for (size_t i = 0; i < 6; i++ ){
		d2sum_dbeta2(i,i) += 1000000;
	}

	Eigen::MatrixXd d2sum_dbetadx(m_poses.size() * 12, 6 * odo_edges.size());
	d2sum_dbetadx = Eigen::MatrixXd::Zero(m_poses.size() * 12, 6 * odo_edges.size());

	for (int i = 0; i < odo_edges.size() ; i++)
	{
		Eigen::Matrix<double, 6, 1> relative_pose_measurement_odo;
		relative_pose_tait_bryan_wc_case1(
			relative_pose_measurement_odo,
			poses_desired[odo_edges[i].first].px,
			poses_desired[odo_edges[i].first].py,
			poses_desired[odo_edges[i].first].pz,
			poses_desired[odo_edges[i].first].om,
			poses_desired[odo_edges[i].first].fi,
			poses_desired[odo_edges[i].first].ka,
			poses_desired[odo_edges[i].second].px,
			poses_desired[odo_edges[i].second].py,
			poses_desired[odo_edges[i].second].pz,
			poses_desired[odo_edges[i].second].om,
			poses_desired[odo_edges[i].second].fi,
			poses_desired[odo_edges[i].second].ka);
		Eigen::Matrix<double, 12, 6, Eigen::RowMajor> d2sum_dbetadx_temp;
		relative_pose_obs_eq_tait_bryan_wc_case1_d2sum_dbetadx(
			d2sum_dbetadx_temp,
			poses[odo_edges[i].first].px,
			poses[odo_edges[i].first].py,
			poses[odo_edges[i].first].pz,
			poses[odo_edges[i].first].om,
			poses[odo_edges[i].first].fi,
			poses[odo_edges[i].first].ka,
			poses[odo_edges[i].second].px,
			poses[odo_edges[i].second].py,
			poses[odo_edges[i].second].pz,
			poses[odo_edges[i].second].om,
			poses[odo_edges[i].second].fi,
			poses[odo_edges[i].second].ka,
			relative_pose_measurement_odo(0,0),
			relative_pose_measurement_odo(1,0),
			relative_pose_measurement_odo(2,0),
			relative_pose_measurement_odo(3,0),
			relative_pose_measurement_odo(4,0),
			relative_pose_measurement_odo(5,0));

		int raw = odo_edges[i].first * 6;
		int cal = i * 6;
		for(size_t r = 0; r < 6; r++){
			for(size_t c = 0; c < 6; c++){
				d2sum_dbetadx(raw + r, cal + c) = d2sum_dbetadx_temp(r,c);
			}
		}
		raw = odo_edges[i].second * 6;
		for(size_t r = 0; r < 6; r++){
			for(size_t c = 0; c < 6; c++){
				d2sum_dbetadx(raw + r, cal + c) = d2sum_dbetadx_temp(r + 6,c);
			}
		}
	}

	Eigen::MatrixXd cov_x(6 * odo_edges.size(), 6 * odo_edges.size());
	cov_x = Eigen::MatrixXd::Zero(6 * odo_edges.size(), 6 * odo_edges.size());

	for(int i = 0 ; i < 6 * odo_edges.size(); i+=6){
		cov_x(i,i)     = 0.005 * 0.005;
		cov_x(i+1,i+1) = 0.05 * 0.05;
		cov_x(i+2,i+2) = 0.000000001 * 0.000000001;
		cov_x(i+3,i+3) = 0.000000001 * 0.000000001;
		cov_x(i+4,i+4) = 0.000000001 * 0.000000001;
		cov_x(i+5,i+5) = 0.01 * 0.01;
	}
	if(is_loop_closure_added){
		int i = 6 * (odo_edges.size()-1);
		cov_x(i,i)     = 0.01*0.01;
		cov_x(i+1,i+1) = 0.01*0.01;
		cov_x(i+2,i+2) = 0.01*0.01;
		cov_x(i+3,i+3) = 0.01 * 0.01;
		cov_x(i+4,i+4) = 0.01 * 0.01;
		cov_x(i+5,i+5) = 0.01 * 0.01;
	}

	cov_b = d2sum_dbeta2.inverse() * d2sum_dbetadx * cov_x * d2sum_dbetadx.transpose() * d2sum_dbeta2.inverse();
}


#ifndef INCLUDED_numeric_rand_xform_HH
#define INCLUDED_numeric_rand_xform_HH

#include "scheme/numeric/util.hh"
#include <Eigen/Geometry>
#include <random>

namespace scheme { namespace numeric {

template<class T>
void
rand_xform(
	std::mt19937 & rng,
	Eigen::Transform<T,3,Eigen::Affine> & x,
	T cart_bound = 512.0
){
	std::uniform_real_distribution<> runif;
	std::normal_distribution<> rnorm;
	Eigen::Quaterniond qrand( rnorm(rng), rnorm(rng), rnorm(rng), rnorm(rng) );
	qrand.normalize();
	Eigen::Matrix3d m = qrand.matrix();

	x.data()[3] = 0; x.data()[7] = 0; x.data()[11] = 0; x.data()[15] = 1;
	for(int i = 0; i <  3; ++i) x.data()[i] = m.data()[i-0];
	for(int i = 4; i <  7; ++i) x.data()[i] = m.data()[i-1];
	for(int i = 8; i < 11; ++i) x.data()[i] = m.data()[i-2];
	x.data()[12] = runif(rng) * cart_bound - cart_bound/2.0;
	x.data()[13] = runif(rng) * cart_bound - cart_bound/2.0;
	x.data()[14] = runif(rng) * cart_bound - cart_bound/2.0;
}

template<class T>
void
rand_xform(
	std::mt19937 & rng,
	Eigen::Transform<T,3,Eigen::AffineCompact> & x,
	T cart_bound = 512.0
){
	std::uniform_real_distribution<> runif;
	std::normal_distribution<> rnorm;
	Eigen::Quaterniond qrand( rnorm(rng), rnorm(rng), rnorm(rng), rnorm(rng) );
	qrand.normalize();
	Eigen::Matrix3d m = qrand.matrix();
	for(int i = 0; i < 9; ++i) x.data()[i] = m.data()[i];

	x.data()[ 9] = runif(rng) * cart_bound - cart_bound/2.0;
	x.data()[10] = runif(rng) * cart_bound - cart_bound/2.0;
	x.data()[11] = runif(rng) * cart_bound - cart_bound/2.0;
}

template<class X>
X rand_xform(
	std::mt19937 & rng,
	scalar<X> cart_bound = 512.0
){
	X x;
	rand_xform(rng,x,cart_bound);
	return x;
}

template<class T>
void
rand_xform_cartnormal(
	std::mt19937 & rng,
	Eigen::Transform<T,3,Eigen::AffineCompact> & x,
	T const & cart_sd
){
	std::uniform_real_distribution<> runif;
	std::normal_distribution<> rnorm;
	Eigen::Quaterniond qrand( rnorm(rng), rnorm(rng), rnorm(rng), rnorm(rng) );
	qrand.normalize();
	Eigen::Matrix3d m = qrand.matrix();
	for(int i = 0; i < 9; ++i) x.data()[i] = m.data()[i];
	x.data()[ 9] = rnorm(rng) * cart_sd;
	x.data()[10] = rnorm(rng) * cart_sd;
	x.data()[11] = rnorm(rng) * cart_sd;
}

template<class T>
void
rand_xform_quat(
	std::mt19937 & rng,
	Eigen::Transform<T,3,Eigen::AffineCompact> & x,
	double cart_bound, double quat_bound
){
	std::uniform_real_distribution<> runif;
	std::normal_distribution<> rnorm;

	assert( quat_bound < sqrt(3.0)/2.0 );

	{ // ori part
		Eigen::Quaterniond qrand( 0.0, rnorm(rng), rnorm(rng), rnorm(rng) );
		double scale = 1.0 - runif(rng)*runif(rng);
		double len = qrand.norm();
		qrand.x() *= scale * quat_bound / len;
		qrand.y() *= scale * quat_bound / len;
		qrand.z() *= scale * quat_bound / len;
		qrand.w() = sqrt( 1.0 - qrand.squaredNorm() );
		assert( fabs( qrand.norm() - 1.0) < 0.00001 );

		qrand.normalize();
		Eigen::Matrix3d m = qrand.matrix();
		for(int i = 0; i < 9; ++i) x.data()[i] = m.data()[i];
	}
	{ // cart part
		x.data()[ 9] = rnorm(rng);
		x.data()[10] = rnorm(rng);
		x.data()[11] = rnorm(rng);
		double scale = 1.0 - runif(rng)*runif(rng);
		double len = sqrt( x.data()[9]*x.data()[9] + x.data()[10]*x.data()[10] + x.data()[11]*x.data()[11] );
		x.data()[ 9] *= scale * cart_bound / len;
		x.data()[10] *= scale * cart_bound / len;
		x.data()[11] *= scale * cart_bound / len;
	}
}

template<class T>
void
rand_xform_sphere(
	std::mt19937 & rng,
	Eigen::Transform<T,3,Eigen::AffineCompact> & x,
	T const cart_radius,
	T const ang_radius
){
	std::normal_distribution<> rnorm;
	std::uniform_real_distribution<> runif;

	float ang = (1.0 - runif(rng)*runif(rng)) * ang_radius;
	Eigen::Matrix<float,1,3> axis( rnorm(rng), rnorm(rng), rnorm(rng) );
	axis.normalize();
	Eigen::AngleAxis<T> aa( ang, axis );
	x = Eigen::Transform<T,3,Eigen::AffineCompact> ( aa );

	Eigen::AngleAxis<T> aa2( x.rotation() );

	// std::cout << ang << " " << ang_radius << " " << aa2.angle() << std::endl;

	Eigen::Matrix<float,1,3> delta( 9e9, 9e9, 9e9 );
	while( delta.squaredNorm() > cart_radius*cart_radius ){
		delta = 2.0 * cart_radius * Eigen::Matrix<float,1,3>( runif(rng)-0.5, runif(rng)-0.5, runif(rng)-0.5 );
	}

	x.translation() = delta;

}

}}

#endif


/*
 * Copyright 2011-2012 Mario Mulansky
 * Copyright 2012-2013 Karsten Ahnert
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 *
 * Example for self defined vector type.
 */

#include <vector>

#include <boost/numeric/odeint.hpp>

//[my_vector
template< size_t MAX_N >
class my_vector
{
    typedef std::vector< double > vector;

public:
    typedef vector::iterator iterator;
    typedef vector::const_iterator const_iterator;

public:
    my_vector( const size_t N )
        : m_v( N )
    {
        m_v.reserve( MAX_N );
    }

    my_vector()
        : m_v()
    {
        m_v.reserve( MAX_N );
    }

// ... [ implement container interface ]
//]
    const double & operator[]( const size_t n ) const
    { return m_v[n]; }

    double & operator[]( const size_t n )
    { return m_v[n]; }

    iterator begin()
    { return m_v.begin(); }

    const_iterator begin() const
    { return m_v.begin(); }

    iterator end()
    { return m_v.end(); }

    const_iterator end() const
    { return m_v.end(); }

    size_t size() const
    { return m_v.size(); }

    void resize( const size_t n )
    { m_v.resize( n ); }

private:
    std::vector< double > m_v;

};

//[my_vector_resizeable
// define my_vector as resizeable

namespace boost { namespace numeric { namespace odeint {

template<size_t N>
struct is_resizeable< my_vector<N> >
{
    typedef boost::true_type type;
    static const bool value = type::value;
};

} } }
//]

typedef my_vector<3> state_type;

void lorenz( const state_type &x , state_type &dxdt , const double t )
{
    const double sigma( 10.0 );
    const double R( 28.0 );
    const double b( 8.0 / 3.0 );

    dxdt[0] = sigma * ( x[1] - x[0] );
    dxdt[1] = R * x[0] - x[1] - x[0] * x[2];
    dxdt[2] = -b * x[2] + x[0] * x[1];
}

using namespace boost::numeric::odeint;

int main()
{
    state_type x(3);
    x[0] = 5.0 ; x[1] = 10.0 ; x[2] = 10.0;

    // make sure resizing is ON
    BOOST_STATIC_ASSERT( is_resizeable<state_type>::value == true );

    // my_vector works with range_algebra as it implements
    // the required parts of a container interface
    // no further work is required

    integrate_const( runge_kutta4< state_type >() , lorenz , x , 0.0 , 10.0 , 0.1 );
}


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

/*
 Copyright (C) 2014 Peter Caspers

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

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

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file zabrinterpolation.hpp
    \brief ZABR interpolation interpolation between discrete points
*/

#ifndef quantlib_zabr_interpolation_hpp
#define quantlib_zabr_interpolation_hpp

#include <ql/math/interpolations/xabrinterpolation.hpp>
#include <ql/experimental/volatility/zabrsmilesection.hpp>

#include <boost/assign/list_of.hpp>

namespace QuantLib {

namespace detail {

template <typename Evaluation> struct ZabrSpecs {
    Size dimension() { return 5; }
    Real eps() { return 0.000001; }
    void defaultValues(std::vector<Real> &params,
                       std::vector<bool> &paramIsFixed, const Real &forward,
                       const Real expiryTime, const std::vector<Real>& addParams) {
        if (params[1] == Null<Real>())
            params[1] = 0.5;
        if (params[0] == Null<Real>())
            // adapt alpha to beta level
            params[0] =
                0.2 *
                (params[1] < 0.9999 ? std::pow(forward, 1.0 - params[1]) : 1.0);
        if (params[2] == Null<Real>())
            params[2] = std::sqrt(0.4);
        if (params[3] == Null<Real>())
            params[3] = 0.0;
        if (params[4] == Null<Real>())
            params[4] = 1.0;
    }
    void guess(Array &values, const std::vector<bool> &paramIsFixed,
               const Real &forward, const Real expiryTime,
               const std::vector<Real> &r, const std::vector<Real>& addParams) {
        Size j = 0;
        if (!paramIsFixed[1])
            values[1] = (1.0 - 2E-6) * r[j++] + 1E-6;
        if (!paramIsFixed[0]) {
            values[0] = (1.0 - 2E-6) * r[j++] + 1E-6; // lognormal vol guess
            // adapt this to beta level
            if (values[1] < 0.999)
                values[0] *= std::pow(forward, 1.0 - values[1]);
        }
        if (!paramIsFixed[2])
            values[2] = 1.5 * r[j++] + 1E-6;
        if (!paramIsFixed[3])
            values[3] = (2.0 * r[j++] - 1.0) * (1.0 - 1E-6);
        if (!paramIsFixed[4])
            values[4] = r[j++] * 2.0;
    }
    Real eps1() { return .0000001; }
    Real eps2() { return .9999; }
    Real dilationFactor() { return 0.001; }
    Array inverse(const Array &y, const std::vector<bool> &,
                  const std::vector<Real> &, const Real) {
        Array x(5);
        x[0] = y[0] < 25.0 + eps1() ? std::sqrt(y[0] - eps1())
                                    : (y[0] - eps1() + 25.0) / 10.0;
        x[1] = std::sqrt(-std::log(y[1]));
        x[2] = std::tan(M_PI*(y[4]/5.0-0.5));
        x[3] = std::asin(y[3] / eps2());
        x[4] = std::tan(M_PI*(y[4]/1.9-0.5));
        return x;
    }
    Array direct(const Array &x, const std::vector<bool> &,
                 const std::vector<Real> &, const Real) {
        Array y(5);
        y[0] = std::fabs(x[0]) < 5.0 ? x[0] * x[0] + eps1()
                                     : (10.0 * std::fabs(x[0]) - 25.0) + eps1();
        y[1] = std::fabs(x[1]) < std::sqrt(-std::log(eps1()))
                   ? std::exp(-(x[1] * x[1]))
                   : eps1();
        // limit nu to 5.00
        y[2] = (std::atan(x[2])/M_PI + 0.5) * 5.0;
        y[3] = std::fabs(x[3]) < 2.5 * M_PI
                   ? eps2() * std::sin(x[3])
                   : eps2() * (x[3] > 0.0 ? 1.0 : (-1.0));
        // limit gamma to 1.9
        y[4] = (std::atan(x[4])/M_PI + 0.5) * 1.9;
        return y;
    }
    Real weight(const Real strike, const Real forward, const Real stdDev,
                const std::vector<Real> &addParams) {
        return blackFormulaStdDevDerivative(strike, forward, stdDev, 1.0);
    }
    typedef ZabrSmileSection<Evaluation> type;
    ext::shared_ptr<type> instance(const Time t, const Real &forward,
                                     const std::vector<Real> &params,
                                     const std::vector<Real> &addParams) {
        return ext::make_shared<type>(t, forward, params);
    }
};
} // end namespace detail

//! zabr smile interpolation between discrete volatility points.
template <class Evaluation> class ZabrInterpolation : public Interpolation {
  public:
    template <class I1, class I2>
    ZabrInterpolation(
        const I1 &xBegin, // x = strikes
        const I1 &xEnd,
        const I2 &yBegin, // y = volatilities
        Time t,           // option expiry
        const Real &forward, Real alpha, Real beta, Real nu, Real rho,
        Real gamma, bool alphaIsFixed, bool betaIsFixed, bool nuIsFixed,
        bool rhoIsFixed, bool gammaIsFixed, bool vegaWeighted = true,
        const ext::shared_ptr<EndCriteria> &endCriteria =
            ext::shared_ptr<EndCriteria>(),
        const ext::shared_ptr<OptimizationMethod> &optMethod =
            ext::shared_ptr<OptimizationMethod>(),
        const Real errorAccept = 0.0020, const bool useMaxError = false,
        const Size maxGuesses = 50) {
            impl_ = ext::shared_ptr<
                Interpolation::Impl>(new detail::XABRInterpolationImpl<
                I1, I2,
                detail::ZabrSpecs<Evaluation> >(
                xBegin, xEnd, yBegin, t, forward,
                boost::assign::list_of(alpha)(beta)(nu)(rho)(gamma),
                boost::assign::list_of(alphaIsFixed)(betaIsFixed)(nuIsFixed)(
                    rhoIsFixed)(gammaIsFixed),
                vegaWeighted, endCriteria, optMethod, errorAccept, useMaxError,
                maxGuesses));
            coeffs_ = ext::dynamic_pointer_cast<detail::XABRCoeffHolder<
                detail::ZabrSpecs<Evaluation> > >(impl_);
    }
    Real expiry() const { return coeffs_->t_; }
    Real forward() const { return coeffs_->forward_; }
    Real alpha() const { return coeffs_->params_[0]; }
    Real beta() const { return coeffs_->params_[1]; }
    Real nu() const { return coeffs_->params_[2]; }
    Real rho() const { return coeffs_->params_[3]; }
    Real gamma() const { return coeffs_->params_[4]; }
    Real rmsError() const { return coeffs_->error_; }
    Real maxError() const { return coeffs_->maxError_; }
    const std::vector<Real> &interpolationWeights() const {
        return coeffs_->weights_;
    }
    EndCriteria::Type endCriteria() { return coeffs_->XABREndCriteria_; }

  private:
    ext::shared_ptr<detail::XABRCoeffHolder<detail::ZabrSpecs<Evaluation> > > coeffs_;
};

//! no arbtrage sabr interpolation factory and traits
template<class Evaluation> class Zabr {
  public:
    Zabr(Time t,
         Real forward,
         Real alpha,
         Real beta,
         Real nu,
         Real rho,
         Real gamma,
         bool alphaIsFixed,
         bool betaIsFixed,
         bool nuIsFixed,
         bool rhoIsFixed,
         bool gammaIsFixed,
         bool vegaWeighted = false,
         const ext::shared_ptr<EndCriteria>& endCriteria = ext::shared_ptr<EndCriteria>(),
         const ext::shared_ptr<OptimizationMethod>& optMethod =
             ext::shared_ptr<OptimizationMethod>(),
         const Real errorAccept = 0.0020,
         const bool useMaxError = false,
         const Size maxGuesses = 50)
    : t_(t), forward_(forward), alpha_(alpha), beta_(beta), nu_(nu), rho_(rho),
      alphaIsFixed_(alphaIsFixed), betaIsFixed_(betaIsFixed), nuIsFixed_(nuIsFixed),
      rhoIsFixed_(rhoIsFixed), gammaIsFixed_(gammaIsFixed), vegaWeighted_(vegaWeighted),
      endCriteria_(endCriteria), optMethod_(optMethod), errorAccept_(errorAccept),
      useMaxError_(useMaxError), maxGuesses_(maxGuesses) {}
    template <class I1, class I2>
    Interpolation interpolate(const I1 &xBegin, const I1 &xEnd,
                              const I2 &yBegin) const {
        return ZabrInterpolation<Evaluation>(
            xBegin, xEnd, yBegin, t_, forward_, alpha_, beta_, nu_, rho_,
            gamma_, alphaIsFixed_, betaIsFixed_, nuIsFixed_, rhoIsFixed_,
            gammaIsFixed_, vegaWeighted_, endCriteria_, optMethod_,
            errorAccept_, useMaxError_, maxGuesses_);
    }
    static const bool global = true;

  private:
    Time t_;
    Real forward_;
    Real alpha_, beta_, nu_, rho_, gamma_;
    bool alphaIsFixed_, betaIsFixed_, nuIsFixed_, rhoIsFixed_, gammaIsFixed_;
    bool vegaWeighted_;
    const ext::shared_ptr<EndCriteria> endCriteria_;
    const ext::shared_ptr<OptimizationMethod> optMethod_;
    const Real errorAccept_;
    const bool useMaxError_;
    const Size maxGuesses_;
};
}

#endif