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;