export_ceres_robust.cpp

//
// Created by michal on 14.01.2022.
//


#include <ceres/loss_function.h>
#include <fstream>

int main()
{
    const double max_residual = 5;
    const int steps = 100;

    const auto cauchy = ceres::CauchyLoss(1.0);
    const auto huber = ceres::HuberLoss(1.0);
    const auto tukey = ceres::TukeyLoss(1.0);
    const auto soft = ceres::SoftLOneLoss(1.0);
    const auto arctan = ceres::ArctanLoss(1.0);
    const auto tolerance = ceres::TolerantLoss(1.0,1.0);
    const auto trivial = ceres::TrivialLoss();




    std::vector<double> residuals;
    std::vector<double> trivial_cost;
    std::vector<double> cauchy_cost;
    std::vector<double> huber_cost;
    std::vector<double> tukey_cost;
    std::vector<double> soft_cost;
    std::vector<double> arctan_cost;
    std::vector<double> tolerance_cost;

    std::vector<double> residuals_p1;
    std::vector<double> trivial_cost_p1;
    std::vector<double> cauchy_cost_p1;
    std::vector<double> huber_cost_p1;
    std::vector<double> tukey_cost_p1;
    std::vector<double> soft_cost_p1;
    std::vector<double> arctan_cost_p1;
    std::vector<double> tolerance_cost_p1;

    std::vector<double> residuals_p2;
    std::vector<double> trivial_cost_p2;
    std::vector<double> cauchy_cost_p2;
    std::vector<double> huber_cost_p2;
    std::vector<double> tukey_cost_p2;
    std::vector<double> soft_cost_p2;
    std::vector<double> arctan_cost_p2;
    std::vector<double> tolerance_cost_p2;

    for (int i =0; i < steps; i++)
    {
        double residual = max_residual * static_cast<double>(i)/static_cast<double>(steps);
        residuals.push_back(residual);

        double rsq = residual*residual;
        double loss_r[3];

        trivial.Evaluate(rsq, loss_r);
        trivial_cost.push_back(loss_r[0]);
        trivial_cost_p1.push_back(loss_r[1]);
        trivial_cost_p2.push_back(loss_r[2]);

        huber.Evaluate(rsq, loss_r);
        huber_cost.push_back(loss_r[0]);
        huber_cost_p1.push_back(loss_r[1]);
        huber_cost_p2.push_back(loss_r[2]);


        cauchy.Evaluate(rsq, loss_r);
        cauchy_cost.push_back(loss_r[0]);
        cauchy_cost_p1.push_back(loss_r[1]);
        cauchy_cost_p2.push_back(loss_r[2]);


        tukey.Evaluate(rsq, loss_r);
        tukey_cost.push_back(loss_r[0]);
        tukey_cost_p1.push_back(loss_r[1]);
        tukey_cost_p2.push_back(loss_r[2]);

        soft.Evaluate(rsq, loss_r);
        soft_cost.push_back(loss_r[0]);
        soft_cost_p1.push_back(loss_r[1]);
        soft_cost_p2.push_back(loss_r[2]);


        arctan.Evaluate(rsq, loss_r);
        arctan_cost.push_back(loss_r[0]);
        arctan_cost_p1.push_back(loss_r[1]);
        arctan_cost_p2.push_back(loss_r[2]);


        tolerance.Evaluate(rsq, loss_r);
        tolerance_cost.push_back(loss_r[0]);
        tolerance_cost_p1.push_back(loss_r[1]);
        tolerance_cost_p2.push_back(loss_r[2]);

    }

    std::ofstream py_draw ("/tmp/draw.py");
    py_draw << "import matplotlib.pyplot as plt"<<std::endl;
    py_draw << "import numpy as np"<<std::endl;
    py_draw << "residuals = np.array([";
    for (auto r : residuals){
        py_draw << r <<",";
    }
    py_draw<<"])"<<std::endl;

    auto export_fun = [&](const std::string& type, const std::vector<double> & vec){
        py_draw << type+" = np.array([";
        for (auto r : vec){
            py_draw << r <<",";
        }
        py_draw<<"])"<<std::endl;
    };

    export_fun("trivial_cost", trivial_cost);
    export_fun("cauchy_cost", cauchy_cost);
    export_fun("huber_cost", huber_cost);
    export_fun("tukey_cost", tukey_cost);
    export_fun("soft_cost", soft_cost);
    export_fun("arctan_cost", arctan_cost);
    export_fun("tolerance_cost", tolerance_cost);

    export_fun("trivial_cost_p1", trivial_cost_p1);
    export_fun("cauchy_cost_p1", cauchy_cost_p1);
    export_fun("huber_cost_p1", huber_cost_p1);
    export_fun("tukey_cost_p1", tukey_cost_p1);
    export_fun("soft_cost_p1", soft_cost_p1);
    export_fun("arctan_cost_p1", arctan_cost_p1);
    export_fun("tolerance_cost_p1", tolerance_cost_p1);

    export_fun("trivial_cost_p2", trivial_cost_p2);
    export_fun("cauchy_cost_p2", cauchy_cost_p2);
    export_fun("huber_cost_p2", huber_cost_p2);
    export_fun("tukey_cost_p2", tukey_cost_p2);
    export_fun("soft_cost_p2", soft_cost_p2);
    export_fun("arctan_cost_p2", arctan_cost_p2);
    export_fun("tolerance_cost_p2", tolerance_cost_p2);

    py_draw<<"plt.subplot(131)"<<std::endl;

    py_draw<<"plt.plot(residuals,trivial_cost)"<<std::endl;
    py_draw<<"plt.plot(residuals,cauchy_cost)"<<std::endl;
    py_draw<<"plt.plot(residuals,huber_cost)"<<std::endl;
    py_draw<<"plt.plot(residuals,tukey_cost)"<<std::endl;
    py_draw<<"plt.plot(residuals,soft_cost)"<<std::endl;
    py_draw<<"plt.plot(residuals,arctan_cost)"<<std::endl;
    py_draw<<"plt.plot(residuals,tolerance_cost)"<<std::endl;
    py_draw<<"plt.legend(['trivial_cost', 'cauchy_cost','huber_cost','tukey_cost', 'soft_cost', 'arctan_cost', 'tolerance_cost'])"<<std::endl;
    py_draw<<"plt.subplot(132)"<<std::endl;

    py_draw<<"plt.plot(residuals,trivial_cost_p1)"<<std::endl;
    py_draw<<"plt.plot(residuals,cauchy_cost_p1)"<<std::endl;
    py_draw<<"plt.plot(residuals,huber_cost_p1)"<<std::endl;
    py_draw<<"plt.plot(residuals,tukey_cost_p1)"<<std::endl;
    py_draw<<"plt.plot(residuals,soft_cost_p1)"<<std::endl;
    py_draw<<"plt.plot(residuals,arctan_cost_p1)"<<std::endl;
    py_draw<<"plt.plot(residuals,tolerance_cost_p1)"<<std::endl;
    py_draw<<"plt.legend(['trivial_cost', 'cauchy_cost','huber_cost','tukey_cost', 'soft_cost', 'arctan_cost', 'tolerance_cost'])"<<std::endl;
    py_draw<<"plt.subplot(133)"<<std::endl;

    py_draw<<"plt.plot(residuals,trivial_cost_p2)"<<std::endl;
    py_draw<<"plt.plot(residuals,cauchy_cost_p2)"<<std::endl;
    py_draw<<"plt.plot(residuals,huber_cost_p2)"<<std::endl;
    py_draw<<"plt.plot(residuals,tukey_cost_p2)"<<std::endl;
    py_draw<<"plt.plot(residuals,soft_cost_p2)"<<std::endl;
    py_draw<<"plt.plot(residuals,arctan_cost_p2)"<<std::endl;
    py_draw<<"plt.plot(residuals,tolerance_cost_p2)"<<std::endl;
    py_draw<<"plt.legend(['trivial_cost', 'cauchy_cost','huber_cost','tukey_cost', 'soft_cost', 'arctan_cost', 'tolerance_cost'])"<<std::endl;

    py_draw<<"plt.show()"<<std::endl;
}