hello_wolf/processor_range_bearing.cpp

@@ -15,7 +15,7 @@ namespace wolf
 {
 
 ProcessorRangeBearing::ProcessorRangeBearing(ProcessorParamsBasePtr _params) :
-        ProcessorBase("RANGE BEARING", _params)
+        ProcessorBase("ProcessorRangeBearing", _params)
 {
     //
 }
@@ -163,7 +163,7 @@ bool ProcessorRangeBearing::storeCapture(CaptureBasePtr _cap_ptr)
 #include "core/processor/processor_factory.h"
 namespace wolf
 {
-WOLF_REGISTER_PROCESSOR("RANGE BEARING", ProcessorRangeBearing)
-WOLF_REGISTER_PROCESSOR_AUTO("RANGE BEARING", ProcessorRangeBearing)
+WOLF_REGISTER_PROCESSOR("ProcessorRangeBearing", ProcessorRangeBearing)
+WOLF_REGISTER_PROCESSOR_AUTO("ProcessorRangeBearing", ProcessorRangeBearing)
 } // namespace wolf
 

hello_wolf/sensor_range_bearing.cpp

@@ -14,7 +14,7 @@ namespace wolf
 WOLF_PTR_TYPEDEFS(SensorRangeBearing);
 
 SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXs& _extrinsics, const Eigen::Vector2s& _noise_std) :
-        SensorBase("RANGE BEARING",
+        SensorBase("SensorRangeBearing",
                    std::make_shared<StateBlock>(_extrinsics.head<2>(), true),
                    std::make_shared<StateAngle>(_extrinsics(2), true),
                    nullptr,
@@ -41,6 +41,6 @@ SensorRangeBearing::~SensorRangeBearing()
 #include "core/sensor/sensor_factory.h"
 namespace wolf
 {
-WOLF_REGISTER_SENSOR("RANGE BEARING", SensorRangeBearing)
-WOLF_REGISTER_SENSOR_AUTO("RANGE BEARING", SensorRangeBearing)
+WOLF_REGISTER_SENSOR("SensorRangeBearing", SensorRangeBearing)
+WOLF_REGISTER_SENSOR_AUTO("SensorRangeBearing", SensorRangeBearing)
 } // namespace wolf

hello_wolf/yaml/hello_wolf_config.yaml

@@ -12,14 +12,14 @@ config:
     
   sensors:
     
-    - type:               "ODOM 2D"
+    - type:               "SensorOdom2D"
       plugin:             "core"
       name:               "sen odom"
       extrinsic: 
         pose:             [0,0, 0]
       follow:             "hello_wolf/yaml/sensor_odom_2D.yaml"         # config parameters in this file
 
-    - type:               "RANGE BEARING"
+    - type:               "SensorRangeBearing"
       plugin:             "core"
       name:               "sen rb"  
       extrinsic:
@@ -29,20 +29,19 @@ config:
           
   processors:
     
-    - type:               "ODOM 2D"
+    - type:               "ProcessorOdom2D"
       plugin:             "core"
       name:               "prc odom"
       sensor_name:        "sen odom"                                    # attach processor to this sensor
       follow:             hello_wolf/yaml/processor_odom_2D.yaml        # config parameters in this file
     
-    - type:               "RANGE BEARING"
+    - type:               "ProcessorRangeBearing"
       plugin:             "core"
       name:               "prc rb"
       sensor_name:        "sen rb"                                      # attach processor to this sensor
       follow:             hello_wolf/yaml/processor_range_bearing.yaml  # config parameters in this file
     
     
-files:
-#    - "/Users/jsola/dev/wolf lib/core/lib/libhellowolf.dylib"
+
   
     
\ No newline at end of file

include/core/utils/params_server.hpp

@@ -40,14 +40,19 @@ public:
         _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);
-//        }
-//    }
+    void addParams(std::map<std::string, std::string> params)
+    {
+        _params.insert(params.begin(), params.end());
+    }
+
+   // 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 {

include/core/yaml/parser_yaml.hpp

@@ -355,6 +355,8 @@ void ParserYAML::walkTreeR(YAML::Node n, std::vector<std::string>& tags, std::st
       }
         break;
     }
+      case YAML::NodeType::Null :
+        break;
     default:
         assert(1 == 0 && "Unsupported node Type at walkTreeR.");
         break;

src/problem/problem.cpp

@@ -84,7 +84,8 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
     std::string frame_structure = _server.getParam<std::string> ("problem/frame_structure");
     int dim                     = _server.getParam<int>         ("problem/dimension");
     auto problem                = Problem::create(frame_structure, dim);
-    // cout << "PRINTING SERVER MAP" << endl;
+    //
+    //  cout << "PRINTING SERVER MAP" << endl;
     // _server.print();
     // cout << "-----------------------------------" << endl;
     WOLF_TRACE("Setting up problem with frame structure {" + frame_structure + "} and dimension " + std::to_string(dim) + "D");
@@ -149,10 +150,10 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
     }
 
     // Prior
-    Eigen::VectorXs prior_state = _server.getParam<Eigen::VectorXs>("problem/prior/state");
-    Eigen::MatrixXs prior_cov   = _server.getParam<Eigen::MatrixXs>("problem/prior/cov");
-    Scalar prior_time_tolerance = _server.getParam<Scalar>("problem/prior/time_tolerance");
-    Scalar prior_ts             = _server.getParam<Scalar>("problem/prior/timestamp");
+    Eigen::VectorXs prior_state = _server.getParam<Eigen::VectorXs> ("problem/prior/state");
+    Eigen::MatrixXs prior_cov   = _server.getParam<Eigen::MatrixXs> ("problem/prior/cov");
+    Scalar prior_time_tolerance = _server.getParam<Scalar>          ("problem/prior/time_tolerance");
+    Scalar prior_ts             = _server.getParam<Scalar>          ("problem/prior/timestamp");
 
     WOLF_TRACE("prior timestamp:\n"     , prior_ts);
     WOLF_TRACE("prior state:\n"         , prior_state.transpose());

src/processor/processor_tracker.cpp

@@ -87,8 +87,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
             // Issue KF callback with new KF
             getProblem()->keyFrameCallback(kfrm, shared_from_this(), params_tracker_->time_tolerance);
 
-            // Update pointers
             resetDerived();
+
+            // Update pointers
             origin_ptr_ = incoming_ptr_;
             last_ptr_   = incoming_ptr_;
             incoming_ptr_ = nullptr;
@@ -175,9 +176,9 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
                 last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
                 last_ptr_->getFrame()->setKey();
 
-                // make F; append incoming to new F
-                FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
-                incoming_ptr_->link(frm);
+                // // make F; append incoming to new F
+                // FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, incoming_ptr_->getTimeStamp());
+                // incoming_ptr_->link(frm);
 
                 // Establish factors
                 establishFactors();
@@ -187,6 +188,11 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
 
                 // Update pointers
                 resetDerived();
+
+                // make F; append incoming to new F
+                FrameBasePtr frm = getProblem()->emplaceFrame(NON_ESTIMATED, last_ptr_->getFrame()->getState(), incoming_ptr_->getTimeStamp());
+                incoming_ptr_->link(frm);
+
                 origin_ptr_ = last_ptr_;
                 last_ptr_   = incoming_ptr_;
                 incoming_ptr_ = nullptr;

wolf_scripts/rename.sh

@@ -50,8 +50,13 @@ types=$(getTypes | sort | uniq)
 # done
 for target in $(find include/ src/ test/ -type f); do
     for ctype in $types; do
+        filename=$(echo $target | rev | cut -d '/' -f1 | cut -d '.' -f2 | rev)
+        snake2camel $filename
         oldtype=$ctype
         ctype=$(echo $ctype | sed -r "s/([^\ 0-9])([^\ 0-9]+)\ ?/\1\L\2/g")
-        sed -ri "s/(Capture|Feature|Processor|Landmark|Factor|Sensor)(.*)\"${oldtype}\"/\1\2\"\1${ctype}\"/" $target
+        # echo $target $oldtype $ctype
+        # sed -ri "s/(Capture|Feature|Processor|Landmark|Factor|Sensor)(.*)\"${oldtype}\"/\1\2\"\1${ctype}\"/" $target
+      sed -ri "s/(Capture|Feature|Processor|Landmark|Factor|Sensor)(.*)\"${oldtype}\"/\1\2\"\1${ctype}\"/" $target
+      sed -ri "s%\"${oldtype}\"%\"${new_camel}\"%" $target
     done
-done
\ No newline at end of file
+done