diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index 21efe737be2efa0bed17b92ba5e9531698243a11..54a1d5f221e89a0479850d1cb2b7c0a778acd307 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -104,6 +104,10 @@ TARGET_LINK_LIBRARIES(test_sparsification ${PROJECT_NAME})
 ADD_EXECUTABLE(test_sparsification_topology test_sparsification_topology.cpp)
 TARGET_LINK_LIBRARIES(test_sparsification_topology ${PROJECT_NAME})
 
+# Sparsification convergence test
+ADD_EXECUTABLE(test_sparsification_convergence test_sparsification_convergence.cpp)
+TARGET_LINK_LIBRARIES(test_sparsification_convergence ${PROJECT_NAME})
+
 # Comparing analytic and autodiff odometry constraints
 #ADD_EXECUTABLE(test_analytic_odom_constraint test_analytic_odom_constraint.cpp)
 #TARGET_LINK_LIBRARIES(test_analytic_odom_constraint ${PROJECT_NAME})
diff --git a/src/examples/test_sparsification_convergence.cpp b/src/examples/test_sparsification_convergence.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5564514cc90492563ed986df13b318760bcbcea1
--- /dev/null
+++ b/src/examples/test_sparsification_convergence.cpp
@@ -0,0 +1,299 @@
+// Sparsification example creating wolf tree from imported graph from .txt file
+
+//C includes for sleep, time and main args
+#include "unistd.h"
+
+//std includes
+#include <cstdlib>
+#include <fstream>
+#include <string>
+#include <iostream>
+#include <memory>
+#include <random>
+#include <cmath>
+#include <queue>
+#include <chrono>
+#include <ctime>
+#include <sys/stat.h>
+
+//Wolf includes
+#include "wolf.h"
+#include "factory.h"
+#include "sensor_factory.h"
+#include "capture_void.h"
+#include "feature_odom_2D.h"
+#include "constraint_base.h"
+#include "ceres_wrapper/ceres_manager.h"
+#include "pruning/problem_pruning.h"
+
+// EIGEN
+//#include <Eigen/CholmodSupport>
+#include <Eigen/StdVector> // Eigen in std vector
+
+using namespace wolf;
+
+Scalar computeMutualInformation(const Eigen::MatrixXs& Sigma_Tik, const std::map<FrameBasePtr, unsigned int>& fr_2_col, const ConstraintBasePtr& ctr_ptr)
+{
+    FrameBasePtr frame_ptr1 = ctr_ptr->getFrameOtherPtr();
+    FrameBasePtr frame_ptr2 = ctr_ptr->getCapturePtr()->getFramePtr();
+    Eigen::MatrixXs Sigma_Tik_12(frame_ptr1->getSize() + frame_ptr2->getSize(), frame_ptr1->getSize() + frame_ptr2->getSize());
+
+    Sigma_Tik_12.topLeftCorner(    frame_ptr1->getSize(), frame_ptr1->getSize()) = Sigma_Tik.block(fr_2_col.at(frame_ptr1), fr_2_col.at(frame_ptr1), frame_ptr1->getSize(), frame_ptr1->getSize());
+    Sigma_Tik_12.topRightCorner(   frame_ptr1->getSize(), frame_ptr2->getSize()) = Sigma_Tik.block(fr_2_col.at(frame_ptr1), fr_2_col.at(frame_ptr2), frame_ptr1->getSize(), frame_ptr2->getSize());
+    Sigma_Tik_12.bottomLeftCorner( frame_ptr2->getSize(), frame_ptr1->getSize()) = Sigma_Tik.block(fr_2_col.at(frame_ptr2), fr_2_col.at(frame_ptr1), frame_ptr2->getSize(), frame_ptr1->getSize());
+    Sigma_Tik_12.bottomRightCorner(frame_ptr2->getSize(), frame_ptr2->getSize()) = Sigma_Tik.block(fr_2_col.at(frame_ptr2), fr_2_col.at(frame_ptr2), frame_ptr2->getSize(), frame_ptr2->getSize());
+
+    Scalar info = 0.5 * log(Sigma_Tik_12.topLeftCorner(frame_ptr1->getSize(), frame_ptr1->getSize()).determinant() *
+                            Sigma_Tik_12.bottomRightCorner(frame_ptr2->getSize(), frame_ptr2->getSize()).determinant() /
+                            Sigma_Tik_12.determinant());
+    return info;
+}
+
+void print_wrong_input()
+{
+    std::cout << "Please call me with: [./test_sparsification_topology DATASET T N (folder run_best)], where:" << std::endl;
+    std::cout << "    DATASET: manhattan, killian or intel" << std::endl;
+    std::cout << "    T pruning period: ALL, all, c_all, 3, c_3, 4, c_4, 5, c_5, 10 or c_10" << std::endl;
+    std::cout << "    N amount of problems (0: all)" << std::endl;
+    std::cout << "    folder where to put the logfiles (default: empty) " << std::endl;
+    std::cout << "EXIT due to bad user input" << std::endl << std::endl;
+}
+
+int main(int argc, char** argv) 
+{
+    //Welcome message
+    std::cout << std::endl << " ========= WOLF SPARSIFICATION AND TOPOLOGIES TEST ===========" << std::endl << std::endl;
+    auto start = std::chrono::system_clock::now();
+
+    std::vector<std::string> T_options({"3","4","5","10","c_3","c_4","c_5","c_10"});
+    std::vector<std::string> T_options_all({"3","4","5","10"});
+    std::vector<std::string> T_options_c_all({"c_3","c_4","c_5","c_10"});
+
+    // inputs
+    if (argc < 4)
+    {
+        print_wrong_input();
+        return -1;
+    }
+
+	std::string pruning_T_string = argv[2];
+    std::string dataset(argv[1]);
+    unsigned int N_problems_desired = atoi(argv[3]);
+    std::vector<std::string> T_options_combine;
+    if (pruning_T_string.compare("all") == 0)
+        T_options_combine = T_options_all;
+    if (pruning_T_string.compare("c_all") == 0)
+        T_options_combine = T_options_c_all;
+    if (pruning_T_string.compare("ALL") == 0)
+        T_options_combine = T_options;
+    else
+        for (auto T : T_options)
+            if (T.compare(pruning_T_string) == 0)
+            {
+                T_options_combine.push_back(pruning_T_string);
+                break;
+            }
+    if (T_options_combine.empty())
+    {
+        print_wrong_input();
+        return -1;
+    }
+    std::string folder;
+    if (argc > 4)
+        folder = std::string(argv[4]) + std::string("/");
+
+    // input variables
+    char const * dataset_path = std::getenv("DATASET_PATH");
+    std::string root_path = std::getenv("DATASET_PATH") + std::string("/logfiles/") + dataset + std::string("/topology_test/");
+
+    // An experiment for each pruning_T
+    for (auto T_str : T_options_combine)
+    {
+        std::cout << std::endl << " ===== Pruning ratio " << T_str << " topology test =====" << std::endl << std::endl;
+
+        // load file containing all original problems
+        std::ifstream problems_file;
+        // load
+        std::string file_path = root_path + "SG_FD_MI1_" + T_str + "_originals.txt";
+        problems_file.open(file_path.c_str(), std::ifstream::in);
+        if (!problems_file.is_open())
+        {
+            printf("\nError opening file: %s\n",file_path.c_str());
+            return -1;
+        }
+
+        // Necessary variables
+        ProblemPtr problem_ptr = wolf::Problem::create("PO_2D");
+        SensorBasePtr sensor_ptr = problem_ptr->installSensor("ODOM 2D", "Odometry", Eigen::VectorXs::Zero(3), IntrinsicsBasePtr());
+        problem_ptr->emplaceFrame("PO 2D", KEY_FRAME, Eigen::Vector3s::Zero(), TimeStamp(0.0));
+
+
+        // COMPARED TOPOLOGIES
+        Scalar max_time = 0.1;
+        unsigned int max_iter = 10000000;
+        std::map<std::string,PruningOptions> options_map;
+        //                                                                 local Topol  Topol        optim     initial             ----------------------------- only for optimization ----------------------------------------------
+        //                   name                      dataset  pruning    solve Method Size   Gamma method    guess    connected (max_iter  max_time  min_gradient  Hessian   init_rho rho_decrease min_gradient_rho cyclic apply_closed_form formulation)
+        options_map.emplace("IP-Id",    PruningOptions(dataset, 0,         true, MI,    N_FILL,0.75, IP,       ID,      false,     max_iter, max_time, 1e-9,         ANALYTIC, 1,       0.5,         1,               true,  true,             0));
+//        options_map.emplace("IP-Id",    PruningOptions(dataset, 0,         true, MI,    N_FILL,0.75, IP,       ID,      false,     max_iter, max_time, 1e-9,         ANALYTIC, 0.5,     0.5,         0.1,             true,  true,             0));
+//        options_map.emplace("IP2-Id",   PruningOptions(dataset, 0,         true, MI,    N_FILL,0.75, IP,       ID,      false,     max_iter, max_time, 1e-9,         ANALYTIC, 0.5,     0.5,         0.5,             true,  true,             0));
+//        options_map.emplace("IP3-Id",   PruningOptions(dataset, 0,         true, MI,    N_FILL,0.75, IP,       ID,      false,     max_iter, max_time, 1e-9,         ANALYTIC, 0.5,     0.5,         1,               true,  true,             0));
+//        options_map.emplace("IP4-Id",   PruningOptions(dataset, 0,         true, MI,    N_FILL,0.75, IP,       ID,      false,     max_iter, max_time, 1e-9,         ANALYTIC, 1,       0.5,         0.1,             true,  true,             0));
+//        options_map.emplace("IP5-Id",   PruningOptions(dataset, 0,         true, MI,    N_FILL,0.75, IP,       ID,      false,     max_iter, max_time, 1e-9,         ANALYTIC, 1,       0.5,         0.5,             true,  true,             0));
+//        options_map.emplace("IP4-Id",   PruningOptions(dataset, 0,         true, MI,    N_FILL,0.75, IP,       ID,      false,     max_iter, max_time, 1e-9,         ANALYTIC, 0.5,     0.1,         0.1,             true,  true,             0));
+//        options_map.emplace("IP5-Id",   PruningOptions(dataset, 0,         true, MI,    N_FILL,0.75, IP,       ID,      false,     max_iter, max_time, 1e-9,         ANALYTIC, 0.5,     0.1,         0.5,             true,  true,             0));
+//        options_map.emplace("IP6-Id",   PruningOptions(dataset, 0,         true, MI,    N_FILL,0.75, IP,       ID,      false,     max_iter, max_time, 1e-9,         ANALYTIC, 0.5,     0.1,         1,               true,  true,             0));
+//        options_map.emplace("IP7-Id",   PruningOptions(dataset, 0,         true, MI,    N_FILL,0.75, IP,       ID,      false,     max_iter, max_time, 1e-9,         ANALYTIC, 0.1,     0.5,         0.1,             true,  true,             0));
+//        options_map.emplace("IP8-Id",   PruningOptions(dataset, 0,         true, MI,    N_FILL,0.75, IP,       ID,      false,     max_iter, max_time, 1e-9,         ANALYTIC, 0.1,     0.5,         0.5,             true,  true,             0));
+//        options_map.emplace("IP9-Id",   PruningOptions(dataset, 0,         true, MI,    N_FILL,0.75, IP,       ID,      false,     max_iter, max_time, 1e-9,         ANALYTIC, 0.1,     0.5,         1,               true,  true,             0));
+//        options_map.emplace("IP10-Id",  PruningOptions(dataset, 0,         true, MI,    N_FILL,0.75, IP,       ID,      false,     max_iter, max_time, 1e-9,         ANALYTIC, 0.1,     0.1,         0.1,             true,  true,             0));
+//        options_map.emplace("IP11-Id",  PruningOptions(dataset, 0,         true, MI,    N_FILL,0.75, IP,       ID,      false,     max_iter, max_time, 1e-9,         ANALYTIC, 0.1,     0.1,         0.5,             true,  true,             0));
+//        options_map.emplace("IP12-Id",  PruningOptions(dataset, 0,         true, MI,    N_FILL,0.75, IP,       ID,      false,     max_iter, max_time, 1e-9,         ANALYTIC, 0.1,     0.1,         1,               true,  true,             0));
+        options_map.emplace("FD-Id",    PruningOptions(dataset, 0,         true, MI,    N_FILL,0.75, FD,       ID,      false,     max_iter, max_time, 1e-9,         ANALYTIC, 0.0,     0.0,         0.0,             true,  true,             1));
+        options_map.emplace("FD-ODB",   PruningOptions(dataset, 0,         true, MI,    N_FILL,0.75, FD,       ODB,     false,     max_iter, max_time, 1e-9,         ANALYTIC, 0.0,     0.0,         0.0,             true,  true,             1));
+        options_map.emplace("FD-FFD",   PruningOptions(dataset, 0,         true, MI,    N_FILL,0.75, FD,       FFD,     false,     max_iter, max_time, 1e-9,         ANALYTIC, 0.0,     0.0,         0.0,             true,  true,             1));
+
+        unsigned int N_options = options_map.size();
+        unsigned int N_problems(0);
+
+        // LOGFILES
+        std::map<std::string,std::ofstream> convergence_logfiles;
+        for (auto pair_option : options_map)
+        {
+            // evolutions [KLD,t,MB]
+            std::string filepath = root_path + "/results/" + folder + pair_option.first + "_" + T_str + std::string("_convergence.txt");
+            convergence_logfiles[pair_option.first].open(filepath, std::ofstream::out);
+            if (!convergence_logfiles[pair_option.first].is_open())
+                std::cout << std::endl << "Failed to open the problem sizes file " << filepath << std::endl;
+        }
+
+        ////////////////////////////////////////////////////////////////////////
+        // EXPERIMENT : Solving all original problems using different topologies
+        int cols = 0, rows = 0;
+        double buff[100];
+        std::string line;
+        while (std::getline(problems_file, line) && !problems_file.eof())
+        {
+            // mean
+            int elements = 0;
+            std::stringstream stream(line);
+            while(! stream.eof())
+                stream >> buff[elements++];
+
+            Eigen::VectorXs mean(elements);
+            for (int i = 0; i < elements; i++)
+                mean(i) = buff[i];
+
+            // Lambda
+            Eigen::MatrixXs Lambda(elements,elements);
+            for (int row = 0; row < elements; row++)
+            {
+                int col = 0;
+                std::getline(problems_file, line);
+                std::stringstream stream(line);
+                while(! stream.eof())
+                    stream >> Lambda(row,col++);
+            }
+
+            // space
+            std::getline(problems_file, line);
+
+            // TEST TOPOLOGIES ///////////////////////////
+            // INDICES
+            //std::cout << "INDICES\n";
+            unsigned int node_size = 3;
+            unsigned int markov_size = mean.size();
+            unsigned int markov_nodes = mean.size() / node_size;
+            std::vector<FrameBasePtr> frame_vector;
+
+            // Only for topologies to be designed (i.e. MB > 3)
+            if (markov_nodes > 2)
+            {
+                // BUILD MARKOV BLANKET
+                FrameBaseList markov_blanket;
+                for (auto i = 0; i<markov_nodes; i++)
+                    markov_blanket.push_back(problem_ptr->emplaceFrame("PO 2D", KEY_FRAME, mean.segment(i*node_size,node_size), TimeStamp(i)));
+
+                // INDEXES
+                std::map<StateBlockPtr, unsigned int> sb_2_col;
+                std::map<FrameBasePtr,  unsigned int> fr_2_col;
+                unsigned int state_size = computeStateBlockMap(markov_blanket, sb_2_col, fr_2_col);
+
+                // PROJECTION
+                //std::cout << "PROJECTION" << std::endl;
+                unsigned int projected_size = markov_size - node_size;
+                Eigen::JacobiSVD<Eigen::MatrixXs> UDUT(Lambda, Eigen::ComputeFullU | Eigen::ComputeFullV);
+                Eigen::MatrixXs D  = UDUT.singularValues().head(projected_size).asDiagonal();
+                Eigen::MatrixXs iD = UDUT.singularValues().head(projected_size).cwiseInverse().asDiagonal();
+                Eigen::MatrixXs U  = UDUT.matrixU().leftCols(projected_size).real();
+
+                if (D.determinant() < 1e-9)
+                    std::cout << "Projected Lambda is close to be ill-defined. Det(D) = " << D.determinant() << std::endl << "Singular values: " << UDUT.singularValues().transpose() << std::endl;
+
+                // topologies vector
+                std::map<std::string, std::vector<unsigned int>> topologies;
+                std::map<std::string, Eigen::MatrixXs> Lambdas_approx;
+                std::map<std::string, Scalar> KLDs;
+
+                // COMPUTE TOPOLOGY
+                ConstraintBaseList new_ctrs = computeTopology(markov_blanket, Lambda, mean, iD, U, sb_2_col, fr_2_col, options_map.begin()->second, options_map.begin()->second.optim_options_, sensor_ptr);
+
+                // SPARSIFICATION
+                // std::cout << "SPARSIFICATION" << std::endl;
+                std::map<ConstraintBasePtr, unsigned int> new_ctr_2_row;
+                unsigned int new_meas_size = computeConstraintMap(new_ctrs, new_ctr_2_row);
+
+                Eigen::SparseMatrixs J(new_meas_size, markov_size);
+                computeJacobians(new_ctrs, sb_2_col, new_ctr_2_row, mean, J);
+                Eigen::MatrixXs JU = J*U;
+
+                // Rank
+                Eigen::FullPivLU<Eigen::MatrixXs> JU_decomp(JU);
+
+                // Only optimization cases
+                if (JU_decomp.rank() != JU.rows())
+                    // COMPARE SPARSIFICATIONS
+                    for (auto& option_pair : options_map)
+                    {
+                        if (option_pair.second.gamma_topology_ < 1.1 && option_pair.second.topology_size_ == N_CLT)
+                            std::cout << option_pair.first << " is not solved in closed form... JU rank: " << JU_decomp.rank() << "JU.rows() " << JU.rows() << std::endl;
+
+                        Eigen::MatrixXs logs = solveConstrainedOptimization(new_ctrs, Lambda, J, JU, iD, new_ctr_2_row, fr_2_col, option_pair.second.optim_options_, true);
+
+                        convergence_logfiles[option_pair.first] << markov_nodes << "\n" << logs.row(0) << "\n" << logs.row(1) << std::endl; // store logs
+                    }
+
+                // REMOVE NODES
+                //std::cout << "remove nodes...\n";
+                for (auto fr : markov_blanket)
+                    fr->remove();
+
+                // stop
+                N_problems++;
+                if (N_problems == N_problems_desired)
+                    break;
+
+                // report
+                if (N_problems % 10 == 0)
+                    std::cout << N_problems << " problems solved\n";
+            }
+        }
+        // CLOSE LOGFILES
+        for (auto& log_pair : convergence_logfiles)
+            log_pair.second.close();
+
+        //End message
+        std::cout << std::endl << " ===== Pruning ratio " << T_str << " report: =====" << std::endl << std::endl;
+        std::cout << "\tProblems:\n\t\t[total]\t\t" << N_problems << std::endl;
+    }
+
+    auto end = std::chrono::system_clock::now();
+    std::chrono::duration<double> elapsed_seconds = end-start;
+    std::time_t end_time = std::chrono::system_clock::to_time_t(end);
+    std::cout << " =========================== END ===============================" << std::endl << std::endl;
+    std::cout << "Finished at " << std::ctime(&end_time)
+              << "elapsed time: " << int(int(elapsed_seconds.count()) / 3600) << "h " << int((int(elapsed_seconds.count()) % 3600) / 60) << "min " << int(elapsed_seconds.count()) % 60 << "s - ("<< elapsed_seconds.count() << "s)\n";
+
+    //exit
+    return 0;
+}
diff --git a/src/examples/test_sparsification_topology.cpp b/src/examples/test_sparsification_topology.cpp
index a44dba9ced593477137980df068512fa3d81f77d..8c7eba60c085e5ada05de125d3b4cf731c3f40fe 100644
--- a/src/examples/test_sparsification_topology.cpp
+++ b/src/examples/test_sparsification_topology.cpp
@@ -63,10 +63,12 @@ bool isIncludedIn(const std::vector<unsigned int>& A, const std::vector<unsigned
 
 void print_wrong_input()
 {
-    std::cout << "Please call me with: [./test_sparsification_topology DATASET T N], where:" << std::endl;
+    std::cout << "Please call me with: [./test_sparsification_topology DATASET T N (folder run_best)], where:" << std::endl;
     std::cout << "    DATASET: manhattan, killian or intel" << std::endl;
     std::cout << "    T pruning period: ALL, all, c_all, 3, c_3, 4, c_4, 5, c_5, 10 or c_10" << std::endl;
     std::cout << "    N amount of problems (0: all)" << std::endl;
+    std::cout << "    folder where to put the logfiles (default: empty) " << std::endl;
+    std::cout << "    run_best if BEST topologies should be computed (default: true)" << std::endl;
     std::cout << "EXIT due to bad user input" << std::endl << std::endl;
 }
 
@@ -110,8 +112,11 @@ int main(int argc, char** argv)
         return -1;
     }
     std::string folder;
-    if (argc == 5)
+    if (argc > 4)
         folder = std::string(argv[4]) + std::string("/");
+    bool run_BEST = true;
+    if (argc > 5)
+        run_BEST = atoi(argv[5]) != 0;
 
     // input variables
     char const * dataset_path = std::getenv("DATASET_PATH");
@@ -147,7 +152,6 @@ int main(int argc, char** argv)
         //                   name                      dataset  pruning    solve Method Size   Gamma method    guess    connected (max_iter  max_time  min_gradient  Hessian   init_rho rho_decrease min_gradient_rho cyclic apply_closed_form formulation)
         options_map.emplace("CLT_MI",   PruningOptions(dataset, 0,         true, MI,    N_CLT, 1.0,  NO_OPTIM, NO_INIT, false));
         options_map.emplace("CLT_TD2",  PruningOptions(dataset, 0,         true, TD2,   N_CLT, 1.0,  NO_OPTIM, NO_INIT, false));
-        options_map.emplace("CLT_BEST", PruningOptions(dataset, 0,         true, BEST,  N_CLT, 1.0,  NO_OPTIM, NO_INIT, false));
 
         options_map.emplace("SG_MI",    PruningOptions(dataset, 0,         true, MI,    N_CLT, 1.5,  FD,       ODB,     false,     max_iter, max_time, 0.001,        ANALYTIC, 0.0,     0.0,         0.0,             true,  true,             1));
         options_map.emplace("SG_MI2",   PruningOptions(dataset, 0,         true, MI2,   N_CLT, 1.5,  FD,       ODB,     false,     max_iter, max_time, 0.001,        ANALYTIC, 0.0,     0.0,         0.0,             true,  true,             1));
@@ -156,13 +160,25 @@ int main(int argc, char** argv)
         options_map.emplace("SG_TD2",   PruningOptions(dataset, 0,         true, TD2,   N_CLT, 1.5,  FD,       ODB,     false,     max_iter, max_time, 0.001,        ANALYTIC, 0.0,     0.0,         0.0,             true,  true,             1));
         //options_map.emplace("SG_MOR",   PruningOptions(dataset, 0,         true, MOR,   N_CLT, 1.5,  FD,       ODB,     false,     max_iter, max_time, 0.001,        ANALYTIC, 0.0,     0.0,         0.0,             true,  true,             1));
         options_map.emplace("SG_OD1",   PruningOptions(dataset, 0,         true, OD1,   N_CLT, 1.5,  FD,       ODB,     false,     max_iter, max_time, 0.001,        ANALYTIC, 0.0,     0.0,         0.0,             true,  true,             1));
-        //options_map.emplace("SG_OD2",   PruningOptions(dataset, 0,         true, OD2,   N_CLT, 1.5,  FD,       ODB,     false,     max_iter, max_time, 0.001,        ANALYTIC, 0.0,     0.0,         0.0,             true,  true,             1));
+        options_map.emplace("SG_OD2",   PruningOptions(dataset, 0,         true, OD2,   N_CLT, 1.5,  FD,       ODB,     false,     max_iter, max_time, 0.001,        ANALYTIC, 0.0,     0.0,         0.0,             true,  true,             1));
         options_map.emplace("SG_OD3",   PruningOptions(dataset, 0,         true, OD3,   N_CLT, 1.5,  FD,       ODB,     false,     max_iter, max_time, 0.001,        ANALYTIC, 0.0,     0.0,         0.0,             true,  true,             1));
-        options_map.emplace("SG_BEST",  PruningOptions(dataset, 0,         true, BEST,  N_CLT, 1.5,  FD,       ODB,     false,     max_iter, max_time, 0.001,        ANALYTIC, 0.0,     0.0,         0.0,             true,  true,             1));
 
         options_map.emplace("SGF_MI",   PruningOptions(dataset, 0,         true, MI,    N_FILL,0.75, FD,       ODB,     false,     max_iter, max_time, 0.001,        ANALYTIC, 0.0,     0.0,         0.0,             true,  true,             1));
+        options_map.emplace("SGF_MI2",  PruningOptions(dataset, 0,         true, MI2,   N_FILL,0.75, FD,       ODB,     false,     max_iter, max_time, 0.001,        ANALYTIC, 0.0,     0.0,         0.0,             true,  true,             1));
+        options_map.emplace("SGF_MI3",  PruningOptions(dataset, 0,         true, MI3,   N_FILL,0.75, FD,       ODB,     false,     max_iter, max_time, 0.001,        ANALYTIC, 0.0,     0.0,         0.0,             true,  true,             1));
         options_map.emplace("SGF_TD1",  PruningOptions(dataset, 0,         true, TD1,   N_FILL,0.75, FD,       ODB,     false,     max_iter, max_time, 0.001,        ANALYTIC, 0.0,     0.0,         0.0,             true,  true,             1));
-        options_map.emplace("SGF_BEST", PruningOptions(dataset, 0,         true, BEST,  N_FILL,0.75, FD,       ODB,     false,     max_iter, max_time, 0.001,        ANALYTIC, 0.0,     0.0,         0.0,             true,  true,             1));
+        options_map.emplace("SGF_TD2",  PruningOptions(dataset, 0,         true, TD2,   N_FILL,0.75, FD,       ODB,     false,     max_iter, max_time, 0.001,        ANALYTIC, 0.0,     0.0,         0.0,             true,  true,             1));
+        //options_map.emplace("SGF_MOR",  PruningOptions(dataset, 0,         true, MOR,   N_FILL,0.75, FD,       ODB,     false,     max_iter, max_time, 0.001,        ANALYTIC, 0.0,     0.0,         0.0,             true,  true,             1));
+        options_map.emplace("SGF_OD1",  PruningOptions(dataset, 0,         true, OD1,   N_FILL,0.75, FD,       ODB,     false,     max_iter, max_time, 0.001,        ANALYTIC, 0.0,     0.0,         0.0,             true,  true,             1));
+        options_map.emplace("SGF_OD2",  PruningOptions(dataset, 0,         true, OD2,   N_FILL,0.75, FD,       ODB,     false,     max_iter, max_time, 0.001,        ANALYTIC, 0.0,     0.0,         0.0,             true,  true,             1));
+        options_map.emplace("SGF_OD3",  PruningOptions(dataset, 0,         true, OD3,   N_FILL,0.75, FD,       ODB,     false,     max_iter, max_time, 0.001,        ANALYTIC, 0.0,     0.0,         0.0,             true,  true,             1));
+
+        if (run_BEST)
+        {
+            options_map.emplace("CLT_BEST", PruningOptions(dataset, 0,         true, BEST,  N_CLT, 1.0,  NO_OPTIM, NO_INIT, false));
+            options_map.emplace("SG_BEST",  PruningOptions(dataset, 0,         true, BEST,  N_CLT, 1.5,  FD,       ODB,     false,     max_iter, max_time, 0.001,        ANALYTIC, 0.0,     0.0,         0.0,             true,  true,             1));
+            options_map.emplace("SGF_BEST", PruningOptions(dataset, 0,         true, BEST,  N_FILL,0.75, FD,       ODB,     false,     max_iter, max_time, 0.001,        ANALYTIC, 0.0,     0.0,         0.0,             true,  true,             1));
+        }
 
         unsigned int N_options = options_map.size();
 
@@ -192,11 +208,17 @@ int main(int argc, char** argv)
         if (!analysis_logfile.is_open())
             std::cout << std::endl << "Failed to open the analysis file " << std::endl;
 
+        // Example
+        std::ofstream example_logfile(root_path + "/results/" + folder + T_str + std::string("_example.txt"));
+        if (!example_logfile.is_open())
+            std::cout << std::endl << "Failed to open the analysis file " << std::endl;
+
         // evaluation variables
         unsigned int N_problems(0), N_CLT_BEST_is_CLT_MI(0), N_CLT_BEST_is_CLT_TD(0), N_SG_BEST_contains_CLT_MI(0), N_SG_BEST_contains_CLT_TD(0), N_SG_BEST_contains_MostMI(0), N_SG_BEST_contains_LeastMI(0);
         std::map<std::string, unsigned int> N_SG_BEST_is_option;
         std::vector<unsigned int> N_SG_BEST_contains_CLT_MI_MB, N_SG_BEST_contains_CLT_TD_MB, N_SG_BEST_contains_MostMI_MB, N_SG_BEST_contains_LeastMI_MB, N_problems_MB;
 
+
         ////////////////////////////////////////////////////////////////////////
         // EXPERIMENT : Solving all original problems using different topologies
         int cols = 0, rows = 0;
@@ -262,23 +284,15 @@ int main(int argc, char** argv)
 
                 // topologies vector
                 std::map<std::string, std::vector<unsigned int>> topologies;
+                std::map<std::string, Eigen::MatrixXs> Lambdas_approx;
+                std::map<std::string, Scalar> KLDs;
 
                 // COMPARE TOPOLOGIES
                 for (auto& option_pair : options_map)
                 {
-                //for (auto t = 0; t<N_options; t++)
-                //{
-    //                if (markov_nodes >= 8 && ( t== 2 ||t == N_options-1))
-    //                {
-    //                    std::cout << "not performing BEST topologies since too many combinations\n";
-    //                    KLD_logfiles[t] << -1 << " ";
-    //                    t_logfiles[t] << -1 << " ";
-    //                    continue;
-    //                }
-
                     // COMPUTE TOPOLOGY
                     // reduce the optimization weight just for computing topology (it only has sense for BEST)
-                    option_pair.second.optim_options_.max_iter_ = 10*markov_nodes;
+                    option_pair.second.optim_options_.max_iter_ = 100*markov_nodes;
 
                     clock_t t1 = clock();
                     ConstraintBaseList new_ctrs = computeTopology(markov_blanket, Lambda, mean, iD, U, sb_2_col, fr_2_col, option_pair.second, option_pair.second.optim_options_, sensor_ptr);
@@ -306,8 +320,6 @@ int main(int argc, char** argv)
                             Eigen::MatrixXs inv_omega_k = JU.middleRows(new_ctr_2_row.at(ctr), ctr->getSize()) *
                                                           iD *
                                                           JU.middleRows(new_ctr_2_row.at(ctr), ctr->getSize()).transpose();
-                            //std::cout << "det(inv_omega_k) = " << inv_omega_k.determinant() << std::endl << "inv_omega_k:\n" << inv_omega_k << std::endl;
-
                             // Set information matrix
                             setSPD(inv_omega_k);
                             ctr->getFeaturePtr()->setMeasurementCovariance(inv_omega_k);
@@ -336,86 +348,113 @@ int main(int argc, char** argv)
                     KLD_logfiles[option_pair.first] << KLD << " ";
                     t_logfiles[option_pair.first] << t_topology << " ";
 
-                    // STORE TOPOLOGY INDEXES
+                    // STORE TOPOLOGY INDEXES and LAMBDA APPROX
                     for (auto ctr : new_ctrs)
+                    {
                         topologies[option_pair.first].push_back(100*ctr->getCapturePtr()->getFramePtr()->getTimeStamp().get()+ctr->getFrameOtherPtr()->getTimeStamp().get());
-
+                        Lambdas_approx[option_pair.first] = Eigen::MatrixXs(A_approx.transpose() * A_approx);
+                        KLDs[option_pair.first] = KLD;
+                    }
                 }
 
                 // ANALYSIS ///////////////////////////////////////////////////////////////
                 // Resize vectors MB
-                while (N_SG_BEST_contains_CLT_MI_MB.size() <= markov_nodes)
-                    N_SG_BEST_contains_CLT_MI_MB.push_back(0);
-                while (N_SG_BEST_contains_CLT_TD_MB.size() <= markov_nodes)
-                    N_SG_BEST_contains_CLT_TD_MB.push_back(0);
-                while (N_SG_BEST_contains_MostMI_MB.size() <= markov_nodes)
-                    N_SG_BEST_contains_MostMI_MB.push_back(0);
-                while (N_SG_BEST_contains_LeastMI_MB.size() <= markov_nodes)
-                    N_SG_BEST_contains_LeastMI_MB.push_back(0);
                 while (N_problems_MB.size() <= markov_nodes)
                     N_problems_MB.push_back(0);
 
-                // CLT-BEST is CLT-MI
-                if (isIncludedIn(topologies["CLT_MI"],topologies["CLT_BEST"]))
-                    N_CLT_BEST_is_CLT_MI++;
-
-                // CLT-BEST is CLT-TD2
-                if (isIncludedIn(topologies["CLT_TD2"],topologies["CLT_BEST"]))
-                    N_CLT_BEST_is_CLT_TD++;
-
-                // SG-BEST includes CLT-MI [total & MB]
-                if (isIncludedIn(topologies["CLT_MI"],topologies["SG_BEST"]))
+                if (run_BEST)
                 {
-                    N_SG_BEST_contains_CLT_MI++;
-                    N_SG_BEST_contains_CLT_MI_MB[markov_nodes]++;
-                }
-
-                // SG-BEST includes CLT-TD [total & MB]
-                if (isIncludedIn(topologies["CLT_TD2"],topologies["SG_BEST"]))
-                {
-                    N_SG_BEST_contains_CLT_TD++;
-                    N_SG_BEST_contains_CLT_TD_MB[markov_nodes]++;
-                }
+                    while (N_SG_BEST_contains_CLT_MI_MB.size() <= markov_nodes)
+                        N_SG_BEST_contains_CLT_MI_MB.push_back(0);
+                    while (N_SG_BEST_contains_CLT_TD_MB.size() <= markov_nodes)
+                        N_SG_BEST_contains_CLT_TD_MB.push_back(0);
+                    while (N_SG_BEST_contains_MostMI_MB.size() <= markov_nodes)
+                        N_SG_BEST_contains_MostMI_MB.push_back(0);
+                    while (N_SG_BEST_contains_LeastMI_MB.size() <= markov_nodes)
+                        N_SG_BEST_contains_LeastMI_MB.push_back(0);
+
+                    // CLT-BEST is CLT-MI
+                    if (isIncludedIn(topologies["CLT_MI"],topologies["CLT_BEST"]))
+                        N_CLT_BEST_is_CLT_MI++;
+
+                    // CLT-BEST is CLT-TD2
+                    if (isIncludedIn(topologies["CLT_TD2"],topologies["CLT_BEST"]))
+                        N_CLT_BEST_is_CLT_TD++;
+
+                    // SG-BEST includes CLT-MI [total & MB]
+                    if (isIncludedIn(topologies["CLT_MI"],topologies["SG_BEST"]))
+                    {
+                        N_SG_BEST_contains_CLT_MI++;
+                        N_SG_BEST_contains_CLT_MI_MB[markov_nodes]++;
+                    }
 
-                // SG-BEST is SG-option
-                for (auto& option_pair : options_map)
-                    if (option_pair.first.compare("CLT_MI") != 0 &&
-                        option_pair.first.compare("CLT_TD2") != 0 &&
-                        option_pair.first.compare("CLT_BEST") != 0 &&
-                        option_pair.first.compare("SG_BEST") != 0 )
-                        if (isIncludedIn(topologies[option_pair.first],topologies["SG_BEST"]))
-                            N_SG_BEST_is_option[option_pair.first]++;
-
-                // SG-BEST includes Most MI & Least MI [total & MB]
-                Scalar most_MI(-1e12), least_MI(1e12);
-                unsigned int most_MI_factor, least_MI_factor;
-                Eigen::MatrixXs Sigma_Tik = (Lambda + Eigen::MatrixXs::Identity(markov_size,markov_size)).inverse();
-                for (auto fr_it1 = markov_blanket.begin(); fr_it1 != markov_blanket.end(); fr_it1++)
-                    for (auto fr_it2 = std::next(fr_it1); fr_it2 != markov_blanket.end(); fr_it2++)
+                    // SG-BEST includes CLT-TD [total & MB]
+                    if (isIncludedIn(topologies["CLT_TD2"],topologies["SG_BEST"]))
                     {
-                        ConstraintBasePtr ctr = createRelative2DConstraint(*fr_it1, *fr_it2, mean, fr_2_col, Eigen::Matrix3s::Identity(), sensor_ptr);
-                        Scalar MI = computeMutualInformation(Sigma_Tik, fr_2_col, ctr);
-                        if (MI > most_MI)
-                        {
-                            most_MI = MI;
-                            most_MI_factor = 100*ctr->getCapturePtr()->getFramePtr()->getTimeStamp().get()+ctr->getFrameOtherPtr()->getTimeStamp().get();
-                        }
-                        if (MI < least_MI)
+                        N_SG_BEST_contains_CLT_TD++;
+                        N_SG_BEST_contains_CLT_TD_MB[markov_nodes]++;
+                    }
+
+                    // SG-BEST is SG-option
+                    for (auto& option_pair : options_map)
+                        if (option_pair.first.compare("CLT_MI") != 0 &&
+                            option_pair.first.compare("CLT_TD2") != 0 &&
+                            option_pair.first.compare("CLT_BEST") != 0 &&
+                            option_pair.first.compare("SG_BEST") != 0 )
+                            if (isIncludedIn(topologies[option_pair.first],topologies["SG_BEST"]))
+                                N_SG_BEST_is_option[option_pair.first]++;
+
+                    // SG-BEST includes Most MI & Least MI [total & MB]
+                    Scalar most_MI(-1e12), least_MI(1e12);
+                    unsigned int most_MI_factor, least_MI_factor;
+                    Eigen::MatrixXs Sigma_Tik = (Lambda + Eigen::MatrixXs::Identity(markov_size,markov_size)).inverse();
+                    for (auto fr_it1 = markov_blanket.begin(); fr_it1 != markov_blanket.end(); fr_it1++)
+                        for (auto fr_it2 = std::next(fr_it1); fr_it2 != markov_blanket.end(); fr_it2++)
                         {
-                            least_MI = MI;
-                            least_MI_factor = 100*ctr->getCapturePtr()->getFramePtr()->getTimeStamp().get()+ctr->getFrameOtherPtr()->getTimeStamp().get();
+                            ConstraintBasePtr ctr = createRelative2DConstraint(*fr_it1, *fr_it2, mean, fr_2_col, Eigen::Matrix3s::Identity(), sensor_ptr);
+                            Scalar MI = computeMutualInformation(Sigma_Tik, fr_2_col, ctr);
+                            if (MI > most_MI)
+                            {
+                                most_MI = MI;
+                                most_MI_factor = 100*ctr->getCapturePtr()->getFramePtr()->getTimeStamp().get()+ctr->getFrameOtherPtr()->getTimeStamp().get();
+                            }
+                            if (MI < least_MI)
+                            {
+                                least_MI = MI;
+                                least_MI_factor = 100*ctr->getCapturePtr()->getFramePtr()->getTimeStamp().get()+ctr->getFrameOtherPtr()->getTimeStamp().get();
+                            }
                         }
-                    }
 
-                if (std::find(topologies["SG_BEST"].begin(), topologies["SG_BEST"].end(), most_MI_factor) != topologies["SG_BEST"].end())
-                {
-                    N_SG_BEST_contains_MostMI++;
-                    N_SG_BEST_contains_MostMI_MB[markov_nodes]++;
+                    if (std::find(topologies["SG_BEST"].begin(), topologies["SG_BEST"].end(), most_MI_factor) != topologies["SG_BEST"].end())
+                    {
+                        N_SG_BEST_contains_MostMI++;
+                        N_SG_BEST_contains_MostMI_MB[markov_nodes]++;
+                    }
+                    if (std::find(topologies["SG_BEST"].begin(), topologies["SG_BEST"].end(), least_MI_factor) != topologies["SG_BEST"].end())
+                    {
+                        N_SG_BEST_contains_LeastMI++;
+                        N_SG_BEST_contains_LeastMI_MB[markov_nodes]++;
+                    }
                 }
-                if (std::find(topologies["SG_BEST"].begin(), topologies["SG_BEST"].end(), least_MI_factor) != topologies["SG_BEST"].end())
+
+                // STORE EXAMPLE
+                if (markov_nodes == 5 &&
+                    KLDs["SGF_MI"]  > KLDs["SGF_MI2"] &&
+                    KLDs["SGF_MI"]  > KLDs["SGF_MI3"] &&
+                    KLDs["SGF_MI3"] > KLDs["SGF_TD1"] &&
+                    KLDs["SGF_MI3"] > KLDs["SGF_OD3"] &&
+                    KLDs["SGF_OD3"] > KLDs["SGF_BEST"])
                 {
-                    N_SG_BEST_contains_LeastMI++;
-                    N_SG_BEST_contains_LeastMI_MB[markov_nodes]++;
+                    example_logfile << "-------------- Lambda:" << std::endl << Lambda << std::endl;
+                    for (auto& option_pair : options_map)
+                    {
+                        example_logfile << option_pair.first << std::endl;
+                        example_logfile << "topology" << std::endl;
+                        for (auto factor : topologies[option_pair.first])
+                            example_logfile << factor << std::endl;
+                        example_logfile << "KLD" << std::endl << KLDs[option_pair.first] << std::endl;
+                        example_logfile << "Lambda_approx:" << std::endl << Lambdas_approx[option_pair.first] << std::endl;
+                    }
                 }
 
                 // REMOVE NODES
@@ -438,33 +477,35 @@ int main(int argc, char** argv)
             }
         }
         // STORE ANALYSIS
-        auto N_MB = N_SG_BEST_contains_CLT_MI_MB.size()-1;
+        auto N_MB = N_problems_MB.size()-1;
         analysis_logfile << "Problems:\n\t[total]\t" << N_problems << std::endl;
-        for (auto MB = 4; MB <= N_MB; MB++)
+        for (auto MB = 3; MB <= N_MB; MB++)
             analysis_logfile << "\t[" << MB << "]\t\t" << N_problems_MB[MB] << "\t: " << (100.*N_problems_MB[MB])/N_problems << "%" << std::endl;
 
-        analysis_logfile << "SG-BEST contains CLT-MI:\n\t[total]\t" << N_SG_BEST_contains_CLT_MI << "\t: " << (100.*N_SG_BEST_contains_CLT_MI)/N_problems << "%" << std::endl;
-        for (auto MB = 4; MB <= N_MB; MB++)
-            analysis_logfile << "\t[" << MB << "]\t\t" << N_SG_BEST_contains_CLT_MI_MB[MB] << "\t: " << (100.*N_SG_BEST_contains_CLT_MI_MB[MB])/N_problems_MB[MB] << "%" << std::endl;
-
-        analysis_logfile << "SG-BEST contains CLT-TD:\n\t[total]\t" << N_SG_BEST_contains_CLT_TD << "\t: " << (100.*N_SG_BEST_contains_CLT_TD)/N_problems << "%" << std::endl;
-        for (auto MB = 4; MB <= N_MB; MB++)
-            analysis_logfile << "\t[" << MB << "]\t\t" << N_SG_BEST_contains_CLT_TD_MB[MB] << "\t: " << (100.*N_SG_BEST_contains_CLT_TD_MB[MB])/N_problems_MB[MB] << "%" << std::endl;
+        if (run_BEST)
+        {
+            analysis_logfile << "SG-BEST contains CLT-MI:\n\t[total]\t" << N_SG_BEST_contains_CLT_MI << "\t: " << (100.*N_SG_BEST_contains_CLT_MI)/N_problems << "%" << std::endl;
+            for (auto MB = 3; MB <= N_MB; MB++)
+                analysis_logfile << "\t[" << MB << "]\t\t" << N_SG_BEST_contains_CLT_MI_MB[MB] << "\t: " << (100.*N_SG_BEST_contains_CLT_MI_MB[MB])/N_problems_MB[MB] << "%" << std::endl;
 
-        analysis_logfile << "SG-BEST contains Most MI:\n\t[total]\t" << N_SG_BEST_contains_MostMI << "\t: " << (100.*N_SG_BEST_contains_MostMI)/N_problems << "%" << std::endl;
-        for (auto MB = 4; MB <= N_MB; MB++)
-            analysis_logfile << "\t[" << MB << "]\t\t" << N_SG_BEST_contains_MostMI_MB[MB] << "\t: " << (100.*N_SG_BEST_contains_MostMI_MB[MB])/N_problems_MB[MB] << "%" << std::endl;
+            analysis_logfile << "SG-BEST contains CLT-TD:\n\t[total]\t" << N_SG_BEST_contains_CLT_TD << "\t: " << (100.*N_SG_BEST_contains_CLT_TD)/N_problems << "%" << std::endl;
+            for (auto MB = 3; MB <= N_MB; MB++)
+                analysis_logfile << "\t[" << MB << "]\t\t" << N_SG_BEST_contains_CLT_TD_MB[MB] << "\t: " << (100.*N_SG_BEST_contains_CLT_TD_MB[MB])/N_problems_MB[MB] << "%" << std::endl;
 
-        analysis_logfile << "SG-BEST contains Least MI:\n\t[total]\t" << N_SG_BEST_contains_LeastMI << "\t: " << (100.*N_SG_BEST_contains_LeastMI)/N_problems << "%" << std::endl;
-        for (auto MB = 4; MB <= N_MB; MB++)
-            analysis_logfile << "\t[" << MB << "]\t\t" << N_SG_BEST_contains_LeastMI_MB[MB] << "\t: " << (100.*N_SG_BEST_contains_LeastMI_MB[MB])/N_problems_MB[MB] << "%" << std::endl;
+            analysis_logfile << "SG-BEST contains Most MI:\n\t[total]\t" << N_SG_BEST_contains_MostMI << "\t: " << (100.*N_SG_BEST_contains_MostMI)/N_problems << "%" << std::endl;
+            for (auto MB = 3; MB <= N_MB; MB++)
+                analysis_logfile << "\t[" << MB << "]\t\t" << N_SG_BEST_contains_MostMI_MB[MB] << "\t: " << (100.*N_SG_BEST_contains_MostMI_MB[MB])/N_problems_MB[MB] << "%" << std::endl;
 
-        analysis_logfile << "CLT-BEST = CLT-MI:\t" << N_CLT_BEST_is_CLT_MI << "\t: " << (100.*N_CLT_BEST_is_CLT_MI)/N_problems << "%" << std::endl;
-        analysis_logfile << "CLT-BEST = CLT-TD:\t" << N_CLT_BEST_is_CLT_TD << "\t: " << (100.*N_CLT_BEST_is_CLT_TD)/N_problems << "%" << std::endl;
+            analysis_logfile << "SG-BEST contains Least MI:\n\t[total]\t" << N_SG_BEST_contains_LeastMI << "\t: " << (100.*N_SG_BEST_contains_LeastMI)/N_problems << "%" << std::endl;
+            for (auto MB = 3; MB <= N_MB; MB++)
+                analysis_logfile << "\t[" << MB << "]\t\t" << N_SG_BEST_contains_LeastMI_MB[MB] << "\t: " << (100.*N_SG_BEST_contains_LeastMI_MB[MB])/N_problems_MB[MB] << "%" << std::endl;
 
-        for (auto& pair : N_SG_BEST_is_option)
-            analysis_logfile << "SG-BEST = " << pair.first << ":\t" << pair.second << "\t: " << (100.*pair.second)/N_problems << "%" << std::endl;
+            analysis_logfile << "CLT-BEST = CLT-MI:\t" << N_CLT_BEST_is_CLT_MI << "\t: " << (100.*N_CLT_BEST_is_CLT_MI)/N_problems << "%" << std::endl;
+            analysis_logfile << "CLT-BEST = CLT-TD:\t" << N_CLT_BEST_is_CLT_TD << "\t: " << (100.*N_CLT_BEST_is_CLT_TD)/N_problems << "%" << std::endl;
 
+            for (auto& pair : N_SG_BEST_is_option)
+                analysis_logfile << "SG-BEST = " << pair.first << ":\t" << pair.second << "\t: " << (100.*pair.second)/N_problems << "%" << std::endl;
+        }
 
         // CLOSE LOGFILES
         for (auto& log_pair : KLD_logfiles)
@@ -473,34 +514,38 @@ int main(int argc, char** argv)
             log_pair.second.close();
         MB_sizes_logfile.close();
         analysis_logfile.close();
+        example_logfile.close();
 
         //End message
         std::cout << std::endl << " ===== Pruning ratio " << T_str << " report: =====" << std::endl << std::endl;
         std::cout << "\tProblems:\n\t\t[total]\t\t" << N_problems << std::endl;
-        for (auto MB = 4; MB <= N_MB; MB++)
+        for (auto MB = 3; MB <= N_MB; MB++)
                 std::cout << "\t\t[" << MB << "]\t\t" << N_problems_MB[MB] << "\t: " << (100.*N_problems_MB[MB])/N_problems << "%" << std::endl;
 
-        std::cout << "\tSG-BEST contains CLT-MI:\n\t\t[total]\t\t" << N_SG_BEST_contains_CLT_MI << "\t: " << (100.*N_SG_BEST_contains_CLT_MI)/N_problems << "%" << std::endl;
-        for (auto MB = 4; MB <= N_MB; MB++)
-            std::cout << "\t\t[" << MB << "]\t\t" << N_SG_BEST_contains_CLT_MI_MB[MB] << "\t: " << (100.*N_SG_BEST_contains_CLT_MI_MB[MB])/N_problems_MB[MB] << "%" << std::endl;
+        if (run_BEST)
+        {
+            std::cout << "\tSG-BEST contains CLT-MI:\n\t\t[total]\t\t" << N_SG_BEST_contains_CLT_MI << "\t: " << (100.*N_SG_BEST_contains_CLT_MI)/N_problems << "%" << std::endl;
+            for (auto MB = 3; MB <= N_MB; MB++)
+                std::cout << "\t\t[" << MB << "]\t\t" << N_SG_BEST_contains_CLT_MI_MB[MB] << "\t: " << (100.*N_SG_BEST_contains_CLT_MI_MB[MB])/N_problems_MB[MB] << "%" << std::endl;
 
-        std::cout << "\tSG-BEST contains CLT-TD:\n\t\t[total]\t\t" << N_SG_BEST_contains_CLT_TD << "\t: " << (100.*N_SG_BEST_contains_CLT_TD)/N_problems << "%" << std::endl;
-        for (auto MB = 4; MB <= N_MB; MB++)
-            std::cout << "\t\t[" << MB << "]\t\t" << N_SG_BEST_contains_CLT_TD_MB[MB] << "\t: " << (100.*N_SG_BEST_contains_CLT_TD_MB[MB])/N_problems_MB[MB] << "%" << std::endl;
+            std::cout << "\tSG-BEST contains CLT-TD:\n\t\t[total]\t\t" << N_SG_BEST_contains_CLT_TD << "\t: " << (100.*N_SG_BEST_contains_CLT_TD)/N_problems << "%" << std::endl;
+            for (auto MB = 3; MB <= N_MB; MB++)
+                std::cout << "\t\t[" << MB << "]\t\t" << N_SG_BEST_contains_CLT_TD_MB[MB] << "\t: " << (100.*N_SG_BEST_contains_CLT_TD_MB[MB])/N_problems_MB[MB] << "%" << std::endl;
 
-        std::cout << "\tSG-BEST contains Most MI:\n\t\t[total]\t\t" << N_SG_BEST_contains_MostMI << "\t: " << (100.*N_SG_BEST_contains_MostMI)/N_problems << "%" << std::endl;
-        for (auto MB = 4; MB <= N_MB; MB++)
-            std::cout << "\t\t[" << MB << "]\t\t" << N_SG_BEST_contains_MostMI_MB[MB] << "\t: " << (100.*N_SG_BEST_contains_MostMI_MB[MB])/N_problems_MB[MB] << "%" << std::endl;
+            std::cout << "\tSG-BEST contains Most MI:\n\t\t[total]\t\t" << N_SG_BEST_contains_MostMI << "\t: " << (100.*N_SG_BEST_contains_MostMI)/N_problems << "%" << std::endl;
+            for (auto MB = 3; MB <= N_MB; MB++)
+                std::cout << "\t\t[" << MB << "]\t\t" << N_SG_BEST_contains_MostMI_MB[MB] << "\t: " << (100.*N_SG_BEST_contains_MostMI_MB[MB])/N_problems_MB[MB] << "%" << std::endl;
 
-        std::cout << "\tSG-BEST contains Least MI:\n\t\t[total]\t\t" << N_SG_BEST_contains_LeastMI << "\t: " << (100.*N_SG_BEST_contains_LeastMI)/N_problems << "%" << std::endl;
-        for (auto MB = 4; MB <= N_MB; MB++)
-            std::cout << "\t\t[" << MB << "]\t\t" << N_SG_BEST_contains_LeastMI_MB[MB] << "\t: " << (100.*N_SG_BEST_contains_LeastMI_MB[MB])/N_problems_MB[MB] << "%" << std::endl;
+            std::cout << "\tSG-BEST contains Least MI:\n\t\t[total]\t\t" << N_SG_BEST_contains_LeastMI << "\t: " << (100.*N_SG_BEST_contains_LeastMI)/N_problems << "%" << std::endl;
+            for (auto MB = 3; MB <= N_MB; MB++)
+                std::cout << "\t\t[" << MB << "]\t\t" << N_SG_BEST_contains_LeastMI_MB[MB] << "\t: " << (100.*N_SG_BEST_contains_LeastMI_MB[MB])/N_problems_MB[MB] << "%" << std::endl;
 
-        std::cout << "\tCLT-BEST = CLT-MI:\t" << N_CLT_BEST_is_CLT_MI << "\t: " << (100.*N_CLT_BEST_is_CLT_MI)/N_problems << "%" << std::endl;
-        std::cout << "\tCLT-BEST = CLT-TD:\t" << N_CLT_BEST_is_CLT_TD << "\t: " << (100.*N_CLT_BEST_is_CLT_TD)/N_problems << "%" << std::endl;
+            std::cout << "\tCLT-BEST = CLT-MI:\t" << N_CLT_BEST_is_CLT_MI << "\t: " << (100.*N_CLT_BEST_is_CLT_MI)/N_problems << "%" << std::endl;
+            std::cout << "\tCLT-BEST = CLT-TD:\t" << N_CLT_BEST_is_CLT_TD << "\t: " << (100.*N_CLT_BEST_is_CLT_TD)/N_problems << "%" << std::endl;
 
-        for (auto& pair : N_SG_BEST_is_option)
-            std::cout << "\tSG-BEST = " << pair.first << ":\t" << pair.second << "\t: " << (100.*pair.second)/N_problems << "%" << std::endl;
+            for (auto& pair : N_SG_BEST_is_option)
+                std::cout << "\tSG-BEST = " << pair.first << ":\t" << pair.second << "\t: " << (100.*pair.second)/N_problems << "%" << std::endl;
+        }
     }
 
     auto end = std::chrono::system_clock::now();
diff --git a/src/pruning/factor_descent.h b/src/pruning/factor_descent.h
index 8445b8c67a84aece507e3896f3b6349934f388c2..58a62d5c4e3a3364622fa9f343892bd328aafd63 100644
--- a/src/pruning/factor_descent.h
+++ b/src/pruning/factor_descent.h
@@ -629,7 +629,7 @@ void topologyDescent(const Eigen::MatrixXs& J, const Eigen::MatrixXs& Sigma, con
                 prev_Omega_k = prev_Omega.block(ctr_pair_it->second,ctr_pair_it->second, ctr_pair_it->first->getSize(), ctr_pair_it->first->getSize());
                 assignSparseBlock(Omega_k,Omega,ctr_pair_it->second,ctr_pair_it->second);
                 Eigen::MatrixXs Lambda = J.transpose()*Omega*J;
-                if (Lambda.determinant() < 1e-9)
+                while (Lambda.determinant() < 1e-9)
                     Lambda += 0.1*Eigen::MatrixXs::Identity(sigma_size, sigma_size);
 
                 Scalar KLD = 0.5*((Lambda*Sigma).trace() - logdet(Lambda*Sigma, std::string("topologyDescent")) - sigma_size);
diff --git a/src/pruning/problem_pruning.cpp b/src/pruning/problem_pruning.cpp
index 8e13db7e4ba7d312e5a39448fe3b0e47f002392b..a05dbb322f0c9042e3c10dc249b44dd8694dce12 100644
--- a/src/pruning/problem_pruning.cpp
+++ b/src/pruning/problem_pruning.cpp
@@ -48,88 +48,6 @@ ProblemPruning::ProblemPruning(const std::string& _frame_structure, const std::s
         if (!originals_file_.is_open())
             std::cout << std::endl << "Failed to open the originals file " << filepath << std::endl;
     }
-
-    // comparison between all methods
-    if (options_.optim_options_.optim_metod_ == COMP)
-    {
-        // IP variants                                                                                                                                                        Hessian   init_rho rho_dec grad_rho cyclic closed_form formulation
-//        variants_options_.push_back(PruningOptimOptions(IP, ID,  options_.optim_options_.max_iter_, options_.optim_options_.max_time_, options_.optim_options_.min_gradient_, ANALYTIC, 1,       0.5,    10,       0,     false,      0));
-//        variants_names_.push_back(std::string("IP0-Id"));
-//        variants_options_.push_back(PruningOptimOptions(IP, ID,  options_.optim_options_.max_iter_, options_.optim_options_.max_time_, options_.optim_options_.min_gradient_, ANALYTIC, 1,       0.3,    10,       0,     false,      0));
-//        variants_names_.push_back(std::string("IP1-Id"));
-//        variants_options_.push_back(PruningOptimOptions(IP, ID,  options_.optim_options_.max_iter_, options_.optim_options_.max_time_, options_.optim_options_.min_gradient_, ANALYTIC, 1,       0.1,    10,       0,     false,      0));
-//        variants_names_.push_back(std::string("IP2-Id"));
-        variants_options_.push_back(PruningOptimOptions(IP, ODB, options_.optim_options_.max_iter_, options_.optim_options_.max_time_, options_.optim_options_.min_gradient_, ANALYTIC, 0.1,     0.5,    1,        0,     false,      1));
-        variants_names_.push_back(std::string("IP0-ODB"));
-//        variants_options_.push_back(PruningOptimOptions(IP, ODB, options_.optim_options_.max_iter_, options_.optim_options_.max_time_, options_.optim_options_.min_gradient_, ANALYTIC, 1,       0.5,    1,        0,     false,      1));
-//        variants_names_.push_back(std::string("IP1-ODB"));
-
-        // FD variants
-      //variants_options_.push_back(PruningOptimOptions(FD, ID,  options_.optim_options_.max_iter_, options_.optim_options_.max_time_, options_.optim_options_.min_gradient_, ANALYTIC, 0,       0,      0,       0,     false,      0));
-      //variants_names_.push_back(std::string("FD00-Id"));
-      //variants_options_.push_back(PruningOptimOptions(FD, ID,  options_.optim_options_.max_iter_, options_.optim_options_.max_time_, options_.optim_options_.min_gradient_, ANALYTIC, 0,       0,      0,       1,     false,      0));
-      //variants_names_.push_back(std::string("FD10-Id"));
-      //variants_options_.push_back(PruningOptimOptions(FD, ID,  options_.optim_options_.max_iter_, options_.optim_options_.max_time_, options_.optim_options_.min_gradient_, ANALYTIC, 0,       0,      0,       2,     false,      0));
-      //variants_names_.push_back(std::string("FD20-Id"));
-      //variants_options_.push_back(PruningOptimOptions(FD, ID,  options_.optim_options_.max_iter_, options_.optim_options_.max_time_, options_.optim_options_.min_gradient_, ANALYTIC, 0,       0,      0,       0,     false,      1));
-      //variants_names_.push_back(std::string("FD01-Id"));
-      //variants_options_.push_back(PruningOptimOptions(FD, ID,  options_.optim_options_.max_iter_, options_.optim_options_.max_time_, options_.optim_options_.min_gradient_, ANALYTIC, 0,       0,      0,       1,     false,      1));
-      //variants_names_.push_back(std::string("FD11-Id"));
-      //variants_options_.push_back(PruningOptimOptions(FD, ID,  options_.optim_options_.max_iter_, options_.optim_options_.max_time_, options_.optim_options_.min_gradient_, ANALYTIC, 0,       0,      0,       2,     false,      1));
-      //variants_names_.push_back(std::string("FD21-Id"));
-//        variants_options_.push_back(PruningOptimOptions(FD, ODB, options_.optim_options_.max_iter_, options_.optim_options_.max_time_, options_.optim_options_.min_gradient_, ANALYTIC, 0,       0,      0,       0,     false,      0));
-//        variants_names_.push_back(std::string("FD00-ODB"));
-//        variants_options_.push_back(PruningOptimOptions(FD, ODB, options_.optim_options_.max_iter_, options_.optim_options_.max_time_, options_.optim_options_.min_gradient_, ANALYTIC, 0,       0,      0,       1,     false,      0));
-//        variants_names_.push_back(std::string("FD10-ODB"));
-//        variants_options_.push_back(PruningOptimOptions(FD, ODB, options_.optim_options_.max_iter_, options_.optim_options_.max_time_, options_.optim_options_.min_gradient_, ANALYTIC, 0,       0,      0,       2,     false,      0));
-//        variants_names_.push_back(std::string("FD20-ODB"));
-//        variants_options_.push_back(PruningOptimOptions(FD, ODB, options_.optim_options_.max_iter_, options_.optim_options_.max_time_, options_.optim_options_.min_gradient_, ANALYTIC, 0,       0,      0,       0,     false,      1));
-//        variants_names_.push_back(std::string("FD01-ODB"));
-//        variants_options_.push_back(PruningOptimOptions(FD, ODB, options_.optim_options_.max_iter_, options_.optim_options_.max_time_, options_.optim_options_.min_gradient_, ANALYTIC, 0,       0,      0,       1,     false,      1));
-//        variants_names_.push_back(std::string("FD11-ODB"));
-//        variants_options_.push_back(PruningOptimOptions(FD, ODB, options_.optim_options_.max_iter_, options_.optim_options_.max_time_, options_.optim_options_.min_gradient_, ANALYTIC, 0,       0,      0,       2,     false,      1));
-//        variants_names_.push_back(std::string("FD21-ODB"));
-      //variants_options_.push_back(PruningOptimOptions(FD, ID,  options_.optim_options_.max_iter_, options_.optim_options_.max_time_, options_.optim_options_.min_gradient_, ANALYTIC, 0,       0,      0,       0,     true,       0));
-      //variants_names_.push_back(std::string("FD00cf-Id"));
-      //variants_options_.push_back(PruningOptimOptions(FD, ID,  options_.optim_options_.max_iter_, options_.optim_options_.max_time_, options_.optim_options_.min_gradient_, ANALYTIC, 0,       0,      0,       1,     true,       0));
-      //variants_names_.push_back(std::string("FD10cf-Id"));
-      //variants_options_.push_back(PruningOptimOptions(FD, ID,  options_.optim_options_.max_iter_, options_.optim_options_.max_time_, options_.optim_options_.min_gradient_, ANALYTIC, 0,       0,      0,       2,     true,       0));
-      //variants_names_.push_back(std::string("FD20cf-Id"));
-      //variants_options_.push_back(PruningOptimOptions(FD, ID,  options_.optim_options_.max_iter_, options_.optim_options_.max_time_, options_.optim_options_.min_gradient_, ANALYTIC, 0,       0,      0,       0,     true,       1));
-      //variants_names_.push_back(std::string("FD01cf-Id"));
-      //variants_options_.push_back(PruningOptimOptions(FD, ID,  options_.optim_options_.max_iter_, options_.optim_options_.max_time_, options_.optim_options_.min_gradient_, ANALYTIC, 0,       0,      0,       1,     true,       1));
-      //variants_names_.push_back(std::string("FD11cf-Id"));
-      //variants_options_.push_back(PruningOptimOptions(FD, ID,  options_.optim_options_.max_iter_, options_.optim_options_.max_time_, options_.optim_options_.min_gradient_, ANALYTIC, 0,       0,      0,       2,     true,       1));
-      //variants_names_.push_back(std::string("FD21cf-Id"));
-//        variants_options_.push_back(PruningOptimOptions(FD, ODB, options_.optim_options_.max_iter_, options_.optim_options_.max_time_, options_.optim_options_.min_gradient_, ANALYTIC, 0,       0,      0,       0,     true,       0));
-//        variants_names_.push_back(std::string("FD00cf-ODB"));
-//        variants_options_.push_back(PruningOptimOptions(FD, ODB, options_.optim_options_.max_iter_, options_.optim_options_.max_time_, options_.optim_options_.min_gradient_, ANALYTIC, 0,       0,      0,       1,     true,       0));
-//        variants_names_.push_back(std::string("FD10cf-ODB"));
-//        variants_options_.push_back(PruningOptimOptions(FD, ODB, options_.optim_options_.max_iter_, options_.optim_options_.max_time_, options_.optim_options_.min_gradient_, ANALYTIC, 0,       0,      0,       2,     true,       0));
-//        variants_names_.push_back(std::string("FD20cf-ODB"));
-        variants_options_.push_back(PruningOptimOptions(FD, ODB, options_.optim_options_.max_iter_, options_.optim_options_.max_time_, options_.optim_options_.min_gradient_, ANALYTIC, 0,       0,      0,       0,     true,       1));
-        variants_names_.push_back(std::string("FD01cf-ODB"));
-//        variants_options_.push_back(PruningOptimOptions(FD, ODB, options_.optim_options_.max_iter_, options_.optim_options_.max_time_, options_.optim_options_.min_gradient_, ANALYTIC, 0,       0,      0,       1,     true,       1));
-//        variants_names_.push_back(std::string("FD11cf-ODB"));
-        variants_options_.push_back(PruningOptimOptions(FD, ODB, options_.optim_options_.max_iter_, options_.optim_options_.max_time_, options_.optim_options_.min_gradient_, ANALYTIC, 0,       0,      0,       2,     true,       1));
-        variants_names_.push_back(std::string("FD21cf-ODB"));
-
-        log_files_.resize(variants_names_.size());
-
-        for (auto i = 0; i < variants_names_.size(); i++)
-        {
-            std::string filepath = getenv("DATASET_PATH") + std::string("/logfiles/") + options_.dataset_ + std::string("/") + variants_names_[i] + (options_.connected_ ? "_c" : "") + std::string("_log_file.txt");
-            //std::cout << "filepath:" << filepath << std::endl;
-            log_files_[i].open(filepath, std::ofstream::out); //open log file
-            if (!log_files_[i].is_open())
-                std::cout << std::endl << "Failed to open the log file " << filepath << std::endl;
-        }
-    }
-    else
-    {
-        variants_options_.push_back(options_.optim_options_);
-        variants_names_.push_back(name_);
-    }
 }
 
 ProblemPruningPtr ProblemPruning::create(const std::string& _frame_structure, const std::string& _name, const PruningOptions& _options) // USE THIS AS A CONSTRUCTOR!
@@ -177,9 +95,6 @@ ProblemPruning::~ProblemPruning()
         // Close file
         problems_file_.close();
     }
-    if (options_.optim_options_.optim_metod_ == COMP)
-        for (auto i = 0; i < log_files_.size(); i++)
-            log_files_[i].close(); //close log files
 
     // close original distributions file
     if (options_.pruning_T_ != 0)
@@ -410,14 +325,19 @@ Scalar ProblemPruning::computeKLD(const Eigen::MatrixXs& _Sigma_bl, const Eigen:
     // RMSE
     RMSEs_.push_back(std::sqrt( error.squaredNorm() / (_mean_bl.size() / 3)));
 
+    // N factors
+    ConstraintBaseList all_ctr;
+    trajectory_ptr_->getConstraintList(all_ctr);
+
     // print
     std::cout << name_  << (name_.length() > 8 ? "\t" : "\t\t");
     std::cout << std::fixed;
     std::cout << std::setprecision(3);
     std::cout << "KLD = " << KLD;
-    std::cout << " (" << 0.5 * Lambda_Sigma.trace() - 0.5 * Scalar(_mean_bl.size()) << " & " << -0.5*logdet(Lambda_Sigma) << " & " << 0.5*_alpha*error.transpose() * Lambda * error << ")";
+    //std::cout << " (" << 0.5 * Lambda_Sigma.trace() - 0.5 * Scalar(_mean_bl.size()) << " & " << -0.5*logdet(Lambda_Sigma) << " & " << 0.5*_alpha*error.transpose() * Lambda * error << ")";
     std::cout << "\tRMSE = " << RMSEs_.back();
-    std::cout << "\ttime= " << t_sparsification_ << std::endl;
+    std::cout << "\ttime= " << t_sparsification_;
+    std::cout << "\tN factors = " << all_ctr.size() << std::endl;
     if (_print_level > 0)
     {
         //std::cout << "\terror = " << error.transpose() << std::endl;
@@ -479,397 +399,6 @@ SensorBasePtr ProblemPruning::getSensorPtr() const
     return sensor_ptr_;
 }
 
-void ProblemPruning::sparsifyOld()
-{
-    std::cout << "sparsifying Old" << name_ << std::endl;
-
-    if (last_removed_fr_it_ == trajectory_ptr_->getFrameList().end() && !trajectory_ptr_->getFrameList().empty())
-        last_removed_fr_it_ = trajectory_ptr_->getFrameList().begin();
-
-    if (options_.pruning_T_ != 0)
-    {
-        // list of frames to be removed
-        FrameBaseList all_removed_frames_list;
-        for (auto fr_it = last_removed_fr_it_; *fr_it != trajectory_ptr_->getLastKeyFramePtr(); fr_it++)
-            // check if it has to be marginalized
-            if (!(*fr_it)->isFixed() && (*fr_it)->getTimeStamp().getSeconds() % options_.pruning_T_ != 1)
-            {
-                all_removed_frames_list.push_back(*fr_it);
-                last_removed_fr_it_ = std::next(fr_it);
-            }
-
-        //std::cout << "ALL FRAMES TO BE REMOVED: ";
-        //for (auto rem_frame : all_removed_frames_list)
-        //    std::cout << rem_frame->id() << " ";
-        //std::cout << std::endl;
-
-        // Until no more frames to be removed...
-        while (!all_removed_frames_list.empty())
-        {
-            // Current removed frames list
-            FrameBaseList removed_frame_list;
-
-            // Connected nodes
-            if (options_.connected_)
-                removed_frame_list = connectedFrames(all_removed_frames_list.front(), all_removed_frames_list);
-            // One by one
-            else
-                removed_frame_list.push_back(all_removed_frames_list.front());
-
-            // Remove current removed from all list
-            for (auto fr_ptr : removed_frame_list)
-                all_removed_frames_list.remove(fr_ptr);
-
-            //std::cout << "REMOVING FRAMES: ";
-            //for (auto rem_frame : removed_frame_list)
-            //    std::cout << rem_frame->id() << " ";
-            //std::cout << std::endl;
-            //std::cout << "ALL FRAMES TO BE REMOVED: ";
-            //for (auto rem_frame : all_removed_frames_list)
-            //    std::cout << rem_frame->id() << " ";
-            //std::cout << std::endl;
-
-            // INDICES
-            //std::cout << "INDICES\n";
-            // Markov Blanket & intrafactors
-            ConstraintBaseList intra_factors;
-            FrameBaseList markov_blanket;
-            markovBlanket(removed_frame_list, markov_blanket, intra_factors);
-
-            // Old constraint map
-            std::map<ConstraintBasePtr, unsigned int> old_ctr_2_row;
-            unsigned int meas_size = computeConstraintMap(intra_factors, old_ctr_2_row);
-
-            // Removed node frame and state blocks maps
-            FrameBaseList original_nodes(markov_blanket);
-            unsigned int removed_size = 0;
-            for (auto rem_frame : removed_frame_list)
-            {
-                original_nodes.push_back(rem_frame);// removed node at the end
-                removed_size += rem_frame->getSize();
-            }
-
-            // state block and frame maps
-            std::map<StateBlockPtr, unsigned int> sb_2_col;
-            std::map<FrameBasePtr,  unsigned int> fr_2_col;
-            unsigned int state_size = computeStateBlockMap(original_nodes, sb_2_col, fr_2_col);
-            unsigned int markov_size = state_size - removed_size;
-            unsigned int node_size = markov_blanket.front()->getSize();
-
-            //std::cout << "Markov blanket: ";
-            //for (auto fr_it= markov_blanket.begin(); fr_it != markov_blanket.end(); fr_it++)
-            //  std::cout << (*fr_it)->id() << " ";
-            //std::cout << std::endl;
-
-            // BUILD PROBLEM
-            //std::cout << "BUILD PROBLEM" << std::endl;
-            // mean & normalized jacobian
-            Eigen::VectorXs mean = getStateBlocksMean(sb_2_col, state_size);
-            Eigen::VectorXs residual(meas_size);
-            Eigen::SparseMatrixs A(meas_size,state_size);
-
-            //std::cout << "mean: " << mean.transpose() << std::endl;
-
-            // OPTIONALLY SOLVE LOCAL PROBLEM (fixing the first node)
-            if (options_.local_solve_)
-            {
-                computeAb(intra_factors, sb_2_col, old_ctr_2_row, mean, A, residual);
-                Eigen::SparseMatrixs Aaux = A.rightCols(state_size-node_size);
-                Eigen::SparseQR<Eigen::SparseMatrixs, Eigen::COLAMDOrdering<int>> solver;
-                solver.compute(Aaux);
-                assert(solver.info()==Eigen::Success);
-                Eigen::VectorXs delta_mean = solver.solve(-residual);
-                //std::cout << "Solved local problem! delta_x = " << delta_mean.transpose() << std::endl;
-                mean.tail(state_size-node_size) += delta_mean;
-            }
-
-            // BUILD SPARSIFICATION PROBLEM
-            // jacobians & residual
-            A.setZero();
-            computeAb(intra_factors, sb_2_col, old_ctr_2_row, mean, A, residual);
-            //std::cout << "A: " << std::endl << Eigen::MatrixXs(A) << std::endl;
-
-            // Information matrix
-            Eigen::MatrixXs Lambda_original = A.transpose() * A;
-            //std::cout << "Lambda_original: " << std::endl << Lambda_original << std::endl;
-
-            // Marginalization
-            Eigen::MatrixXs Lambda = Lambda_original.topLeftCorner(markov_size, markov_size) -
-                                     Lambda_original.topRightCorner(markov_size, removed_size) *
-                                     Lambda_original.bottomRightCorner(removed_size, removed_size).inverse() *
-                                     Lambda_original.bottomLeftCorner(removed_size, markov_size);
-            //std::cout << "Exact Lambda: " << std::endl << Lambda << std::endl;
-
-            // PROJECTION
-            unsigned int projected_size = markov_size - node_size;
-            Eigen::JacobiSVD<Eigen::MatrixXs> UDUT(Lambda, Eigen::ComputeFullU | Eigen::ComputeFullV);
-            Eigen::MatrixXs D  = UDUT.singularValues().head(projected_size).asDiagonal();
-            Eigen::MatrixXs iD = UDUT.singularValues().head(projected_size).cwiseInverse().asDiagonal();
-            Eigen::MatrixXs U  = UDUT.matrixU().leftCols(projected_size).real();
-
-            // TOPOLOGY
-            //std::cout << "TOPOLOGY" << std::endl;
-            typedef std::pair<FrameBasePtr, FrameBasePtr> FramePair;
-            std::list<FramePair> topology;
-            if (markov_blanket.size() > 2)
-            {
-                // Tikhonov regularization
-                Eigen::MatrixXs Sigma_Tik = (Lambda + Eigen::MatrixXs::Identity(markov_size,markov_size)).inverse();
-                // all pairs with its mutual information
-                std::list<std::pair<double, FramePair>> frame_info_pairs;
-                for (auto fr_it1 = markov_blanket.begin(); fr_it1 != markov_blanket.end(); fr_it1++)
-                    for (auto fr_it2 = std::next(fr_it1); fr_it2 != markov_blanket.end(); fr_it2++)
-                    {
-                        Eigen::MatrixXs Sigma_Tik_12((*fr_it1)->getSize() + (*fr_it2)->getSize(), (*fr_it1)->getSize() + (*fr_it2)->getSize());
-
-                        Sigma_Tik_12.topLeftCorner(    (*fr_it1)->getSize(), (*fr_it1)->getSize()) = Sigma_Tik.block(fr_2_col.at(*fr_it1), fr_2_col.at(*fr_it1), (*fr_it1)->getSize(), (*fr_it1)->getSize());
-                        Sigma_Tik_12.topRightCorner(   (*fr_it1)->getSize(), (*fr_it2)->getSize()) = Sigma_Tik.block(fr_2_col.at(*fr_it1), fr_2_col.at(*fr_it2), (*fr_it1)->getSize(), (*fr_it2)->getSize());
-                        Sigma_Tik_12.bottomLeftCorner( (*fr_it2)->getSize(), (*fr_it1)->getSize()) = Sigma_Tik.block(fr_2_col.at(*fr_it2), fr_2_col.at(*fr_it1), (*fr_it2)->getSize(), (*fr_it1)->getSize());
-                        Sigma_Tik_12.bottomRightCorner((*fr_it2)->getSize(), (*fr_it2)->getSize()) = Sigma_Tik.block(fr_2_col.at(*fr_it2), fr_2_col.at(*fr_it2), (*fr_it2)->getSize(), (*fr_it2)->getSize());
-
-                        double info = 0.5 * log(Sigma_Tik_12.topLeftCorner((*fr_it1)->getSize(), (*fr_it1)->getSize()).determinant() *
-                                                Sigma_Tik_12.bottomRightCorner((*fr_it2)->getSize(), (*fr_it2)->getSize()).determinant() /
-                                                Sigma_Tik_12.determinant());
-                        frame_info_pairs.push_back(std::pair<double, FramePair>(info, std::pair<FrameBasePtr, FrameBasePtr>(*fr_it1,*fr_it2)));
-                    }
-                // sort
-                frame_info_pairs.sort();
-                frame_info_pairs.reverse();
-
-                // Chow-Liu Tree
-                unsigned int N_tree = markov_blanket.size()-1;
-                // add most informative pair
-                topology.push_back(frame_info_pairs.front().second);
-                std::list<FrameBasePtr> added_frames({topology.front().first, topology.front().second});
-                //std::cout << "BUILDING CLT" << std::endl;
-                //std::cout << "\tFirst link from " << topology.front().first->getTimeStamp().getSeconds() << " to " << topology.front().second->getTimeStamp().getSeconds() << " with IG = " << frame_info_pairs.front().first << std::endl;
-                frame_info_pairs.pop_front();
-                // build tree
-                while (topology.size() != N_tree)
-                {
-                    for (auto pair_it = frame_info_pairs.begin(); pair_it != frame_info_pairs.end(); pair_it++)
-                    {
-                        //std::cout << "\tCandidate from " << pair_it->second.first->getTimeStamp().getSeconds() << " to " << pair_it->second.second->getTimeStamp().getSeconds() << " with IG = " << pair_it->first << std::endl;
-
-                        // both frames already added
-                        if ( std::find(added_frames.begin(), added_frames.end(), pair_it->second.first) != added_frames.end() &&
-                             std::find(added_frames.begin(), added_frames.end(), pair_it->second.second) != added_frames.end() )
-                            continue;
-                        // neither of both added
-                        if ( std::find(added_frames.begin(), added_frames.end(), pair_it->second.first) == added_frames.end() &&
-                             std::find(added_frames.begin(), added_frames.end(), pair_it->second.second) == added_frames.end() )
-                            continue;
-
-                        // add pair
-                        topology.push_back(pair_it->second);
-                        if ( std::find(added_frames.begin(), added_frames.end(), pair_it->second.first) == added_frames.end())
-                            added_frames.push_back(pair_it->second.first);
-                        else
-                            added_frames.push_back(pair_it->second.second);
-
-                        //std::cout << "\t\tADDED!" << std::endl;
-                        // remove info_pair
-                        frame_info_pairs.erase(pair_it);
-                        break;
-                    }
-                }
-                //std::cout << "CLT built!" << std::endl << std::endl;
-                assert(added_frames.size() == markov_blanket.size());
-
-                // complement tree -> sub-graph
-                for (auto pair_it = frame_info_pairs.begin(); topology.size() != int(N_tree * options_.gamma_topology_) && pair_it != frame_info_pairs.end(); pair_it++)
-                    topology.push_back(pair_it->second);
-
-            }
-            else
-                topology.push_back(std::pair<FrameBasePtr, FrameBasePtr>(markov_blanket.front(), markov_blanket.back()));
-
-            // BUILD TOPOLOGY
-            //std::cout << "BUILD TOPOLOGY" << std::endl;
-            std::list<ConstraintBasePtr> new_ctrs;
-            for (auto fr_pair : topology)
-            {
-                FrameBasePtr from_frame = fr_pair.first;
-                FrameBasePtr to_frame   = fr_pair.second;
-
-                // measurement mean
-                Eigen::VectorXs from_frame_pose = mean.segment(fr_2_col.at(from_frame), from_frame->getSize());
-                Eigen::VectorXs to_frame_pose   = mean.segment(fr_2_col.at(to_frame), to_frame->getSize());
-
-                Eigen::Matrix3s R = Eigen::Matrix3s::Identity();
-                R.topLeftCorner(2,2) = Eigen::Rotation2Ds(-from_frame_pose(2)).matrix();
-                Eigen::Vector3s new_meas = R*(to_frame_pose - from_frame_pose);
-
-                // create & store constraint
-                new_ctrs.push_back(createConstraint(from_frame, to_frame, new_meas, Eigen::Matrix3s::Identity(), sensor_ptr_));
-            }
-
-            if (new_ctrs.size() > 1)
-            {
-                //std::cout << "SG built! " << new_ctrs.size() <<  std::endl;
-                //for (auto ctr : new_ctrs)
-                //    std::cout << "\t" << ctr->getCapturePtr()->getFramePtr()->getTimeStamp().get() << "-" << ctr->getFrameOtherPtr()->getTimeStamp().get() << std::endl;
-            }
-            // new constraints map
-            std::map<ConstraintBasePtr, unsigned int> new_ctr_2_row;
-            unsigned int new_meas_size = computeConstraintMap(new_ctrs, new_ctr_2_row);
-
-            Eigen::SparseMatrixs J(node_size*topology.size(), markov_size);
-            Eigen::VectorXs b = Eigen::VectorXs::Zero(J.rows());
-            computeAb(new_ctrs, sb_2_col, new_ctr_2_row, mean, J, b);
-
-            // SPARSIFICATION
-            //std::cout << "SPARSIFICATION" << std::endl;
-            clock_t clock_0_ = clock();
-            Eigen::MatrixXs JU = J*U;
-
-            if (D.determinant() < 1e-9)
-                std::cout << "Projected Lambda is close to be ill-defined. Det(D) = " << D.determinant() << std::endl << "Singular values: " << UDUT.singularValues().transpose() << std::endl;
-
-            // Rank
-            Eigen::FullPivLU<Eigen::MatrixXs> JU_decomp(JU);
-            // 1 - closed form
-            if (JU_decomp.rank() == JU.rows())
-            {
-                for (auto ctr : new_ctrs)
-                {
-                    Eigen::MatrixXs inv_omega_k = JU.middleRows(new_ctr_2_row.at(ctr), ctr->getSize()) *
-                                                  iD *
-                                                  JU.middleRows(new_ctr_2_row.at(ctr), ctr->getSize()).transpose();
-                    //std::cout << "det(inv_omega_k) = " << inv_omega_k.determinant() << std::endl << "inv_omega_k:\n" << inv_omega_k << std::endl;
-
-                    // Set information matrix
-                    setSPD(inv_omega_k);
-                    ctr->getFeaturePtr()->setMeasurementCovariance(inv_omega_k);
-                }
-            }
-            // 2 - optimization
-            else
-            {
-                // if COMP: several variants, OTHERWISE only one variant
-                for (auto variant = 0; variant < variants_names_.size(); variant++)
-                {
-                    //std::cout << "Variant " << variants_names_[variant] << std::endl;
-
-                    bool store_logs = options_.optim_options_.optim_metod_ == COMP;
-                    Eigen::MatrixXs logs;
-
-                    // Initial guess
-                    switch (variants_options_[variant].initial_guess_)
-                    {
-                        case ID:
-                            for (auto ctr : new_ctrs)
-                                ctr->getFeaturePtr()->setMeasurementInfo(Eigen::MatrixXs::Identity(ctr->getSize(),ctr->getSize()));
-                                //insertSparseBlock(Eigen::MatrixXs::Identity(ctr->getSize(),ctr->getSize()), Omega, new_ctr_2_row[ctr], new_ctr_2_row[ctr]);
-                            break;
-                        case ODB:
-                            for (auto ctr : new_ctrs)
-                                ctr->getFeaturePtr()->setMeasurementInfo(initODB(ctr,J,Lambda,new_ctr_2_row,fr_2_col));
-                                //insertSparseBlock(initODB(ctr,J,Lambda,new_ctr_2_row,fr_2_col), Omega, new_ctr_2_row[ctr], new_ctr_2_row[ctr]);
-                            break;
-                        case NO_INIT:
-                            break;
-                    }
-
-                    // sparsification
-                    switch (variants_options_[variant].optim_metod_)
-                    {
-                        case IP:
-                            //logs = interiorPoint(JU, iD, new_ctr_2_row, Omega, variants_options_[variant], store_logs);
-                            break;
-                        case FD:
-                        {
-                            logs = factorDescent(JU, iD, new_ctr_2_row, variants_options_[variant], store_logs);
-                            break;
-                        }
-                        case FDOLD:
-                        {
-                            Eigen::SparseMatrixs Omega(new_meas_size,new_meas_size);
-                            for (auto ctr : new_ctrs)
-                                insertSparseBlock(ctr->getFeaturePtr()->getMeasurementInfo(), Omega, new_ctr_2_row[ctr], new_ctr_2_row[ctr]);
-                            logs = factorDescentOld(JU, iD, new_ctr_2_row, Omega, variants_options_[variant], store_logs);
-
-                            for (auto ctr : new_ctrs)
-                            {
-                                Eigen::MatrixXs Omega_k = Omega.block(new_ctr_2_row[ctr], new_ctr_2_row[ctr],ctr->getSize(), ctr->getSize());
-                                setSPD(Omega_k);
-                                ctr->getFeaturePtr()->setMeasurementInfo(Omega_k);
-                            }
-                            break;
-                        }
-                        case COMP:
-                        {
-                            assert(true && "not reachable option!");
-                            break;
-                        }
-                        case NO_OPTIM:
-                            break;
-                    }
-
-                    // store logs
-                    if (store_logs)
-                        log_files_[variant] << markov_blanket.size() << "\n" << logs.row(0) << "\n" << logs.row(1) << std::endl; // store logs
-
-                    // fill covariance of constriants
-                }
-            }
-            t_sparsification_ += ((Scalar) clock() - clock_0_) / CLOCKS_PER_SEC;
-
-            // CHECK KLD
-            /*if (markov_blanket.size() > 2)
-            {
-                Eigen::SparseMatrixs A_approx(new_meas_size,markov_size);
-                Eigen::VectorXs residual_approx(new_meas_size);
-                computeAb(new_ctrs, sb_2_col, new_ctr_2_row, mean, A_approx, residual_approx);
-                Eigen::MatrixXs Lambda_approx = U.transpose() * A_approx.transpose() * A_approx * U;
-                Eigen::MatrixXs Lambda_Sigma = Lambda_approx * D.inverse();
-                Scalar KLD = 0.5 * (Lambda_Sigma.trace() - logdet(Lambda_Sigma) - Scalar(Lambda_Sigma.rows()));
-                if (KLD > 1e3)
-                {
-                    std::cout << "A_approx: " << std::endl << Eigen::MatrixXs(A_approx) << std::endl;
-                    std::cout << "Lambda_approx: " << std::endl << Eigen::MatrixXs(A_approx.transpose() * A_approx) << std::endl;
-                    std::cout << "Lambda: " << std::endl << Lambda << std::endl;
-                    std::cout << "Lambda_Sigma: " << std::endl << Lambda_Sigma << std::endl;
-                    std::cout << "Ut * Lambda_approx * U: " << std::endl << Lambda_approx << std::endl;
-                    std::cout << "D.inv: " << std::endl << D.inverse() << std::endl;
-                    std::cout << "Sparsification " << name_ << " KLD: " << KLD << std::endl;
-                }
-            }*/
-
-            // STORE PROBLEM SIZES
-            MB_sizes_.push_back(markov_blanket.size());
-
-            // REMOVE MARGINALIZED NODE
-            // remove frames and constraints
-            //std::cout << "REMOVE NODES" << std::endl;
-            markov_blanket.clear();
-            original_nodes.clear();
-            fr_2_col.clear();
-            sb_2_col.clear();
-            old_ctr_2_row.clear();
-            new_ctr_2_row.clear();
-            //std::cout << "Removing frame " << removed_frame->id() << " owned by " << removed_frame.use_count() << std::endl;
-            //std::cout << "State blocks:" << std::endl;
-            //for (auto sb : removed_frame->getStateBlockVec())
-            //  if (sb)
-            //      std::cout << "\t" << sb->getPtr() << std::endl;
-            //std::cout << "Removed constraints: " << std::endl;
-            for (auto ctr : intra_factors)
-            {
-                ctr->remove();
-                //std::cout << "\t" << ctr->id() << " owned by " << ctr.use_count() << std::endl;
-            }
-            intra_factors.clear();
-            // remove from wolf tree
-            while (!removed_frame_list.empty())
-            {
-                removed_frame_list.front()->remove();
-                removed_frame_list.erase(removed_frame_list.begin());
-            }
-        }
-    }
-}
-
 Eigen::MatrixXs solveConstrainedOptimization(ConstraintBaseList& new_ctrs,
                                              const Eigen::MatrixXs& Lambda,
                                              const Eigen::SparseMatrixs& J,
@@ -892,6 +421,14 @@ Eigen::MatrixXs solveConstrainedOptimization(ConstraintBaseList& new_ctrs,
             for (auto ctr : new_ctrs)
                 ctr->getFeaturePtr()->setMeasurementInfo(initODB(ctr,J,Lambda,new_ctr_2_row,fr_2_col));
             break;
+        case FFD:
+        {
+            for (auto ctr : new_ctrs)
+                ctr->getFeaturePtr()->setMeasurementInfo(1e-6*Eigen::MatrixXs::Identity(ctr->getSize(),ctr->getSize()));
+            PruningOptimOptions initFDOptions(FD, NO_INIT, new_ctrs.size(), 1e3, 0, ANALYTIC, 0, 0, 0, true, true, 1);
+            factorDescent(JU, iD, new_ctr_2_row, initFDOptions, false);
+            break;
+        }
         case NO_INIT:
             break;
     }
@@ -918,11 +455,6 @@ Eigen::MatrixXs solveConstrainedOptimization(ConstraintBaseList& new_ctrs,
             }
             return logs;
         }
-        case COMP:
-        {
-            assert(true && "not reachable option!");
-            break;
-        }
         case NO_OPTIM:
             break;
     }
@@ -945,9 +477,9 @@ std::list<ConstraintBasePtr> computeTopology(const FrameBaseList& markov_blanket
     unsigned int N_tree = markov_blanket.size()-1;
     unsigned int N_subgraph = 0;
     if (options.topology_size_ == N_CLT)
-        N_subgraph = (long unsigned int)(N_tree * options.gamma_topology_);
+        N_subgraph = std::ceil(Scalar(N_tree) * options.gamma_topology_);
     if (options.topology_size_ == N_FILL)
-        N_subgraph = (long unsigned int)(options.gamma_topology_ * markov_blanket.size() * (markov_blanket.size() - 1) / 2);
+        N_subgraph = std::ceil(options.gamma_topology_ * Scalar(markov_blanket.size()) * Scalar((markov_blanket.size() - 1)) / 2.);
     if (N_subgraph < N_tree)
         N_subgraph = N_tree;
 
@@ -1094,19 +626,15 @@ std::list<ConstraintBasePtr> computeTopology(const FrameBaseList& markov_blanket
             case MI2:
             {
                 // Downdate Sigma with topology factors
-                Eigen::MatrixXs Lambda_down = Lambda + Eigen::MatrixXs::Identity(markov_size,markov_size);
                 Eigen::MatrixXs Sigma_Tik_down = Sigma_Tik;
                 for (auto ctr : new_ctrs)
                 {
                     Eigen::SparseMatrixs Jk(ctr->getSize(), markov_size);
                     computeJacobians(ConstraintBaseList({ctr}), sb_2_col, std::map<ConstraintBasePtr, unsigned int>({{ctr, 0}}), mean, Jk);
-                    Eigen::MatrixXs Omega_k = (Jk * U * iD * U.transpose() * Jk.transpose()).inverse();
+                    Eigen::MatrixXs inv_Omega_k = Jk * U * iD * U.transpose() * Jk.transpose();
 
-                    //Lambda_down -= Jk.transpose() * Omega_k * Jk;
-                    Sigma_Tik_down += Sigma_Tik_down * Jk.transpose() * (Omega_k + Jk * Sigma_Tik_down * Jk.transpose()).inverse() * Jk * Sigma_Tik_down;
+                    Sigma_Tik_down += Sigma_Tik_down * Jk.transpose() * (inv_Omega_k + Jk * Sigma_Tik_down * Jk.transpose()).inverse() * Jk * Sigma_Tik_down;
                 }
-                // Recompute mutual information
-                //Sigma_Tik_down = Lambda_down.inverse();
                 for (auto& ctr_pair : all_ctr_info)
                     ctr_pair.first = computeMutualInformation(Sigma_Tik_down, fr_2_col, ctr_pair.second);
 
@@ -1124,21 +652,18 @@ std::list<ConstraintBasePtr> computeTopology(const FrameBaseList& markov_blanket
             case MI3:
             {
                 // Downdate Sigma with topology factors
-                Eigen::MatrixXs Lambda_down = Lambda + Eigen::MatrixXs::Identity(markov_size,markov_size);
                 Eigen::MatrixXs Sigma_Tik_down = Sigma_Tik;
                 for (auto ctr : new_ctrs)
                 {
                     Eigen::SparseMatrixs Jk(ctr->getSize(), markov_size);
                     computeJacobians(ConstraintBaseList({ctr}), sb_2_col, std::map<ConstraintBasePtr, unsigned int>({{ctr, 0}}), mean, Jk);
-                    Eigen::MatrixXs Omega_k = (Jk * U * iD * U.transpose() * Jk.transpose()).inverse();
+                    Eigen::MatrixXs inv_Omega_k = Jk * U * iD * U.transpose() * Jk.transpose();
 
-                    //Lambda_down -= Jk.transpose() * Omega_k * Jk;
-                    Sigma_Tik_down += Sigma_Tik_down * Jk.transpose() * (Omega_k + Jk * Sigma_Tik_down * Jk.transpose()).inverse() * Jk * Sigma_Tik_down;
+                    //Lambda_down -= Jk.transpose() * inv_Omega_k.inverse() * Jk;
+                    Sigma_Tik_down += Sigma_Tik_down * Jk.transpose() * (inv_Omega_k + Jk * Sigma_Tik_down * Jk.transpose()).inverse() * Jk * Sigma_Tik_down;
                 }
                 while (new_ctrs.size() != N_subgraph)
                 {
-                    // Recompute mutual information
-                    //Sigma_Tik_down = Lambda_down.inverse();
                     for (auto& ctr_pair : all_ctr_info)
                         ctr_pair.first = computeMutualInformation(Sigma_Tik_down, fr_2_col, ctr_pair.second);
 
@@ -1155,7 +680,6 @@ std::list<ConstraintBasePtr> computeTopology(const FrameBaseList& markov_blanket
                     computeJacobians(ConstraintBaseList({new_ctrs.back()}), sb_2_col, std::map<ConstraintBasePtr, unsigned int>({{new_ctrs.back(), 0}}), mean, Jk);
                     Eigen::MatrixXs Omega_k = (Jk * U * iD * U.transpose() * Jk.transpose()).inverse();
 
-                    //Lambda_down -= Jk.transpose() * Omega_k * Jk;
                     Sigma_Tik_down += Sigma_Tik_down * Jk.transpose() * (Omega_k + Jk * Sigma_Tik_down * Jk.transpose()).inverse() * Jk * Sigma_Tik_down;
                 }
                 break;
@@ -1306,13 +830,7 @@ std::list<ConstraintBasePtr> computeTopology(const FrameBaseList& markov_blanket
 
 void ProblemPruning::sparsify()
 {
-    if (variants_options_[0].optim_metod_ == FDOLD)
-    {
-        sparsifyOld();
-        return;
-    }
-
-    std::cout << "sparsifying " << name_ << std::endl;
+    //std::cout << "sparsifying " << name_ << std::endl;
 
     if (last_removed_fr_it_ == trajectory_ptr_->getFrameList().end() && !trajectory_ptr_->getFrameList().empty())
         last_removed_fr_it_ = trajectory_ptr_->getFrameList().begin();
@@ -1443,14 +961,14 @@ void ProblemPruning::sparsify()
                 std::cout << "Projected Lambda is close to be ill-defined. Det(D) = " << D.determinant() << std::endl << "Singular values: " << UDUT.singularValues().transpose() << std::endl;
 
             // TOPOLOGY
+            clock_t clock_0_ = clock();
             //std::cout << "TOPOLOGY" << std::endl;
 
             // compute topology
-            ConstraintBaseList new_ctrs = computeTopology(markov_blanket, Lambda, mean, iD, U, sb_2_col, fr_2_col, options_, variants_options_[0], sensor_ptr_);
+            ConstraintBaseList new_ctrs = computeTopology(markov_blanket, Lambda, mean, iD, U, sb_2_col, fr_2_col, options_, options_.optim_options_, sensor_ptr_);
 
             // SPARSIFICATION
             //std::cout << "SPARSIFICATION" << std::endl;
-            clock_t clock_0_ = clock();
             // new constraints map
             //std::cout << "new constraints map " << new_ctrs.size() << std::endl;
             std::map<ConstraintBasePtr, unsigned int> new_ctr_2_row;
@@ -1479,19 +997,8 @@ void ProblemPruning::sparsify()
             // 2 - optimization
             else
             {
-                // if COMP: several variants, OTHERWISE only one variant
-                for (unsigned int variant = 0; variant < variants_names_.size(); variant++)
-                {
-                    //std::cout << "Variant " << variants_names_[variant] << std::endl;
-
-                    bool store_logs = options_.optim_options_.optim_metod_ == COMP;
-
-                    Eigen::MatrixXs logs = solveConstrainedOptimization(new_ctrs, Lambda, J, JU, iD, new_ctr_2_row, fr_2_col, variants_options_[variant], store_logs);
-
-                    // store logs
-                    if (store_logs)
-                        log_files_[variant] << markov_blanket.size() << "\n" << logs.row(0) << "\n" << logs.row(1) << std::endl; // store logs
-                }
+                //std::cout << "Variant " << name_ << std::endl;
+                Eigen::MatrixXs logs = solveConstrainedOptimization(new_ctrs, Lambda, J, JU, iD, new_ctr_2_row, fr_2_col, options_.optim_options_, false);
             }
             t_sparsification_ += ((Scalar) clock() - clock_0_) / CLOCKS_PER_SEC;
 
diff --git a/src/pruning/problem_pruning.h b/src/pruning/problem_pruning.h
index 438a7dde676de1dc5481c0b57a6187fa29e16502..d18a7ae0ba1c82111aeeff4543aa02f315eeb10a 100644
--- a/src/pruning/problem_pruning.h
+++ b/src/pruning/problem_pruning.h
@@ -104,13 +104,13 @@ typedef enum {
     NO_INIT,
     ID,
     ODB,
+    FFD
 } initMethod;
 typedef enum {
     NO_OPTIM,
     IP,
     FD,
-    FDOLD,
-    COMP
+    FDOLD
 } optimMethod;
 typedef enum {
     ANALYTIC,
@@ -244,9 +244,6 @@ class ProblemPruning : public Problem
         SensorBasePtr sensor_ptr_;
         Scalar t_sparsification_;
         FrameBaseIter last_removed_fr_it_;
-        std::vector<std::ofstream> log_files_;
-        std::vector<PruningOptimOptions> variants_options_;
-        std::vector<std::string> variants_names_;
         std::ofstream problems_file_;
         std::ofstream originals_file_;
         std::vector<Scalar> MB_sizes_;
@@ -272,7 +269,6 @@ class ProblemPruning : public Problem
 
         Scalar computeKLD(const Eigen::MatrixXs& _Sigma_bl, const Eigen::VectorXs& _mean_bl, const int& _print_level, const double& _alpha = 1.0);
 
-        void sparsifyOld();
         void sparsify();
 
         void markovBlanket(const FrameBaseList& removed_frames_list, FrameBaseList& markov_blanket, ConstraintBaseList& intra_factors) const;