CMakeLists.txt

@@ -185,10 +185,10 @@ SET(HDRS_UTILS
   include/core/utils/converter.h
   include/core/utils/eigen_assert.h
   include/core/utils/eigen_predicates.h
-  include/core/utils/loader.hpp
+  include/core/utils/loader.h
   include/core/utils/logging.h
   include/core/utils/make_unique.h
-  include/core/utils/params_server.hpp
+  include/core/utils/params_server.h
   include/core/utils/singleton.h
   include/core/utils/utils_gtest.h
   include/core/utils/converter_utils.h
@@ -287,7 +287,7 @@ SET(HDRS_SOLVER
   )
 
 SET(HDRS_YAML
-  include/core/yaml/parser_yaml.hpp
+  include/core/yaml/parser_yaml.h
   include/core/yaml/yaml_conversion.h
   )
 #SOURCES
@@ -321,6 +321,8 @@ SET(SRCS_MATH
   )
 SET(SRCS_UTILS
   src/utils/converter_utils.cpp
+  src/utils/params_server.cpp
+  src/utils/loader.cpp
   )
 
 SET(SRCS_CAPTURE
@@ -369,6 +371,7 @@ SET(SRCS_SOLVER
   src/solver/solver_manager.cpp
   )
 SET(SRCS_YAML
+  src/yaml/parser_yaml.cpp
   src/yaml/processor_odom_3d_yaml.cpp
   src/yaml/sensor_odom_2d_yaml.cpp
   src/yaml/sensor_odom_3d_yaml.cpp

hello_wolf/hello_wolf_autoconf.cpp

@@ -9,7 +9,7 @@
 // wolf core includes
 #include "core/common/wolf.h"
 #include "core/capture/capture_odom_2d.h"
-#include "core/yaml/parser_yaml.hpp"
+#include "core/yaml/parser_yaml.h"
 #include "core/ceres_wrapper/ceres_manager.h"
 
 // hello wolf local includes

hello_wolf/sensor_range_bearing.h

@@ -9,7 +9,7 @@
 #define HELLO_WOLF_SENSOR_RANGE_BEARING_H_
 
 #include "core/sensor/sensor_base.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
 
 namespace wolf
 {

include/core/capture/capture_base.h

@@ -101,7 +101,7 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
         static std::shared_ptr<classType> emplace(FrameBasePtr _frm_ptr, T&&... all);
 
     protected:
-        SizeEigen computeCalibSize() const;
+        virtual SizeEigen computeCalibSize() const;
 
     private:
         FeatureBasePtr addFeature(FeatureBasePtr _ft_ptr);

include/core/ceres_wrapper/ceres_manager.h

@@ -8,7 +8,7 @@
 
 //wolf includes
 #include "core/solver/solver_manager.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
 #include "core/ceres_wrapper/cost_function_wrapper.h"
 #include "core/ceres_wrapper/local_parametrization_wrapper.h"
 #include "core/ceres_wrapper/create_numeric_diff_cost_function.h"
@@ -84,6 +84,8 @@ class CeresManager : public SolverManager
 
         void check();
 
+        const Eigen::SparseMatrixd computeHessian() const;
+
     protected:
 
         std::string solveImpl(const ReportVerbosity report_level) override;

include/core/common/params_base.h

 #ifndef PARAMS_BASE_H_
 #define PARAMS_BASE_H_
 
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
 
 namespace wolf {
   struct ParamsBase

include/core/factor/factor_block_absolute.h

@@ -41,7 +41,7 @@ class FactorBlockAbsolute : public FactorAnalytic
                             ProcessorBasePtr _processor_ptr = nullptr,
                             bool _apply_loss_function = false,
                             FactorStatus _status = FAC_ACTIVE) :
-            FactorAnalytic("BLOCK ABS",
+            FactorAnalytic("FactorBlockAbsolute",
                            nullptr,
                            nullptr,
                            nullptr,

include/core/factor/factor_block_difference.h

@@ -37,8 +37,12 @@ class FactorBlockDifference : public FactorAnalytic
          *
          */
         FactorBlockDifference(
-                            StateBlockPtr _sb1_ptr,
-                            StateBlockPtr _sb2_ptr,
+                            const StateBlockPtr& _sb1_ptr,
+                            const StateBlockPtr& _sb2_ptr,
+                            const FrameBasePtr& _frame_other = nullptr,
+                            const CaptureBasePtr& _cap_other = nullptr,
+                            const FeatureBasePtr& _feat_other = nullptr,
+                            const LandmarkBasePtr& _lmk_other = nullptr,
                             unsigned int _start_idx1 = 0,
                             int _size1 = -1,
                             unsigned int _start_idx2 = 0,
@@ -46,11 +50,11 @@ class FactorBlockDifference : public FactorAnalytic
                             ProcessorBasePtr _processor_ptr = nullptr,
                             bool _apply_loss_function = false,
                             FactorStatus _status = FAC_ACTIVE) :
-            FactorAnalytic("BLOCK ABS",
-                           nullptr,
-                           nullptr,
-                           nullptr,
-                           nullptr,
+            FactorAnalytic("FactorBlockDifference",
+                           _frame_other,
+                           _cap_other,
+                           _feat_other,
+                           _lmk_other,
                            _processor_ptr,
                            _apply_loss_function,
                            _status,
@@ -146,10 +150,16 @@ inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Ma
     assert(_st_vector[1].size() == sb2_size_ && "Wrong StateBlock size");
     assert(_st_vector[0].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
     assert(_st_vector[1].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
-
-    // normalized jacobian
-    _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
-    _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
+    assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb1_.rows() && "Wrong jacobian sb1 or covariance size");
+    assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb2_.rows() && "Wrong jacobian sb2 or covariance size");
+
+    // normalized jacobian, computed according to the _compute_jacobian flag
+    if (_compute_jacobian[0]){
+        _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
+    }
+    if (_compute_jacobian[1]){
+        _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
+    }
 }
 
 inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
@@ -163,10 +173,16 @@ inline void FactorBlockDifference::evaluateJacobians(const std::vector<Eigen::Ma
     assert(_st_vector[1].size() == sb2_size_ && "Wrong StateBlock size");
     assert(_st_vector[0].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
     assert(_st_vector[1].size() >= getMeasurement().size() && "StateBlock size and measurement size should match");
-
-    // normalized jacobian
-    _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
-    _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
+    assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb1_.rows() && "Wrong jacobian sb1 or covariance size");
+    assert(getMeasurementSquareRootInformationUpper().cols() == J_res_sb2_.rows() && "Wrong jacobian sb2 or covariance size");
+
+    // normalized jacobian, computed according to the _compute_jacobian flag
+    if (_compute_jacobian[0]){
+        _jacobians[0] = getMeasurementSquareRootInformationUpper() * J_res_sb1_;
+    }
+    if (_compute_jacobian[1]){
+        _jacobians[1] = getMeasurementSquareRootInformationUpper() * J_res_sb2_;
+    }
 }
 
 inline void FactorBlockDifference::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& _pure_jacobians) const

include/core/problem/problem.h

@@ -18,7 +18,7 @@ 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/utils/params_server.h"
 #include "core/sensor/sensor_factory.h"
 #include "core/processor/processor_factory.h"
 #include "core/processor/is_motion.h"
@@ -46,8 +46,7 @@ class Problem : public std::enable_shared_from_this<Problem>
         HardwareBasePtr     hardware_ptr_;
         TrajectoryBasePtr   trajectory_ptr_;
         MapBasePtr          map_ptr_;
-        IsMotionPtr  processor_motion_ptr_;
-//        IsMotionPtr         is_motion_ptr_;
+        std::list<IsMotionPtr>  processor_is_motion_list_;
         std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXd> covariances_;
         SizeEigen state_size_, state_cov_size_, dim_;
         std::map<FactorBasePtr, Notification> factor_notification_map_;
@@ -147,15 +146,15 @@ class Problem : public std::enable_shared_from_this<Problem>
     protected:
         /** \brief Set the processor motion
          *
-         * Set the processor motion.
+         * Add a new processor of type is motion to the processor is motion list.
          */
-        void setProcessorMotion(IsMotionPtr _processor_motion_ptr);
-        IsMotionPtr setProcessorMotion(const std::string& _unique_processor_name);
-        void clearProcessorMotion();
+        void addProcessorIsMotion(IsMotionPtr _processor_motion_ptr);
+        void clearProcessorIsMotion(IsMotionPtr proc);
 
     public:
-        IsMotionPtr& getProcessorMotion();
-
+        IsMotionPtr getProcessorIsMotion();
+        std::list<IsMotionPtr> getProcessorIsMotionList();
+        
         // Trajectory branch ----------------------------------
         TrajectoryBasePtr getTrajectory() const;
         virtual FrameBasePtr setPrior(const Eigen::VectorXd& _prior_state, //
@@ -271,10 +270,10 @@ class Problem : public std::enable_shared_from_this<Problem>
         // Zero state provider
         Eigen::VectorXd zeroState ( ) const;
         bool priorIsSet() const;
+        void setPriorIsSet(bool _prior_is_set);
         // Perturb state
         void            perturb(double amplitude = 0.01);
 
-
         // Map branch -----------------------------------------
         MapBasePtr getMap() const;
         void loadMap(const std::string& _filename_dot_yaml);
@@ -350,7 +349,7 @@ class Problem : public std::enable_shared_from_this<Problem>
                    bool constr_by = false, //
                    bool metric = true, //
                    bool state_blocks = false) const;
-        bool check(int verbose_level) const;
+        bool check(int verbose_level = 0) const;
         bool check(bool verbose, std::ostream& stream) const;
 
 };
@@ -367,9 +366,22 @@ inline bool Problem::priorIsSet() const
     return prior_is_set_;
 }
 
-inline IsMotionPtr& Problem::getProcessorMotion()
+inline void Problem::setPriorIsSet(bool _prior_is_set)
+{
+    prior_is_set_ = _prior_is_set;
+}
+
+inline IsMotionPtr Problem::getProcessorIsMotion()
+{
+    if (!processor_is_motion_list_.empty())
+        return processor_is_motion_list_.front();
+    return nullptr;
+}
+
+
+inline std::list<IsMotionPtr> Problem::getProcessorIsMotionList()
 {
-    return processor_motion_ptr_;
+    return processor_is_motion_list_;
 }
 
 inline std::map<StateBlockPtr,Notification> Problem::consumeStateBlockNotificationMap()

include/core/processor/is_motion.h

@@ -43,6 +43,12 @@ class IsMotion
         TimeStamp       getCurrentTimeStamp() const;
         Eigen::VectorXd getState(const TimeStamp& _ts) const;
 
+        std::string getStateStructure(){return state_structure_;};
+        void setStateStructure(std::string _state_structure){state_structure_ = _state_structure;};
+    
+    protected:
+        std::string state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames)
+
 };
 
 }

include/core/processor/processor_base.h

@@ -117,13 +117,18 @@ public:
     *
     * elements are ordered from most recent to oldest
     */
-    std::map<TimeStamp,T> getContainer();
+    const std::map<TimeStamp,T>& getContainer();
 
     /**\brief Remove all packs in the buffer with a time stamp older than the specified
     *
     */
     void removeUpTo(const TimeStamp& _time_stamp);
 
+    /**\brief Remove all packs in the buffer with a time stamp older than the specified
+    *
+    */
+    void removeUpToLower(const TimeStamp& _time_stamp);
+
     /**\brief Clear the buffer
     *
     */
@@ -503,7 +508,7 @@ void Buffer<T>::add(const TimeStamp& _time_stamp, const T& _element)
 }
 
 template <typename T>
-std::map<TimeStamp,T> Buffer<T>::getContainer()
+const std::map<TimeStamp,T>& Buffer<T>::getContainer()
 {
     return container_;
 }
@@ -533,6 +538,13 @@ inline void Buffer<T>::removeUpTo(const TimeStamp& _time_stamp)
     container_.erase(container_.begin(), post); // erasing by range
 }
 
+template <typename T>
+inline void Buffer<T>::removeUpToLower(const TimeStamp& _time_stamp)
+{
+    Buffer::Iterator post = container_.lower_bound(_time_stamp);
+    container_.erase(container_.begin(), post); // erasing by range
+}
+
 template <typename T>
 inline bool Buffer<T>::doubleCheckTimeTolerance(const TimeStamp& _time_stamp1, const double& _time_tolerance1,
                                 const TimeStamp& _time_stamp2, const double& _time_tolerance2)

include/core/processor/processor_motion.h

@@ -13,7 +13,7 @@
 #include "core/processor/processor_base.h"
 #include "core/processor/is_motion.h"
 #include "core/common/time_stamp.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
 
 // std
 #include <iomanip>
@@ -148,6 +148,7 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
     // This is the main public interface
     public:
         ProcessorMotion(const std::string& _type,
+                        std::string _state_structure,
                         int _dim,
                         SizeEigen _state_size,
                         SizeEigen _delta_size,
@@ -505,11 +506,12 @@ inline void ProcessorMotion::getCurrentState(Eigen::VectorXd& _x) const
     assert(origin_ptr_ && "Trying to access origin_ptr_ but it is nullptr!");
 
     // ensure proper size of the provided reference
-    _x.resize( getProblem()->getFrameStructureSize() );
+    Eigen::VectorXd curr_x = origin_ptr_->getFrame()->getState(state_structure_);
+    _x.resize( curr_x.size() );
 
     // do get timestamp and state corrected by possible self-calibrations
     double Dt = getCurrentTimeStamp() - origin_ptr_->getTimeStamp();
-    statePlusDelta(origin_ptr_->getFrame()->getState(), last_ptr_->getDeltaCorrected(origin_ptr_->getCalibration()), Dt, _x);
+    statePlusDelta(curr_x, last_ptr_->getDeltaCorrected(origin_ptr_->getCalibration()), Dt, _x);
 }
 
 inline const Eigen::MatrixXd ProcessorMotion::getCurrentDeltaPreintCov() const

include/core/processor/processor_odom_2d.h

@@ -12,7 +12,7 @@
 #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"
+#include "core/utils/params_server.h"
 
 namespace wolf {
 

include/core/sensor/sensor_factory.h

@@ -16,7 +16,7 @@ struct ParamsSensorBase;
 
 // wolf
 #include "core/common/factory.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
 
 namespace wolf
 {

include/core/sensor/sensor_odom_2d.h

@@ -3,7 +3,7 @@
 
 //wolf includes
 #include "core/sensor/sensor_base.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
 
 namespace wolf {
 

include/core/sensor/sensor_odom_3d.h

@@ -10,7 +10,7 @@
 
 //wolf includes
 #include "core/sensor/sensor_base.h"
-#include "core/utils/params_server.hpp"
+#include "core/utils/params_server.h"
 
 namespace wolf {
 

include/core/state_block/has_state_blocks.h

@@ -15,6 +15,10 @@
 namespace wolf
 {
 
+/// State of nodes containing several state blocks
+typedef std::unordered_map<std::string, Eigen::VectorXd> State;
+
+
 class HasStateBlocks
 {
     public:
@@ -62,11 +66,13 @@ class HasStateBlocks
         void removeStateBlocks(ProblemPtr _problem);
 
         // States
-        virtual void    setState(const Eigen::VectorXd& _state, const bool _notify = true);
-        Eigen::VectorXd getState() const;
-        void            getState(Eigen::VectorXd& _state) const;
-        unsigned int    getSize() const;
-        unsigned int    getLocalSize() const;
+        inline void setState(const Eigen::VectorXd& _state, std::string _sub_structure="", const bool _notify = true);
+        void getState(Eigen::VectorXd& _state, std::string structure="") const;
+        Eigen::VectorXd getState(std::string structure="") const;
+        unsigned int getSize(std::string _sub_structure="") const;
+        unsigned int getLocalSize(std::string _sub_structure="") const;
+
+        State getStateComposite();
 
         // Perturb state
         void            perturb(double amplitude = 0.01);
@@ -198,60 +204,100 @@ inline bool HasStateBlocks::isFixed() const
     return fixed;
 }
 
-inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const bool _notify)
+inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, std::string _sub_structure, const bool _notify)
 {
-    int size = getSize();
-
+    if (_sub_structure == ""){
+        _sub_structure = structure_;
+    }
+    int size = getSize(_sub_structure);
     assert(_state.size() == size && "In FrameBase::setState wrong state size");
 
     unsigned int index = 0;
-    for (const char key : getStructure())
+    for (const char key : _sub_structure)
     {
         const auto& sb = getStateBlock(key);
-
+        if (!sb){
+            WOLF_ERROR("Stateblock key ", key, " not in the structure");
+        }
         sb->setState(_state.segment(index, sb->getSize()), _notify); // do not notify if state block is not estimated by the solver
         index += sb->getSize();
     }
 
 }
 
-inline void HasStateBlocks::getState(Eigen::VectorXd& _state) const
+// _sub_structure can be either stateblock structure of the node or a subset of this structure
+inline void HasStateBlocks::getState(Eigen::VectorXd& _state, std::string _sub_structure) const
 {
-    _state.resize(getSize());
+    if (_sub_structure == ""){
+        _sub_structure = structure_;
+    }
+    _state.resize(getSize(_sub_structure));
 
     unsigned int index = 0;
-    for (const char key : getStructure())
+    for (const char key : _sub_structure)
     {
         const auto& sb = getStateBlock(key);
-
+        if (!sb){
+            WOLF_ERROR("Stateblock key ", key, " not in the structure");
+        }
         _state.segment(index,sb->getSize()) = sb->getState();
         index += sb->getSize();
     }
 
 }
 
-inline Eigen::VectorXd HasStateBlocks::getState() const
+inline Eigen::VectorXd HasStateBlocks::getState(std::string _sub_structure) const
 {
     Eigen::VectorXd state;
 
-    getState(state);
+    getState(state, _sub_structure);
+
+    return state;
+}
 
+inline State HasStateBlocks::getStateComposite()
+{
+    State state;
+    for (auto& pair_key_kf : state_block_map_)
+    {
+        state.emplace(pair_key_kf.first, pair_key_kf.second->getState());
+    }
     return state;
 }
 
-inline unsigned int HasStateBlocks::getSize() const
+inline unsigned int HasStateBlocks::getSize(std::string _sub_structure) const
 {
+    if (_sub_structure == ""){
+        _sub_structure = structure_;
+    }
     unsigned int size = 0;
-    for (const auto& pair_key_sb : getStateBlockMap())
-            size += pair_key_sb.second->getSize();
+    for (const char key : _sub_structure)
+    {
+        const auto& sb = getStateBlock(key);
+        if (!sb){
+            WOLF_ERROR("Stateblock key ", key, " not in the structure");
+        }
+        size += sb->getSize();
+    }
+
     return size;
 }
 
-inline unsigned int HasStateBlocks::getLocalSize() const
+inline unsigned int HasStateBlocks::getLocalSize(std::string _sub_structure) const
 {
+    if (_sub_structure == ""){
+        _sub_structure = structure_;
+    }
     unsigned int size = 0;
-    for (const auto& pair_key_sb : getStateBlockMap())
-            size += pair_key_sb.second->getLocalSize();
+    for (const char key : _sub_structure)
+    {
+        const auto& sb = getStateBlock(key);
+        if (!sb){
+            WOLF_ERROR("Stateblock key ", key, " not in the structure");
+        }
+        size += sb->getLocalSize();
+    }
+
     return size;
 }
 

include/core/state_block/state_block.h

@@ -74,6 +74,10 @@ public:
          **/
         Eigen::VectorXd getState() const;
 
+        /** \brief Returns the state vector data array
+         **/
+        double* getStateData();
+
         /** \brief Sets the state vector
          **/
         void setState(const Eigen::VectorXd& _state, const bool _notify = true);
@@ -283,6 +287,11 @@ inline void StateBlock::resetLocalParamUpdated()
     local_param_updated_.store(false);
 }
 
+inline double* StateBlock::getStateData()
+{
+    return state_.data();
+}
+
 }// namespace wolf
 
 #endif

include/core/utils/loader.hpp → include/core/utils/loader.h

-#ifndef LOADER_HPP
-#define LOADER_HPP
-#include <dlfcn.h>
+#ifndef LOADER_H
+#define LOADER_H
+
 #include <string>
-#include <stdexcept>
+
 class Loader{
 protected:
     std::string path_;
 public:
-    Loader(std::string _file){
-        this->path_ = _file;
-    }
+    Loader(std::string _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_ + "\n" + "Error info: " + dlerror());
-    }
-    void close(){
-        if(this->resource_) dlclose(this->resource_);
-    }
+    LoaderRaw(std::string _file);
+    ~LoaderRaw();
+    void load();
+    void close();
 };
 // class LoaderPlugin: public Loader{
 //     ClassLoader* resource_;
@@ -42,4 +29,4 @@ public:
 //         delete this->resource_;
 //     }
 // };
-#endif
\ No newline at end of file
+#endif

include/core/utils/params_server.hpp → include/core/utils/params_server.h

-#ifndef PARAMS_SERVER_HPP
-#define PARAMS_SERVER_HPP
+#ifndef PARAMS_SERVER_H
+#define PARAMS_SERVER_H
 
 #include "core/utils/converter.h"
-//#include "core/yaml/parser_yaml.hpp"
 
-#include <vector>
-#include <regex>
 #include <map>
 #include <exception>
 
@@ -14,52 +11,40 @@ namespace wolf{
 class MissingValueException : public std::runtime_error
 {
 public:
-  MissingValueException(std::string msg) : std::runtime_error(msg) {}
+  MissingValueException(std::string _msg) : std::runtime_error(_msg) {}
 };
 
 class ParamsServer{
-    std::map<std::string, std::string> _params;
+    std::map<std::string, std::string> params_;
 public:
-    ParamsServer(){
-        _params = std::map<std::string, std::string>();
-    }
-    ParamsServer(std::map<std::string, std::string> params){
-        _params = params;
-    }
+    ParamsServer();
+    ParamsServer(std::map<std::string, std::string> _params);
     ~ParamsServer(){
         //
     }
 
-    void print(){
-        for(auto it : _params)
-            std::cout << it.first << "~~" << it.second << std::endl;
-    }
+    void print();
 
 
-    void addParam(std::string key, std::string value){
-        _params.insert(std::pair<std::string, std::string>(key, value));
-    }
+    void addParam(std::string _key, std::string _value);
 
-    void addParams(std::map<std::string, std::string> params)
-    {
-        _params.insert(params.begin(), params.end());
-    }
+    void addParams(std::map<std::string, std::string> _params);
 
    // 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);
+   //     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);
+    T getParam(std::string _key) const {
+        if(params_.find(_key) != params_.end()){
+            return converter<T>::convert(params_.find(_key)->second);
         }else{
-            throw MissingValueException("The following key: '" + key + "' has not been found in the parameters server.");
+            throw MissingValueException("The following key: '" + _key + "' has not been found in the parameters server.");
         }
     }