WIP for RAS

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})

src/examples/test_sparsification_convergence.cpp

+// 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;
+}

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);

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;