diff --git a/CMakeLists.txt b/CMakeLists.txt index b0d49b02649503f3de85cc0ef857d2be42793899..99471e5090eb86db7c050f35cf5becdcfc349014 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,7 @@ CMAKE_MINIMUM_REQUIRED(VERSION 2.6) if(COMMAND cmake_policy) - cmake_policy(SET CMP0005 NEW) + cmake_policy(SET CMP0005 NEW) cmake_policy(SET CMP0003 NEW) endif(COMMAND cmake_policy) # MAC OSX RPATH @@ -191,8 +191,8 @@ IF (SPDLOG_INCLUDE_DIR) MESSAGE(STATUS "Found spdlog: ${SPDLOG_INCLUDE_DIR}") ELSE (SPDLOG_INCLUDE_DIR) MESSAGE(FATAL_ERROR "Could not find spdlog") -ENDIF (SPDLOG_INCLUDE_DIR) - +ENDIF (SPDLOG_INCLUDE_DIR) + INCLUDE_DIRECTORIES(${EIGEN_INCLUDE_DIRS}) include_directories("include") include_directories(.) @@ -243,6 +243,9 @@ SET(HDRS_UTILS include/core/utils/logging.h include/core/utils/make_unique.h include/core/utils/singleton.h + include/core/utils/params_server.hpp + include/core/utils/converter.h + include/core/utils/loader.hpp ) SET(HDRS_PROBLEM include/core/problem/problem.h @@ -311,6 +314,7 @@ SET(HDRS_LANDMARK include/core/landmark/landmark_match.h ) SET(HDRS_PROCESSOR + include/core/processor/autoconf_processor_factory.h include/core/processor/diff_drive_tools.h include/core/processor/diff_drive_tools.hpp include/core/processor/motion_buffer.h @@ -332,6 +336,7 @@ SET(HDRS_PROCESSOR include/core/processor/track_matrix.h ) SET(HDRS_SENSOR + include/core/sensor/autoconf_sensor_factory.h include/core/sensor/sensor_base.h include/core/sensor/sensor_diff_drive.h include/core/sensor/sensor_factory.h @@ -347,8 +352,12 @@ SET(HDRS_DTASSC include/core/association/association_solver.h include/core/association/association_tree.h include/core/association/matrix.h + include/core/association/association_nnls.h ) +SET(HDRS_YAML + include/core/yaml/parser_yaml.hpp + ) #SOURCES SET(SRCS_PROBLEM src/problem/problem.cpp @@ -457,18 +466,19 @@ IF (Ceres_FOUND) src/solver/solver_manager.cpp ) ELSE(Ceres_FOUND) - SET(HDRS_WRAPPER) - SET(SRCS_WRAPPER) + SET(HDRS_WRAPPER) + SET(SRCS_WRAPPER) ENDIF(Ceres_FOUND) #SUBDIRECTORIES add_subdirectory(hello_wolf) +add_subdirectory(hello_plugin) IF (cereal_FOUND) - ADD_SUBDIRECTORY(serialization/cereal) +ADD_SUBDIRECTORY(serialization/cereal) ENDIF(cereal_FOUND) IF (Suitesparse_FOUND) - #DOES NOTHING?! + #DOES NOTHING?! #ADD_SUBDIRECTORY(solver_suitesparse) ENDIF(Suitesparse_FOUND) # LEAVE YAML FILES ALWAYS IN THE LAST POSITION !! @@ -487,6 +497,7 @@ ENDIF(YAMLCPP_FOUND) # create the shared library ADD_LIBRARY(${PROJECT_NAME} SHARED + ${SRCS} ${SRCS_BASE} ${SRCS_CAPTURE} ${SRCS_COMMON} @@ -508,7 +519,7 @@ ADD_LIBRARY(${PROJECT_NAME} ${SRCS_WRAPPER} ${SRCS_YAML} ) -TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT}) +TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT} dl) #Link the created libraries #============================================================= @@ -632,12 +643,12 @@ IF (UNIX) COMMAND rm ARGS -rf ${CMAKE_SOURCE_DIR}/build/* TARGET distclean - ) + ) ELSE(UNIX) ADD_CUSTOM_COMMAND( COMMENT "distclean only implemented in unix" TARGET distclean - ) + ) ENDIF(UNIX) ADD_CUSTOM_TARGET (uninstall @echo uninstall package) @@ -648,12 +659,12 @@ IF (UNIX) COMMAND xargs ARGS rm < install_manifest.txt TARGET uninstall - ) + ) ELSE(UNIX) ADD_CUSTOM_COMMAND( COMMENT "uninstall only implemented in unix" TARGET uninstall - ) + ) ENDIF(UNIX) IF (UNIX) @@ -670,4 +681,3 @@ ELSE(UNIX) COMMENT "packaging only implemented in unix" TARGET uninstall) ENDIF(UNIX) - diff --git a/cmake_modules/wolfConfig.cmake b/cmake_modules/wolfConfig.cmake index 82bb7cf0a5523302eb3d6c7fd5064e382a6c59f5..c9321c445315da6e354b1f8423b7368745c14816 100644 --- a/cmake_modules/wolfConfig.cmake +++ b/cmake_modules/wolfConfig.cmake @@ -156,7 +156,6 @@ list(APPEND EIGEN_INCLUDE_DIR_HINTS /usr/include/eigen3) # match and reject with an explanation below. find_package(Eigen3 ${wolf_EIGEN_VERSION} QUIET) - # Flag set with currently found Eigen version. set(EIGEN_VERSION @EIGEN_VERSION@) if (EIGEN3_FOUND) diff --git a/hello_plugin/CMakeLists.txt b/hello_plugin/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..8df392f87658baa3beb7af2a98055c8a88dde949 --- /dev/null +++ b/hello_plugin/CMakeLists.txt @@ -0,0 +1,16 @@ +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 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 new file mode 100644 index 0000000000000000000000000000000000000000..f15d005abd80bf110809135922228661c0ab2f6c --- /dev/null +++ b/hello_plugin/hello_plugin.cpp @@ -0,0 +1,182 @@ +/* + * 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 new file mode 100644 index 0000000000000000000000000000000000000000..14c4a1fa4753080997625511d5aa092611ce0101 --- /dev/null +++ b/hello_plugin/params.yaml @@ -0,0 +1,30 @@ +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 new file mode 100644 index 0000000000000000000000000000000000000000..9f21c611d63849fa95ab701010dbe8c975fc3b91 --- /dev/null +++ b/hello_plugin/params_autoconf.cpp @@ -0,0 +1,71 @@ +/* + * 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 new file mode 100644 index 0000000000000000000000000000000000000000..0d80a5802f5da772f59c29adf12257c8f79663d3 --- /dev/null +++ b/hello_plugin/params_conf.yaml @@ -0,0 +1,8 @@ +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 diff --git a/hello_wolf/CMakeLists.txt b/hello_wolf/CMakeLists.txt index df4b2b136f5215ead121440939fedff4e428af32..20309f55ccc7ee673ecb67fc7f5de039369d9282 100644 --- a/hello_wolf/CMakeLists.txt +++ b/hello_wolf/CMakeLists.txt @@ -13,22 +13,21 @@ SET(HDRS_PROCESSOR ${HDRS_PROCESSOR} ) SET(SRCS_PROCESSOR ${SRCS_PROCESSOR} -# ${CMAKE_CURRENT_SOURCE_DIR}/hello_wolf.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/capture_range_bearing.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/feature_range_bearing.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/landmark_point_2D.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/processor_range_bearing.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/sensor_range_bearing.cpp +# ${CMAKE_CURRENT_SOURCE_DIR}/hello_wolf.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/capture_range_bearing.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/feature_range_bearing.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/landmark_point_2D.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/processor_range_bearing.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/sensor_range_bearing.cpp ) ADD_EXECUTABLE(hello_wolf hello_wolf.cpp) TARGET_LINK_LIBRARIES(hello_wolf ${PROJECT_NAME}) - +add_library(sensor_odom SHARED ../src/sensor/sensor_odom_2D.cpp ../src/processor/processor_odom_2D.cpp) +TARGET_LINK_LIBRARIES(sensor_odom ${PROJECT_NAME}) +add_library(range_bearing SHARED sensor_range_bearing.cpp processor_range_bearing.cpp) +TARGET_LINK_LIBRARIES(range_bearing ${PROJECT_NAME}) - - # These lines always at the end SET(HDRS_PROCESSOR ${HDRS_PROCESSOR} PARENT_SCOPE ) SET(SRCS_PROCESSOR ${SRCS_PROCESSOR} PARENT_SCOPE ) - - diff --git a/hello_wolf/processor_range_bearing.cpp b/hello_wolf/processor_range_bearing.cpp index 0c3d59b9d08a9d1042e986601e9d2360d83494e7..b75801bde13cff3a003e0ba746bec5fb864bb813 100644 --- a/hello_wolf/processor_range_bearing.cpp +++ b/hello_wolf/processor_range_bearing.cpp @@ -118,6 +118,24 @@ ProcessorBasePtr ProcessorRangeBearing::create(const std::string& _unique_name, return prc; } +ProcessorBasePtr ProcessorRangeBearing::createAutoConf(const std::string& _unique_name, + const paramsServer& _server, + const SensorBasePtr _sensor_ptr) +{ + SensorRangeBearingPtr sensor_rb = std::static_pointer_cast<SensorRangeBearing>(_sensor_ptr); + ProcessorParamsRangeBearingPtr params = std::make_shared<ProcessorParamsRangeBearing>(); + params->voting_active = _server.getParam<bool>(_unique_name + "/voting_active", "false"); + params->time_tolerance = _server.getParam<double>(_unique_name + "/time_tolerance", "0.01"); + + // construct processor + ProcessorRangeBearingPtr prc = std::make_shared<ProcessorRangeBearing>(sensor_rb, params); + + // setup processor + prc->setName(_unique_name); + + return prc; +} + Eigen::Vector2s ProcessorRangeBearing::observe(const Eigen::Vector2s& lmk_w) const { return polar(toSensor(lmk_w)); @@ -175,4 +193,9 @@ namespace wolf { WOLF_REGISTER_PROCESSOR("RANGE BEARING", ProcessorRangeBearing) } // namespace wolf +#include "core/processor/autoconf_processor_factory.h" +namespace wolf +{ +WOLF_REGISTER_PROCESSOR_AUTO("RANGE BEARING", ProcessorRangeBearing) +} // namespace wolf diff --git a/hello_wolf/processor_range_bearing.h b/hello_wolf/processor_range_bearing.h index 0dd2b7951fb63f0b1e487603f95ae2ff79eaaf2f..30b2bd8654a8bc8117f761e492f889fb44918a4a 100644 --- a/hello_wolf/processor_range_bearing.h +++ b/hello_wolf/processor_range_bearing.h @@ -40,6 +40,9 @@ class ProcessorRangeBearing : public ProcessorBase static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr); + static ProcessorBasePtr createAutoConf(const std::string& _unique_name, + const paramsServer& _server, + const SensorBasePtr _sensor_ptr = nullptr); protected: // Implementation of pure virtuals from ProcessorBase diff --git a/hello_wolf/sensor_range_bearing.cpp b/hello_wolf/sensor_range_bearing.cpp index ef2c5900a21aa44c885937f65a4f48dfb4974683..74f2c0e46cec59d1be7ba297547f927223d80c94 100644 --- a/hello_wolf/sensor_range_bearing.cpp +++ b/hello_wolf/sensor_range_bearing.cpp @@ -42,6 +42,16 @@ SensorBasePtr SensorRangeBearing::create(const std::string& _unique_name, // return sensor; } +SensorBasePtr SensorRangeBearing::createAutoConf(const std::string& _unique_name, // + const paramsServer& _server) +{ + Eigen::Vector2s noise_std(0.1,1.0); + SensorRangeBearingPtr sensor = std::make_shared<SensorRangeBearing>(Eigen::Vector3s(1,1,0), noise_std); + sensor->setName(_unique_name); + // sensor->node_name_ = _unique_name; + return sensor; +} + } /* namespace wolf */ // Register in the SensorFactory @@ -50,4 +60,8 @@ namespace wolf { WOLF_REGISTER_SENSOR("RANGE BEARING", SensorRangeBearing) } // namespace wolf - +#include "core/sensor/autoconf_sensor_factory.h" +namespace wolf +{ +WOLF_REGISTER_SENSOR_AUTO("RANGE BEARING", SensorRangeBearing) +} // namespace wolf diff --git a/hello_wolf/sensor_range_bearing.h b/hello_wolf/sensor_range_bearing.h index 5b60f0bb28ce8ba4a75f00f07a6ef735b770a252..5b80ff6b8f03ab7b9151252f29b99341212975b6 100644 --- a/hello_wolf/sensor_range_bearing.h +++ b/hello_wolf/sensor_range_bearing.h @@ -9,6 +9,7 @@ #define HELLO_WOLF_SENSOR_RANGE_BEARING_H_ #include "core/sensor/sensor_base.h" +#include "core/utils/params_server.hpp" namespace wolf { @@ -18,6 +19,16 @@ struct IntrinsicsRangeBearing : public IntrinsicsBase { Scalar noise_range_metres_std = 0.05; Scalar noise_bearing_degrees_std = 0.5; + IntrinsicsRangeBearing() + { + //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE. + } + IntrinsicsRangeBearing(std::string _unique_name, const paramsServer& _server): + IntrinsicsBase(_unique_name, _server) + { + noise_range_metres_std = _server.getParam<Scalar>(_unique_name + "/noise_range_metres_std", "0.05"); + noise_bearing_degrees_std = _server.getParam<Scalar>(_unique_name + "/noise_bearing_degrees_std", "0.5"); + } }; WOLF_PTR_TYPEDEFS(SensorRangeBearing) @@ -32,6 +43,8 @@ class SensorRangeBearing : public SensorBase static SensorBasePtr create(const std::string& _unique_name, // const Eigen::VectorXs& _extrinsics, // const IntrinsicsBasePtr _intrinsics); + static SensorBasePtr createAutoConf(const std::string& _unique_name, // + const paramsServer& _server); }; } /* namespace wolf */ diff --git a/include/core/ceres_wrapper/ceres_manager.h b/include/core/ceres_wrapper/ceres_manager.h index 2fc1254ad800781ab36827d4354d902446abce36..0454c140eda8ed520a04625fa196cebd41fbbcdd 100644 --- a/include/core/ceres_wrapper/ceres_manager.h +++ b/include/core/ceres_wrapper/ceres_manager.h @@ -46,6 +46,8 @@ class CeresManager : public SolverManager ~CeresManager(); + static SolverManagerPtr create(const ProblemPtr& wolf_problem, const paramsServer& _server); + ceres::Solver::Summary getSummary(); std::unique_ptr<ceres::Problem>& getCeresProblem() diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h index 3ce328ead448872d8a6cb2bff8d51d49f1ee9cfa..e6371f588ee2dbbe1bf977e9c790a189aebe2387 100644 --- a/include/core/common/wolf.h +++ b/include/core/common/wolf.h @@ -11,6 +11,7 @@ // Enable project-specific definitions and macros #include "internal/config.h" #include "core/utils/logging.h" +#include "core/utils/params_server.hpp" //includes from Eigen lib #include <Eigen/Dense> @@ -364,7 +365,16 @@ bool makePosDef(Eigen::Matrix<T,N,N,RC>& M, const T& eps = Constants::EPS) } //=================================================== +struct ParamsBase +{ + ParamsBase() = default; + ParamsBase(std::string _unique_name, const paramsServer&) + { + // + } + virtual ~ParamsBase() = default; +}; } // namespace wolf #endif /* WOLF_H_ */ diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h index 64e3412133cea5f8597ccf0328a4d53bbe79b43e..6b2e32db108077716f372deb7fc4b2fc4438a6c6 100644 --- a/include/core/problem/problem.h +++ b/include/core/problem/problem.h @@ -18,6 +18,9 @@ struct ProcessorParamsBase; #include "core/common/wolf.h" #include "core/frame/frame_base.h" #include "core/state_block/state_block.h" +#include "core/utils/params_server.hpp" +#include "core/sensor/autoconf_sensor_factory.h" +#include "core/processor/autoconf_processor_factory.h" // std includes #include <mutex> @@ -58,6 +61,7 @@ class Problem : public std::enable_shared_from_this<Problem> public: static ProblemPtr create(const std::string& _frame_structure, SizeEigen _dim); // USE THIS AS A CONSTRUCTOR! + static ProblemPtr autoSetup(const std::string& _frame_structure, SizeEigen _dim, const std::string& _yaml_file); virtual ~Problem(); // Properties ----------------------------------------- @@ -91,7 +95,12 @@ class Problem : public std::enable_shared_from_this<Problem> const std::string& _unique_sensor_name, // const Eigen::VectorXs& _extrinsics, // const std::string& _intrinsics_filename); - + /** + Custom installSensor using the parameters server + */ + SensorBasePtr installSensor(const std::string& _sen_type, // + const std::string& _unique_sensor_name, + const paramsServer& _server); /** \brief get a sensor pointer by its name * \param _sensor_name The sensor name, as it was installed with installSensor() */ @@ -125,7 +134,14 @@ class Problem : public std::enable_shared_from_this<Problem> const std::string& _corresponding_sensor_name, // const std::string& _params_filename = ""); - /** \brief Set the processor motion + /** + Custom installProcessor to be used with parameters server + */ + ProcessorBasePtr installProcessor(const std::string& _prc_type, // + const std::string& _unique_processor_name, // + const std::string& _corresponding_sensor_name, // + const paramsServer& _server); + /** \brief Set the processor motion * * Set the processor motion. */ diff --git a/include/core/processor/autoconf_processor_factory.h b/include/core/processor/autoconf_processor_factory.h new file mode 100644 index 0000000000000000000000000000000000000000..0fc1b5de4ac4722bf204959483ab35cc392a6ac5 --- /dev/null +++ b/include/core/processor/autoconf_processor_factory.h @@ -0,0 +1,185 @@ +/** + * \file processor_factory.h + * + * Created on: May 4, 2016 + * \author: jsola + */ + +#ifndef AUTOCONF_PROCESSOR_FACTORY_H_ +#define AUTOCONF_PROCESSOR_FACTORY_H_ + +namespace wolf +{ +class ProcessorBase; +struct ProcessorParamsBase; +} + +// wolf +#include "core/common/factory.h" + +// std + +namespace wolf +{ +/** \brief Processor factory class + * + * This factory can create objects of classes deriving from ProcessorBase. + * + * Specific object creation is invoked by create(TYPE, params), and the TYPE of processor is identified with a string. + * For example, the following processor types are implemented, + * - "ODOM 3D" for ProcessorOdom3D + * - "ODOM 2D" for ProcessorOdom2D + * - "GPS" for ProcessorGPS + * + * The rule to make new TYPE strings unique is that you skip the prefix 'Processor' from your class name, + * and you build a string in CAPITALS with space separators. + * - ProcessorImageFeature -> ````"IMAGE"```` + * - ProcessorLaser2D -> ````"LASER 2D"```` + * - etc. + * + * The methods to create specific processors are called __creators__. + * Creators must be registered to the factory before they can be invoked for processor creation. + * + * This documentation shows you how to: + * - Access the Factory + * - Register and unregister creators + * - Create processors + * - Write a processor creator for ProcessorOdom2D (example). + * + * #### Accessing the Factory + * The ProcessorFactory class is a singleton: it can only exist once in your application. + * To obtain an instance of it, use the static method get(), + * + * \code + * ProcessorFactory::get() + * \endcode + * + * You can then call the methods you like, e.g. to create a processor, you type: + * + * \code + * ProcessorFactory::get().create(...); // see below for creating processors ... + * \endcode + * + * #### Registering processor creators + * Prior to invoking the creation of a processor of a particular type, + * you must register the creator for this type into the factory. + * + * Registering processor creators into the factory is done through registerCreator(). + * You provide a processor type string (above), and a pointer to a static method + * that knows how to create your specific processor, e.g.: + * + * \code + * ProcessorFactory::get().registerCreator("ODOM 2D", ProcessorOdom2D::create); + * \endcode + * + * The method ProcessorOdom2D::create() exists in the ProcessorOdom2D class as a static method. + * All these ProcessorXxx::create() methods need to have exactly the same API, regardless of the processor type. + * This API includes a processor name, and a pointer to a base struct of parameters, ProcessorParamsBasePtr, + * that can be derived for each derived processor. + * + * Here is an example of ProcessorOdom2D::create() extracted from processor_odom_2D.h: + * + * \code + * static ProcessorBasePtr create(const std::string& _name, ProcessorParamsBasePtr _params) + * { + * // cast _params to good type + * ProcessorParamsOdom2D* params = (ProcessorParamsOdom2D*)_params; + * + * ProcessorBasePtr prc = new ProcessorOdom2D(params); + * prc->setName(_name); // pass the name to the created ProcessorOdom2D. + * return prc; + * } + * \endcode + * + * #### Achieving automatic registration + * Currently, registering is performed in each specific ProcessorXxxx source file, processor_xxxx.cpp. + * For example, in processor_odom_2D.cpp we find the line: + * + * \code + * const bool registered_odom_2D = ProcessorFactory::get().registerCreator("ODOM 2D", ProcessorOdom2D::create); + * \endcode + * + * which is a static invocation (i.e., it is placed at global scope outside of the ProcessorOdom2D class). + * Therefore, at application level, all processors that have a .cpp file compiled are automatically registered. + * + * #### Unregister processor creators + * The method unregisterCreator() unregisters the ProcessorXxx::create() method. + * It only needs to be passed the string of the processor type. + * + * \code + * ProcessorFactory::get().unregisterCreator("ODOM 2D"); + * \endcode + * + * #### Creating processors + * Prior to invoking the creation of a processor of a particular type, + * you must register the creator for this type into the factory. + * + * To create a ProcessorOdom2D, you type: + * + * \code + * ProcessorFactory::get().create("ODOM 2D", "main odometry", params_ptr); + * \endcode + * + * #### Example 1 : using the Factories alone + * We provide the necessary steps to create a processor of class ProcessorOdom2D in our application, + * and bind it to a SensorOdom2D: + * + * \code + * #include "sensor_odom_2D.h" // provides SensorOdom2D and SensorFactory + * #include "processor_odom_2D.h" // provides ProcessorOdom2D and ProcessorFactory + * + * // Note: SensorOdom2D::create() is already registered, automatically. + * // Note: ProcessorOdom2D::create() is already registered, automatically. + * + * // First create the sensor (See SensorFactory for details) + * SensorBasePtr sensor_ptr = SensorFactory::get().create ( "ODOM 2D" , "Main odometer" , extrinsics , &intrinsics ); + * + * // To create a odometry integrator, provide a type="ODOM 2D", a name="main odometry", and a pointer to the parameters struct: + * + * ProcessorParamsOdom2D params({...}); // fill in the derived struct (note: ProcessorOdom2D actually has no input params) + * + * ProcessorBasePtr processor_ptr = + * ProcessorFactory::get().create ( "ODOM 2D" , "main odometry" , ¶ms ); + * + * // Bind processor to sensor + * sensor_ptr->addProcessor(processor_ptr); + * \endcode + * + * #### Example 2: Using the helper API in class Problem + * The WOLF uppermost node, Problem, makes the creation of sensors and processors, and the binding between them, even simpler. + * + * The creation is basically replicating the factories' API. The binding is accomplished by passing the sensor name to the Processor installer. + * + * The example 1 above can be accomplished as follows (we obviated for simplicity all the parameter creation), + * + * \code + * #include "sensor_odom_2D.h" + * #include "processor_odom_2D.h" + * #include "problem.h" + * + * Problem problem(FRM_PO_2D); + * problem.installSensor ( "ODOM 2D" , "Main odometer" , extrinsics , &intrinsics ); + * problem.installProcessor ( "ODOM 2D" , "Odometry" , "Main odometer" , ¶ms ); + * \endcode + * + * You can also check the code in the example file ````src/examples/test_wolf_factories.cpp````. + */ + +typedef Factory<ProcessorBase, + const std::string&, + const paramsServer&, + const SensorBasePtr> AutoConfProcessorFactory; +template<> +inline std::string AutoConfProcessorFactory::getClass() +{ + return "AutoConfProcessorFactory"; +} + + +#define WOLF_REGISTER_PROCESSOR_AUTO(ProcessorType, ProcessorName) \ + namespace{ const bool WOLF_UNUSED ProcessorName##AutoConf##Registered = \ + wolf::AutoConfProcessorFactory::get().registerCreator(ProcessorType, ProcessorName::createAutoConf); }\ + +} /* namespace wolf */ + +#endif /* PROCESSOR_FACTORY_H_ */ diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h index c0e3143be3a782da577954a3f981a58a26d8feab..a6f8ac588195b3402ba2aebab998a8d8f6fcf1f7 100644 --- a/include/core/processor/processor_base.h +++ b/include/core/processor/processor_base.h @@ -11,6 +11,7 @@ class SensorBase; #include "core/common/node_base.h" #include "core/common/time_stamp.h" #include "core/frame/frame_base.h" +#include "core/utils/params_server.hpp" // std #include <memory> @@ -105,10 +106,9 @@ class PackKeyFrameBuffer * * Derive from this struct to create structs of processor parameters. */ -struct ProcessorParamsBase +struct ProcessorParamsBase : public ParamsBase { ProcessorParamsBase() = default; - ProcessorParamsBase(bool _voting_active, Scalar _time_tolerance, bool _voting_aux_active = false) : @@ -118,6 +118,12 @@ struct ProcessorParamsBase { // } + ProcessorParamsBase(std::string _unique_name, const paramsServer& _server): + ParamsBase(_unique_name, _server) + { + voting_active = _server.getParam<bool>(_unique_name + "/voting_active", "false"); + time_tolerance = _server.getParam<Scalar>(_unique_name + "/time_tolerance", "0"); + } virtual ~ProcessorParamsBase() = default; diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h index 4a506e040b41556f75b73ee5db93e2bd0107e1c7..2b5f1a4942f662d78b3fd97acb9714f8a00ff9ee 100644 --- a/include/core/processor/processor_diff_drive.h +++ b/include/core/processor/processor_diff_drive.h @@ -10,6 +10,7 @@ #include "core/processor/processor_motion.h" #include "core/processor/diff_drive_tools.h" +#include "core/utils/params_server.hpp" namespace wolf { @@ -17,18 +18,13 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsDiffDrive); struct ProcessorParamsDiffDrive : public ProcessorParamsMotion { -// ProcessorParamsDiffDrive(const Scalar _time_tolerance, -// const Scalar _dist_travel_th, -// const Scalar _theta_traveled_th, -// const Scalar _cov_det_th, -// const Scalar _unmeasured_perturbation_std = 0.0001) : -// dist_traveled_th_(_dist_travel_th), -// theta_traveled_th_(_theta_traveled_th), -// cov_det_th_(_cov_det_th), -// unmeasured_perturbation_std_(_unmeasured_perturbation_std) -// { -// time_tolerance = _time_tolerance; -// } + Scalar unmeasured_perturbation_std = 0.0001; + ProcessorParamsDiffDrive() = default; + ProcessorParamsDiffDrive(std::string _unique_name, const paramsServer& _server): + ProcessorParamsMotion(_unique_name, _server) + { + unmeasured_perturbation_std = _server.getParam<Scalar>(_unique_name + "/unmeasured_perturbation_std", "0.0001"); + } }; /** diff --git a/include/core/processor/processor_frame_nearest_neighbor_filter.h b/include/core/processor/processor_frame_nearest_neighbor_filter.h index ea4963588e9cc203d8cfae706362e6d00ee4a3d2..dffee75750a452891432aa255447ad1ffb8491b2 100644 --- a/include/core/processor/processor_frame_nearest_neighbor_filter.h +++ b/include/core/processor/processor_frame_nearest_neighbor_filter.h @@ -4,6 +4,7 @@ // Wolf related headers #include "core/processor/processor_loopclosure_base.h" #include "core/state_block/state_block.h" +#include "core/utils/params_server.hpp" namespace wolf{ @@ -38,7 +39,20 @@ struct ProcessorParamsFrameNearestNeighborFilter : public ProcessorParamsLoopClo { // } - + ProcessorParamsFrameNearestNeighborFilter(std::string _unique_name, const paramsServer& _server): + ProcessorParamsLoopClosure(_unique_name, _server) + { + buffer_size_ = _server.getParam<int>(_unique_name + "/buffer_size"); + sample_step_degree_ = _server.getParam<int>(_unique_name + "/sample_step_degree"); + auto distance_type_str = _server.getParam<std::string>(_unique_name + "/distance_type"); + if(distance_type_str.compare("LC_POINT_ELLIPSE")) distance_type_ = LoopclosureDistanceType::LC_POINT_ELLIPSE; + else if(distance_type_str.compare("LC_ELLIPSE_ELLIPSE")) distance_type_ = LoopclosureDistanceType::LC_ELLIPSE_ELLIPSE; + else if(distance_type_str.compare("LC_POINT_ELLIPSOID")) distance_type_ = LoopclosureDistanceType::LC_POINT_ELLIPSOID; + else if(distance_type_str.compare("LC_ELLIPSOID_ELLIPSOID")) distance_type_ = LoopclosureDistanceType::LC_ELLIPSOID_ELLIPSOID; + else if(distance_type_str.compare("LC_MAHALANOBIS_DISTANCE")) distance_type_ = LoopclosureDistanceType::LC_MAHALANOBIS_DISTANCE; + else throw std::runtime_error("Failed to fetch a valid value for the enumerate LoopclosureDistanceType. Value provided: " + distance_type_str); + probability_ = _server.getParam<Scalar>(_unique_name + "/probability"); + } virtual ~ProcessorParamsFrameNearestNeighborFilter() = default; int buffer_size_; diff --git a/include/core/processor/processor_loopclosure_base.h b/include/core/processor/processor_loopclosure_base.h index 00c63856dba087990a250854be11290b8631d9bf..a5dc8589156b6b525b0a9b551ec3dc55ab7f5af7 100644 --- a/include/core/processor/processor_loopclosure_base.h +++ b/include/core/processor/processor_loopclosure_base.h @@ -10,6 +10,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsLoopClosure); struct ProcessorParamsLoopClosure : public ProcessorParamsBase { + using ProcessorParamsBase::ProcessorParamsBase; // virtual ~ProcessorParamsLoopClosure() = default; // add neccesery parameters for loop closure initialisation here and initialize diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h index de315304c278b1bc526f7206694af3a8c9a1d12a..5b971769b72a357d2b775fa4503670ada36711c9 100644 --- a/include/core/processor/processor_motion.h +++ b/include/core/processor/processor_motion.h @@ -12,6 +12,7 @@ #include "core/capture/capture_motion.h" #include "core/processor/processor_base.h" #include "core/common/time_stamp.h" +#include "core/utils/params_server.hpp" // std #include <iomanip> @@ -23,11 +24,21 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsMotion); struct ProcessorParamsMotion : public ProcessorParamsBase { + Scalar max_time_span = 0.5; + unsigned int max_buff_length = 10; + Scalar dist_traveled = 5; + Scalar angle_turned = 0.5; Scalar unmeasured_perturbation_std = 1e-4; - Scalar max_time_span = 0.5; - unsigned int max_buff_length = 10; - Scalar dist_traveled = 5; - Scalar angle_turned = 0.5; + ProcessorParamsMotion() = default; + ProcessorParamsMotion(std::string _unique_name, const paramsServer& _server): + ProcessorParamsBase(_unique_name, _server) + { + max_time_span = _server.getParam<Scalar>(_unique_name + "/max_time_span", "0.5"); + max_buff_length = _server.getParam<unsigned int>(_unique_name + "/max_buff_length", "10"); + dist_traveled = _server.getParam<Scalar>(_unique_name + "/dist_traveled", "5"); + angle_turned = _server.getParam<Scalar>(_unique_name + "/angle_turned", "0.5"); + unmeasured_perturbation_std = _server.getParam<Scalar>(_unique_name + "/unmeasured_perturbation_std", "1e-4"); + } }; /** \brief class for Motion processors diff --git a/include/core/processor/processor_odom_2D.h b/include/core/processor/processor_odom_2D.h index d33a812a198382f10614212333340e5739368634..90d47452d1334d731b39c6154edfef8400573b0a 100644 --- a/include/core/processor/processor_odom_2D.h +++ b/include/core/processor/processor_odom_2D.h @@ -12,17 +12,24 @@ #include "core/capture/capture_odom_2D.h" #include "core/factor/factor_odom_2D.h" #include "core/math/rotations.h" +#include "core/utils/params_server.hpp" namespace wolf { - + WOLF_PTR_TYPEDEFS(ProcessorOdom2D); WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdom2D); struct ProcessorParamsOdom2D : public ProcessorParamsMotion { Scalar cov_det = 1.0; // 1 rad^2 + ProcessorParamsOdom2D() = default; + ProcessorParamsOdom2D(std::string _unique_name, const wolf::paramsServer & _server): + ProcessorParamsMotion(_unique_name, _server) + { + cov_det = _server.getParam<Scalar>(_unique_name + "/cov_det", "1.0"); + unmeasured_perturbation_std = _server.getParam<Scalar>(_unique_name + "/unmeasured_perturbation_std", "0.001"); + } }; - class ProcessorOdom2D : public ProcessorMotion { public: @@ -78,6 +85,7 @@ class ProcessorOdom2D : public ProcessorMotion // Factory method public: static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr); + static ProcessorBasePtr createAutoConf(const std::string& _unique_name, const paramsServer& _server, const SensorBasePtr sensor_ptr = nullptr); }; inline Eigen::VectorXs ProcessorOdom2D::deltaZero() const diff --git a/include/core/processor/processor_odom_3D.h b/include/core/processor/processor_odom_3D.h index 8d3f86397514e0eb602545cd215d4b1f9fa0b63b..38683c57f04c5b6908bac19e26b5cc9ee3353e49 100644 --- a/include/core/processor/processor_odom_3D.h +++ b/include/core/processor/processor_odom_3D.h @@ -21,7 +21,12 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdom3D); struct ProcessorParamsOdom3D : public ProcessorParamsMotion { - // + ProcessorParamsOdom3D() = default; + ProcessorParamsOdom3D(std::string _unique_name, const paramsServer& _server): + ProcessorParamsMotion(_unique_name, _server) + { + // + } }; WOLF_PTR_TYPEDEFS(ProcessorOdom3D); diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h index dab99e7982b9bd2e8755165e2c49fe54c89232d1..56be18b64f9e71b2dabe5b4fde223174711326ce 100644 --- a/include/core/processor/processor_tracker.h +++ b/include/core/processor/processor_tracker.h @@ -12,13 +12,21 @@ #include "core/capture/capture_motion.h" namespace wolf { - + WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTracker); struct ProcessorParamsTracker : public ProcessorParamsBase { - unsigned int min_features_for_keyframe; ///< minimum nbr. of features to vote for keyframe - int max_new_features; ///< maximum nbr. of new features to be processed when adding a keyframe (-1: unlimited. 0: none.) + unsigned int min_features_for_keyframe; ///< minimum nbr. of features to vote for keyframe + int max_new_features; ///< maximum nbr. of new features to be processed when adding a keyframe (-1: unlimited. 0: none.) + + ProcessorParamsTracker() = default; + ProcessorParamsTracker(std::string _unique_name, const wolf::paramsServer & _server): + ProcessorParamsBase(_unique_name, _server) + { + min_features_for_keyframe = _server.getParam<unsigned int>(_unique_name + "/min_features_for_keyframe", "1"); + max_new_features = _server.getParam<int>(_unique_name + "/max_new_features", "-1"); + } }; WOLF_PTR_TYPEDEFS(ProcessorTracker); diff --git a/include/core/processor/processor_tracker_feature.h b/include/core/processor/processor_tracker_feature.h index 4fa99aa2676b9aea273378faf9324720f0255636..055e2ddf87220a65289c09e20746dd4da990541d 100644 --- a/include/core/processor/processor_tracker_feature.h +++ b/include/core/processor/processor_tracker_feature.h @@ -22,9 +22,9 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeature); struct ProcessorParamsTrackerFeature : public ProcessorParamsTracker { - // + using ProcessorParamsTracker::ProcessorParamsTracker; }; - + WOLF_PTR_TYPEDEFS(ProcessorTrackerFeature); /** \brief Feature tracker processor diff --git a/include/core/processor/processor_tracker_landmark.h b/include/core/processor/processor_tracker_landmark.h index 23c2be119a8d91de8aeb84061b8dc914cfd76d31..0e7e2e0a2b7bac5161caa2d7969766a2b57f2581 100644 --- a/include/core/processor/processor_tracker_landmark.h +++ b/include/core/processor/processor_tracker_landmark.h @@ -20,6 +20,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerLandmark); struct ProcessorParamsTrackerLandmark : public ProcessorParamsTracker { + using ProcessorParamsTracker::ProcessorParamsTracker; // }; diff --git a/include/core/sensor/autoconf_sensor_factory.h b/include/core/sensor/autoconf_sensor_factory.h new file mode 100644 index 0000000000000000000000000000000000000000..e52366e334187e9f318bb9a514c1e7fdde6b3b21 --- /dev/null +++ b/include/core/sensor/autoconf_sensor_factory.h @@ -0,0 +1,227 @@ +/** + * \file sensor_factory.h + * + * Created on: Apr 25, 2016 + * \author: jsola + */ + +#ifndef AUTOCONF_SENSOR_FACTORY_H_ +#define AUTOCONF_SENSOR_FACTORY_H_ + +namespace wolf +{ +class SensorBase; +struct IntrinsicsBase; +} + +// wolf +#include "core/common/factory.h" + +namespace wolf +{ + +/** \brief Sensor factory class + * + * This factory can create objects of classes deriving from SensorBase. + * + * Specific object creation is invoked by ````create(TYPE, params ... )````, and the TYPE of sensor is identified with a string. + * Currently, the following sensor types are implemented, + * - "CAMERA" for SensorCamera + * - "ODOM 2D" for SensorOdom2D + * - "GPS FIX" for SensorGPSFix + * + * The rule to make new TYPE strings unique is that you skip the prefix 'Sensor' from your class name, + * and you build a string in CAPITALS with space separators, e.g.: + * - SensorCamera -> ````"CAMERA"```` + * - SensorLaser2D -> ````"LASER 2D"```` + * - etc. + * + * The methods to create specific sensors are called __creators__. + * Creators must be registered to the factory before they can be invoked for sensor creation. + * + * This documentation shows you how to: + * - Access the factory + * - Register and unregister creators + * - Create sensors + * - Write a sensor creator for SensorCamera (example). + * + * #### Accessing the factory + * The SensorFactory class is a <a href="http://stackoverflow.com/questions/1008019/c-singleton-design-pattern#1008289">singleton</a>: it can only exist once in your application. + * To obtain an instance of it, use the static method get(), + * + * \code + * SensorFactory::get() + * \endcode + * + * You can then call the methods you like, e.g. to create a sensor, you type: + * + * \code + * SensorFactory::get().create(...); // see below for creating sensors ... + * \endcode + * + * #### Registering sensor creators + * Prior to invoking the creation of a sensor of a particular type, + * you must register the creator for this type into the factory. + * + * Registering sensor creators into the factory is done through registerCreator(). + * You provide a sensor type string (above), and a pointer to a static method + * that knows how to create your specific sensor, e.g.: + * + * \code + * SensorFactory::get().registerCreator("CAMERA", SensorCamera::create); + * \endcode + * + * The method SensorCamera::create() exists in the SensorCamera class as a static method. + * All these ````SensorXxx::create()```` methods need to have exactly the same API, regardless of the sensor type. + * This API includes a sensor name, a vector of extrinsic parameters, + * and a pointer to a base struct of intrinsic parameters, IntrinsicsBasePtr, + * that can be derived for each derived sensor: + * + * \code + * static SensorBasePtr create(const std::string& _name, Eigen::VectorXs& _extrinsics_pq, IntrinsicsBasePtr _intrinsics) + * \endcode + * + * See further down for an implementation example. + * + * #### Achieving automatic registration + * Currently, registering is performed in each specific SensorXxxx source file, sensor_xxxx.cpp. + * For example, in sensor_camera.cpp we find the line: + * + * \code + * const bool registered_camera = SensorFactory::get().registerCreator("CAMERA", SensorCamera::create); + * \endcode + * + * which is a static invocation (i.e., it is placed at global scope outside of the SensorCamera class). + * Therefore, at application level, all sensors that have a .cpp file compiled are automatically registered. + * + * #### Unregistering sensor creators + * The method unregisterCreator() unregisters the SensorXxx::create() method. It only needs to be passed the string of the sensor type. + * + * \code + * SensorFactory::get().unregisterCreator("CAMERA"); + * \endcode + * + * #### Creating sensors + * Note: Prior to invoking the creation of a sensor of a particular type, + * you must register the creator for this type into the factory. + * + * To create e.g. a SensorCamera, you type: + * + * \code + * SensorFactory::get().create("CAMERA", "Front-left camera", extrinsics, intrinsics_ptr); + * \endcode + * + * where ABSOLUTELY ALL input parameters are important. In particular, the sensor name "Front-left camera" will be used to identify this camera + * and to assign it the appropriate processors. DO NOT USE IT WITH DUMMY PARAMETERS! + * + * #### See also + * - IntrinsicsFactory: to create intrinsic structs deriving from IntrinsicsBase directly from YAML files. + * - ProcessorFactory: to create processors that will be bound to sensors. + * - Problem::installSensor() : to install sensors in WOLF Problem. + * + * #### Example 1: writing a specific sensor creator + * Here is an example of SensorCamera::create() extracted from sensor_camera.cpp: + * + * \code + * static SensorBasePtr create(const std::string& _name, Eigen::VectorXs& _extrinsics_pq, IntrinsicsBasePtr _intrinsics) + * { + * // check extrinsics vector + * assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D."); + * + * // cast instrinsics to good type + * IntrinsicsCamera* intrinsics_ptr = (IntrinsicsCamera*) _intrinsics; + * + * // Do create the SensorCamera object, and complete its setup + * SensorCamera* sen_ptr = new SensorCamera(_extrinsics_pq, intrinsics_ptr); + * sen_ptr->setName(_unique_name); + * + * return sen_ptr; + * } + * \endcode + * + * #### Example 2: registering a sensor creator into the factory + * Registration can be done manually or automatically. It involves the call to static functions. + * It is advisable to put these calls within unnamed namespaces. + * + * - __Manual registration__: you control registration at application level. + * Put the code either at global scope (you must define a dummy variable for this), + * \code + * namespace { + * const bool registered_camera = SensorFactory::get().registerCreator("CAMERA", SensorCamera::create); + * } + * main () { ... } + * \endcode + * or inside your main(), where a direct call is possible: + * \code + * main () { + * SensorFactory::get().registerCreator("CAMERA", SensorCamera::create); + * ... + * } + * \endcode + * + * - __Automatic registration__: registration is performed at library level. + * Put the code at the last line of the sensor_xxx.cpp file, + * \code + * namespace { + * const bool registered_camera = SensorFactory::get().registerCreator("CAMERA", SensorCamera::create); + * } + * \endcode + * Automatic registration is recommended in wolf, and implemented in the classes shipped with it. + * You are free to comment out these lines and place them wherever you consider it more convenient for your design. + * + * #### Example 2: creating sensors + * We finally provide the necessary steps to create a sensor of class SensorCamera in our application: + * + * \code + * #include "sensor_factory.h" + * #include "sensor_camera.h" // provides SensorCamera + * + * // Note: SensorCamera::create() is already registered, automatically. + * + * using namespace wolf; + * int main() { + * + * // To create a camera, provide: + * // a type = "CAMERA", + * // a name = "Front-left camera", + * // an extrinsics vector, and + * // a pointer to the intrinsics struct: + * + * Eigen::VectorXs extrinsics_1(7); // give it some values... + * IntrinsicsCamera intrinsics_1({...}); // see IntrinsicsFactory to fill in the derived struct + * + * SensorBasePtr camera_1_ptr = + * SensorFactory::get().create ( "CAMERA" , "Front-left camera" , extrinsics_1 , &intrinsics_1 ); + * + * // A second camera... with a different name! + * + * Eigen::VectorXs extrinsics_2(7); + * IntrinsicsCamera intrinsics_2({...}); + * + * SensorBasePtr camera_2_ptr = + * SensorFactory::get().create( "CAMERA" , "Front-right camera" , extrinsics_2 , &intrinsics_2 ); + * + * return 0; + * } + * \endcode + * + * You can also check the code in the example file ````src/examples/test_wolf_factories.cpp````. + */ + +typedef Factory<SensorBase, + const std::string&, + const paramsServer&> AutoConfSensorFactory; + +template<> +inline std::string AutoConfSensorFactory::getClass() +{ + return "AutoConfSensorFactory"; +} + +#define WOLF_REGISTER_SENSOR_AUTO(SensorType, SensorName) \ + namespace{ const bool WOLF_UNUSED SensorName##AutConf##Registered = \ + AutoConfSensorFactory::get().registerCreator(SensorType, SensorName::createAutoConf); } \ + +} /* namespace wolf */ + +#endif /* SENSOR_FACTORY_H_ */ diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h index 9567967d48d4f2ea831158ee38b5524e76548610..2e05d2fe57dfb3494cd3b33c2d697510ff493849 100644 --- a/include/core/sensor/sensor_base.h +++ b/include/core/sensor/sensor_base.h @@ -21,9 +21,10 @@ namespace wolf { * * Derive from this struct to create structs of sensor intrinsic parameters. */ -struct IntrinsicsBase +struct IntrinsicsBase: public ParamsBase { virtual ~IntrinsicsBase() = default; + using ParamsBase::ParamsBase; }; class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBase> diff --git a/include/core/sensor/sensor_diff_drive.h b/include/core/sensor/sensor_diff_drive.h index 5cd9e848da2412fbac1daa8455699d1cc0783aa5..c1cbd7c382c58244e0e6a12f31b27d472b3507de 100644 --- a/include/core/sensor/sensor_diff_drive.h +++ b/include/core/sensor/sensor_diff_drive.h @@ -11,6 +11,7 @@ //wolf includes #include "core/sensor/sensor_base.h" #include "core/processor/diff_drive_tools.h" +#include "core/utils/params_server.hpp" namespace wolf { @@ -33,6 +34,32 @@ struct IntrinsicsDiffDrive : public IntrinsicsBase Scalar left_gain_ = 0.01; Scalar right_gain_ = 0.01; + IntrinsicsDiffDrive() + { + //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE. + } + IntrinsicsDiffDrive(std::string _unique_name, const paramsServer& _server): + IntrinsicsBase(_unique_name, _server) + { + + left_radius_ = _server.getParam<Scalar>(_unique_name + "/left_radius_"); + right_radius_ = _server.getParam<Scalar>(_unique_name + "/right_radius_"); + separation_ = _server.getParam<Scalar>(_unique_name + "/separation_"); + + auto model_str = _server.getParam<std::string>(_unique_name + "/model"); + if(model_str.compare("Two_Factor_Model")) model_ = DiffDriveModel::Two_Factor_Model; + else if(model_str.compare("Three_Factor_Model")) model_ = DiffDriveModel::Three_Factor_Model; + else if(model_str.compare("Five_Factor_Model")) model_ = DiffDriveModel::Five_Factor_Model; + else throw std::runtime_error("Failed to fetch a valid value for the enumerate DiffDriveModel. Value provided: " + model_str); + + factors_ = _server.getParam<Eigen::VectorXs>(_unique_name + "/factors", "[1,1,1]"); + + left_resolution_ = _server.getParam<Scalar>(_unique_name + "/left_resolution_"); + right_resolution_ = _server.getParam<Scalar>(_unique_name + "/right_resolution_"); + + left_gain_ = _server.getParam<Scalar>(_unique_name + "/left_gain", "0.01"); + right_gain_ = _server.getParam<Scalar>(_unique_name + "/right_gain", "0.01"); + } virtual ~IntrinsicsDiffDrive() = default; }; diff --git a/include/core/sensor/sensor_odom_2D.h b/include/core/sensor/sensor_odom_2D.h index d7eec490c8a01eda75f24ca845ea53061a727de1..02218e8698790d207e61594befd403bca916862a 100644 --- a/include/core/sensor/sensor_odom_2D.h +++ b/include/core/sensor/sensor_odom_2D.h @@ -1,9 +1,9 @@ - #ifndef SENSOR_ODOM_2D_H_ #define SENSOR_ODOM_2D_H_ //wolf includes #include "core/sensor/sensor_base.h" +#include "core/utils/params_server.hpp" namespace wolf { @@ -15,6 +15,16 @@ struct IntrinsicsOdom2D : public IntrinsicsBase Scalar k_disp_to_disp; ///< ratio of displacement variance to displacement, for odometry noise calculation Scalar k_rot_to_rot; ///< ratio of rotation variance to rotation, for odometry noise calculation + IntrinsicsOdom2D() + { + //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE. + } + IntrinsicsOdom2D(std::string _unique_name, const paramsServer& _server): + IntrinsicsBase(_unique_name, _server) + { + k_disp_to_disp = _server.getParam<Scalar>(_unique_name + "/k_disp_to_disp"); + k_rot_to_rot = _server.getParam<Scalar>(_unique_name + "/k_rot_to_rot"); + } }; WOLF_PTR_TYPEDEFS(SensorOdom2D); @@ -30,24 +40,25 @@ class SensorOdom2D : public SensorBase SensorOdom2D(Eigen::VectorXs _extrinsics, IntrinsicsOdom2DPtr _intrinsics); virtual ~SensorOdom2D(); - + /** \brief Returns displacement noise factor - * + * * Returns displacement noise factor - * - **/ + * + **/ Scalar getDispVarToDispNoiseFactor() const; /** \brief Returns rotation noise factor - * + * * Returns rotation noise factor - * - **/ + * + **/ Scalar getRotVarToRotNoiseFactor() const; - + public: static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsBasePtr _intrinsics); + static SensorBasePtr createAutoConf(const std::string& _unique_name, const paramsServer& _server); }; diff --git a/include/core/sensor/sensor_odom_3D.h b/include/core/sensor/sensor_odom_3D.h index af3301e1acc5bb46fd21f1fce81c0bf78bc42e7a..dffaf433ebd1ff29a258aec53e5478874b51f7c7 100644 --- a/include/core/sensor/sensor_odom_3D.h +++ b/include/core/sensor/sensor_odom_3D.h @@ -10,6 +10,7 @@ //wolf includes #include "core/sensor/sensor_base.h" +#include "core/utils/params_server.hpp" namespace wolf { @@ -22,7 +23,19 @@ struct IntrinsicsOdom3D : public IntrinsicsBase Scalar k_rot_to_rot; ///< ratio of rotation variance to rotation, for odometry noise calculation Scalar min_disp_var; Scalar min_rot_var; - + IntrinsicsOdom3D() + { + //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE. + } + IntrinsicsOdom3D(std::string _unique_name, const paramsServer& _server): + IntrinsicsBase(_unique_name, _server) + { + k_disp_to_disp = _server.getParam<Scalar>(_unique_name + "/k_disp_to_disp"); + k_disp_to_rot = _server.getParam<Scalar>(_unique_name + "/k_disp_to_rot"); + k_rot_to_rot = _server.getParam<Scalar>(_unique_name + "/k_rot_to_rot"); + min_disp_var = _server.getParam<Scalar>(_unique_name + "/min_disp_var"); + min_rot_var = _server.getParam<Scalar>(_unique_name + "/min_rot_var"); + } virtual ~IntrinsicsOdom3D() = default; }; diff --git a/include/core/solver/solver_factory.h b/include/core/solver/solver_factory.h new file mode 100644 index 0000000000000000000000000000000000000000..36a551b668a1f4f1c595d98ea61b1534f54c670e --- /dev/null +++ b/include/core/solver/solver_factory.h @@ -0,0 +1,228 @@ +/** + * \file solver_factory.h + * + * Created on: Dec 17, 2018 + * \author: jcasals + */ + +#ifndef SOLVER_FACTORY_H_ +#define SOLVER_FACTORY_H_ + +namespace wolf +{ +class SensorBase; +struct IntrinsicsBase; +} + +// wolf +#include "core/common/factory.h" +#include "core/solver/solver_manager.h" + +namespace wolf +{ + +/** \brief Sensor factory class + * + * This factory can create objects of classes deriving from SensorBase. + * + * Specific object creation is invoked by ````create(TYPE, params ... )````, and the TYPE of sensor is identified with a string. + * Currently, the following sensor types are implemented, + * - "CAMERA" for SensorCamera + * - "ODOM 2D" for SensorOdom2D + * - "GPS FIX" for SensorGPSFix + * + * The rule to make new TYPE strings unique is that you skip the prefix 'Sensor' from your class name, + * and you build a string in CAPITALS with space separators, e.g.: + * - SensorCamera -> ````"CAMERA"```` + * - SensorLaser2D -> ````"LASER 2D"```` + * - etc. + * + * The methods to create specific sensors are called __creators__. + * Creators must be registered to the factory before they can be invoked for sensor creation. + * + * This documentation shows you how to: + * - Access the factory + * - Register and unregister creators + * - Create sensors + * - Write a sensor creator for SensorCamera (example). + * + * #### Accessing the factory + * The SensorFactory class is a <a href="http://stackoverflow.com/questions/1008019/c-singleton-design-pattern#1008289">singleton</a>: it can only exist once in your application. + * To obtain an instance of it, use the static method get(), + * + * \code + * SensorFactory::get() + * \endcode + * + * You can then call the methods you like, e.g. to create a sensor, you type: + * + * \code + * SensorFactory::get().create(...); // see below for creating sensors ... + * \endcode + * + * #### Registering sensor creators + * Prior to invoking the creation of a sensor of a particular type, + * you must register the creator for this type into the factory. + * + * Registering sensor creators into the factory is done through registerCreator(). + * You provide a sensor type string (above), and a pointer to a static method + * that knows how to create your specific sensor, e.g.: + * + * \code + * SensorFactory::get().registerCreator("CAMERA", SensorCamera::create); + * \endcode + * + * The method SensorCamera::create() exists in the SensorCamera class as a static method. + * All these ````SensorXxx::create()```` methods need to have exactly the same API, regardless of the sensor type. + * This API includes a sensor name, a vector of extrinsic parameters, + * and a pointer to a base struct of intrinsic parameters, IntrinsicsBasePtr, + * that can be derived for each derived sensor: + * + * \code + * static SensorBasePtr create(const std::string& _name, Eigen::VectorXs& _extrinsics_pq, IntrinsicsBasePtr _intrinsics) + * \endcode + * + * See further down for an implementation example. + * + * #### Achieving automatic registration + * Currently, registering is performed in each specific SensorXxxx source file, sensor_xxxx.cpp. + * For example, in sensor_camera.cpp we find the line: + * + * \code + * const bool registered_camera = SensorFactory::get().registerCreator("CAMERA", SensorCamera::create); + * \endcode + * + * which is a static invocation (i.e., it is placed at global scope outside of the SensorCamera class). + * Therefore, at application level, all sensors that have a .cpp file compiled are automatically registered. + * + * #### Unregistering sensor creators + * The method unregisterCreator() unregisters the SensorXxx::create() method. It only needs to be passed the string of the sensor type. + * + * \code + * SensorFactory::get().unregisterCreator("CAMERA"); + * \endcode + * + * #### Creating sensors + * Note: Prior to invoking the creation of a sensor of a particular type, + * you must register the creator for this type into the factory. + * + * To create e.g. a SensorCamera, you type: + * + * \code + * SensorFactory::get().create("CAMERA", "Front-left camera", extrinsics, intrinsics_ptr); + * \endcode + * + * where ABSOLUTELY ALL input parameters are important. In particular, the sensor name "Front-left camera" will be used to identify this camera + * and to assign it the appropriate processors. DO NOT USE IT WITH DUMMY PARAMETERS! + * + * #### See also + * - IntrinsicsFactory: to create intrinsic structs deriving from IntrinsicsBase directly from YAML files. + * - ProcessorFactory: to create processors that will be bound to sensors. + * - Problem::installSensor() : to install sensors in WOLF Problem. + * + * #### Example 1: writing a specific sensor creator + * Here is an example of SensorCamera::create() extracted from sensor_camera.cpp: + * + * \code + * static SensorBasePtr create(const std::string& _name, Eigen::VectorXs& _extrinsics_pq, IntrinsicsBasePtr _intrinsics) + * { + * // check extrinsics vector + * assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D."); + * + * // cast instrinsics to good type + * IntrinsicsCamera* intrinsics_ptr = (IntrinsicsCamera*) _intrinsics; + * + * // Do create the SensorCamera object, and complete its setup + * SensorCamera* sen_ptr = new SensorCamera(_extrinsics_pq, intrinsics_ptr); + * sen_ptr->setName(_unique_name); + * + * return sen_ptr; + * } + * \endcode + * + * #### Example 2: registering a sensor creator into the factory + * Registration can be done manually or automatically. It involves the call to static functions. + * It is advisable to put these calls within unnamed namespaces. + * + * - __Manual registration__: you control registration at application level. + * Put the code either at global scope (you must define a dummy variable for this), + * \code + * namespace { + * const bool registered_camera = SensorFactory::get().registerCreator("CAMERA", SensorCamera::create); + * } + * main () { ... } + * \endcode + * or inside your main(), where a direct call is possible: + * \code + * main () { + * SensorFactory::get().registerCreator("CAMERA", SensorCamera::create); + * ... + * } + * \endcode + * + * - __Automatic registration__: registration is performed at library level. + * Put the code at the last line of the sensor_xxx.cpp file, + * \code + * namespace { + * const bool registered_camera = SensorFactory::get().registerCreator("CAMERA", SensorCamera::create); + * } + * \endcode + * Automatic registration is recommended in wolf, and implemented in the classes shipped with it. + * You are free to comment out these lines and place them wherever you consider it more convenient for your design. + * + * #### Example 2: creating sensors + * We finally provide the necessary steps to create a sensor of class SensorCamera in our application: + * + * \code + * #include "sensor_factory.h" + * #include "sensor_camera.h" // provides SensorCamera + * + * // Note: SensorCamera::create() is already registered, automatically. + * + * using namespace wolf; + * int main() { + * + * // To create a camera, provide: + * // a type = "CAMERA", + * // a name = "Front-left camera", + * // an extrinsics vector, and + * // a pointer to the intrinsics struct: + * + * Eigen::VectorXs extrinsics_1(7); // give it some values... + * IntrinsicsCamera intrinsics_1({...}); // see IntrinsicsFactory to fill in the derived struct + * + * SensorBasePtr camera_1_ptr = + * SensorFactory::get().create ( "CAMERA" , "Front-left camera" , extrinsics_1 , &intrinsics_1 ); + * + * // A second camera... with a different name! + * + * Eigen::VectorXs extrinsics_2(7); + * IntrinsicsCamera intrinsics_2({...}); + * + * SensorBasePtr camera_2_ptr = + * SensorFactory::get().create( "CAMERA" , "Front-right camera" , extrinsics_2 , &intrinsics_2 ); + * + * return 0; + * } + * \endcode + * + * You can also check the code in the example file ````src/examples/test_wolf_factories.cpp````. + */ + +typedef Factory<SolverManager, + const ProblemPtr&, + const paramsServer&> SolverFactory; + +template<> +inline std::string SolverFactory::getClass() +{ + return "SolverFactory"; +} + +#define WOLF_REGISTER_SOLVER(SolverName) \ + namespace{ const bool WOLF_UNUSED SolverName##Registered = \ + wolf::SolverFactory::get().registerCreator("Solver", SolverName::create); } \ + +} /* namespace wolf */ + +#endif /* SENSOR_FACTORY_H_ */ diff --git a/include/core/utils/converter.h b/include/core/utils/converter.h new file mode 100644 index 0000000000000000000000000000000000000000..1824bbf870095023d17789374d4e244bf9e2434a --- /dev/null +++ b/include/core/utils/converter.h @@ -0,0 +1,252 @@ +#ifndef CONVERTER_H +#define CONVERTER_H +#include <vector> +// Eigen +#include <eigen3/Eigen/Dense> +#include <eigen3/Eigen/Geometry> +#include <regex> +#include <iostream> + +namespace utils{ + template <typename A> + using list = std::vector<A>; + // template <typename A> + // class toString<A>{}; + static inline std::vector<std::string> splitter(std::string val){ + std::vector<std::string> cont = std::vector<std::string>(); + std::stringstream ss(val); + std::string token; + while (std::getline(ss, token, ',')) { + cont.push_back(token); + } + return cont; + } + static inline std::vector<std::string> getMatches(std::string val, std::regex exp){ + std::smatch res; + auto v = std::vector<std::string>(); + std::string::const_iterator searchStart( val.cbegin() ); + while ( std::regex_search( searchStart, val.cend(), res, exp ) ) { + v.push_back(res[0]); + searchStart = res.suffix().first; + } + return v; + } + static inline std::vector<std::string> pairSplitter(std::string val){ + std::regex exp("(\\{[^\\{:]:.*?\\})"); + return getMatches(val, exp); + } + static inline std::array<std::string,2> splitMatrixStringRepresentation(std::string matrix){ + std::regex rgx("\\[\\[((?:[0-9]+,?)+)\\],((?:(?:[0-9]+\\.)?[0-9]+,?)+)\\]"); + std::regex rgxStatic("\\[((?:(?:[0-9]+\\.)?[0-9]+,?)+)\\]"); + std::smatch matches; + std::smatch matchesStatic; + std::array<std::string,2> values = {{"[]","[]"}}; + if(std::regex_match(matrix, matches, rgx)) { + values[0] = "[" + matches[1].str() + "]"; + values[1] = "[" + matches[2].str() + "]"; + }else if(std::regex_match(matrix, matchesStatic, rgxStatic)){ + values[1] = "[" + matchesStatic[1].str() + "]"; + }else{ + throw std::runtime_error("Invalid string representation of a Matrix. Correct format is [([num,num],)?(num(,num)*)?]. String provided: " + matrix); + } + return values; + } + static inline std::string splitMapStringRepresentation(std::string str_map){ + std::smatch mmatches; + std::regex rgxM("\\[((?:(?:\\{[^\\{:]+:[^:\\}]+\\}),?)*)\\]"); + // auto v = std::vector<std::string>(); + std::string result = ""; + if(std::regex_match(str_map, mmatches, rgxM)) { + // v = splitter(mmatches[1].str()); + result = mmatches[1].str(); + } else { + throw std::runtime_error("Invalid string representation of a Map. Correct format is [({id:value})?(,{id:value})*]. String provided: " + str_map); + } + return result; + } +} +namespace wolf{ + +template<typename T> +struct converter{ + static T convert(std::string val){ + assert(1 == 0 && "There is no general convert for arbitrary T !!!"); + } +}; +template<typename A> +struct converter<utils::list<A>>{ + static utils::list<A> convert(std::string val){ + std::regex rgxP("\\[([^,]+)?(,[^,]+)*\\]"); + utils::list<A> result = utils::list<A>(); + if(std::regex_match(val, rgxP)) { + std::string aux = val.substr(1,val.size()-2); + auto l = utils::getMatches(aux, std::regex("([^,]+),?")); + for(auto it : l){ + result.push_back(converter<A>::convert(it)); + } + } else throw std::runtime_error("Invalid string format representing a list-like structure. Correct format is [(value)?(,value)*]. String provided: " + val); + return result; + } + static std::string convert(utils::list<A> val){ + std::string aux = ""; + bool first = true; + for(auto it : val){ + if(not first) aux += "," + it; + else{ + first = false; + aux = it; + } + } + return "[" + aux + "]"; + } +}; +template<> +struct converter<int>{ + static int convert(std::string val){ + return stod(val); + } +}; +template<> +struct converter<unsigned int>{ + static unsigned int convert(std::string val){ + return stod(val); + } +}; +template<> +struct converter<double>{ + static double convert(std::string val){ + return stod(val); + } +}; +template<> +struct converter<bool>{ + static bool convert(std::string val){ + if(val == "true") return true; + else if (val == "false") return false; + else throw std::runtime_error("Invalid conversion to bool (Must be either \"true\" or \"false\"). String provided: " + val); + } +}; +template<> +struct converter<std::string>{ + static std::string convert(std::string val){ + return val; + } + template<typename T> + static std::string convert(T val){ + assert(1 == 0 && "There is no general convert to string for arbitrary T !!!"); + return ""; + } + static std::string convert(int val){ + return std::to_string(val); + } + static std::string convert(double val){ + return std::to_string(val); + } + template<typename A> + static std::string convert(utils::list<A> val){ + std::string result = ""; + for(auto it : val){ + result += "," + converter<std::string>::convert(it); + } + if(result.size() > 0) result = result.substr(1,result.size()); + return "[" + result + "]"; + } + template<typename A, typename B> + static std::string convert(std::pair<A,B> val){ + return "{" + converter<std::string>::convert(val.first) + ":" + converter<std::string>::convert(val.second) + "}"; + } + template<typename A, typename B> + static std::string convert(std::map<A,B> val){ + std::string result = ""; + for(auto it : val){ + result += "," + converter<std::string>::convert(it); + } + if(result.size() > 0) result = result.substr(1,result.size()); + return "[" + result + "]"; + } + template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols> + static std::string convert(Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> val){ + std::string result = ""; + for(int i = 0; i < val.rows() ; ++i){ + for(int j = 0; j < val.cols(); ++j){ + result += "," + converter<std::string>::convert(val(i,j)); + } + } + if(result.size() > 0) result = result.substr(1,result.size()); + if(_Rows == Eigen::Dynamic and _Cols == Eigen::Dynamic){ + result = "[" + std::to_string(val.rows()) + "," + std::to_string(val.cols()) + "]," + result; + } + return "[" + result + "]"; + } +}; +template<typename A, typename B> +struct converter<std::pair<A,B>>{ + static std::pair<A,B> convert(std::string val){ + std::regex rgxP("\\{([^\\{:]+):([^\\}]+)\\}"); + std::smatch matches; + if(std::regex_match(val, matches, rgxP)) { + return std::pair<A,B>(converter<A>::convert(matches[1].str()), converter<B>::convert(matches[2].str())); + } else throw std::runtime_error("Invalid string format representing a pair. Correct format is {identifier:value}. String provided: " + val); + } +}; +// TODO: WARNING!! For some reason when trying to specialize converter to std::array +// it defaults to the generic T type, thus causing an error! + +template<typename A, unsigned int N> +struct converter<std::array<A, N>>{ + static std::array<A,N> convert(std::string val){ + // std::vector<std::string> aux = utils::splitter(val); + auto aux = converter<utils::list<A>>::convert(val); + std::array<A,N> rtn = std::array<A,N>(); + if(N != aux.size()) throw std::runtime_error("Error in trying to transform literal string to Array. Invalid argument size. Required size " + + std::to_string(N) + "; provided size " + std::to_string(aux.size())); + for (int i = 0; i < N; ++i) { + rtn[i] = aux[i]; + } + return rtn; + } +}; +template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols> +struct converter<Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>>{ + typedef Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> Matrix; + static Matrix convert(std::string val){ + auto splittedValues = utils::splitMatrixStringRepresentation(val); + auto dimensions = converter<std::vector<int>>::convert(splittedValues[0]); + auto values = converter<std::vector<_Scalar>>::convert(splittedValues[1]); + Matrix m = Matrix(); + if(_Rows == Eigen::Dynamic and _Cols == Eigen::Dynamic) { + if(dimensions.size() != 2) throw std::runtime_error("Invalid string representing Eigen Matrix. Missing dimensions. String provided: " + val); + m.resize(dimensions[0],dimensions[1]); + }else if(_Rows == Eigen::Dynamic){ + int nrows = (int) values.size() / _Cols; + m.resize(nrows, _Cols); + }else if(_Cols == Eigen::Dynamic){ + int ncols = (int) values.size() / _Rows; + m.resize(_Rows, ncols); + } + if(m.rows()*m.cols() != values.size()) throw std::runtime_error("The literal string provides " + std::to_string(values.size()) + " values but " + + "the Eigen matrix is of dimensions " + + std::to_string(m.rows()) + "x" + std::to_string(m.cols())); + else{ + for (int i = 0; i < m.rows(); i++) + for (int j = 0; j < m.cols(); j++) + m(i, j) = values[(int)(i * m.cols() + j)]; + } + return m; + } +}; +template<typename A> +struct converter<std::map<std::string,A>>{ + static std::map<std::string,A> convert(std::string val){ + auto str_map = utils::splitMapStringRepresentation(val); + auto v = utils::pairSplitter(str_map); + auto map = std::map<std::string, A>(); + for(auto it : v){ + auto p = converter<std::pair<std::string, A>>::convert(it); + map.insert(std::pair<std::string, A>(p.first, p.second)); + } + return map; + } +}; +} +#endif \ No newline at end of file diff --git a/include/core/utils/loader.hpp b/include/core/utils/loader.hpp new file mode 100644 index 0000000000000000000000000000000000000000..4f36eeecb9cda277e16bb495349ea047b546b6ce --- /dev/null +++ b/include/core/utils/loader.hpp @@ -0,0 +1,43 @@ +#ifndef LOADER_HPP +#define LOADER_HPP +#include <dlfcn.h> +class Loader{ +protected: + std::string path_; +public: + Loader(std::string _file){ + this->path_ = _file; + } + virtual void load() = 0; + virtual void close() = 0; +}; +class LoaderRaw: public Loader{ + void* resource_; +public: + LoaderRaw(std::string _file): + Loader(_file) + { + // + } + ~LoaderRaw(){ + this->close(); + } + void load(){ + this->resource_ = dlopen(this->path_.c_str(), RTLD_LAZY); + if(not this->resource_) + throw std::runtime_error("Couldn't load resource with path " + this->path_); + } + void close(){ + if(this->resource_) dlclose(this->resource_); + } +}; +// class LoaderPlugin: public Loader{ +// ClassLoader* resource_; +// void load(){ +// this->resource_ = new ClassLoader(this->path_); +// } +// void close(){ +// delete this->resource_; +// } +// }; +#endif \ No newline at end of file diff --git a/include/core/utils/params_server.hpp b/include/core/utils/params_server.hpp new file mode 100644 index 0000000000000000000000000000000000000000..74fb8f3e7c5f9dc12ea19cc7e85eb43dfbf011fd --- /dev/null +++ b/include/core/utils/params_server.hpp @@ -0,0 +1,88 @@ +#ifndef PARAMS_SERVER_HPP +#define PARAMS_SERVER_HPP +#include <vector> +#include <regex> +#include <map> +#include "core/utils/converter.h" +namespace wolf{ +class paramsServer{ + struct ParamsInitSensor{ + std::string _type; + std::string _name; + }; + struct ParamsInitProcessor{ + std::string _type; + std::string _name; + std::string _name_assoc_sensor; + }; + std::map<std::string, std::string> _params; + std::map<std::string,ParamsInitSensor> _paramsSens; + std::map<std::string,ParamsInitProcessor> _paramsProc; +public: + paramsServer(){ + _params = std::map<std::string, std::string>(); + _paramsSens = std::map<std::string,ParamsInitSensor>(); + _paramsProc = std::map<std::string,ParamsInitProcessor>(); + } + paramsServer(std::map<std::string, std::string> params, + std::vector<std::array<std::string,2>> sensors, + std::vector<std::array<std::string,3>> procs){ + _params = params; + _paramsSens = std::map<std::string,ParamsInitSensor>(); + _paramsProc = std::map<std::string,ParamsInitProcessor>(); + for(auto it : sensors) { + ParamsInitSensor pSensor = {it.at(0), it.at(1)}; + _paramsSens.insert(std::pair<std::string,ParamsInitSensor>(it.at(1), pSensor)); + } + for(auto it : procs) { + ParamsInitProcessor pProcs = {it.at(0), it.at(1), it.at(2)}; + _paramsProc.insert(std::pair<std::string,ParamsInitProcessor>(it.at(1), pProcs)); + } + } + ~paramsServer(){ + // + } + void print(){ + for(auto it : _params) + std::cout << it.first << "~~" << it.second << std::endl; + } + void addInitParamsSensor(std::string type, std::string name){ + ParamsInitSensor params = {type, name}; + _paramsSens.insert(std::pair<std::string, ParamsInitSensor>(type + "/" + name + "/", params)); + } + void addInitParamsProcessor(std::string type, std::string name, std::string name_assoc_sensor){ + ParamsInitProcessor params = {type, name, name_assoc_sensor}; + _paramsProc.insert(std::pair<std::string, ParamsInitProcessor>(type + "/" + name + "/", params)); + } + void addParam(std::string key, std::string value){ + _params.insert(std::pair<std::string, std::string>(key, value)); + } + template<typename T> + T getParam(std::string key, std::string def_value) const { + if(_params.find(key) != _params.end()){ + return converter<T>::convert(_params.find(key)->second); + }else{ + return converter<T>::convert(def_value); + } + } + template<typename T> + T getParam(std::string key) const { + if(_params.find(key) != _params.end()){ + return converter<T>::convert(_params.find(key)->second); + }else{ + throw std::runtime_error("The following key: '" + key + "' has not been found in the parameters server and no default value was provided."); + } + } + std::vector<ParamsInitSensor> getSensors(){ + std::vector<ParamsInitSensor> rtn = std::vector<ParamsInitSensor>(); + std::transform(this->_paramsSens.begin(), this->_paramsSens.end(), back_inserter(rtn), [](const std::pair<std::string,ParamsInitSensor> v){return v.second;}); + return rtn; + } + std::vector<ParamsInitProcessor> getProcessors(){ + std::vector<ParamsInitProcessor> rtn = std::vector<ParamsInitProcessor>(); + std::transform(this->_paramsProc.begin(), this->_paramsProc.end(), back_inserter(rtn), [](const std::pair<std::string,ParamsInitProcessor> v){return v.second;}); + return rtn; + } +}; +} +#endif \ No newline at end of file diff --git a/include/core/yaml/parser_yaml.hpp b/include/core/yaml/parser_yaml.hpp new file mode 100644 index 0000000000000000000000000000000000000000..f93d882cb54837e1755e0bc0f127f01ce8d12ca7 --- /dev/null +++ b/include/core/yaml/parser_yaml.hpp @@ -0,0 +1,324 @@ +#ifndef PARSER_YAML_HPP +#define PARSER_YAML_HPP +#include "yaml-cpp/yaml.h" +#include <vector> +#include <regex> +#include <map> +#include <iostream> +#include <algorithm> + +using namespace std; +namespace { + // std::string remove_ws( const std::string& str ){ + // std::string str_no_ws ; + // for( char c : str ) if( !std::isspace(c) ) str_no_ws += c ; + // return str_no_ws ; + // } + string parseSequence(YAML::Node n){ + assert(n.Type() != YAML::NodeType::Map && "Trying to parse as a Sequence a Map node"); + if(n.Type() == YAML::NodeType::Scalar) return n.Scalar(); + string aux = "["; + bool first = true; + for(auto it : n){ + if(first) { + aux = aux + parseSequence(it); + first = false; + }else{ + aux = aux + "," + parseSequence(it); + } + } + aux = aux + "]"; + return aux; + } + std::string mapToString(std::map<std::string,std::string> _map){ + std::string result = ""; + auto v = std::vector<string>(); + std::transform(_map.begin(), _map.end(), back_inserter(v), [](const std::pair<string,string> p){return "{" + p.first + ":" + p.second + "}";}); + auto concat = [](string ac, string str)-> string { + return ac + str + ","; + }; + string aux = ""; + string accumulated = std::accumulate(v.begin(), v.end(), aux, concat); + if(accumulated.size() > 1) accumulated = accumulated.substr(0,accumulated.size() - 1); + else accumulated = ""; + return "[" + accumulated + "]"; + } +} +class parserYAML { + struct ParamsInitSensor{ + string _type; + string _name; + YAML::Node n; + }; + struct ParamsInitProcessor{ + string _type; + string _name; + string _name_assoc_sensor; + YAML::Node n; + }; + map<string, string> _params; + string _active_name; + vector<ParamsInitSensor> _paramsSens; + vector<ParamsInitProcessor> _paramsProc; + vector<string> _files; + string _file; + bool _relative_path; + string _path_root; + vector<array<string, 2>> _callbacks; +public: + parserYAML(){ + _params = map<string, string>(); + _active_name = ""; + _paramsSens = vector<ParamsInitSensor>(); + _paramsProc = vector<ParamsInitProcessor>(); + _file = ""; + _files = vector<string>(); + _path_root = ""; + _relative_path = false; + _callbacks = vector<array<string, 2>>(); + } + parserYAML(string file){ + _params = map<string, string>(); + _active_name = ""; + _paramsSens = vector<ParamsInitSensor>(); + _paramsProc = vector<ParamsInitProcessor>(); + _files = vector<string>(); + _file = file; + _path_root = ""; + _relative_path = false; + _callbacks = vector<array<string, 2>>(); + } + parserYAML(string file, string path_root){ + _params = map<string, string>(); + _active_name = ""; + _paramsSens = vector<ParamsInitSensor>(); + _paramsProc = vector<ParamsInitProcessor>(); + _files = vector<string>(); + _file = file; + _path_root = path_root; + _relative_path = true; + _callbacks = vector<array<string, 2>>(); + } + ~parserYAML(){ + // + } + void walkTree(string file); + void walkTree(string file, vector<string>& tags); + void walkTree(string file, vector<string>& tags, string hdr); + void walkTreeR(YAML::Node n, vector<string>& tags, string hdr); + void updateActiveName(string tag); + void parseFirstLevel(string file); + string tagsToString(vector<string>& tags); + vector<array<string, 2>> sensorsSerialization(); + vector<array<string, 3>> processorsSerialization(); + vector<string> getFiles(); + vector<array<string, 2>> getCallbacks(); + map<string,string> getParams(); + void parse(); + map<string, string> fetchAsMap(YAML::Node); +}; +string parserYAML::tagsToString(vector<std::string> &tags){ + string hdr = ""; + for(auto it : tags){ + hdr = hdr + "/" + it; + } + return hdr; +} +void parserYAML::walkTree(string file){ + YAML::Node n; + // cout << "RELATIVE? " << _relative_path << " path root " << _path_root << " file " << file << endl; + if(not _relative_path) n = YAML::LoadFile(file); + else n = YAML::LoadFile(_path_root + file); + vector<string> hdrs = vector<string>(); + walkTreeR(n, hdrs, ""); +} +void parserYAML::walkTree(string file, vector<string>& tags){ + YAML::Node n; + // cout << "RELATIVE? " << _relative_path << " path root " << _path_root << " file " << file << endl; + if(not _relative_path) n = YAML::LoadFile(file); + else n = YAML::LoadFile(_path_root + file); + walkTreeR(n, tags, ""); +} +void parserYAML::walkTree(string file, vector<string>& tags, string hdr){ + YAML::Node n; + // cout << "RELATIVE? " << _relative_path << " path root " << _path_root << " file " << file << endl; + if(not _relative_path) n = YAML::LoadFile(file); + else n = YAML::LoadFile(_path_root + file); + walkTreeR(n, tags, hdr); +} +void parserYAML::walkTreeR(YAML::Node n, vector<string>& tags, string hdr){ + switch (n.Type()) { + case YAML::NodeType::Scalar : { + regex r("^@.*"); + if(regex_match(n.Scalar(), r)){ + string str = n.Scalar(); + // cout << "SUBSTR " << str.substr(1,str.size() - 1); + walkTree(str.substr(1,str.size() - 1), tags, hdr); + }else{ + // std::copy(tags.begin(), tags.end(), std::ostream_iterator<string>(std::cout, "¬")); + // cout << "«»" << n.Scalar() << endl; + _params.insert(pair<string,string>(hdr, n.Scalar())); + } + break; + } + case YAML::NodeType::Sequence : { + // cout << tags[tags.size() - 1] << "«»" << kv << endl; + // std::vector<double> vi = n.as<std::vector<double>>(); + string aux = parseSequence(n); + _params.insert(pair<string,string>(hdr, aux)); + break; + } + case YAML::NodeType::Map : { + for(const auto& kv : n){ + //If the key's value starts with a $ (i.e. $key) then its value is parsed as a literal map, + //otherwise the parser recursively parses the map. + regex r("^\\$.*"); + if(not regex_match(kv.first.as<string>(), r)){ + /* + If key=="follow" then the parser will assume that the value is a path and will parse + the (expected) yaml file at the specified path. Note that this does not increase the header depth. + The following example shows how the header remains unafected: + @my_main_config | @some_path + - cov_det: 1 | - my_value : 23 + - follow: "some_path" | + - var: 1.2 | + Resulting map: + cov_det -> 1 + my_value-> 23 + var: 1.2 + Instead of: + cov_det -> 1 + follow/my_value-> 23 + var: 1.2 + Which would result from the following yaml files + @my_main_config | @some_path + - cov_det: 1 | - my_value : 23 + - $follow: "some_path" | + - var: 1.2 | + */ + regex rr("follow"); + if(not regex_match(kv.first.as<string>(), rr)) { + tags.push_back(kv.first.as<string>()); + if(tags.size() == 2) this->updateActiveName(kv.first.as<string>()); + walkTreeR(kv.second, tags, hdr +"/"+ kv.first.as<string>()); + tags.pop_back(); + if(tags.size() == 1) this->updateActiveName(""); + }else{ + walkTree(kv.second.as<string>(), tags, hdr); + } + }else{ + string key = kv.first.as<string>(); + key = key.substr(1,key.size() - 1); + auto fm = fetchAsMap(kv.second); + _params.insert(pair<string,string>(hdr + "/" + key, mapToString(fm))); + } + } + break; + } + default: + assert(1 == 0 && "Unsupported node Type at walkTreeR"); + break; + } +} +void parserYAML::updateActiveName(string tag){ + this->_active_name = tag; +} +void parserYAML::parseFirstLevel(string file){ + YAML::Node n; + // cout << "RELATIVE? " << _relative_path << " path root " << _path_root << " file " << file << endl; + if(not _relative_path) n = YAML::LoadFile(file); + else n = YAML::LoadFile(_path_root + file); + YAML::Node n_config = n["config"]; + assert(n_config.Type() == YAML::NodeType::Map && "trying to parse config node but found a non-Map node"); + for(const auto& kv : n_config["sensors"]){ + ParamsInitSensor pSensor = {kv["type"].Scalar(), kv["name"].Scalar(), kv}; + _paramsSens.push_back(pSensor); + } + for(const auto& kv : n_config["processors"]){ + ParamsInitProcessor pProc = {kv["type"].Scalar(), kv["name"].Scalar(), kv["sensor name"].Scalar(), kv}; + _paramsProc.push_back(pProc); + } + for(const auto& kv : n_config["callbacks"]){ + _callbacks.push_back({{kv[0].as<std::string>(), kv[1].as<std::string>()}}); + } + YAML::Node n_files = n["files"]; + assert(n_files.Type() == YAML::NodeType::Sequence && "trying to parse files node but found a non-Sequence node"); + for(const auto& kv : n_files){ + _files.push_back(kv.Scalar()); + } +} +vector<array<string, 2>> parserYAML::sensorsSerialization(){ + vector<array<string, 2>> aux = vector<array<string, 2>>(); + for(auto it : this->_paramsSens) + aux.push_back({{it._type,it._name}}); + return aux; +} +vector<array<string, 3>> parserYAML::processorsSerialization(){ + vector<array<string, 3>> aux = vector<array<string, 3>>(); + for(auto it : this->_paramsProc) + aux.push_back({{it._type,it._name,it._name_assoc_sensor}}); + return aux; +} +vector<string> parserYAML::getFiles(){ + return this->_files; +} +vector<array<string, 2>> parserYAML::getCallbacks(){ + return this->_callbacks; +} +map<string,string> parserYAML::getParams(){ + map<string,string> rtn = _params; + return rtn; +} +void parserYAML::parse(){ + this->parseFirstLevel(this->_file); + for(auto it : _paramsSens){ + vector<string> tags = vector<string>(); + // this->walkTreeR(it.n , tags , it._type + "/" + it._name); + this->walkTreeR(it.n , tags , it._name); + } + for(auto it : _paramsProc){ + vector<string> tags = vector<string>(); + // this->walkTreeR(it.n , tags , it._type + "/" + it._name); + this->walkTreeR(it.n , tags , it._name); + } +} +map<string, string> parserYAML::fetchAsMap(YAML::Node n){ + assert(n.Type() == YAML::NodeType::Map && "trying to fetch as Map a non-Map node"); + auto m = map<string, string>(); + for(const auto& kv : n){ + string key = kv.first.as<string>(); + switch (kv.second.Type()) { + case YAML::NodeType::Scalar : { + string value = kv.second.Scalar(); + m.insert(pair<string,string>(key, value)); + break; + } + case YAML::NodeType::Sequence : { + // cout << tags[tags.size() - 1] << "«»" << kv << endl; + // std::vector<double> vi = n.as<std::vector<double>>(); + string aux = parseSequence(kv.second); + m.insert(pair<string,string>(key, aux)); + break; + } + case YAML::NodeType::Map : { + m = fetchAsMap(kv.second); + auto rtn = vector<string>(); + std::transform(m.begin(), m.end(), back_inserter(rtn), [](const std::pair<string,string> v){return "{" + v.first + ":" + v.second + "}";}); + auto concat = [](string ac, string str)-> string { + return ac + str + ","; + }; + string aux = ""; + string accumulated = std::accumulate(rtn.begin(), rtn.end(), aux, concat); + if(accumulated.size() > 1) accumulated = accumulated.substr(0,accumulated.size() - 1); + else accumulated = ""; + m.insert(pair<string,string>(key, "[" + accumulated + "]")); + break; + } + default: + assert(1 == 0 && "Unsupported node Type at fetchAsMap"); + break; + } + } + return m; +} +#endif \ No newline at end of file diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp index d7f7f34f8852e02c2ad26d17c21a9269aca6377f..681f3d59ee5e3f674d277182790d9247ac41aa4c 100644 --- a/src/ceres_wrapper/ceres_manager.cpp +++ b/src/ceres_wrapper/ceres_manager.cpp @@ -40,6 +40,14 @@ CeresManager::~CeresManager() removeFactor(fac_2_residual_idx_.begin()->first); } +SolverManagerPtr CeresManager::create(const ProblemPtr &wolf_problem, const paramsServer &_server) +{ + ceres::Solver::Options opt; + opt.max_num_iterations = _server.getParam<int>("max_num_iterations","1000"); + // CeresManager m = CeresManager(wolf_problem, opt); + return std::make_shared<CeresManager>(wolf_problem, opt); +} + std::string CeresManager::solveImpl(const ReportVerbosity report_level) { // Check @@ -434,4 +442,8 @@ void CeresManager::check() } } // namespace wolf +#include "core/solver/solver_factory.h" +namespace wolf { + WOLF_REGISTER_SOLVER(CeresManager) +} // namespace wolf diff --git a/src/examples/test_apriltag.cpp b/src/examples/test_apriltag.cpp index 593938e9dd14f7b260c67d98d40df6fe5d101c0a..66ab9995d34cea1b6df3333a1d9f1f2e88434a64 100644 --- a/src/examples/test_apriltag.cpp +++ b/src/examples/test_apriltag.cpp @@ -54,7 +54,7 @@ int main(int argc, char *argv[]) std::string wolf_root = _WOLF_ROOT_DIR; // Wolf problem - ProblemPtr problem = Problem::create("PO 3D"); + ProblemPtr problem = Problem::create("PO", 3); ceres::Solver::Options options; options.function_tolerance = 1e-6; options.max_num_iterations = 100; diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index 9099cefa5a10d3b8203fd3cb17d09f3af5923023..62c035cf0c7adfc58f2a1ee8c23ac7483d5f0564 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -11,6 +11,9 @@ #include "core/sensor/sensor_factory.h" #include "core/processor/processor_factory.h" #include "core/state_block/state_block.h" +#include "core/yaml/parser_yaml.hpp" +#include "core/utils/params_server.hpp" +#include "core/utils/loader.hpp" // IRI libs includes @@ -63,12 +66,43 @@ void Problem::setup() map_ptr_ -> setProblem(shared_from_this()); } - ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim) +ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim) { ProblemPtr p(new Problem(_frame_structure, _dim)); // We use `new` and not `make_shared` since the Problem constructor is private and cannot be passed to `make_shared`. p->setup(); return p->shared_from_this(); } +ProblemPtr Problem::autoSetup(const std::string& _frame_structure, SizeEigen _dim, const std::string& _yaml_file) +{ + auto p = Problem::create(_frame_structure, _dim); + // string file = "/home/jcasals/catkin_ws/src/wolf_ros_wrapper/src/params.yaml"; + parserYAML parser = parserYAML(_yaml_file); + parser.parse(); + paramsServer server = paramsServer(parser.getParams(), parser.sensorsSerialization(), parser.processorsSerialization()); + // cout << "PRINTING SERVER MAP" << endl; + // server.print(); + // cout << "-----------------------------------" << endl; + auto loaders = vector<Loader*>(); + for(auto it : parser.getFiles()) { + cout << "LOADING " << it << endl; + auto l = new LoaderRaw(it); + l->load(); + loaders.push_back(l); + } + //TODO: To be fixed. This prior should be set in here, but now it is set externally. + // setPrior(Eigen::Vector3s::Zero(), 0.1*Eigen::Matrix3s::Identity(), TimeStamp(), Scalar(0.1)); + 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,p->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,p->installProcessor(s._type, s._name, s._name_assoc_sensor, server))); + } + return p; +} Problem::~Problem() { @@ -107,7 +141,14 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type, // return installSensor(_sen_type, _unique_sensor_name, _extrinsics, IntrinsicsBasePtr()); } - + SensorBasePtr Problem::installSensor(const std::string& _sen_type, // + const std::string& _unique_sensor_name, // + const paramsServer& _server) + { + SensorBasePtr sen_ptr = AutoConfSensorFactory::get().create(uppercase(_sen_type), _unique_sensor_name, _server); + addSensor(sen_ptr); + return sen_ptr; + } ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, // const std::string& _unique_processor_name, // SensorBasePtr _corresponding_sensor_ptr, // @@ -154,6 +195,29 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, // } } +ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, // + const std::string& _unique_processor_name, // + const std::string& _corresponding_sensor_name, // + const paramsServer& _server) +{ + SensorBasePtr sen_ptr = getSensor(_corresponding_sensor_name); + if (sen_ptr == nullptr) + throw std::runtime_error("Sensor not found. Cannot bind Processor."); + ProcessorBasePtr prc_ptr = AutoConfProcessorFactory::get().create(uppercase(_prc_type), _unique_processor_name, _server, sen_ptr); + prc_ptr->configure(sen_ptr); + sen_ptr->addProcessor(prc_ptr); + + // setting the origin in all processor motion if origin already setted + if (prc_ptr->isMotion() && prior_is_set_) + (std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame()); + + // setting the main processor motion + if (prc_ptr->isMotion() && processor_motion_ptr_ == nullptr) + processor_motion_ptr_ = std::static_pointer_cast<ProcessorMotion>(prc_ptr); + + return prc_ptr; +} + SensorBasePtr Problem::getSensor(const std::string& _sensor_name) { auto sen_it = std::find_if(getHardware()->getSensorList().begin(), diff --git a/src/processor/processor_frame_nearest_neighbor_filter.cpp b/src/processor/processor_frame_nearest_neighbor_filter.cpp index 6227ebf2cdedc56e95fa52b0025dc597c0fd6be3..b4f6303b128b37ba74104cddf02dd82e328c08e8 100644 --- a/src/processor/processor_frame_nearest_neighbor_filter.cpp +++ b/src/processor/processor_frame_nearest_neighbor_filter.cpp @@ -2,7 +2,6 @@ namespace wolf { - ProcessorFrameNearestNeighborFilter::ProcessorFrameNearestNeighborFilter(ParamsPtr _params_NNF): ProcessorLoopClosureBase("FRAME NEAREST NEIGHBOR FILTER", _params_NNF), params_NNF(_params_NNF) diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp index 218711e63885ceb7dbbd593b30e2a20faa9b631f..9b19d4e8c4eda8db40eac71574534f3419880634 100644 --- a/src/processor/processor_odom_2D.cpp +++ b/src/processor/processor_odom_2D.cpp @@ -190,6 +190,26 @@ ProcessorBasePtr ProcessorOdom2D::create(const std::string& _unique_name, const return prc_ptr; } +ProcessorBasePtr ProcessorOdom2D::createAutoConf(const std::string& _unique_name, const paramsServer& _server, const SensorBasePtr sensor_ptr) +{ + + ProcessorOdom2DPtr prc_ptr; + + std::shared_ptr<ProcessorParamsOdom2D> params; + params = std::make_shared<ProcessorParamsOdom2D>(); + params->voting_active = _server.getParam<bool>(_unique_name + "/voting_active", "true"); + params->time_tolerance = _server.getParam<double>(_unique_name + "/time_tolerance", "0.1"); + params->max_time_span = _server.getParam<double>(_unique_name + "/max_time_span", "999"); + params->dist_traveled = _server.getParam<double>(_unique_name + "/dist_traveled", "0.95"); // Will make KFs automatically every 1m displacement + params->angle_turned = _server.getParam<double>(_unique_name + "/angle_turned", "999"); + params->cov_det = _server.getParam<double>(_unique_name + "/cov_det", "999"); + params->unmeasured_perturbation_std = _server.getParam<double>(_unique_name + "/unmeasured_perturbation_std", "0.0001"); + + prc_ptr = std::make_shared<ProcessorOdom2D>(params); + prc_ptr->setName(_unique_name); + + return prc_ptr; +} } @@ -198,3 +218,7 @@ ProcessorBasePtr ProcessorOdom2D::create(const std::string& _unique_name, const namespace wolf { WOLF_REGISTER_PROCESSOR("ODOM 2D", ProcessorOdom2D) } // namespace wolf +#include "core/processor/autoconf_processor_factory.h" +namespace wolf { + WOLF_REGISTER_PROCESSOR_AUTO("ODOM 2D", ProcessorOdom2D) +} // namespace wolf diff --git a/src/sensor/sensor_odom_2D.cpp b/src/sensor/sensor_odom_2D.cpp index e3e5d860f2960f8775a292c371b345d51d2dd70f..f6b408c7ec9bb68544e5b5f24019bde710954c93 100644 --- a/src/sensor/sensor_odom_2D.cpp +++ b/src/sensor/sensor_odom_2D.cpp @@ -58,10 +58,28 @@ SensorBasePtr SensorOdom2D::create(const std::string& _unique_name, const Eigen: return odo; } -} // namespace wolf +SensorBasePtr SensorOdom2D::createAutoConf(const std::string& _unique_name, const paramsServer& _server) +{ + // Eigen::VectorXs _extrinsics_po = Eigen::Vector3s(0,0,0); + Eigen::VectorXs _extrinsics_po = _server.getParam<Eigen::VectorXs>(_unique_name + "/extrinsic/pos", "[0,0,0]"); + assert(_extrinsics_po.size() == 3 && "Bad extrinsics vector length. Should be 3 for 2D."); + SensorOdom2DPtr odo; + IntrinsicsOdom2D params; + params.k_disp_to_disp = _server.getParam<double>(_unique_name + "/intrinsic/k_disp_to_disp", "1"); + params.k_rot_to_rot = _server.getParam<double>(_unique_name + "/intrinsic/k_rot_to_rot", "1"); + odo = std::make_shared<SensorOdom2D>(_extrinsics_po, params); + odo->setName(_unique_name); + return odo; +} + +} // Register in the SensorFactory #include "core/sensor/sensor_factory.h" namespace wolf { WOLF_REGISTER_SENSOR("ODOM 2D", SensorOdom2D) } // namespace wolf +#include "core/sensor/autoconf_sensor_factory.h" +namespace wolf { +WOLF_REGISTER_SENSOR_AUTO("ODOM 2D", SensorOdom2D) +} // namespace wolf diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index f0d94c387440b4ca27655abd3dee9f79cabcb080..9a9d1d36147dc655deda32e39b7dcb4eb9286cba 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -52,6 +52,10 @@ target_link_libraries(gtest_capture_base ${PROJECT_NAME}) wolf_add_gtest(gtest_factor_autodiff gtest_factor_autodiff.cpp) target_link_libraries(gtest_factor_autodiff ${PROJECT_NAME}) +# Converter from String to various types used by the parameters server +wolf_add_gtest(gtest_converter gtest_converter.cpp) +target_link_libraries(gtest_converter ${PROJECT_NAME}) + # FeatureBase classes test wolf_add_gtest(gtest_eigen_predicates gtest_eigen_predicates.cpp) target_link_libraries(gtest_eigen_predicates ${PROJECT_NAME}) @@ -81,6 +85,14 @@ target_link_libraries(gtest_motion_buffer ${PROJECT_NAME}) wolf_add_gtest(gtest_pack_KF_buffer gtest_pack_KF_buffer.cpp) target_link_libraries(gtest_pack_KF_buffer ${PROJECT_NAME}) +# Parameters server +wolf_add_gtest(gtest_param_server gtest_param_server.cpp ${CMAKE_CURRENT_LIST_DIR}) +target_link_libraries(gtest_param_server ${PROJECT_NAME}) + +# YAML parser +wolf_add_gtest(gtest_parser_yaml gtest_parser_yaml.cpp) +target_link_libraries(gtest_parser_yaml ${PROJECT_NAME}) + # Problem class test # TODO: TO BE FIXED # wolf_add_gtest(gtest_problem gtest_problem.cpp) diff --git a/test/gtest_converter.cpp b/test/gtest_converter.cpp new file mode 100644 index 0000000000000000000000000000000000000000..69bb7ce355646daadaf3b91a1e816e0f0c163587 --- /dev/null +++ b/test/gtest_converter.cpp @@ -0,0 +1,50 @@ +#include "utils_gtest.h" +#include "core/utils/converter.h" + +using namespace std; +using namespace wolf; + +TEST(Converter, ParseToVector) +{ + string v1 = "[3,4,5,6,7,8,9,10,11]"; + vector<int> v = converter<vector<int>>::convert(v1); + ASSERT_EQ(v.size(),9); + ASSERT_EQ(v[0],3); + ASSERT_EQ(v[1],4); + ASSERT_EQ(v[2],5); + ASSERT_EQ(v[3],6); + ASSERT_EQ(v[4],7); + ASSERT_EQ(v[5],8); + ASSERT_EQ(v[6],9); + ASSERT_EQ(v[7],10); + ASSERT_EQ(v[8],11); + vector<string> vs {"a","b","c"}; + ASSERT_EQ(converter<string>::convert(vs), "[a,b,c]"); + +} +TEST(Converter, ParseToEigenMatrix) +{ + string v1 = "[[3,3],3,4,5,6,7,8,9,10,11]"; + auto v = Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, 3, 5, 5>(); + EXPECT_NO_THROW(([=, &v]{v = converter<Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, 3, 5, 5>>::convert(v1);}())); + EXPECT_EQ(v.size(),9); + string v2 = "[[3,3],3,4,5,6,7,8,9,10,11,12]"; + EXPECT_THROW(([=, &v]{v = converter<Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, 3, 5, 5>>::convert(v2);}()), std::runtime_error); + string v3 = "[[3],3,4,5,6,7,8,9,10,11]"; + EXPECT_THROW(([=, &v]{v = converter<Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, 3, 5, 5>>::convert(v3);}()), std::runtime_error); +} +TEST(Converter, ParseToMap) +{ + string str = "[{x:1},{y:[1,23,3]},{z:3}]"; + EXPECT_THROW(([=]{auto a = converter<std::map<std::string, double>>::convert(str);}()), std::invalid_argument); + string str2 = "[{x:[1]},{y:[1,23,3]},{z:[3]}]"; + EXPECT_NO_THROW(([=]{auto a = converter<std::map<std::string, std::vector<int>>>::convert(str2);}())); + map<string, vector<int>> m = {{"x",vector<int>{1,2}}, {"y",vector<int>{}}, {"z",vector<int>{3}}}; + ASSERT_EQ(converter<string>::convert(m), "[{x:[1,2]},{y:[]},{z:[3]}]"); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/gtest_param_server.cpp b/test/gtest_param_server.cpp new file mode 100644 index 0000000000000000000000000000000000000000..ab349ceb38fc304d6ad581a157d98fe4de42d390 --- /dev/null +++ b/test/gtest_param_server.cpp @@ -0,0 +1,35 @@ +#include "utils_gtest.h" +#include "core/utils/converter.h" +#include "core/common/wolf.h" +#include "core/yaml/parser_yaml.hpp" +#include "core/utils/params_server.hpp" + +using namespace std; +using namespace wolf; + +std::string wolf_root = _WOLF_ROOT_DIR; + +parserYAML parse(string _file, string _path_root) +{ + parserYAML parser = parserYAML(_file, _path_root); + parser.parse(); + return parser; +} + +TEST(ParamsServer, Default) +{ + auto parser = parse("/test/params1.yaml", wolf_root); + auto params = parser.getParams(); + paramsServer server = paramsServer(params, parser.sensorsSerialization(), parser.processorsSerialization()); + EXPECT_EQ(server.getParam<double>("should_not_exist", "2.6"), 2.6); + EXPECT_EQ(server.getParam<bool>("my_proc_test/voting_active", "true"), false); + EXPECT_NE(server.getParam<unsigned int>("my_proc_test/time_tolerance", "23"), 23); + EXPECT_THROW({ server.getParam<unsigned int>("test error"); }, std::runtime_error); + EXPECT_NE(server.getParam<unsigned int>("my_proc_test/time_tolerance"), 23); + EXPECT_EQ(server.getParam<bool>("my_proc_test/voting_active"), false); +} +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/gtest_parser_yaml.cpp b/test/gtest_parser_yaml.cpp new file mode 100644 index 0000000000000000000000000000000000000000..4af9e7760ef8d8d9b14fb6da4240852ee7919885 --- /dev/null +++ b/test/gtest_parser_yaml.cpp @@ -0,0 +1,44 @@ +#include "utils_gtest.h" +#include "core/utils/converter.h" +#include "core/common/wolf.h" +#include "core/yaml/parser_yaml.hpp" + +using namespace std; +using namespace wolf; + +std::string wolf_root = _WOLF_ROOT_DIR; + +parserYAML parse(string _file, string _path_root) +{ + parserYAML parser = parserYAML(_file, _path_root); + parser.parse(); + return parser; +} + +TEST(ParserYAML, RegularParse) +{ + auto parser = parse("/test/params1.yaml", wolf_root); + auto params = parser.getParams(); + // for(auto it : params) + // cout << it.first << " %% " << it.second << endl; + EXPECT_EQ(params["odom/intrinsic/k_rot_to_rot"], "0.1"); + EXPECT_EQ(params["processor1/sensor name"], "odom"); +} +TEST(ParserYAML, ParseMap) +{ + auto parser = parse("/test/params2.yaml", wolf_root); + auto params = parser.getParams(); + EXPECT_EQ(params["processor1/mymap"], "[{k1:v1},{k2:v2},{k3:[v3,v4,v5]}]"); +} +TEST(ParserYAML, JumpFile) +{ + auto parser = parse("/test/params3.yaml", wolf_root); + auto params = parser.getParams(); + EXPECT_EQ(params["my_proc_test/max_buff_length"], "100"); + EXPECT_EQ(params["my_proc_test/jump/voting_active"], "false"); +} +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/params1.yaml b/test/params1.yaml new file mode 100644 index 0000000000000000000000000000000000000000..834984f6cc03f111dd446cca4e802603b81ae961 --- /dev/null +++ b/test/params1.yaml @@ -0,0 +1,30 @@ +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: "/test/params3.1.yaml" +files: + - "/home/jcasals/workspace/wip/wolf/lib/libsensor_odo.so" + - "/home/jcasals/workspace/wip/wolf/lib/librange_bearing.so" \ No newline at end of file diff --git a/test/params2.yaml b/test/params2.yaml new file mode 100644 index 0000000000000000000000000000000000000000..d58014fbabc36387be1a96cc4244cff954ac2820 --- /dev/null +++ b/test/params2.yaml @@ -0,0 +1,33 @@ +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" + $mymap: + k1: v1 + k2: v2 + k3: [v3,v4,v5] + - + type: "RANGE BEARING" + name: "rb_processor" + sensor name: "rb" + - + type: "ODOM 2D" + name: "my_proc_test" + sensor name: "odom" +files: + - "/home/jcasals/workspace/wip/wolf/lib/libsensor_odo.so" + - "/home/jcasals/workspace/wip/wolf/lib/librange_bearing.so" \ No newline at end of file diff --git a/test/params3.1.yaml b/test/params3.1.yaml new file mode 100644 index 0000000000000000000000000000000000000000..bd9b495c830e56f17f11abbb5b2109d780f4e438 --- /dev/null +++ b/test/params3.1.yaml @@ -0,0 +1,8 @@ +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 diff --git a/test/params3.yaml b/test/params3.yaml new file mode 100644 index 0000000000000000000000000000000000000000..3ee99dcc40a597e0b83a346aedd1e6c63fa52754 --- /dev/null +++ b/test/params3.yaml @@ -0,0 +1,31 @@ +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: "/test/params3.1.yaml" + jump: "@/test/params3.1.yaml" +files: + - "/home/jcasals/workspace/wip/wolf/lib/libsensor_odo.so" + - "/home/jcasals/workspace/wip/wolf/lib/librange_bearing.so" \ No newline at end of file diff --git a/wolf_scripts/out b/wolf_scripts/out index 93fc67671cc2375e1857e43c0b57ff52ad2b7b35..2b7857b6ba924b96ce256a0a7661b7079c429a2a 100644 --- a/wolf_scripts/out +++ b/wolf_scripts/out @@ -132,7 +132,7 @@ -=============================================================== +====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/problem.h ================================== -=============================================================== -+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/new_processor_factory.h ================================== ++====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/autoconf_processor_factory.h ================================== -=============================================================== +====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/state_homogeneous_3D.h ================================== -=============================================================== @@ -146,7 +146,7 @@ -=============================================================== +====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/processor_tracker_feature.h ================================== -=============================================================== -+====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/new_sensor_factory.h ================================== ++====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/autoconf_sensor_factory.h ================================== -=============================================================== +====================== /home/jcasals/workspace/wip/wolf/src/core/include/core/constraint_analytic.h ================================== -=============================================================== @@ -273,8 +273,8 @@ #include "factory.h" #include "sensor_factory.h" #include "processor_factory.h" -#include "new_sensor_factory.h" -#include "new_processor_factory.h" +#include "autoconf_sensor_factory.h" +#include "autoconf_processor_factory.h" #include "constraint_base.h" #include "state_block.h" #include "processor_motion.h"