#include <iostream>
#include <fstream>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <geometry/draw.hpp>
#include <imgproc/gradient_adapter.hpp>
#include <imgproc/derivative_gradient.hpp>
#include <imgproc/susan.hpp>
#include <imgproc/rcmg.hpp>
#include <imgproc/quadratureG2.hpp>
#include <imgproc/quadratureS.hpp>
#include <imgproc/quadratureSF.hpp>
#include <imgproc/quadratureLGF.hpp>
#include <imgproc/pc_sqf.hpp>
#include <imgproc/pc_lgf.hpp>
#include <imgproc/pc_matlab.hpp>
#include <imgproc/image_operator.hpp>
#include <utility/matlab_helpers.hpp>

#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/format.hpp>
#include <boost/lexical_cast.hpp>

#define WRITE_IMAGE_FILES
//#define SHOW_IMAGES

using namespace lsfm;
using namespace std;
namespace fs = boost::filesystem;

std::string path = "./orientation/", fname;
int stepsi = -1, steps = 45;
double magTh = 0.1;

constexpr int ENTRY_HALF_RANGE = 1;

template<class GT, class MT = int, class DT = double>
struct Entry {
    Entry() :  flags(0) {}
    Entry(const cv::Ptr<GradientI<uchar, GT, MT, DT>>& a, const std::string& b, int f = 0)
        : grad(a), name(b), flags(f) {}

    cv::Ptr<GradientI<uchar, GT, MT, DT>> grad;
    std::string name;
    int flags;

    inline bool hr() const {
        return flags & ENTRY_HALF_RANGE;
    }

    inline cv::Mat process(const cv::Mat src) {
        grad->process(src);
        return grad->magnitude();
    }

    cv::Mat fixGrad(const cv::Mat &m) const {
        cv::Mat mag;
        if (m.type() != CV_32F)
            m.convertTo(mag, CV_32F);
        else
            m.copyTo(mag);

        double vmin, vmax;
        cv::minMaxIdx(mag, &vmin, &vmax);
        // laplace
        if (vmin < 0) {
            vmax = std::max(vmax, -vmin);
            mag = cv::abs(mag);
            mag *= 255.0 / vmax;
        }
        else {
            mag -= vmin;
            mag *= 255.0 / (vmax - vmin);
        }
        mag.convertTo(mag, CV_8U);
        return mag;
    }
};

double error(const cv::Mat &gt, const cv::Mat &dir, bool hr = false) {
    cv::Mat res = cv::abs(gt - dir);
    cv::subtract(2 * CV_PI, res, res, res > CV_PI);
    if (hr)
        cv::subtract(CV_PI, res, res, res > CV_PI / 2);
#ifdef WRITE_IMAGE_FILES
    cv::Mat write = res / CV_PI * 2550;
    write.convertTo(write, CV_8U);
    if (stepsi < 0)
        cv::imwrite(path + fname + "-error.png", write);
    else
        cv::imwrite(path + boost::str(boost::format("%04d-") % stepsi) + fname + "-error.png", write);
#endif
#ifdef SHOW_IMAGES
    imshow("error", res / CV_PI * 10);
    cv::waitKey();
#endif
    return cv::sum(res)[0];
}

template<class GT, class MT = int, class DT = float>
double testSingle(Entry<GT, MT, DT> &e, const cv::Mat &img, const cv::Mat &mask, double rot = 0, double noise = 0) {

    RotateOperator<double> op_rot(rot, cv::Point2d(127, 127), cv::INTER_CUBIC);
    RotateOperator<double> op_rot_mask(rot, cv::Point2d(127, 127), cv::INTER_NEAREST);
    GaussianNoiseOperator op_noise(noise);

    cv::Mat gt(mask.size(),CV_64F);
    gt.setTo(0);

    gt.setTo(-rot, mask == 1);
    gt.setTo(-rot + CV_PI/2, mask == 2);

    if (e.hr()) {
        gt.setTo(-rot, mask == 3);
        gt.setTo(-rot + CV_PI / 2, mask == 4);
    }
    else
    {
        gt.setTo(-rot + CV_PI, mask == 3);
        gt.setTo(-rot -  CV_PI / 2, mask == 4);
    }

    cv::Mat nimg;
    img.copyTo(nimg);

    //cv::imshow("img", img);

    cv::Mat nmask;
    mask.copyTo(nmask);
    if (rot > 0) {
        op_rot(nimg);
        op_rot_mask(nmask);
        op_rot_mask(gt);
    }

    int num = static_cast<int>(cv::sum(gt > 0)[0]) / 255;
    if (noise > 0)
        op_noise(nimg);

    e.grad->process(nimg);
    cv::Mat dir = e.grad->direction();
    dir.setTo(0, nmask == 0);

#ifdef WRITE_IMAGE_FILES
    cv::imwrite(path + boost::str(boost::format("%04d-") % stepsi) + fname + ".png", nimg);
    cv::imwrite(path + boost::str(boost::format("%04d-") % stepsi) + fname + "-mask.png", nmask > 0);
#endif
#ifdef SHOW_IMAGES
    cv::imshow("img", nimg);
    cv::imshow("mask", nmask > 0);
    //cv::imshow("gt", gt > 0);
    cv::imshow("cA", quiverDir<DT, uchar>(nimg, gt, Range<double>(-CV_PI, CV_PI), nmask > 0, 2, 2, 1, 8, 1));
    cv::imshow("cB", quiverDir<DT, uchar>(nimg, dir, Range<double>(-CV_PI, CV_PI), nmask > 0, 2, 2, 1, 8, 1));
    //cv::imshow(e.name + "-mask", nmask > 0);
    std::cout << rot << std::endl;
#endif

    return error(gt, dir, e.hr()) / num;

}

cv::Mat boxImage() {
    cv::Mat img(256, 256, CV_8U);
    img.setTo(85);
    img.rowRange(47, 208).colRange(47, 208).setTo(170);
    return img;
}

template<class GT, class MT = int, class DT = float>
double testBox(Entry<GT,MT,DT> &e, cv::Mat img, cv::Mat mask, double blur = 0, double noise = 0) {
    GaussianBlurOperator op_blur(blur);

    if (blur > 0)
        op_blur(img);

    // remove borders dependong on blur
    int ks = cvRound(blur * 6 + 1) | 1;
    // make sure that border has at least the size of greatest filter kernel (7x7)
    if (ks < 9)
        ks = 9;
    mask.rowRange(47 - ks, 47 + ks).colRange(47 - ks, 47 + ks).setTo(0);
    mask.rowRange(207 - ks, 207 + ks).colRange(47 - ks, 47 + ks).setTo(0);
    mask.rowRange(47 - ks, 47 + ks).colRange(207 - ks, 207 + ks).setTo(0);
    mask.rowRange(207 - ks, 207 + ks).colRange(207 - ks, 207 + ks).setTo(0);

    cv::Mat tmp = mask.rowRange(47 + ks, 207 - ks).colRange(47 - ks, 47 + ks);
    tmp.setTo(1, tmp > 0);

    tmp = mask.rowRange(47 - ks, 47 + ks).colRange(47 + ks, 207 - ks);
    tmp.setTo(2, tmp > 0);

    tmp = mask.rowRange(47 + ks, 207 - ks).colRange(207 - ks, 207 + ks);
    tmp.setTo(3, tmp > 0);

    tmp = mask.rowRange(207 - ks, 207 + ks).colRange(47 + ks, 207 - ks);
    tmp.setTo(4, tmp > 0);

    double sum = 0;
    for (stepsi = 0; stepsi <= steps; ++stepsi)
        sum += testSingle(e, img, mask, stepsi * CV_PI / (4 *steps), noise);

    return sum / steps;
}

cv::Mat diskImage() {
    cv::Mat img(512, 512, CV_8U);
    img.setTo(85);
    cv::circle(img, cv::Point(256, 256), 150, 170, -1, CV_AA);
    return img;
}

template<class GT, class MT = int, class DT = double>
double testDisk(Entry<GT, MT, DT> &e, cv::Mat img, cv::Mat mask, double blur = 0, double noise = 0) {
    stepsi = -1;
    GaussianBlurOperator op_blur(blur);
    GaussianNoiseOperator op_noise(noise);

    cv::Mat gt(512, 512, CV_64F);
    for (int y = 0; y != 512; ++y)
        for (int x = 0; x != 512; ++x)
            gt.at<double>(y, x) = std::atan2(static_cast<double>(256-y), static_cast<double>(256-x));

    if (e.hr()) {
        cv::add(gt, CV_PI, gt, gt < -CV_PI / 2);
        cv::subtract(gt, CV_PI, gt, gt > CV_PI / 2);
    }

    if (blur > 0)
        op_blur(img);

    int num = static_cast<int>(cv::sum(mask)[0]) / 255;
    gt.setTo(0, mask == 0);

    if (noise > 0)
        op_noise(img);

    e.grad->process(img);
    cv::Mat dir = e.grad->direction();
    dir.setTo(0, mask == 0);

#ifdef WRITE_IMAGE_FILES
    cv::imwrite(path + fname + ".png", img);
    cv::imwrite(path + fname + "-mask.png", mask);
#endif
#ifdef SHOW_IMAGES
    cv::imshow("img", img);
    cv::imshow("mask", mask > 0);
    cv::imshow("A", quiverDir<DT, uchar>(img, gt, Range<double>(-CV_PI, CV_PI), mask, 4, 4, 1, 8, 1));
    cv::imshow("B", quiverDir<DT, uchar>(img, dir, Range<double>(-CV_PI, CV_PI), mask, 4, 4, 1, 8, 1));
    //cv::imshow(e.name + "-mask", mask > 0);

    //cv::Mat res = e.fixGrad(e.process(img));
    //imshow(e.name + " - grad", res);
    //imshow(e.name + " - grad", quiverDir<DT, MT>(res, e.grad->direction(), e.grad->directionRange(), e.grad->magnitude(), 1, 1, 4, 1, 0.5));
#endif
    return error(gt, dir) / num;
}

int main(int argc, char** argv)
{
    std::vector<Entry<short>> gradI;
    gradI.push_back(Entry<short>(new SusanGradient<short, int, double>, "Susan (37)", ENTRY_HALF_RANGE));
    gradI.push_back(Entry<short>(new SusanGradient<short, int, double>(20,true), "Susan (3x3)", ENTRY_HALF_RANGE));
    gradI.push_back(Entry<short>(new RCMGradient<uchar, 1, short, int, double>(3,1), "RMG (3x3)",ENTRY_HALF_RANGE));
    gradI.push_back(Entry<short>(new RCMGradient<uchar, 1, short, int, double>(5, 3), "RMG (5x5)", ENTRY_HALF_RANGE));

    std::vector<Entry<double, double>> gradF;
    gradF.push_back(Entry<double, double>(new DerivativeGradient<uchar, double, double, double, RobertsDerivative>, "Roberts (2x2)"));
    gradF.push_back(Entry<double, double>(new DerivativeGradient<uchar, double, double, double, PrewittDerivative>, "Prewitt (3x3)"));
    gradF.push_back(Entry<double, double>(new DerivativeGradient<uchar, double, double, double, ScharrDerivative>, "Scharr (3x3)"));

    gradF.push_back(Entry<double, double>(new DerivativeGradient<uchar, double, double, double, SobelDerivative>, "Sobel (3x3)"));
    gradF.push_back(Entry<double, double>(new DerivativeGradient<uchar, double, double, double, SobelDerivative>({ NV("grad_kernel_size",5)}), "Sobel (5x5)"));
    gradF.push_back(Entry<double, double>(new DerivativeGradient<uchar, double, double, double, SobelDerivative>({ NV("grad_kernel_size",7) }), "Sobel (7x7)"));
    gradF.push_back(Entry<double, double>(new DerivativeGradient<uchar, double, double, double, SobelDerivative>({ NV("grad_kernel_size",9) }), "Sobel (9x9)"));

    gradF.push_back(Entry<double, double>(new DerivativeGradient<uchar, double, double, double, GaussianDerivative>({ NV("grad_kernel_size",3), NV("grad_range",1.5) }), "Gauss (3x3)"));
    gradF.push_back(Entry<double, double>(new DerivativeGradient<uchar, double, double, double, GaussianDerivative>({ NV("grad_kernel_size",5), NV("grad_range",2.3) }), "Gauss (5x5)"));
    gradF.push_back(Entry<double, double>(new DerivativeGradient<uchar, double, double, double, GaussianDerivative>({ NV("grad_kernel_size",7), NV("grad_range",3.0) }), "Gauss (7x7)"));
    gradF.push_back(Entry<double, double>(new DerivativeGradient<uchar, double, double, double, GaussianDerivative>({ NV("grad_kernel_size",9), NV("grad_range",3.5) }), "Gauss (9x9)"));

    gradF.push_back(Entry<double, double>(new GradientOdd<QuadratureG2<uchar, double>>({ NV("grad_kernel_size",3), NV("grad_kernel_spacing",1.24008) }), "QF StG (3x3)", ENTRY_HALF_RANGE));
    gradF.push_back(Entry<double, double>(new GradientOdd<QuadratureG2<uchar, double>>({ NV("grad_kernel_size", 5), NV("grad_kernel_spacing", 1.008) }), "QF StG (5x5)", ENTRY_HALF_RANGE));
    gradF.push_back(Entry<double, double>(new GradientOdd<QuadratureG2<uchar, double>>({ NV("grad_kernel_size", 7), NV("grad_kernel_spacing", 0.873226) }), "QF StG (7x7)", ENTRY_HALF_RANGE));
    gradF.push_back(Entry<double, double>(new GradientOdd<QuadratureG2<uchar, double>>({ NV("grad_kernel_size", 9), NV("grad_kernel_spacing", 0.781854) }), "QF StG (9x9)", ENTRY_HALF_RANGE));

    gradF.push_back(Entry<double, double>(new GradientOdd<QuadratureS<uchar, double, double>>({ NV("grad_scale", 1), NV("grad_muls", 2), NV("grad_kernel_size", 3), NV("grad_kernel_spacing", 1.2) }), "SQF Po (3x3)"));
    gradF.push_back(Entry<double, double>(new GradientOdd<QuadratureS<uchar, double, double>>({ NV("grad_scale", 1), NV("grad_muls", 2), NV("grad_kernel_size", 5), NV("grad_kernel_spacing", 1.2) }), "SQF Po (5x5)"));
    gradF.push_back(Entry<double, double>(new GradientOdd<QuadratureS<uchar, double, double>>({ NV("grad_scale", 1), NV("grad_muls", 2), NV("grad_kernel_size", 7), NV("grad_kernel_spacing", 1.2) }), "SQF Po (7x7)"));
    gradF.push_back(Entry<double, double>(new GradientOdd<QuadratureS<uchar, double, double>>({ NV("grad_scale", 1), NV("grad_muls", 2), NV("grad_kernel_size", 9), NV("grad_kernel_spacing", 1.2) }), "SQF Po (9x9)"));

    gradF.push_back(Entry<double, double>(new GradientOdd<QuadratureLGF<uchar, double>>({ NV("grad_waveLength", 5), NV("grad_sigmaOnf", 0.55) }), "SQFF LG"));
    gradF.push_back(Entry<double, double>(new GradientOdd<QuadratureSF<uchar, double>>({ NV("grad_scale", 1), NV("grad_muls", 2), NV("grad_kernel_spacing", 1.2) }), "SQFF Po12"));
    gradF.push_back(Entry<double, double>(new GradientOdd<QuadratureSF<uchar, double>>({ NV("grad_scale", 1), NV("grad_muls", 3), NV("grad_kernel_spacing", 2.5) }), "SQFF Po"));

    gradF.push_back(Entry<double, double>(new GradientEnergy<PCLgf<uchar, double>>({ NV("grad_waveLength", 3), NV("grad_sigmaOnf", 0.55) }), "PCF Lg"));
    gradF.push_back(Entry<double, double>(new GradientOdd<PCSqf<uchar, double>>({ NV("grad_scale", 1), NV("grad_muls", 3), NV("grad_kernel_spacing", 2.5) }), "PCF Po"));

    int cols = gradI.size() + gradF.size() + 1;
    int rows = 7;
    std::vector<std::vector<std::string>> table_box, table_disk;
    table_box.resize(cols); table_disk.resize(cols);
    for_each(table_box.begin(), table_box.end(), [&](std::vector<std::string> &row) {
        row.resize(rows);
    });

    for_each(table_disk.begin(), table_disk.end(), [&](std::vector<std::string> &row) {
        row.resize(rows);
    });

    std::cout << "none...";
    table_box[0][0] = "Method";
    table_box[0][1] = "none";

    table_disk[0][0] = "Method";
    table_disk[0][1] = "none";

    DerivativeGradient<uchar, double, double, double, SobelDerivative> sobel;
    cv::Mat box = boxImage();
    sobel.process(box);
    cv::Mat box_mask = sobel.magnitude() > sobel.magnitudeThreshold(magTh);

    cv::Mat disk = diskImage();
    sobel.process(disk);
    cv::Mat disk_mask = sobel.magnitude() > sobel.magnitudeThreshold(magTh);

    int row = 1;
    for_each(gradI.begin(), gradI.end(), [&](Entry<short> &e) {
        table_box[row][0] = e.name;
        table_disk[row][0] = e.name;
        fname = "box-"+ e.name;
        table_box[row][1] = boost::str(boost::format("%.3f") % testBox(e,box.clone(),box_mask.clone(),0,0));
        fname = "disk-"+ e.name;
        table_disk[row++][1] = boost::str(boost::format("%.3f") % testDisk(e,disk.clone(),disk_mask.clone(),0,0));
    });
    for_each(gradF.begin(), gradF.end(), [&](Entry<double, double> &e) {
        table_box[row][0] = e.name;
        table_disk[row][0] = e.name;
        fname = "box-" + e.name;
        table_box[row][1] = boost::str(boost::format("%.3f") % testBox(e, box.clone(), box_mask.clone(), 0, 0));
        fname = "disk-" + e.name;
        table_disk[row++][1] = boost::str(boost::format("%.3f") % testDisk(e, disk.clone(), disk_mask.clone(), 0, 0));
    });

    std::cout << "done" << std::endl << "blur...";
    table_box[0][2] = "blur";
    table_disk[0][2] = "blur";

    row = 1;
    for_each(gradI.begin(), gradI.end(), [&](Entry<short> &e) {
        fname = "box-blur" + e.name;
        table_box[row][2] = boost::str(boost::format("%.3f") % testBox(e, box.clone(), box_mask.clone(), 2, 0));
        fname = "disk-blur" + e.name;
        table_disk[row++][2] = boost::str(boost::format("%.3f") % testDisk(e, disk.clone(), disk_mask.clone(), 2, 0));
    });
    for_each(gradF.begin(), gradF.end(), [&](Entry<double, double> &e) {
        fname = "box-blur-"+ e.name;
        table_box[row][2] = boost::str(boost::format("%.3f") % testBox(e, box.clone(), box_mask.clone(), 2, 0));
        fname = "disk-blur-"+ e.name;
        table_disk[row++][2] = boost::str(boost::format("%.3f") % testDisk(e, disk.clone(), disk_mask.clone(), 2, 0));
    });

    std::cout << "done" << std::endl << "noise 10...";
    table_box[0][3] = "noise10";
    table_disk[0][3] = "noise10";

    row = 1;
    for_each(gradI.begin(), gradI.end(), [&](Entry<short> &e) {
        fname = "box-noise10-"+ e.name;
        table_box[row][3] = boost::str(boost::format("%.3f") % testBox(e, box.clone(), box_mask.clone(), 0, 10));
        fname = "disk-noise10-"+ e.name;
        table_disk[row++][3] = boost::str(boost::format("%.3f") % testDisk(e, disk.clone(), disk_mask.clone(), 0, 10));
    });
    for_each(gradF.begin(), gradF.end(), [&](Entry<double, double> &e) {
        fname = "box-noise10-"+ e.name;
        table_box[row][3] = boost::str(boost::format("%.3f") % testBox(e, box.clone(), box_mask.clone(), 0, 10));
        fname = "disk-noise10-" + e.name;
        table_disk[row++][3] = boost::str(boost::format("%.3f") % testDisk(e, disk.clone(), disk_mask.clone(), 0, 10));
    });

    std::cout << "done" << std::endl << "noise 40...";
    table_box[0][4] = "noise40";
    table_disk[0][4] = "noise40";

    row = 1;
    for_each(gradI.begin(), gradI.end(), [&](Entry<short> &e) {
        fname = "box-noise40-" + e.name;
        table_box[row][4] = boost::str(boost::format("%.3f") % testBox(e, box.clone(), box_mask.clone(), 0, 40));
        fname = "disk-noise40-" + e.name;
        table_disk[row++][4] = boost::str(boost::format("%.3f") % testDisk(e, disk.clone(), disk_mask.clone(), 0, 40));
    });
    for_each(gradF.begin(), gradF.end(), [&](Entry<double, double> &e) {
        fname = "box-noise40-" + e.name;
        table_box[row][4] = boost::str(boost::format("%.3f") % testBox(e, box.clone(), box_mask.clone(), 0, 40));
        fname = "disk-noise40-" + e.name;
        table_disk[row++][4] = boost::str(boost::format("%.3f") % testDisk(e, disk.clone(), disk_mask.clone(), 0, 40));
    });

    std::cout << "done" << std::endl << "blur + noise 10...";
    table_box[0][5] = "bnoise10";
    table_disk[0][5] = "bnoise10";

    row = 1;
    for_each(gradI.begin(), gradI.end(), [&](Entry<short> &e) {
        fname = "box-bnoise10-" + e.name;
        table_box[row][5] = boost::str(boost::format("%.3f") % testBox(e, box.clone(), box_mask.clone(), 2, 10));
        fname = "disk-bnoise10-" + e.name;
        table_disk[row++][5] = boost::str(boost::format("%.3f") % testDisk(e, disk.clone(), disk_mask.clone(), 2, 10));
    });
    for_each(gradF.begin(), gradF.end(), [&](Entry<double, double> &e) {
        fname = "box-bnoise10-" + e.name;
        table_box[row][5] = boost::str(boost::format("%.3f") % testBox(e, box.clone(), box_mask.clone(), 2, 10));
        fname = "disk-bnoise10-" + e.name;
        table_disk[row++][5] = boost::str(boost::format("%.3f") % testDisk(e, disk.clone(), disk_mask.clone(), 2, 10));
    });

    std::cout << "done" << std::endl << "blur + noise 40...";
    table_box[0][6] = "bnoise40";
    table_disk[0][6] = "bnoise40";

    row = 1;
    for_each(gradI.begin(), gradI.end(), [&](Entry<short> &e) {
        fname = "box-bnoise40-" + e.name;
        table_box[row][6] = boost::str(boost::format("%.3f") % testBox(e, box.clone(), box_mask.clone(), 2, 40));
        fname = "disk-bnoise40-" + e.name;
        table_disk[row++][6] = boost::str(boost::format("%.3f") % testDisk(e, disk.clone(), disk_mask.clone(), 2, 40));
    });
    for_each(gradF.begin(), gradF.end(), [&](Entry<double, double> &e) {
        fname = "box-bnoise40-" + e.name;
        table_box[row][6] = boost::str(boost::format("%.3f") % testBox(e, box.clone(), box_mask.clone(), 2, 40));
        fname = "disk-bnoise40-" + e.name;
        table_disk[row++][6] = boost::str(boost::format("%.3f") % testDisk(e, disk.clone(), disk_mask.clone(), 2, 40));
    });
    std::cout << "done" << std::endl;

    std::ofstream ofs;
    ofs.open("gradient_orientation_box.csv");

    for_each(table_box.begin(), table_box.end(), [&](const std::vector<std::string> &row) {
        for_each(row.begin(), row.end(), [&](const std::string &cell) {
            std::cout << cell << "\t";
            ofs << cell << ";";
        });
        std::cout << std::endl;
        ofs << std::endl;
    });

    ofs.close();

    ofs.open("gradient_orientation_disk.csv");

    for_each(table_disk.begin(), table_disk.end(), [&](const std::vector<std::string> &row) {
        for_each(row.begin(), row.end(), [&](const std::string &cell) {
            std::cout << cell << "\t";
            ofs << cell << ";";
        });
        std::cout << std::endl;
        ofs << std::endl;
    });

    ofs.close();

    cv::waitKey();

    return 0;
}


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

// Copyright (c) 2013, 2014 Barend Gehrels, Amsterdam, the Netherlands.

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

//[buffer_with_strategies
//` Shows how the buffer algorithm can be used to create a buffer of a linestring

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/geometries.hpp>
/*<-*/ #include "../examples_utils/create_svg_buffer.hpp" /*->*/

int main()
{
    typedef double coordinate_type;
    typedef boost::geometry::model::d2::point_xy<coordinate_type> point;
    typedef boost::geometry::model::polygon<point> polygon;

    // Declare strategies
    const double buffer_distance = 1.0;
    const int points_per_circle = 36;
    boost::geometry::strategy::buffer::distance_symmetric<coordinate_type> distance_strategy(buffer_distance);
    boost::geometry::strategy::buffer::join_round join_strategy(points_per_circle);
    boost::geometry::strategy::buffer::end_round end_strategy(points_per_circle);
    boost::geometry::strategy::buffer::point_circle circle_strategy(points_per_circle);
    boost::geometry::strategy::buffer::side_straight side_strategy;

    // Declare output
    boost::geometry::model::multi_polygon<polygon> result;

    // Declare/fill a linestring
    boost::geometry::model::linestring<point> ls;
    boost::geometry::read_wkt("LINESTRING(0 0,4 5,7 4,10 6)", ls);

    // Create the buffer of a linestring
    boost::geometry::buffer(ls, result,
                distance_strategy, side_strategy,
                join_strategy, end_strategy, circle_strategy);
    /*<-*/ create_svg_buffer("buffer_linestring.svg", ls, result); /*->*/

    // Declare/fill a multi point
    boost::geometry::model::multi_point<point> mp;
    boost::geometry::read_wkt("MULTIPOINT((3 3),(4 4),(6 2))", mp);

    // Create the buffer of a multi point
    boost::geometry::buffer(mp, result,
                distance_strategy, side_strategy,
                join_strategy, end_strategy, circle_strategy);
    /*<-*/ create_svg_buffer("buffer_multi_point.svg", mp, result); /*->*/

    // Declare/fill a multi_polygon
    boost::geometry::model::multi_polygon<polygon> mpol;
    boost::geometry::read_wkt("MULTIPOLYGON(((0 1,2 5,5 3,0 1)),((1 1,5 2,5 0,1 1)))", mpol);

    // Create the buffer of a multi polygon
    boost::geometry::buffer(mpol, result,
                distance_strategy, side_strategy,
                join_strategy, end_strategy, circle_strategy);
    /*<-*/ create_svg_buffer("buffer_multi_polygon.svg", mpol, result); /*->*/

    return 0;
}

//]


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

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.

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

//[degree_radian
//` Specify two coordinate systems, one in degrees, one in radians.

#include <iostream>
#include <boost/geometry.hpp>

using namespace boost::geometry;

int main()
{
    typedef model::point<double, 2, cs::spherical_equatorial<degree> > degree_point;
    typedef model::point<double, 2, cs::spherical_equatorial<radian> > radian_point;

    degree_point d(4.893, 52.373);
    radian_point r(0.041, 0.8527);

    double dist = distance(d, r);
    std::cout
        << "distance:" << std::endl
        << dist << " over unit sphere" << std::endl
        << dist * 3959  << " over a spherical earth, in miles" << std::endl;

    return 0;
}

//]

//[degree_radian_output
/*`
Output:
[pre
distance:
0.0675272 over unit sphere
267.34 over a spherical earth, in miles
]
*/
//]


#define BOOST_TEST_MODULE variables
#include <boost/test/included/unit_test.hpp>
#include "exprtest.hpp"

double const x = 1;
double const y = 2;

static std::map<std::string, double> symtab() {
    std::map<std::string, double> symtab;
    symtab.insert(std::make_pair("x",  x));
    symtab.insert(std::make_pair("y",  y));
    symtab.insert(std::make_pair("e", -1));
    return symtab;
}

SYMEXPRTEST(var1, "x+y" , symtab(), x+y)
SYMEXPRTEST(var2, "x-y" , symtab(), x-y)
SYMEXPRTEST(var3, "x*y" , symtab(), x*y)
SYMEXPRTEST(var4, "x/y" , symtab(), x/y);
SYMEXPRTEST(var5, "x%y" , symtab(), std::fmod(x,y));
SYMEXPRTEST(var6, "x**y", symtab(), std::pow(x,y));

// Constants have higher priority than variables of the same name
SYMEXPRTEST(var7, "e", symtab(), boost::math::constants::e<double>())


//******************************************************************************
//  Copyright(C) 2008-2013 Intel Corporation. All Rights Reserved.
//
//  The source code, information  and  material ("Material") contained herein is
//  owned  by Intel Corporation or its suppliers or licensors, and title to such
//  Material remains  with Intel Corporation  or its suppliers or licensors. The
//  Material  contains proprietary information  of  Intel or  its  suppliers and
//  licensors. The  Material is protected by worldwide copyright laws and treaty
//  provisions. No  part  of  the  Material  may  be  used,  copied, reproduced,
//  modified, published, uploaded, posted, transmitted, distributed or disclosed
//  in any way  without Intel's  prior  express written  permission. No  license
//  under  any patent, copyright  or  other intellectual property rights  in the
//  Material  is  granted  to  or  conferred  upon  you,  either  expressly,  by
//  implication, inducement,  estoppel or  otherwise.  Any  license  under  such
//  intellectual  property  rights must  be express  and  approved  by  Intel in
//  writing.
//
//  *Third Party trademarks are the property of their respective owners.
//
//  Unless otherwise  agreed  by Intel  in writing, you may not remove  or alter
//  this  notice or  any other notice embedded  in Materials by Intel or Intel's
//  suppliers or licensors in any way.
//
//******************************************************************************
// Content:
//     Intel(R) Math Kernel Library (MKL) overloaded Boost/uBLAS prod()
//******************************************************************************

#ifndef _MKL_BOOST_UBLAS_GEMM_
#define _MKL_BOOST_UBLAS_GEMM_

#include <boost/version.hpp>
#if defined (BOOST_VERSION) && (BOOST_VERSION >= 103401)

#include <boost/numeric/ublas/detail/concepts.hpp>
#include <boost/numeric/ublas/matrix.hpp>

#include "mkl_cblas.h"

#ifndef MKL_BOOST_UBLAS_INLINE
#define MKL_BOOST_UBLAS_INLINE inline
#endif

namespace boost { namespace numeric { namespace ublas {

namespace mkl {

    template<class E1, class E2>
    BOOST_UBLAS_INLINE
    typename matrix_matrix_binary_traits<typename E1::value_type, E1,
                                         typename E2::value_type, E2>::result_type
    gemm(const matrix_expression<E1> &e1,
         const matrix_expression<E2> &e2)
    {
        typedef typename matrix_matrix_binary_traits<typename E1::value_type, E1,
                                                                 typename E2::value_type, E2>::storage_category storage_category;
        typedef typename matrix_matrix_binary_traits<typename E1::value_type, E1,
                                                                 typename E2::value_type, E2>::orientation_category orientation_category;
        return prod (e1, e2, storage_category (), orientation_category ());
    } // For unsupported matrix types.
    template<class T>
    MKL_BOOST_UBLAS_INLINE
    void gemm(const  CBLAS_ORDER Order, const  CBLAS_TRANSPOSE TransA,
            const  CBLAS_TRANSPOSE TransB, const MKL_INT m, const MKL_INT n,
            const MKL_INT k, const T alpha, const T *a,
            const MKL_INT lda, const T *b, const MKL_INT ldb,
            const T beta, T *c, const MKL_INT ldc)
    {}// To resolve externals for unsupported matrix types. Never is called.
    template<>
    MKL_BOOST_UBLAS_INLINE
    void
    gemm(const  CBLAS_ORDER Order, const  CBLAS_TRANSPOSE TransA,
         const  CBLAS_TRANSPOSE TransB, const MKL_INT m, const MKL_INT n,
         const MKL_INT k, const double alpha, const double *a,
         const MKL_INT lda, const double *b, const MKL_INT ldb,
         const double beta, double *c, const MKL_INT ldc)
    {
        cblas_dgemm(Order, TransA, TransB, m, n, k, alpha, a, lda, b, ldb, beta, c, ldc);
    }
    template<>
    MKL_BOOST_UBLAS_INLINE
    void
    gemm(const  CBLAS_ORDER Order, const  CBLAS_TRANSPOSE TransA,
         const  CBLAS_TRANSPOSE TransB, const MKL_INT m, const MKL_INT n,
         const MKL_INT k, const float alpha, const float *a,
         const MKL_INT lda, const float *b, const MKL_INT ldb,
         const float beta, float *c, const MKL_INT ldc)
    {
        cblas_sgemm(Order, TransA, TransB, m, n, k, alpha, a, lda, b, ldb, beta, c, ldc);
    }
    template<>
    MKL_BOOST_UBLAS_INLINE
    void
    gemm(const  CBLAS_ORDER Order, const  CBLAS_TRANSPOSE TransA,
         const  CBLAS_TRANSPOSE TransB, const MKL_INT m, const MKL_INT n,
         const MKL_INT k, const std::complex<double> alpha, const std::complex<double> *a,
         const MKL_INT lda, const std::complex<double> *b, const MKL_INT ldb,
         const std::complex<double> beta, std::complex<double> *c, const MKL_INT ldc)
    {
        cblas_zgemm(Order, TransA, TransB, m, n, k, static_cast<const void *>(&alpha),
            static_cast<const void *>(a), lda, static_cast<const void *>(b), ldb,
            static_cast<const void *>(&beta), static_cast<void *>(c), ldc);
    }
    template<>
    MKL_BOOST_UBLAS_INLINE
    void
    gemm(const  CBLAS_ORDER Order, const  CBLAS_TRANSPOSE TransA,
         const  CBLAS_TRANSPOSE TransB, const MKL_INT m, const MKL_INT n,
         const MKL_INT k, const std::complex<float> alpha, const std::complex<float> *a,
         const MKL_INT lda, const std::complex<float> *b, const MKL_INT ldb,
         const std::complex<float> beta, std::complex<float> *c, const MKL_INT ldc)
    {
        cblas_cgemm(Order, TransA, TransB, m, n, k, static_cast<const void *>(&alpha),
            static_cast<const void *>(a), lda, static_cast<const void *>(b), ldb,
            static_cast<const void *>(&beta), static_cast<void *>(c), ldc);
    }

    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    T *  // M
    pointer(const matrix<T,F,A> &m)
    {
        //return &m(0,0);
        return const_cast<T*>(&m.data().begin()[0]);
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    T *  // trans(M)
    pointer(const matrix_unary2<matrix<T,F,A>,scalar_identity<T> > &m)
    {
        return pointer(m.expression().expression());
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    T *  // trans(conj(M))
    pointer(const matrix_unary2<matrix_unary1<matrix<T,F,A>,scalar_conj<T> > const ,scalar_identity<T> > &m)
    {
        return pointer(m.expression().expression().expression());
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    T *  // conj(trans(M))
    pointer(const matrix_unary1<matrix_unary2<matrix<T,F,A>,scalar_identity<T> >,scalar_conj<T> > &m)
    {
        return pointer(m.expression().expression().expression());
    }

    template<class E>
    MKL_BOOST_UBLAS_INLINE
    MKL_INT
    leading_dimension(const matrix_expression<E> &e, row_major_tag)
    {
        return e().size1();
    }
    template<class E>
    MKL_BOOST_UBLAS_INLINE
    MKL_INT
    leading_dimension(const matrix_expression<E> &e, column_major_tag)
    {
        return e().size2();
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    MKL_INT
    leading_dimension(const matrix<T,F,A> &m, row_major_tag)
    {
        return m.size2();
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    MKL_INT
    leading_dimension(const matrix<T,F,A> &m, column_major_tag)
    {
        return m.size1();
    }

    template<class T>
    MKL_BOOST_UBLAS_INLINE
    CBLAS_ORDER
    storage_layout(T);
    template<>
    MKL_BOOST_UBLAS_INLINE
    CBLAS_ORDER
    storage_layout(row_major_tag) {
        return CblasRowMajor;
    }
    template<>
    MKL_BOOST_UBLAS_INLINE
    CBLAS_ORDER
    storage_layout(column_major_tag) {
        return CblasColMajor;
    }

    template<class T>
    MKL_BOOST_UBLAS_INLINE
    int supported_type(T)
    { return 0; }
    template<>
    MKL_BOOST_UBLAS_INLINE
    int supported_type(float)
    { return 1; }
    template<>
    MKL_BOOST_UBLAS_INLINE
    int supported_type(double)
    { return 2; }
    template<>
    MKL_BOOST_UBLAS_INLINE
    int supported_type(std::complex<float>)
    { return 3; }
    template<>
    MKL_BOOST_UBLAS_INLINE
    int supported_type(std::complex<double>)
    { return 4; }

    template<class E1, class E2, class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    void
    gemm(const  CBLAS_TRANSPOSE transa, const  CBLAS_TRANSPOSE transb,
         const matrix_expression<E1> &a, const matrix_expression<E2> &b,
         matrix<T,F,A> &c)
    {
        typedef T value_type;
        typedef typename F::orientation_category orientation_category;
        if(supported_type(value_type()) > 0) {
            MKL_INT m = a().size1();
            MKL_INT k = a().size2();
            MKL_INT n = b().size2();
            MKL_INT lda = leading_dimension(a(),orientation_category());
            MKL_INT ldb = leading_dimension(b(),orientation_category());
            MKL_INT ldc = leading_dimension(c,orientation_category());

	/*boost 1.57 change the function OneElement definition from 1.0 to 0.0 in boost/numeric/ublas/detail/concepts.hpp
	template<class T>
    T
    OneElement (T) {
        return T(0.0);
    }
		They explain this in boost/numeric/ublas/detail/concepts.hpp
		Replaced the ZeroElement and OneElement functions with the templated versions
		because the former where giving warnings with clang.  Anyway, It seems a bug or something else. so change
	//value_type alpha = OneElement(value_type());  // boost/numeric/ublas/detail/concepts.hpp  to
	*/
            value_type alpha = 1.0;
			value_type beta = ZeroElement(value_type());  // boost/numeric/ublas/detail/concepts.hpp
            CBLAS_ORDER layout = storage_layout(orientation_category());
            T *pa = pointer(a());
            T *pb = pointer(b());
            T *pc = pointer(c);
            gemm(layout, transa, transb, m, n, k, alpha, pa, lda, pb, ldb, beta, pc, ldc);
        } else {
            c = gemm(a,b);
        }
    }

}  // namespace mkl

}}}
#endif  // BOOST_VERSION
#endif  // _MKL_BOOST_UBLAS_GEMM_


/******************************************************************************
 * Copyright (C) 2013 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 ******************************************************************************/

/** \file simulate-offline.cpp
    \brief This file runs a simulation of the calibration problem in batch mode.
  */

#include <iostream>
#include <iomanip>
#include <fstream>
#include <vector>

#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>

#include <Eigen/Core>

#include <sm/kinematics/rotations.hpp>
#include <sm/kinematics/three_point_methods.hpp>

#include <sm/BoostPropertyTree.hpp>

#include <aslam/backend/Optimizer2Options.hpp>
#include <aslam/backend/GaussNewtonTrustRegionPolicy.hpp>
#include <aslam/backend/Optimizer2.hpp>

#include <aslam/calibration/core/OptimizationProblem.h>
#include <aslam/calibration/statistics/UniformDistribution.h>
#include <aslam/calibration/statistics/NormalDistribution.h>
#include <aslam/calibration/data-structures/VectorDesignVariable.h>
#include <aslam/calibration/geometry/Transformation.h>
#include <aslam/calibration/base/Timestamp.h>
#include <aslam-tsvd-solver/aslam-tsvd-solver.h>

#include "aslam/calibration/2dlrf/utils.h"
#include "aslam/calibration/2dlrf/ErrorTermMotion.h"
#include "aslam/calibration/2dlrf/ErrorTermObservation.h"

using namespace aslam::calibration;
using namespace aslam::backend;
using namespace sm::kinematics;
using namespace sm;

typedef aslam::backend::AslamTruncatedSvdSolver LinearSolver;
int main(int argc, char** argv) {
  if (argc != 2) {
    std::cerr << "Usage: " << argv[0] << " <conf_file>" << std::endl;
    return -1;
  }

  // load configuration file
  BoostPropertyTree propertyTree;
  propertyTree.loadXml(argv[1]);

  // steps to simulate
  const size_t steps = propertyTree.getInt("lrf/problem/steps");

  // timestep size
  const double T = propertyTree.getDouble("lrf/problem/timestep");

  // true state
  std::vector<Eigen::Vector3d > x_true;
  x_true.reserve(steps);

  // integrated odometry
  std::vector<Eigen::Vector3d > x_odom;
  x_odom.reserve(steps);

  // true control input
  std::vector<Eigen::Vector3d > u_true;
  const double sineWaveAmplitude = propertyTree.getDouble(
    "lrf/problem/sineWaveAmplitude");
  const double sineWaveFrequency = propertyTree.getDouble(
    "lrf/problem/sineWaveFrequency");
  genSineWavePath(u_true, steps, sineWaveAmplitude, sineWaveFrequency, T);

  // measured control input
  std::vector<Eigen::Vector3d > u_noise;
  u_noise.reserve(steps);

  // number of landmarks
  const size_t nl = propertyTree.getInt("lrf/problem/numLandmarks");

  // playground size
  const Eigen::Vector2d min(propertyTree.getDouble("lrf/problem/groundMinX"),
    propertyTree.getDouble("lrf/problem/groundMinY"));
  const Eigen::Vector2d max(propertyTree.getDouble("lrf/problem/groundMaxX"),
    propertyTree.getDouble("lrf/problem/groundMaxY"));

  // bearing measurements
  std::vector<std::vector<double> > b;
  b.reserve(steps);
  b.push_back(std::vector<double>(nl, 0));

  // range measurements
  std::vector<std::vector<double> > r;
  r.reserve(steps);
  r.push_back(std::vector<double>(nl, 0));

  // covariance matrix for motion model
  Eigen::Matrix3d Q = Eigen::Matrix3d::Zero();
  Q(0, 0) = propertyTree.getDouble("lrf/problem/motion/sigma2_x");
  Q(1, 1) = propertyTree.getDouble("lrf/problem/motion/sigma2_y");
  Q(2, 2) = propertyTree.getDouble("lrf/problem/motion/sigma2_t");

  // covariance matrix for observation model
  Eigen::Matrix2d R = Eigen::Matrix2d::Zero();
  R(0, 0) = propertyTree.getDouble("lrf/problem/observation/sigma2_r");
  R(1, 1) = propertyTree.getDouble("lrf/problem/observation/sigma2_b");

  // landmark positions
  std::vector<Eigen::Vector2d > x_l;
  UniformDistribution<double, 2>(min, max).getSamples(x_l, nl);

  // true calibration parameters
  const Eigen::Vector3d Theta(propertyTree.getDouble("lrf/problem/thetaTrue/x"),
    propertyTree.getDouble("lrf/problem/thetaTrue/y"),
    propertyTree.getDouble("lrf/problem/thetaTrue/t"));

  // guessed calibration parameters
  const Eigen::Vector3d Theta_hat(
    propertyTree.getDouble("lrf/problem/thetaHat/x"),
    propertyTree.getDouble("lrf/problem/thetaHat/y"),
    propertyTree.getDouble("lrf/problem/thetaHat/t"));

  // initial state
  const Eigen::Vector3d x_0(propertyTree.getDouble("lrf/problem/x0/x"),
    propertyTree.getDouble("lrf/problem/x0/y"),
    propertyTree.getDouble("lrf/problem/x0/t"));
  x_true.push_back(x_0);
  x_odom.push_back(x_0);
  u_noise.push_back(Eigen::Vector3d::Zero());

  // simulate
  for (size_t i = 1; i < steps; ++i) {
    Eigen::Matrix3d B = Eigen::Matrix3d::Identity();
    B(0, 0) = cos(x_true[i - 1](2));
    B(0, 1) = -sin(x_true[i - 1](2));
    B(1, 0) = sin(x_true[i - 1](2));
    B(1, 1) = cos(x_true[i - 1](2));
    Eigen::Vector3d xk = x_true[i - 1] + T * B * u_true[i];
    xk(2) = angleMod(xk(2));
    x_true.push_back(xk);
    u_noise.push_back(u_true[i] +
      NormalDistribution<3>(Eigen::Vector3d::Zero(), Q).getSample());
    B(0, 0) = cos(x_odom[i - 1](2));
    B(0, 1) = -sin(x_odom[i - 1](2));
    B(1, 0) = sin(x_odom[i - 1](2));
    B(1, 1) = cos(x_odom[i - 1](2));
    xk = x_odom[i - 1] + T * B * u_noise[i];
    xk(2) = angleMod(xk(2));
    x_odom.push_back(xk);
    const double ct = cos(x_true[i](2));
    const double st = sin(x_true[i](2));
    std::vector<double> rk(nl, 0);
    std::vector<double> bk(nl, 0);
    for (size_t j = 0; j < nl; ++j) {
      const double aa = x_l[j](0) - x_true[i](0) - Theta(0) * ct +
        Theta(1) * st;
      const double bb = x_l[j](1) - x_true[i](1) - Theta(0) * st -
        Theta(1) * ct;
      const double range = sqrt(aa * aa + bb * bb) +
        NormalDistribution<1>(0, R(0, 0)).getSample();
      rk[j] = range;
      bk[j] = angleMod(atan2(bb, aa) - x_true[i](2) - Theta(2) +
        NormalDistribution<1>(0, R(1, 1)).getSample());
    }
    r.push_back(rk);
    b.push_back(bk);
  }

  // landmark guess
  std::vector<Eigen::Vector2d > x_l_hat;
  initLandmarks(x_l_hat, x_odom, Theta_hat, r, b);

  // create optimization problem
  auto problem = boost::make_shared<OptimizationProblem>();

  // create calibration parameters design variable
  auto dv_Theta = boost::make_shared<VectorDesignVariable<3> >(Theta_hat);
  dv_Theta->setActive(true);
  problem->addDesignVariable(dv_Theta, 2);

  // create state design variables
  std::vector<boost::shared_ptr<VectorDesignVariable<3> > > dv_x;
  dv_x.reserve(steps);
  for (size_t i = 0; i < steps; ++i) {
    dv_x.push_back(boost::make_shared<VectorDesignVariable<3> >(x_odom[i]));
    dv_x[i]->setActive(true);
    problem->addDesignVariable(dv_x[i], 0);
  }

  // create landmarks design variables
  std::vector<boost::shared_ptr<VectorDesignVariable<2> > > dv_x_l;
  dv_x_l.reserve(nl);
  for (size_t i = 0; i < nl; ++i) {
    dv_x_l.push_back(boost::make_shared<VectorDesignVariable<2> >(x_l_hat[i]));
    dv_x_l[i]->setActive(true);
    problem->addDesignVariable(dv_x_l[i], 1);
  }

  // set the ordering of the problem
  problem->setGroupsOrdering({0, 1, 2});

  // add motion and observation error terms
  for (size_t i = 1; i < steps; ++i) {
    auto e_mot = boost::make_shared<ErrorTermMotion>(dv_x[i - 1].get(),
      dv_x[i].get(), T, u_noise[i], Q);
    problem->addErrorTerm(e_mot);
    for (size_t j = 0; j < nl; ++j) {
      auto e_obs = boost::make_shared<ErrorTermObservation>(dv_x[i].get(),
        dv_x_l[j].get(), dv_Theta.get(), r[i][j], b[i][j], R);
      problem->addErrorTerm(e_obs);
    }
  }

  // optimization
  std::cout << "Calibration before: " << *dv_Theta << std::endl;
  Optimizer2 optimizer(PropertyTree(propertyTree, "lrf/estimator/optimizer"),
    boost::make_shared<LinearSolver>(PropertyTree(propertyTree,
    "lrf/estimator/optimizer/linearSolver")),
    boost::make_shared<GaussNewtonTrustRegionPolicy>());
  optimizer.setProblem(problem);

  size_t JCols = 0;
  for (auto it = problem->getGroupsOrdering().cbegin();
      it != problem->getGroupsOrdering().cend(); ++it)
    JCols += problem->getGroupDim(*it);
  const size_t dim = problem->getGroupDim(2);
  auto linearSolver = optimizer.getSolver<LinearSolver>();
  linearSolver->setMargStartIndex(JCols - dim);
  const double before = Timestamp::now();
  optimizer.optimize();
  const double after = Timestamp::now();
  std::cout << "Elapsed time [s]: " << after - before << std::endl;
  std::cout << "Calibration after: " << *dv_Theta << std::endl;
  std::cout << "Singular values (scaled): "
    << linearSolver->getSingularValues().transpose() << std::endl;
  std::cout << "Unobservable basis (scaled): " << std::endl
    << linearSolver->getNullSpace() << std::endl;
  std::cout << "Observable basis (scaled): " << std::endl
    << linearSolver->getRowSpace() << std::endl;
  linearSolver->analyzeMarginal();
  std::cout << "SVD rank: " << linearSolver->getSVDRank() << std::endl;
  std::cout << "SVD rank deficiency: " << linearSolver->getSVDRankDeficiency()
    << std::endl;
  std::cout << "SVD tolerance: " << linearSolver->getSVDTolerance()
    << std::endl;
  std::cout << "Singular values: " << optimizer.getSolver<LinearSolver>()
    ->getSingularValues().transpose() << std::endl;
  std::cout << "QR rank: " << optimizer.getSolver<LinearSolver>()->getQRRank()
    << std::endl;
  std::cout << "QR rank deficiency: " << optimizer.getSolver<LinearSolver>()
    ->getQRRankDeficiency() << std::endl;
  std::cout << "QR tolerance: " << linearSolver->getQRTolerance() << std::endl;
  std::cout << "Unobservable basis: " << std::endl
    << linearSolver->getNullSpace() << std::endl;
  std::cout << "Observable basis: " << std::endl << linearSolver->getRowSpace()
    << std::endl;
  std::cout << "Covariance: " << std::endl << linearSolver->getCovariance()
    << std::endl;
  std::cout << "Observable covariance: " << std::endl
    << linearSolver->getRowSpaceCovariance() << std::endl;
  std::cout << "Peak memory usage (MB): " << linearSolver->getPeakMemoryUsage()
    / 1024.0 / 1024.0 << std::endl;
  std::cout << "Memory usage (MB): " << linearSolver->getMemoryUsage() /
    1024.0 / 1024.0 << std::endl;
  std::cout << "Flop count: " << linearSolver->getNumFlops() << std::endl;
  std::cout << "Linear solver time: " << linearSolver->getLinearSolverTime()
    << std::endl;
  std::cout << "Marginal analysis time: "
    << linearSolver->getMarginalAnalysisTime() << std::endl;
  std::cout << "Symbolic factorization time: "
    << linearSolver->getSymbolicFactorizationTime() << std::endl;
  std::cout << "Numeric factorization time: "
    << linearSolver->getNumericFactorizationTime() << std::endl;
  std::cout << "Log2sum of singular values: "
    << linearSolver->getSingularValuesLog2Sum() << std::endl;

  // output results to file
  std::ofstream x_true_log("x_true.txt");
  for (size_t i = 0; i < steps; ++i)
    x_true_log << x_true[i].transpose() << std::endl;
  std::ofstream x_odom_log("x_odom.txt");
  for (size_t i = 0; i < steps; ++i)
    x_odom_log << x_odom[i].transpose() << std::endl;
  std::ofstream x_est_log("x_est.txt");
  for (size_t i = 0; i < steps; ++i)
    x_est_log << *(dv_x[i]) << std::endl;
  std::ofstream l_log("l.txt");
  for (size_t i = 0; i < nl; ++i)
    l_log << x_l[i].transpose() << std::endl;
  std::ofstream l_est_log("l_est.txt");
  for (size_t i = 0; i < nl; ++i)
    l_est_log << *(dv_x_l[i]) << std::endl;

  // align landmarks
  Eigen::MatrixXd l = Eigen::MatrixXd::Zero(3, nl);
  Eigen::MatrixXd l_est = Eigen::MatrixXd::Zero(3, nl);
  for (size_t i = 0; i < nl; ++i) {
    l(0, i) = x_l[i](0);
    l(1, i) = x_l[i](1);
    l_est(0, i) = dv_x_l[i]->getValue()(0);
    l_est(1, i) = dv_x_l[i]->getValue()(1);
  }
  Transformation<double, 3> trans(threePointSvd(l, l_est));
  Eigen::MatrixXd l_est_trans = Eigen::MatrixXd::Zero(3, nl);
  for (size_t i = 0; i < nl; ++i)
    l_est_trans.col(i) = trans(l_est.col(i));
  std::ofstream l_est_trans_log("l_est_trans.txt");
  for (size_t i = 0; i < nl; ++i)
    l_est_trans_log << l_est_trans.col(i).head<2>().transpose() << std::endl;

  // align poses
  std::vector<Eigen::Vector3d > x_est_trans;
  x_est_trans.reserve(steps);
  std::ofstream x_est_trans_log("x_est_trans.txt");
  for (size_t i = 0; i < steps; ++i) {
    Eigen::Vector3d pose((Eigen::Vector3d()
      << dv_x[i]->getValue().head<2>(), 0).finished());
    trans.transform(pose, pose);
    pose(2) = dv_x[i]->getValue()(2);
    x_est_trans.push_back(pose);
    x_est_trans_log << x_est_trans[i].transpose() << std::endl;
  }

  return 0;
}


//
// Created by david on 2018-11-16.
//

#include "matvec_dense.h"
#include <Eigen/Core>
#include <Eigen/LU>
#include <general/class_tic_toc.h>
#include <memory>
#define profile_matrix_product_dense 1

// Function definitions
template<typename Scalar>
using MatrixType = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
template<typename Scalar>
using VectorType = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
template<typename Scalar>
using VectorTypeT = Eigen::Matrix<Scalar, 1, Eigen::Dynamic>;

namespace dense_lu {
    std::optional<Eigen::PartialPivLU<MatrixType<double>>>               lu_real;
    std::optional<Eigen::PartialPivLU<MatrixType<std::complex<double>>>> lu_cplx;
    void                                                                 reset() {
        lu_real.reset();
        lu_cplx.reset();
    }
    template<typename Scalar>
    void init() {
        if constexpr(std::is_same_v<Scalar, double>) { dense_lu::lu_real = Eigen::PartialPivLU<MatrixType<Scalar>>(); }
        if constexpr(std::is_same_v<Scalar, std::complex<double>>) dense_lu::lu_cplx = Eigen::PartialPivLU<MatrixType<Scalar>>();
    }

}

template<typename Scalar>
MatVecDense<Scalar>::~MatVecDense() {
    dense_lu::reset();
}

// Pointer to data constructor, copies the matrix into an internal Eigen matrix.
template<typename Scalar>
MatVecDense<Scalar>::MatVecDense(const Scalar *const A_, const long L_, const bool copy_data, const eig::Form form_, const eig::Side side_)
    : A_ptr(A_), L(L_), form(form_), side(side_) {
    if(copy_data) {
        A_stl.resize(static_cast<size_t>(L * L));
        std::copy(A_ptr, A_ptr + static_cast<size_t>(L * L), A_stl.begin());
        A_ptr = A_stl.data();
    }
    dense_lu::init<Scalar>();
    init_profiling();
}

template<typename Scalar>
void MatVecDense<Scalar>::FactorOP()

/*  Partial pivot LU decomposition
 *  Factors P(A-sigma*I) = LU
 */
{
    if(readyFactorOp) return; // happens only once
    if(not readyShift) throw std::runtime_error("Cannot FactorOP: Shift value sigma has not been set.");
    Eigen::Map<const MatrixType<Scalar>> A_matrix(A_ptr, L, L);
    t_factorOP->tic();
    if constexpr(std::is_same_v<Scalar, eig::real>) { dense_lu::lu_real.value().compute(A_matrix); }
    if constexpr(std::is_same_v<Scalar, eig::cplx>) { dense_lu::lu_cplx.value().compute(A_matrix); }

    readyFactorOp = true;
    t_factorOP->toc();
}

template<typename Scalar>
void MatVecDense<Scalar>::MultOPv(Scalar *x_in_ptr, Scalar *x_out_ptr) {
    assert(readyFactorOp and "FactorOp() has not been run yet.");
    t_multOPv->tic();
    switch(side) {
        case eig::Side::R: {
            Eigen::Map<VectorType<Scalar>> x_in(x_in_ptr, L);
            Eigen::Map<VectorType<Scalar>> x_out(x_out_ptr, L);
            if constexpr(std::is_same_v<Scalar, double>) x_out.noalias() = dense_lu::lu_real.value().solve(x_in);
            if constexpr(std::is_same_v<Scalar, std::complex<double>>) x_out.noalias() = dense_lu::lu_cplx.value().solve(x_in);
            break;
        }
        case eig::Side::L: {
            Eigen::Map<VectorTypeT<Scalar>> x_in(x_in_ptr, L);
            Eigen::Map<VectorTypeT<Scalar>> x_out(x_out_ptr, L);
            if constexpr(std::is_same_v<Scalar, double>) x_out.noalias() = x_in * dense_lu::lu_real.value().inverse();
            if constexpr(std::is_same_v<Scalar, std::complex<double>>) x_out.noalias() = x_in * dense_lu::lu_cplx.value().inverse();
            break;
        }
        case eig::Side::LR: {
            throw std::runtime_error("eigs cannot handle sides L and R simultaneously");
        }
    }
    t_multOPv->toc();
    counter++;
}

template<typename Scalar>
void MatVecDense<Scalar>::MultAx(Scalar *x_in, Scalar *x_out) {
    auto                                 token = t_multAx->tic_token();
    Eigen::Map<const MatrixType<Scalar>> A_matrix(A_ptr, L, L);
    switch(form) {
        case eig::Form::NSYM:
            switch(side) {
                case eig::Side::R: {
                    Eigen::Map<VectorType<Scalar>> x_vec_in(x_in, L);
                    Eigen::Map<VectorType<Scalar>> x_vec_out(x_out, L);
                    x_vec_out.noalias() = A_matrix * x_vec_in;
                    break;
                }
                case eig::Side::L: {
                    Eigen::Map<VectorTypeT<Scalar>> x_vec_in(x_in, L);
                    Eigen::Map<VectorTypeT<Scalar>> x_vec_out(x_out, L);
                    x_vec_out.noalias() = x_vec_in * A_matrix;
                    break;
                }
                case eig::Side::LR: {
                    throw std::runtime_error("eigs cannot handle sides L and R simultaneously");
                }
            }
            break;
        case eig::Form::SYMM: {
            Eigen::Map<VectorType<Scalar>> x_vec_in(x_in, L);
            Eigen::Map<VectorType<Scalar>> x_vec_out(x_out, L);
            x_vec_out.noalias() = A_matrix.template selfadjointView<Eigen::Lower>() * x_vec_in;
            break;
        }
    }
    counter++;
}

template<typename T>
void MatVecDense<T>::MultAx(void *x, int *ldx, void *y, int *ldy, int *blockSize, [[maybe_unused]] primme_params *primme, [[maybe_unused]] int *err) {
    auto                                 token = t_multAx->tic_token();
    Eigen::Map<const MatrixType<Scalar>> A_matrix(A_ptr, L, L);
    switch(form) {
        case eig::Form::NSYM:
            switch(side) {
                case eig::Side::R: {
                    for(int i = 0; i < *blockSize; i++) {
                        T *                            x_in  = static_cast<T *>(x) + *ldx * i;
                        T *                            x_out = static_cast<T *>(y) + *ldy * i;
                        Eigen::Map<VectorType<Scalar>> x_vec_in(x_in, L);
                        Eigen::Map<VectorType<Scalar>> x_vec_out(x_out, L);
                        x_vec_out.noalias() = A_matrix * x_vec_in;
                        counter++;
                    }
                    break;
                }
                case eig::Side::L: {
                    for(int i = 0; i < *blockSize; i++) {
                        T *                            x_in  = static_cast<T *>(x) + *ldx * i;
                        T *                            x_out = static_cast<T *>(y) + *ldy * i;
                        Eigen::Map<VectorType<Scalar>> x_vec_in(x_in, L);
                        Eigen::Map<VectorType<Scalar>> x_vec_out(x_out, L);
                        x_vec_out.noalias() = x_vec_in * A_matrix;
                        counter++;
                    }
                    break;
                }
                case eig::Side::LR: {
                    throw std::runtime_error("eigs cannot handle sides L and R simultaneously");
                }
            }
            break;
        case eig::Form::SYMM: {
            for(int i = 0; i < *blockSize; i++) {
                T *                            x_in  = static_cast<T *>(x) + *ldx * i;
                T *                            x_out = static_cast<T *>(y) + *ldy * i;
                Eigen::Map<VectorType<Scalar>> x_vec_in(x_in, L);
                Eigen::Map<VectorType<Scalar>> x_vec_out(x_out, L);
                x_vec_out.noalias() = A_matrix.template selfadjointView<Eigen::Lower>() * x_vec_in;
                counter++;
            }
            break;
        }
    }
    *err = 0;
}

template<typename Scalar>
void MatVecDense<Scalar>::print() const {
    Eigen::Map<const MatrixType<Scalar>> A_matrix(A_ptr, L, L);
}

template<typename Scalar>
void MatVecDense<Scalar>::set_shift(std::complex<double> sigma_) {
    if(readyShift) { return; }
    sigma = sigma_;
    if(A_stl.empty()) {
        A_stl.resize(static_cast<size_t>(L * L));
        std::copy(A_ptr, A_ptr + static_cast<size_t>(L * L), A_stl.begin());
        A_ptr = A_stl.data();
    }
    Eigen::Map<MatrixType<Scalar>> A_matrix(A_stl.data(), L, L);
    if constexpr(std::is_same_v<Scalar, eig::real>) A_matrix -= Eigen::MatrixXd::Identity(L, L) * std::real(sigma);
    if constexpr(std::is_same_v<Scalar, eig::cplx>) A_matrix -= Eigen::MatrixXd::Identity(L, L) * sigma;
    readyShift = true;
}

template<typename Scalar>
void MatVecDense<Scalar>::set_mode(const eig::Form form_) {
    form = form_;
}
template<typename Scalar>
void MatVecDense<Scalar>::set_side(const eig::Side side_) {
    side = side_;
}
template<typename Scalar>
const eig::Form &MatVecDense<Scalar>::get_form() const {
    return form;
}
template<typename Scalar>
const eig::Side &MatVecDense<Scalar>::get_side() const {
    return side;
}

template<typename Scalar>
void MatVecDense<Scalar>::init_profiling() {
    t_factorOP = std::make_unique<class_tic_toc>(profile_matrix_product_dense, 5, "Time FactorOp");
    t_multOPv  = std::make_unique<class_tic_toc>(profile_matrix_product_dense, 5, "Time MultOpv");
    t_multAx   = std::make_unique<class_tic_toc>(profile_matrix_product_dense, 5, "Time MultAx");
}

// Explicit instantiations

template class MatVecDense<double>;
template class MatVecDense<std::complex<double>>;


/*
 *  Distributed under the MIT License (See accompanying file /LICENSE )
 */
#include "pgcpp/euclid_plane.hpp" // import Ar
#include "pgcpp/persp_plane.hpp"
#include "pgcpp/pg_line.hpp"
#include "pgcpp/pg_point.hpp"
#include <boost/multiprecision/cpp_int.hpp>
#include <doctest/doctest.h>
// #include <iostream>

using namespace fun;

static const auto Zero = doctest::Approx(0).epsilon(0.01);

/*!
 * @brief
 *
 * @tparam PG
 * @param[in] myck
 */
template <typename PG>
void chk_degenerate(const PG& myck)
{
    using Point = typename PG::point_t;
    // using Line = typename PG::line_t;
    using K = Value_type<Point>;

    auto a1 = Point {-1, 0, 3};
    auto a2 = Point {4, -2, 1};
    auto a3 = Point {3, -1, 1};

    auto triangle = std::tuple {std::move(a1), std::move(a2), std::move(a3)};
    const auto trilateral = tri_dual(triangle);

    // const auto& [a1, a2, a3] = triangle;
    const auto& [l1, l2, l3] = trilateral;

    const auto m12 = myck.midpoint(a1, a2);
    const auto m23 = myck.midpoint(a2, a3);
    const auto m13 = myck.midpoint(a1, a3);
    const auto t1 = a1 * m23;
    const auto t2 = a2 * m13;
    const auto t3 = a3 * m12;

    const auto [q1, q2, q3] = myck.tri_quadrance(triangle);
    const auto [s1, s2, s3] = myck.tri_spread(trilateral);

    const auto tqf = sq(q1 + q2 + q3) - 2 * (q1 * q1 + q2 * q2 + q3 * q3);
    const auto tsf =
        sq(s1 + s2 + s3) - 2 * (s1 * s1 + s2 * s2 + s3 * s3) - 4 * s1 * s2 * s3;
    auto a4 = plucker(3, a1, 4, a2);
    const auto tri2 = std::tuple {std::move(a1), std::move(a2), std::move(a4)};

    const auto [qq1, qq2, qq3] = myck.tri_quadrance(tri2);

    const auto tqf2 = Ar(qq1, qq2, qq3); // get 0

    if constexpr (Integral<K>)
    {
        CHECK(!myck.is_parallel(l1, l2));
        CHECK(!myck.is_parallel(l2, l3));
        CHECK(coincident(t1 * t2, t3));
        CHECK(tqf == Ar(q1, q2, q3));
        CHECK(tsf == 0);
        CHECK(tqf2 == 0);
    }
    else
    {
        CHECK(myck.l_infty().dot(l1 * l2) != Zero);
        CHECK(myck.l_infty().dot(l2 * l3) != Zero);
        CHECK(t1.dot(t2 * t3) == Zero);
        CHECK(tqf - Ar(q1, q2, q3) == Zero);
        CHECK(tsf == Zero);
        CHECK(tqf2 == Zero);
    }
}

TEST_CASE("Perspective Euclid plane (cpp_int)")
{
    using boost::multiprecision::cpp_int;

    auto Ire = pg_point<cpp_int>(0, 1, 1);
    auto Iim = pg_point<cpp_int>(1, 0, 0);
    auto l_inf = pg_line<cpp_int>(0, -1, 1);

    const auto P =
        persp_euclid_plane {std::move(Ire), std::move(Iim), std::move(l_inf)};
    chk_degenerate(P);
}

TEST_CASE("Perspective Euclid plane (floating point)")
{
    auto Ire = pg_point {0., 1., 1.};
    auto Iim = pg_point {1., 0., 0.};
    auto l_inf = pg_line {0., -1., 1.};

    const auto P =
        persp_euclid_plane {std::move(Ire), std::move(Iim), std::move(l_inf)};
    chk_degenerate(P);
}


#pragma once

#include <vector>
#include <Eigen/Dense>

/* We use the following indices for orbital types:
   0 = s
   1 = px
   2 = py
   3 = pz

   Therefore, if an index is > 0, it is a p orbital
*/

// This is the number of orbital types we have
const int orbitals_per_atom = 4;

/*! \brief Calculate a fock matrix */
Eigen::MatrixXd calculate_fock_matrix_fast(Eigen::MatrixXd hamiltonian_matrix,
                                           Eigen::MatrixXd interaction_matrix,
                                           Eigen::MatrixXd density_matrix,
                                           double model_dipole);

/*! \brief Calculate MP2 energy */
double calculate_energy_mp2(std::vector<double> E_occ, std::vector<double> E_virt, Eigen::MatrixXd v_tilde_flat);


/**
 * @file spalera_sgd_test.cpp
 * @author Marcus Edel
 *
 * Test file for SGD (stochastic gradient descent).
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <mlpack/core/optimizers/spalera_sgd/spalera_sgd.hpp>
#include <mlpack/methods/logistic_regression/logistic_regression.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace mlpack::optimization;
using namespace mlpack::distribution;
using namespace mlpack::regression;

BOOST_AUTO_TEST_SUITE(SPALeRASGDTest);

/**
 * Run SPALeRA SGD on logistic regression and make sure the results are
 * acceptable.
 */
BOOST_AUTO_TEST_CASE(LogisticRegressionTest)
{
  // Generate a two-Gaussian dataset.
  GaussianDistribution g1(arma::vec("1.0 1.0 1.0"), arma::eye<arma::mat>(3, 3));
  GaussianDistribution g2(arma::vec("9.0 9.0 9.0"), arma::eye<arma::mat>(3, 3));

  arma::mat data(3, 500);
  arma::Row<size_t> responses(500);
  for (size_t i = 0; i < 250; ++i)
  {
    data.col(i) = g1.Random();
    responses[i] = 0;
  }
  for (size_t i = 250; i < 500; ++i)
  {
    data.col(i) = g2.Random();
    responses[i] = 1;
  }

  // Shuffle the dataset.
  arma::uvec indices = arma::shuffle(arma::linspace<arma::uvec>(0,
      data.n_cols - 1, data.n_cols));
  arma::mat shuffledData(3, 500);
  arma::Row<size_t> shuffledResponses(500);
  for (size_t i = 0; i < data.n_cols; ++i)
  {
    shuffledData.col(i) = data.col(indices[i]);
    shuffledResponses[i] = responses[indices[i]];
  }

  // Create a test set.
  arma::mat testData(3, 500);
  arma::Row<size_t> testResponses(500);
  for (size_t i = 0; i < 250; ++i)
  {
    testData.col(i) = g1.Random();
    testResponses[i] = 0;
  }
  for (size_t i = 250; i < 500; ++i)
  {
    testData.col(i) = g2.Random();
    testResponses[i] = 1;
  }

  // Now run mini-batch SGD with a couple of batch sizes.
  for (size_t batchSize = 30; batchSize < 50; batchSize += 5)
  {
    SPALeRASGD<> mbsgd(0.05 / batchSize, batchSize, 10000, 1e-4);
    LogisticRegression<> lr(shuffledData, shuffledResponses, mbsgd, 0.5);

    // Ensure that the error is close to zero.
    const double acc = lr.ComputeAccuracy(data, responses);
    BOOST_REQUIRE_CLOSE(acc, 100.0, 0.5); // 0.5% error tolerance.

    const double testAcc = lr.ComputeAccuracy(testData, testResponses);
    BOOST_REQUIRE_CLOSE(testAcc, 100.0, 0.8); // 0.8% error tolerance.
  }
}

BOOST_AUTO_TEST_SUITE_END();


#include <string>
#include <vector>
#include <boost/filesystem.hpp>
#include <math.h>
#include <algorithm>
#include <opencv2/opencv.hpp>
#include <opencv2/xfeatures2d.hpp>
#include <eigen3/Eigen/Eigenvalues>
#include <opencv2/core/eigen.hpp>
#include <sys/types.h>
#include <sys/stat.h>

#include "../bag_of_words/bow_tools.hpp"
#include "../loop_closure/LC_tools.hpp"
#include "../vo/VO_tools.hpp"
#include "../ba/BA_tools.hpp"

using namespace std;
using namespace cv;
using namespace gtsam;

Eigen::VectorXd K(9);

int main()
{

    Ptr<DescriptorMatcher> matcher_b(new FlannBasedMatcher);
    //create Sift feature point extracter
    Ptr<FeatureDetector> features(new cv::xfeatures2d::SiftFeatureDetector);
    //create Sift descriptor extractor
    Ptr<DescriptorExtractor> descriptors(new cv::xfeatures2d::SiftDescriptorExtractor);

    Ptr<BOWImgDescriptorExtractor> bowDE(new BOWImgDescriptorExtractor(descriptors, matcher_b));
    VO solver("trajectory.txt","../../data/rgbd_dataset_freiburg2_desk/rgb/");
    solver.SetK(525,525,319.5,239.5);

    Mat dictionary;
    build_dictionary(dictionary);
    bowDE->setVocabulary(dictionary);

    map<int, Mat> index_features;
    map<int, vector<KeyPoint>> index_points;
    map<int, Mat> index_descriptors;
    int last_loop = 0;

    NonlinearFactorGraph graph;
    noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.2));

    Values initialEstimate;

    vector<Eigen::Vector3f> all_pose;
    vector<Eigen::Vector3f> all_pose_pre;
    vector<Eigen::Matrix3f> all_R;
    vector<Eigen::Vector3f> all_t;
    vector<Eigen::Matrix3f> after_R;
    vector<Eigen::Vector3f> after_t;
    vector<Eigen::Vector2f> pre_all_inliers1;
    vector<Eigen::Vector2f> pre_all_inliers2;
    vector<vector<Eigen::Vector2f> >  all_2d_inliers;
    vector<map<int, int> >  all_indexes;
    vector<Eigen::Vector3f> all_3d_points;
    map<int, vector<Eigen::Vector2f> > all_frames;
    Eigen::Vector3f inital_pos;
    inital_pos << 0.0, 0.0, 0.0;

    int step_size = 10;
    K = solver.get_K_();
    int file_size = solver.read_file_names();
    Ptr<Feature2D> f2d = xfeatures2d::SIFT::create(0,3,0.04,10,1.6);

    int index = 0;

    for (int files = 0; files < file_size - step_size;
     files+=step_size)
    {
        solver.solve_once(all_R,
                        all_t,
                        pre_all_inliers1,
                        pre_all_inliers2,
                        all_2d_inliers,
                        all_indexes,
                        index_points,
                        index_descriptors,
                        f2d,
                        index,
                        files);

        if (all_R.size() == 9)
        {
            //relative to absolute
            for (int i = 1; i < 9; i++)
            {
                Eigen::Matrix3f R_tmp = all_R[i]*all_R[i-1];
                Eigen::Vector3f t_tmp = all_R[i]*all_t[i-1]+all_t[i];
                all_R[i] = R_tmp;
                all_t[i] = t_tmp;
            }
            get_all_3d_landmarks(all_2d_inliers,all_indexes,all_3d_points,all_frames,all_R,all_t);
            optimization(all_3d_points,all_frames,all_R,all_t,after_R,after_t);
            all_R.clear();
            all_t.clear();
            all_frames.clear();
            all_indexes.clear();
            all_2d_inliers.clear();
            all_3d_points.clear();
            all_pose_pre.push_back(inital_pos);
            for (int i = 0; i < 9; i++)
            {
                Eigen::Vector3f new_pos;
                new_pos = after_R[i]*inital_pos +after_t[i];
                all_pose.push_back(new_pos);
                all_pose_pre.push_back(new_pos);
            }

            inital_pos = all_pose[8];
            if (index == 9)
            {
                noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.3));
                graph.emplace_shared<PriorFactor<Point3> >(0, Point3(0, 0, 0), priorNoise);
                initialEstimate.insert(0, Point3(0.0, 0.0, 0.0));
                Mat bowDescriptor;
                vector<KeyPoint> keypoints;
                bowDE->compute(index_descriptors[0], bowDescriptor);
                index_features[0] = bowDescriptor;
            }
            for (int i = 0; i < all_pose.size(); i++)
            {
                Eigen::Vector3f trans;
                trans = all_pose[i]- all_pose_pre[i];
                graph.emplace_shared<BetweenFactor<Point3> >(index-9+i, index-8+i, Point3(trans[0], trans[1], trans[2]), model);
                initialEstimate.insert(index-8+i, Point3(all_pose[i][0], all_pose[i][1], all_pose[i][2]));
                Mat bowDescriptor;
                bowDE->compute(index_descriptors[index-8+i], bowDescriptor);
                index_features[index-8+i] = bowDescriptor;
                if ((index-8+i) - last_loop >= 20)
                {
                    int frame_looped = find_loop(index_features);
                    if (frame_looped != index_features.size())
                    {
                        Mat R_loop, t_loop;
                        geometric_verification(R_loop, t_loop, K, index_points, index_descriptors, frame_looped,index-8+i);
                        Mat pose_loop = (Mat_<double>(3,1) << all_pose[i][0], all_pose[i][1], all_pose[i][2]);
                        Mat pose_looped = R_loop*pose_loop+t_loop;
                        Mat trans_loop = pose_loop - pose_looped;
                        graph.emplace_shared<BetweenFactor<Point3> >(index-8+i, frame_looped, Point3(trans_loop.at<double>(0,0), trans_loop.at<double>(1,0), trans_loop.at<double>(2,0)), model);
                        GaussNewtonParams parameters;
                        parameters.relativeErrorTol = 1e-5;
                        parameters.maxIterations = 100;
                        GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters);
                        Values result = optimizer.optimize();
                        last_loop = index-8+i;
                    }
                }
            }
            after_R.clear();
            after_t.clear();
            all_pose.clear();
            all_pose_pre.clear();

        }

    }
    return 0;
}


/**
 * \file libs/numeric/ublasx/test/rcond.cpp
 *
 * \brief Test suite for the \c rcond operation.
 *
 * Copyright (c) 2010, Marco Guazzone
 *
 * 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)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

//ATTENTION: test fails
//TODO: fix it

#include <boost/numeric/ublas/banded.hpp>
#include <boost/numeric/ublas/fwd.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/symmetric.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublasx/operation/rcond.hpp>
#include <complex>
#include <cstddef>
#include "libs/numeric/ublasx/test/utils.hpp"

namespace ublas = boost::numeric::ublas;
namespace ublasx = boost::numeric::ublasx;

BOOST_UBLASX_TEST_DEF( norm_1_real_square_dense_matrix_column_major )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: 1-Norm - Real Square Dense Matrix - Column Major");

	typedef double value_type;
	typedef double result_type;
	typedef ublas::matrix<value_type,ublas::column_major> matrix_type;

	const std::size_t n = 3;

	matrix_type A(n,n);
	A(0,0) = 1; A(0,1) = 2; A(0,2) = 3;
	A(1,0) = 4; A(1,1) = 5; A(1,2) = 6;
	A(2,0) = 7; A(2,1) = 8; A(2,2) = 9;

	result_type res;
	result_type expect_res;

	res = ublasx::rcond(A);

	BOOST_UBLASX_DEBUG_TRACE("A = " << A);
	BOOST_UBLASX_DEBUG_TRACE("res = " << res);

	expect_res = 1.5420e-18; // Computed with matlab R2008a, octave 3.2.4 and R 2.11.1
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect_res, 1.0e-18 );
}

BOOST_UBLASX_TEST_DEF( norm_1_real_square_dense_matrix_row_major )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: 1-Norm - Real Square Dense Matrix - Row Major");

	typedef double value_type;
	typedef double result_type;
	typedef ublas::matrix<value_type,ublas::row_major> matrix_type;

	const std::size_t n = 3;

	matrix_type A(n,n);
	A(0,0) = 1; A(0,1) = 2; A(0,2) = 3;
	A(1,0) = 4; A(1,1) = 5; A(1,2) = 6;
	A(2,0) = 7; A(2,1) = 8; A(2,2) = 9;

	result_type res;
	result_type expect_res;

	res = ublasx::rcond(A);

	BOOST_UBLASX_DEBUG_TRACE("A = " << A);
	BOOST_UBLASX_DEBUG_TRACE("res = " << res);

	expect_res = 1.5420e-18; // Computed with matlab R2008a, octave 3.2.4 and R 2.11.1
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect_res, 1.0e-18 );
}

BOOST_UBLASX_TEST_DEF( norm_1_real_upper_triangular_matrix_column_major )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: 1-Norm - Real Upper Triangular Matrix - Column Major");

	typedef double value_type;
	typedef double result_type;
	typedef ublas::triangular_matrix<value_type,ublas::upper,ublas::column_major> matrix_type;

	const std::size_t n = 3;

	matrix_type A(n,n);
	A(0,0) = 1; A(0,1) = 2; A(0,2) = 3;
	            A(1,1) = 4; A(1,2) = 5;
	                        A(2,2) = 6;

	result_type res;
	result_type expect_res;

	res = ublasx::rcond(A);

	BOOST_UBLASX_DEBUG_TRACE("A = " << A);
	BOOST_UBLASX_DEBUG_TRACE("res = " << res);

	expect_res = 0.07142857; // Computed with matlab R2008a, octave 3.2.4 and R 2.11.1
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect_res, 1.0e-7 );
}

BOOST_UBLASX_TEST_DEF( norm_1_real_upper_triangular_matrix_row_major )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: 1-Norm - Real Upper Triangular Matrix - Row Major");

	typedef double value_type;
	typedef double result_type;
	typedef ublas::triangular_matrix<value_type,ublas::upper,ublas::row_major> matrix_type;

	const std::size_t n = 3;

	matrix_type A(n,n);
	A(0,0) = 1; A(0,1) = 2; A(0,2) = 3;
	            A(1,1) = 4; A(1,2) = 5;
	                        A(2,2) = 6;

	result_type res;
	result_type expect_res;

	res = ublasx::rcond(A);

	BOOST_UBLASX_DEBUG_TRACE("A = " << A);
	BOOST_UBLASX_DEBUG_TRACE("res = " << res);

	expect_res = 0.07142857; // Computed with matlab R2008a, octave 3.2.4 and R 2.11.1
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect_res, 1.0e-7 );
}

BOOST_UBLASX_TEST_DEF( norm_1_real_lower_triangular_matrix_column_major )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: 1-Norm - Real Lower Triangular Matrix - Column Major");

	typedef double value_type;
	typedef double result_type;
	typedef ublas::triangular_matrix<value_type,ublas::lower,ublas::column_major> matrix_type;

	const std::size_t n = 3;

	matrix_type A(n,n);
	A(0,0) = 1;
	A(1,0) = 2; A(1,1) = 3;
	A(2,0) = 4; A(2,1) = 5; A(2,2) = 6;

	result_type res;
	result_type expect_res;

	res = ublasx::rcond(A);

	BOOST_UBLASX_DEBUG_TRACE("A = " << A);
	BOOST_UBLASX_DEBUG_TRACE("res = " << res);

	expect_res = 0.0703125; // Computed with matlab R2008a, octave 3.2.4, and R 2.11.1
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect_res, 1.0e-7 );
}

BOOST_UBLASX_TEST_DEF( norm_1_real_lower_triangular_matrix_row_major )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: 1-Norm - Real Lower Triangular Matrix - Row Major");

	typedef double value_type;
	typedef double result_type;
	typedef ublas::triangular_matrix<value_type,ublas::lower,ublas::row_major> matrix_type;

	const std::size_t n = 3;

	matrix_type A(n,n);
	A(0,0) = 1;
	A(1,0) = 2; A(1,1) = 3;
	A(2,0) = 4; A(2,1) = 5; A(2,2) = 6;

	result_type res;
	result_type expect_res;

	res = ublasx::rcond(A);

	BOOST_UBLASX_DEBUG_TRACE("A = " << A);
	BOOST_UBLASX_DEBUG_TRACE("res = " << res);

	expect_res = 0.0703125; // Computed with matlab R2008a, octave 3.2.4, and R 2.11.1
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect_res, 1.0e-7 );
}

BOOST_UBLASX_TEST_DEF( norm_1_real_banded_matrix_column_major )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: 1-Norm - Real Banded Matrix - Column Major");

	typedef double value_type;
	typedef double result_type;
	typedef ublas::banded_matrix<value_type,ublas::column_major> matrix_type;

	const std::size_t n = 4;

	matrix_type A(n,n,1,2);
	A(0,0) = -0.23; A(0,1) = 2.54; A(0,2) = -3.66;         /* 0 */ // 1st row
	A(1,0) = -6.98; A(1,1) = 2.46; A(1,2) = -2.73; A(1,3) = -2.13; // 2nd row
	        /* 0 */ A(2,1) = 2.56; A(2,2) =  2.46; A(2,3) =  4.07; // 3rd row
	        /* 0 */        /* 0 */ A(3,2) = -4.78; A(3,3) = -3.82; // 4th row

	result_type res;
	result_type expect_res;

	res = ublasx::rcond(A);

	BOOST_UBLASX_DEBUG_TRACE("A = " << A);
	BOOST_UBLASX_DEBUG_TRACE("res = " << res);

	expect_res = 0.017728; // Computed with matlab R2008a, octave 3.2.4, and R 2.11.1
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect_res, 1.0e-6 );
}

BOOST_UBLASX_TEST_DEF( norm_1_real_banded_matrix_row_major )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: 1-Norm - Real Banded Matrix - Row Major");

	typedef double value_type;
	typedef double result_type;
	typedef ublas::banded_matrix<value_type,ublas::row_major> matrix_type;

	const std::size_t n = 4;

	matrix_type A(n,n,1,2);
	A(0,0) = -0.23; A(0,1) = 2.54; A(0,2) = -3.66;         /* 0 */ // 1st row
	A(1,0) = -6.98; A(1,1) = 2.46; A(1,2) = -2.73; A(1,3) = -2.13; // 2nd row
	        /* 0 */ A(2,1) = 2.56; A(2,2) =  2.46; A(2,3) =  4.07; // 3rd row
	        /* 0 */        /* 0 */ A(3,2) = -4.78; A(3,3) = -3.82; // 4th row

	result_type res;
	result_type expect_res;

	res = ublasx::rcond(A);

	BOOST_UBLASX_DEBUG_TRACE("A = " << A);
	BOOST_UBLASX_DEBUG_TRACE("res = " << res);

	expect_res = 0.017728; // Computed with matlab R2008a, octave 3.2.4, and R 2.11.1
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect_res, 1.0e-6 );
}

BOOST_UBLASX_TEST_DEF( norm_1_real_lower_symmetric_matrix_column_major )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: 1-Norm - Real Lower Symmetric Matrix - Column Major");

	typedef double value_type;
	typedef double result_type;
	typedef ublas::symmetric_matrix<value_type,ublas::lower,ublas::column_major> matrix_type;

	const std::size_t n = 4;

	matrix_type A(n,n);
	A(0,0) =  2.07;   /* 3.87*/      /* 4.20*/      /*-1.15*/
	A(1,0) =  3.87; A(1,1) = -0.21;  /* 1.87*/      /* 0.63*/
	A(2,0) =  4.20; A(2,1) = 1.87; A(2,2) = 1.15;   /* 2.06*/
	A(3,0) = -1.15; A(3,1) = 0.63; A(3,2) = 2.06; A(3,3) = -1.81;

	result_type res;
	result_type expect_res;

	res = ublasx::rcond(A);

	BOOST_UBLASX_DEBUG_TRACE("A = " << A);
	BOOST_UBLASX_DEBUG_TRACE("res = " << res);

	expect_res = 0.01321232; // Computed with matlab R2008a, octave 3.2.4, and R 2.11.1
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect_res, 1.0e-6 );
}

BOOST_UBLASX_TEST_DEF( norm_1_real_lower_symmetric_matrix_row_major )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: 1-Norm - Real Lower Symmetric Matrix - Row Major");

	typedef double value_type;
	typedef double result_type;
	typedef ublas::symmetric_matrix<value_type,ublas::lower,ublas::row_major> matrix_type;

	const std::size_t n = 4;

	matrix_type A(n,n);
	A(0,0) =  2.07;   /* 3.87*/      /* 4.20*/      /*-1.15*/
	A(1,0) =  3.87; A(1,1) = -0.21;  /* 1.87*/      /* 0.63*/
	A(2,0) =  4.20; A(2,1) = 1.87; A(2,2) = 1.15;   /* 2.06*/
	A(3,0) = -1.15; A(3,1) = 0.63; A(3,2) = 2.06; A(3,3) = -1.81;

	result_type res;
	result_type expect_res;

	res = ublasx::rcond(A);

	BOOST_UBLASX_DEBUG_TRACE("A = " << A);
	BOOST_UBLASX_DEBUG_TRACE("res = " << res);

	expect_res = 0.01321232; // Computed with matlab R2008a, octave 3.2.4, and R 2.11.1
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect_res, 1.0e-6 );
}

BOOST_UBLASX_TEST_DEF( norm_1_real_upper_symmetric_matrix_column_major )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: 1-Norm - Real Upper Symmetric Matrix - Column Major");

	typedef double value_type;
	typedef double result_type;
	typedef ublas::symmetric_matrix<value_type,ublas::upper,ublas::column_major> matrix_type;

	const std::size_t n = 4;

	matrix_type A(n,n);
	A(0,0) =  2.07; A(0,1) =  3.87; A(0,2) = 4.20; A(0,3) = -1.15;
	A(1,0) =  3.87; A(1,1) = -0.21; A(1,2) = 1.87; A(1,3) =  0.63;
	  /* 4.20*/       /* 1.87*/     A(2,2) = 1.15; A(2,3) =  2.06;
	  /*-1.15*/       /* 0.63*/       /* 2.06*/    A(3,3) = -1.81;

	result_type res;
	result_type expect_res;

	res = ublasx::rcond(A);

	BOOST_UBLASX_DEBUG_TRACE("A = " << A);
	BOOST_UBLASX_DEBUG_TRACE("res = " << res);

	expect_res = 0.01321232; // Computed with matlab R2008a, octave 3.2.4, and R 2.11.1
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect_res, 1.0e-6 );
}

BOOST_UBLASX_TEST_DEF( norm_1_real_upper_symmetric_matrix_row_major )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: 1-Norm - Real Upper Symmetric Matrix - Row Major");

	typedef double value_type;
	typedef double result_type;
	typedef ublas::symmetric_matrix<value_type,ublas::upper,ublas::row_major> matrix_type;

	const std::size_t n = 4;

	matrix_type A(n,n);
	A(0,0) =  2.07; A(0,1) =  3.87; A(0,2) = 4.20; A(0,3) = -1.15;
	A(1,0) =  3.87; A(1,1) = -0.21; A(1,2) = 1.87; A(1,3) =  0.63;
	  /* 4.20*/       /* 1.87*/     A(2,2) = 1.15; A(2,3) =  2.06;
	  /*-1.15*/       /* 0.63*/       /* 2.06*/    A(3,3) = -1.81;

	result_type res;
	result_type expect_res;

	res = ublasx::rcond(A);

	BOOST_UBLASX_DEBUG_TRACE("A = " << A);
	BOOST_UBLASX_DEBUG_TRACE("res = " << res);

	expect_res = 0.01321232; // Computed with matlab R2008a, octave 3.2.4, and R 2.11.1
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect_res, 1.0e-6 );
}

BOOST_UBLASX_TEST_DEF( norm_1_complex_lower_hermitian_matrix_column_major )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: 1-Norm - Complex Lower Hermitian Matrix - Column Major");

	typedef std::complex<double> value_type;
	typedef double result_type;
	typedef ublas::hermitian_matrix<value_type,ublas::lower,ublas::column_major> matrix_type;

	const std::size_t n = 4;

	matrix_type A(n,n);
	A(0,0) = value_type(-1.36, 0.00);
	A(1,0) = value_type( 1.58,-0.90); A(1,1) = value_type(-8.87, 0.00);
	A(2,0) = value_type( 2.21, 0.21); A(2,1) = value_type(-1.84, 0.03); A(2,2) = value_type(-4.63, 0.00);
	A(3,0) = value_type( 3.91,-1.50); A(3,1) = value_type(-1.78,-1.18); A(3,2) = value_type( 0.11,-0.11); A(3,3) = value_type(-1.84, 0.00);

	result_type res;
	result_type expect_res;

	res = ublasx::rcond(A);

	BOOST_UBLASX_DEBUG_TRACE("A = " << A);
	BOOST_UBLASX_DEBUG_TRACE("res = " << res);

	expect_res = 0.028470197472865; // Computed with matlab R2008a, octave 3.2.4, and R 2.11.1
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect_res, 1.0e-6 );
}

BOOST_UBLASX_TEST_DEF( norm_1_real_lower_hermitian_matrix_row_major )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: 1-Norm - Real Lower Hermitian Matrix - Row Major");

	typedef std::complex<double> value_type;
	typedef double result_type;
	typedef ublas::hermitian_matrix<value_type,ublas::lower,ublas::row_major> matrix_type;

	const std::size_t n = 4;

	matrix_type A(n,n);
	A(0,0) = value_type(-1.36, 0.00);
	A(1,0) = value_type( 1.58,-0.90); A(1,1) = value_type(-8.87, 0.00);
	A(2,0) = value_type( 2.21, 0.21); A(2,1) = value_type(-1.84, 0.03); A(2,2) = value_type(-4.63, 0.00);
	A(3,0) = value_type( 3.91,-1.50); A(3,1) = value_type(-1.78,-1.18); A(3,2) = value_type( 0.11,-0.11); A(3,3) = value_type(-1.84, 0.00);

	result_type res;
	result_type expect_res;

	res = ublasx::rcond(A);

	BOOST_UBLASX_DEBUG_TRACE("A = " << A);
	BOOST_UBLASX_DEBUG_TRACE("res = " << res);

	expect_res = 0.028470197472865; // Computed with matlab R2008a, octave 3.2.4, and R 2.11.1
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect_res, 1.0e-6 );
}

BOOST_UBLASX_TEST_DEF( norm_1_complex_square_dense_matrix_column_major )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: 1-Norm - Complex Square Dense Matrix - Column Major");

	typedef std::complex<double> value_type;
	typedef double result_type;
	typedef ublas::matrix<value_type,ublas::column_major> matrix_type;

	const std::size_t n = 3;

	matrix_type A(n,n);
	A(0,0) = value_type( 1, 2); A(0,1) = value_type( 3, 4); A(0,2) = value_type( 5, 6);
	A(1,0) = value_type( 7, 8); A(1,1) = value_type( 9,10); A(1,2) = value_type(11,12);
	A(2,0) = value_type(13,14); A(2,1) = value_type(15,16); A(2,2) = value_type(17,18);

	result_type res;
	result_type expect_res;

	res = ublasx::rcond(A);

	BOOST_UBLASX_DEBUG_TRACE("A = " << A);
	BOOST_UBLASX_DEBUG_TRACE("res = " << res);

	//expect_res = 4.3758e-18; // Computed with matlab R2008a and octave 3.2.4 and R 2.11.1
	expect_res = 1.136408e-18; // Computed with R 2.11.1
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect_res, 1.0e-18 );
}

BOOST_UBLASX_TEST_DEF( norm_1_complex_square_dense_matrix_row_major )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: 1-Norm - Complex Square Dense Matrix - Row Major");

	typedef std::complex<double> value_type;
	typedef double result_type;
	typedef ublas::matrix<value_type,ublas::row_major> matrix_type;

	const std::size_t n = 3;

	matrix_type A(n,n);
	A(0,0) = value_type( 1, 2); A(0,1) = value_type( 3, 4); A(0,2) = value_type( 5, 6);
	A(1,0) = value_type( 7, 8); A(1,1) = value_type( 9,10); A(1,2) = value_type(11,12);
	A(2,0) = value_type(13,14); A(2,1) = value_type(15,16); A(2,2) = value_type(17,18);

	result_type res;
	result_type expect_res;

	res = ublasx::rcond(A);

	BOOST_UBLASX_DEBUG_TRACE("A = " << A);
	BOOST_UBLASX_DEBUG_TRACE("res = " << res);

	//expect_res = 4.375805911678970e-18; // Computed with matlab R2008a and octave 3.2.4
	expect_res = 1.136408e-18; // Computed with R 2.11.1
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect_res, 1.0e-18 );
}

BOOST_UBLASX_TEST_DEF( norm_1_complex_upper_triangular_matrix_column_major )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: 1-Norm - Complex Upper Triangular Matrix - Column Major");

	typedef std::complex<double> value_type;
	typedef double result_type;
	typedef ublas::triangular_matrix<value_type,ublas::upper,ublas::column_major> matrix_type;

	const std::size_t n = 3;

	matrix_type A(n,n);
	A(0,0) = value_type( 1, 2); A(0,1) = value_type( 3, 4); A(0,2) = value_type( 5, 6);
	                            A(1,1) = value_type( 7, 8); A(1,2) = value_type( 9,10);
	                                                        A(2,2) = value_type(11,12);

	result_type res;
	result_type expect_res;

	res = ublasx::rcond(A);

	BOOST_UBLASX_DEBUG_TRACE("A = " << A);
	BOOST_UBLASX_DEBUG_TRACE("res = " << res);

	expect_res = 0.059560668674847; // Computed with matlab R2008a, octave 3.2.4
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect_res, 1.0e-7 );
}

BOOST_UBLASX_TEST_DEF( norm_1_complex_upper_triangular_matrix_row_major )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: 1-Norm - Real Upper Triangular Matrix - Row Major");

	typedef std::complex<double> value_type;
	typedef double result_type;
	typedef ublas::triangular_matrix<value_type,ublas::upper,ublas::row_major> matrix_type;

	const std::size_t n = 3;

	matrix_type A(n,n);
	A(0,0) = value_type( 1, 2); A(0,1) = value_type( 3, 4); A(0,2) = value_type( 5, 6);
	                            A(1,1) = value_type( 7, 8); A(1,2) = value_type( 9,10);
	                                                        A(2,2) = value_type(11,12);

	result_type res;
	result_type expect_res;

	res = ublasx::rcond(A);

	BOOST_UBLASX_DEBUG_TRACE("A = " << A);
	BOOST_UBLASX_DEBUG_TRACE("res = " << res);

	expect_res = 0.059560668674847; // Computed with matlab R2008a and octave 3.2.4
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect_res, 1.0e-7 );
}

BOOST_UBLASX_TEST_DEF( norm_1_complex_lower_triangular_matrix_column_major )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: 1-Norm - Complex Lower Triangular Matrix - Column Major");

	typedef std::complex<double> value_type;
	typedef double result_type;
	typedef ublas::triangular_matrix<value_type,ublas::lower,ublas::column_major> matrix_type;

	const std::size_t n = 3;

	matrix_type A(n,n);
	A(0,0) = value_type( 1, 2);
	A(1,0) = value_type( 3, 4); A(1,1) = value_type( 5, 6);
	A(2,0) = value_type( 7, 8); A(2,1) = value_type( 9,10); A(2,2) = value_type(11,12);

	result_type res;
	result_type expect_res;

	res = ublasx::rcond(A);

	BOOST_UBLASX_DEBUG_TRACE("A = " << A);
	BOOST_UBLASX_DEBUG_TRACE("res = " << res);

	expect_res = 0.059544953702088; // Computed with matlab R2008a, octave 3.2.4
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect_res, 1.0e-7 );
}

BOOST_UBLASX_TEST_DEF( norm_1_complex_lower_triangular_matrix_row_major )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: 1-Norm - Complex lower Triangular Matrix - Row Major");

	typedef std::complex<double> value_type;
	typedef double result_type;
	typedef ublas::triangular_matrix<value_type,ublas::lower,ublas::row_major> matrix_type;

	const std::size_t n = 3;

	matrix_type A(n,n);
	A(0,0) = value_type( 1, 2);
	A(1,0) = value_type( 3, 4); A(1,1) = value_type( 5, 6);
	A(2,0) = value_type( 7, 8); A(2,1) = value_type( 9,10); A(2,2) = value_type(11,12);

	result_type res;
	result_type expect_res;

	res = ublasx::rcond(A);

	BOOST_UBLASX_DEBUG_TRACE("A = " << A);
	BOOST_UBLASX_DEBUG_TRACE("res = " << res);

	expect_res = 0.059544953702088; // Computed with matlab R2008a and octave 3.2.4
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect_res, 1.0e-7 );
}

BOOST_UBLASX_TEST_DEF( norm_1_complex_banded_matrix_column_major )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: 1-Norm - Complex Banded Matrix - Column Major");

	typedef std::complex<double> value_type;
	typedef double result_type;
	typedef ublas::banded_matrix<value_type,ublas::column_major> matrix_type;

	const std::size_t n = 4;

	matrix_type A(n,n,1,2);
	A(0,0) = value_type(-1.65, 2.26); A(0,1) = value_type(-2.05,-0.85); A(0,2) = value_type( 0.97,-2.84);
	A(1,0) = value_type( 0.00, 6.30); A(1,1) = value_type(-1.48,-1.75); A(1,2) = value_type(-3.99, 4.01); A(1,3) = value_type( 0.59,-0.48);
			                          A(2,1) = value_type(-0.77, 2.83); A(2,2) = value_type(-1.06, 1.94); A(2,3) = value_type( 3.33,-1.04);
							                                            A(3,2) = value_type( 4.48,-1.09); A(3,3) = value_type(-0.46,-1.72);

	result_type res;
	result_type expect_res;

	res = ublasx::rcond(A);

	BOOST_UBLASX_DEBUG_TRACE("A = " << A);
	BOOST_UBLASX_DEBUG_TRACE("res = " << res);

	expect_res = 0.009594414793018; // Computed with matlab R2008a, octave 3.2.4, and R 2.11.1
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect_res, 1.0e-6 );
}

BOOST_UBLASX_TEST_DEF( norm_1_complex_banded_matrix_row_major )
{
	BOOST_UBLASX_DEBUG_TRACE("Test Case: 1-Norm - Complex Banded Matrix - Row Major");

	typedef std::complex<double> value_type;
	typedef double result_type;
	typedef ublas::banded_matrix<value_type,ublas::row_major> matrix_type;

	const std::size_t n = 4;

	matrix_type A(n,n,1,2);
	A(0,0) = value_type(-1.65, 2.26); A(0,1) = value_type(-2.05,-0.85); A(0,2) = value_type( 0.97,-2.84);
	A(1,0) = value_type( 0.00, 6.30); A(1,1) = value_type(-1.48,-1.75); A(1,2) = value_type(-3.99, 4.01); A(1,3) = value_type( 0.59,-0.48);
			                          A(2,1) = value_type(-0.77, 2.83); A(2,2) = value_type(-1.06, 1.94); A(2,3) = value_type( 3.33,-1.04);
							                                            A(3,2) = value_type( 4.48,-1.09); A(3,3) = value_type(-0.46,-1.72);

	result_type res;
	result_type expect_res;

	res = ublasx::rcond(A);

	BOOST_UBLASX_DEBUG_TRACE("A = " << A);
	BOOST_UBLASX_DEBUG_TRACE("res = " << res);

	expect_res = 0.009594414793018; // Computed with matlab R2008a, octave 3.2.4, and R 2.11.1
	BOOST_UBLASX_TEST_CHECK_CLOSE( res, expect_res, 1.0e-6 );
}

int main()
{
	BOOST_UBLASX_DEBUG_TRACE("Test Suite: 'rcond' operation");

	BOOST_UBLASX_TEST_BEGIN();

	BOOST_UBLASX_TEST_DO( norm_1_real_square_dense_matrix_column_major );
	BOOST_UBLASX_TEST_DO( norm_1_real_square_dense_matrix_row_major );
	BOOST_UBLASX_TEST_DO( norm_1_real_upper_triangular_matrix_column_major );
	BOOST_UBLASX_TEST_DO( norm_1_real_upper_triangular_matrix_row_major );
	BOOST_UBLASX_TEST_DO( norm_1_real_lower_triangular_matrix_column_major );
	BOOST_UBLASX_TEST_DO( norm_1_real_lower_triangular_matrix_row_major );
	BOOST_UBLASX_TEST_DO( norm_1_real_banded_matrix_column_major );
	BOOST_UBLASX_TEST_DO( norm_1_real_banded_matrix_row_major );
	BOOST_UBLASX_TEST_DO( norm_1_real_lower_symmetric_matrix_column_major );
	BOOST_UBLASX_TEST_DO( norm_1_real_lower_symmetric_matrix_row_major );
	BOOST_UBLASX_TEST_DO( norm_1_real_upper_symmetric_matrix_column_major );
	BOOST_UBLASX_TEST_DO( norm_1_real_upper_symmetric_matrix_row_major );
	BOOST_UBLASX_TEST_DO( norm_1_complex_square_dense_matrix_column_major );
	BOOST_UBLASX_TEST_DO( norm_1_complex_square_dense_matrix_row_major );
	BOOST_UBLASX_TEST_DO( norm_1_complex_upper_triangular_matrix_column_major );
	BOOST_UBLASX_TEST_DO( norm_1_complex_upper_triangular_matrix_row_major );
	BOOST_UBLASX_TEST_DO( norm_1_complex_lower_triangular_matrix_column_major );
	BOOST_UBLASX_TEST_DO( norm_1_complex_lower_triangular_matrix_row_major );
	BOOST_UBLASX_TEST_DO( norm_1_complex_banded_matrix_column_major );
	BOOST_UBLASX_TEST_DO( norm_1_complex_banded_matrix_row_major );
	BOOST_UBLASX_TEST_DO( norm_1_complex_lower_hermitian_matrix_column_major );
	BOOST_UBLASX_TEST_DO( norm_1_real_lower_hermitian_matrix_row_major );

	BOOST_UBLASX_TEST_END();
}