From 2b04f262e4841acc91050e048c66fdd50ab7c819 Mon Sep 17 00:00:00 2001 From: jcasals <jcasals@iri.upc.edu> Date: Fri, 5 Jul 2019 13:09:47 +0200 Subject: [PATCH] Remove hello_plugin --- CMakeLists.txt | 1 - hello_plugin/CMakeLists.txt | 16 --- hello_plugin/hello_plugin.cpp | 203 ------------------------------- hello_plugin/params.yaml | 30 ----- hello_plugin/params_autoconf.cpp | 71 ----------- hello_plugin/params_conf.yaml | 8 -- 6 files changed, 329 deletions(-) delete mode 100644 hello_plugin/CMakeLists.txt delete mode 100644 hello_plugin/hello_plugin.cpp delete mode 100644 hello_plugin/params.yaml delete mode 100644 hello_plugin/params_autoconf.cpp delete mode 100644 hello_plugin/params_conf.yaml diff --git a/CMakeLists.txt b/CMakeLists.txt index b54760092..bb7f03212 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -401,7 +401,6 @@ ENDIF(Ceres_FOUND) #SUBDIRECTORIES add_subdirectory(hello_wolf) -add_subdirectory(hello_plugin) IF (cereal_FOUND) ADD_SUBDIRECTORY(serialization/cereal) ENDIF(cereal_FOUND) diff --git a/hello_plugin/CMakeLists.txt b/hello_plugin/CMakeLists.txt deleted file mode 100644 index 36a22f7a7..000000000 --- a/hello_plugin/CMakeLists.txt +++ /dev/null @@ -1,16 +0,0 @@ -INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR}) -# INCLUDE_DIRECTORIES(/home/jcasals/workspace/wolf/src) -# link_directories(/home/jcasals/workspace/wolf/lib) - -ADD_EXECUTABLE(hello_plugin hello_plugin.cpp) -ADD_EXECUTABLE(params_autoconf params_autoconf.cpp) -# target_link_libraries(hello_plugin class_loader boost_system console_bridge wolf yaml-cpp ${CERES_LIBRARIES}) -# target_link_libraries(params_autoconf class_loader boost_system console_bridge wolf yaml-cpp ) -target_link_libraries(hello_plugin wolf hellowolf yaml-cpp ${CERES_LIBRARIES} dl) -target_link_libraries(params_autoconf wolf yaml-cpp dl) - -# These lines always at the end -SET(HDRS_PLUGIN ${HDRS_PLUGIN} PARENT_SCOPE ) -SET(SRCS_PLUGIN ${SRCS_PLUGIN} PARENT_SCOPE ) - - diff --git a/hello_plugin/hello_plugin.cpp b/hello_plugin/hello_plugin.cpp deleted file mode 100644 index cc837fa43..000000000 --- a/hello_plugin/hello_plugin.cpp +++ /dev/null @@ -1,203 +0,0 @@ -/* - * hello_plugin.cpp - * - * Created on: Nov 12, 2018 - * Author: jcasals - */ -#include "core/sensor/sensor_base.h" -#include "core/common/wolf.h" -// #include "sensor_odom_2D.cpp" -#include <yaml-cpp/yaml.h> -#include "core/yaml/parser_yaml.hpp" -#include "core/utils/params_server.hpp" - -#include "../hello_wolf/capture_range_bearing.h" -#include "../hello_wolf/feature_range_bearing.h" -#include "../hello_wolf/factor_range_bearing.h" -#include "../hello_wolf/landmark_point_2D.h" -#include "core/utils/loader.hpp" -#include "core/processor/processor_odom_2D.h" - -#include "core/solver/solver_factory.h" -#include "core/ceres_wrapper/ceres_manager.h" - -using namespace std; -using namespace wolf; -using namespace Eigen; - -int main(int argc, char** argv) { - string file = ""; - if (argc > 1) - file = argv[1]; - parserYAML parser = parserYAML(file); - parser.parse(); - paramsServer server = paramsServer(parser.getParams(), - parser.sensorsSerialization(), parser.processorsSerialization()); - cout << "PRINTING SERVER MAP" << endl; - server.print(); - cout << "-----------------------------------" << endl; - /** - It seems to be a requirement that each file is loaded by its own ClassLoader object, otherwise I get - a segmentation fault. Likewise, it seems that these ClassLoaders must be allocated at the heap, because - the constructor refuses to build an object if I try to do local (stack) allocation, i.e `ClassLoader(it)` is not allowed but `new ClassLoader(it)` is. - **/ - // vector<ClassLoader*> class_loaders = vector<ClassLoader*>(); - // for(auto it : parser.getFiles()) { - // auto c = new ClassLoader(it); - // class_loaders.push_back(c); - // } - auto loaders = vector<Loader*>(); - for (auto it : parser.getFiles()) { - auto l = new LoaderRaw(it); - loaders.push_back(l); - } - ProblemPtr problem = Problem::create("PO", 2); - auto sensorMap = map<string, SensorBasePtr>(); - auto procesorMap = map<string, ProcessorBasePtr>(); - for (auto s : server.getSensors()) { - cout << s._name << " " << s._type << endl; - sensorMap.insert( - pair<string, SensorBasePtr>(s._name, - problem->installSensor(s._type, s._name, server))); - } - for (auto s : server.getProcessors()) { - cout << s._name << " " << s._type << " " << s._name_assoc_sensor - << endl; - procesorMap.insert( - pair<string, ProcessorBasePtr>(s._name, - problem->installProcessor(s._type, s._name, - s._name_assoc_sensor, server))); - } - - problem->print(4, 1, 1, 1); - Vector2s motion_data(1.0, 0.0); // Will advance 1m at each keyframe, will not turn. - Matrix2s motion_cov = 0.1 * Matrix2s::Identity(); - - // landmark observations data - VectorXi ids; - VectorXs ranges, bearings; - - // SET OF EVENTS ======================================================= - std::cout << std::endl; - WOLF_TRACE("======== BUILD PROBLEM ======="); - - // ceres::Solver::Options options; - // options.max_num_iterations = 1000; // We depart far from solution, need a lot of iterations - // CeresManagerPtr ceres = std::make_shared<CeresManager>(problem, options); - auto ceres = SolverFactory::get().create("Solver", problem, server); - // We'll do 3 steps of motion and landmark observations. - - // STEP 1 -------------------------------------------------------------- - - // initialize - TimeStamp t(0.0); // t : 0.0 - Vector3s x(0, 0, 0); - Matrix3s P = Matrix3s::Identity() * 0.1; - problem->setPrior(x, P, t, 0.5); // KF1 : (0,0,0) - auto sensor_rb = sensorMap.find("rb")->second; - // observe lmks - ids.resize(1); - ranges.resize(1); - bearings.resize(1); - ids << 1; // will observe Lmk 1 - ranges << 1.0; // see drawing - bearings << M_PI / 2; - CaptureRangeBearingPtr cap_rb = std::make_shared < CaptureRangeBearing - > (t, sensor_rb, ids, ranges, bearings); - sensor_rb->process(cap_rb); // L1 : (1,2) - - // STEP 2 -------------------------------------------------------------- - t += 1.0; // t : 1.0 - - // motion - auto sensor_odometry = sensorMap.find("odom")->second; - CaptureOdom2DPtr cap_motion = std::make_shared < CaptureOdom2D - > (t, sensor_odometry, motion_data, motion_cov); - sensor_odometry->process(cap_motion); // KF2 : (1,0,0) - - // observe lmks - ids.resize(2); - ranges.resize(2); - bearings.resize(2); - ids << 1, 2; // will observe Lmks 1 and 2 - ranges << sqrt(2.0), 1.0; // see drawing - bearings << 3 * M_PI / 4, M_PI / 2; - cap_rb = std::make_shared < CaptureRangeBearing - > (t, sensor_rb, ids, ranges, bearings); - sensor_rb->process(cap_rb); // L1 : (1,2), L2 : (2,2) - - // STEP 3 -------------------------------------------------------------- - t += 1.0; // t : 2.0 - - // motion - cap_motion->setTimeStamp(t); - sensor_odometry->process(cap_motion); // KF3 : (2,0,0) - // observe lmks - ids.resize(2); - ranges.resize(2); - bearings.resize(2); - ids << 2, 3; // will observe Lmks 2 and 3 - ranges << sqrt(2.0), 1.0; // see drawing - bearings << 3 * M_PI / 4, M_PI / 2; - cap_rb = std::make_shared < CaptureRangeBearing - > (t, sensor_rb, ids, ranges, bearings); - sensor_rb->process(cap_rb); // L1 : (1,2), L2 : (2,2), L3 : (3,2) - problem->print(1, 0, 1, 0); - - // SOLVE ================================================================ - - // SOLVE with exact initial guess - WOLF_TRACE("======== SOLVE PROBLEM WITH EXACT PRIORS =======") - std::string report = ceres->solve( - wolf::SolverManager::ReportVerbosity::FULL); - WOLF_TRACE(report); // should show a very low iteration number (possibly 1) - problem->print(1, 0, 1, 0); - - // PERTURB initial guess - WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======") - for (auto sen : problem->getHardware()->getSensorList()) - for (auto sb : sen->getStateBlockVec()) - if (sb && !sb->isFixed()) - sb->setState( - sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT ! - for (auto kf : problem->getTrajectory()->getFrameList()) - if (kf->isKey()) - for (auto sb : kf->getStateBlockVec()) - if (sb && !sb->isFixed()) - sb->setState( - sb->getState() - + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT ! - for (auto lmk : problem->getMap()->getLandmarkList()) - for (auto sb : lmk->getStateBlockVec()) - if (sb && !sb->isFixed()) - sb->setState( - sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT ! - problem->print(1, 0, 1, 0); - - // SOLVE again - WOLF_TRACE("======== SOLVE PROBLEM WITH PERTURBED PRIORS =======") - report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL); - WOLF_TRACE(report); // should show a very high iteration number (more than 10, or than 100!) - problem->print(1, 0, 1, 0); - - // GET COVARIANCES of all states - WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======") - ceres->computeCovariances( - SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); - for (auto kf : problem->getTrajectory()->getFrameList()) { - if (kf->isKey()) { - Eigen::MatrixXs cov; - WOLF_TRACE("KF", kf->id(), "_cov = \n", kf->getCovariance(cov)); - } - for (auto lmk : problem->getMap()->getLandmarkList()) { - Eigen::MatrixXs cov; - WOLF_TRACE("L", lmk->id(), "_cov = \n", lmk->getCovariance(cov)); - } - } - std::cout << std::endl; - - WOLF_TRACE("======== FINAL PRINT FOR INTERPRETATION =======") - problem->print(4, 1, 1, 1); - - return 0; -} diff --git a/hello_plugin/params.yaml b/hello_plugin/params.yaml deleted file mode 100644 index 14c4a1fa4..000000000 --- a/hello_plugin/params.yaml +++ /dev/null @@ -1,30 +0,0 @@ -config: - sensors: - - - type: "ODOM 2D" - name: "odom" - intrinsic: - k_disp_to_disp: 0.1 - k_rot_to_rot: 0.1 - extrinsic: - pos: [1,2,3] - - - type: "RANGE BEARING" - name: "rb" - processors: - - - type: "ODOM 2D" - name: "processor1" - sensor name: "odom" - - - type: "RANGE BEARING" - name: "rb_processor" - sensor name: "rb" - - - type: "ODOM 2D" - name: "my_proc_test" - sensor name: "odom" - follow: "../hello_plugin/params_conf.yaml" #Config continues in this file -files: - - "/home/jcasals/workspace/wip/wolf/lib/libsensor_odom.so" - - "/home/jcasals/workspace/wip/wolf/lib/librange_bearing.so" \ No newline at end of file diff --git a/hello_plugin/params_autoconf.cpp b/hello_plugin/params_autoconf.cpp deleted file mode 100644 index 9f21c611d..000000000 --- a/hello_plugin/params_autoconf.cpp +++ /dev/null @@ -1,71 +0,0 @@ -/* - * params_autoconf.cpp - * - * Created on: Feb 15, 2019 - * Author: jcasals - */ -#include "core/sensor/sensor_base.h" -#include "core/common/wolf.h" -// #include "sensor_odom_2D.cpp" -#include <yaml-cpp/yaml.h> -#include "core/yaml/parser_yaml.hpp" -#include "core/utils/params_server.hpp" - -#include "../hello_wolf/capture_range_bearing.h" -#include "../hello_wolf/feature_range_bearing.h" -#include "../hello_wolf/factor_range_bearing.h" -#include "../hello_wolf/landmark_point_2D.h" -#include "core/utils/loader.hpp" -#include "core/processor/processor_odom_2D.h" - -#include "core/solver/solver_factory.h" -#include "core/ceres_wrapper/ceres_manager.h" - -using namespace std; -using namespace wolf; -using namespace Eigen; - -int main(int argc, char** argv) { - string file = ""; - if(argc > 1) file = argv[1]; - parserYAML parser = parserYAML(file); - parser.parse(); - paramsServer server = paramsServer(parser.getParams(), parser.sensorsSerialization(), parser.processorsSerialization()); - cout << "PRINTING SERVER MAP" << endl; - server.print(); - cout << "-----------------------------------" << endl; - /** - It seems to be a requirement that each file is loaded by its own ClassLoader object, otherwise I get - a segmentation fault. Likewise, it seems that these ClassLoaders must be allocated at the heap, because - the constructor refuses to build an object if I try to do local (stack) allocation, i.e `ClassLoader(it)` is not allowed but `new ClassLoader(it)` is. - **/ - auto loaders = vector<Loader*>(); - for(auto it : parser.getFiles()) { - auto l = new LoaderRaw(it); - l->load(); - loaders.push_back(l); - } - ProblemPtr problem = Problem::create("PO", 2); - auto sensorMap = map<string, SensorBasePtr>(); - auto procesorMap = map<string, ProcessorBasePtr>(); - for(auto s : server.getSensors()){ - cout << s._name << " " << s._type << endl; - sensorMap.insert(pair<string, SensorBasePtr>(s._name,problem->installSensor(s._type, s._name, server))); - } - for(auto s : server.getProcessors()){ - cout << s._name << " " << s._type << " " << s._name_assoc_sensor << endl; - procesorMap.insert(pair<string, ProcessorBasePtr>(s._name,problem->installProcessor(s._type, s._name, s._name_assoc_sensor, server))); - } - auto prc = ProcessorParamsOdom2D("my_proc_test", server); - - std::cout << "prc.cov_det " << prc.cov_det << std::endl; - std::cout << "prc.unmeasured_perturbation_std " << prc.unmeasured_perturbation_std << std::endl; - std::cout << "prc.angle_turned " << prc.angle_turned << std::endl; - std::cout << "prc.dist_traveled " << prc.dist_traveled << std::endl; - std::cout << "prc.max_buff_length " << prc.max_buff_length << std::endl; - std::cout << "prc.max_time_span " << prc.max_time_span << std::endl; - std::cout << "prc.time_tolerance " << prc.time_tolerance << std::endl; - std::cout << "prc.voting_active " << prc.voting_active << std::endl; - - return 0; -} diff --git a/hello_plugin/params_conf.yaml b/hello_plugin/params_conf.yaml deleted file mode 100644 index 0d80a5802..000000000 --- a/hello_plugin/params_conf.yaml +++ /dev/null @@ -1,8 +0,0 @@ -cov_det: 100 -unmeasured_perturbation_std: 100 -angle_turned: 100 -dist_traveled: 100 -max_buff_length: 100 -max_time_span: 100 -time_tolerance: 100 -voting_active: false \ No newline at end of file -- GitLab