Merge branch 'devel' into plugin/core

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

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)

hello_plugin/CMakeLists.txt

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

hello_plugin/hello_plugin.cpp

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

hello_plugin/params.yaml

+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

hello_plugin/params_autoconf.cpp

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

hello_plugin/params_conf.yaml

+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

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

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
 

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

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

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 */

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

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_ */

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.
          */

include/core/processor/autoconf_processor_factory.h

+/**
+ * \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" , &params );
+ *
+ *     // 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" , &params     );
+ *     \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_ */

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;
 

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

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

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