.gitignore

@@ -4,7 +4,10 @@
 README.txt
 bin/
 build/
+build_debug/
 build_release/
+build-release/
+build-debug/
 lib/
 .idea/
 ./Wolf.user

.gitlab-ci.yml

@@ -6,6 +6,25 @@ stages:
   - deploy_ros
 
 ############ YAML ANCHORS ############
+.print_variables_template: &print_variables_definition
+  # Print variables
+  - echo $CI_COMMIT_BRANCH
+  - echo $WOLF_IMU_BRANCH
+  - echo $WOLF_GNSS_BRANCH
+  - echo $WOLF_LASER_BRANCH
+  - echo $WOLF_VISION_BRANCH
+  - echo $WOLF_APRILTAG_BRANCH
+  - echo $WOLF_BODYDYNAMICS_BRANCH
+  - echo $GNSSUTILS_BRANCH
+  - echo $LASERSCANUTILS_BRANCH
+  - echo $DEPLOY_CI_ROS
+  - echo $WOLF_ROS_IMU_BRANCH
+  - echo $WOLF_ROS_GNSS_BRANCH
+  - echo $WOLF_ROS_LASER_BRANCH
+  - echo $WOLF_ROS_VISION_BRANCH
+  - echo $WOLF_ROS_APRILTAG_BRANCH
+  - echo $WOLF_ROS_BODYDYNAMICS_BRANCH
+
 .preliminaries_template: &preliminaries_definition
   ## Install ssh-agent if not already installed, it is required by Docker.
   ## (change apt-get to yum if you use an RPM-based image)
@@ -80,6 +99,7 @@ license_header:
   stage: license
   image: labrobotica/wolf_deps:20.04
   before_script:  
+    - *print_variables_definition
     - *preliminaries_definition
   script: 
     - *license_header_definition
@@ -89,6 +109,7 @@ build_and_test:bionic:
   stage: build_and_test
   image: labrobotica/wolf_deps:18.04
   script:
+    - *print_variables_definition
     - *build_and_test_definition
 
 ############ UBUNTU 20.04 TESTS ############
@@ -96,6 +117,7 @@ build_and_test:focal:
   stage: build_and_test
   image: labrobotica/wolf_deps:20.04
   script:
+    - *print_variables_definition
     - *build_and_test_definition
 
 ############ DEPLOY PLUGINS ANY BRANCY EXCEPT FOR main ############
@@ -117,6 +139,7 @@ deploy_gnss:
     - if: $CI_COMMIT_BRANCH != "main"
   variables:
     WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    GNSSUTILS_BRANCH: $GNSSUTILS_BRANCH
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/gnss
@@ -141,6 +164,7 @@ deploy_laser:
     - if: $CI_COMMIT_BRANCH != "main"
   variables:
     WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    LASERSCANUTILS_BRANCH: $LASERSCANUTILS_BRANCH
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/laser
@@ -153,6 +177,7 @@ deploy_apriltag:
     - if: $CI_COMMIT_BRANCH != "main"
   variables:
     WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_VISION_BRANCH: $WOLF_VISION_BRANCH
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/apriltag
@@ -165,6 +190,7 @@ deploy_bodydynamics:
     - if: $CI_COMMIT_BRANCH != "main"
   variables:
     WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_IMU_BRANCH: $WOLF_IMU_BRANCH
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/bodydynamics
@@ -177,7 +203,7 @@ deploy_imu_main:
   rules: 
     - if: $CI_COMMIT_BRANCH == "main"
   variables:
-    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_CORE_BRANCH: main
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/imu
@@ -189,7 +215,8 @@ deploy_gnss_main:
   rules: 
     - if: $CI_COMMIT_BRANCH == "main"
   variables:
-    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_CORE_BRANCH: main
+    GNSSUTILS_BRANCH: $GNSSUTILS_BRANCH
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/gnss
@@ -201,7 +228,7 @@ deploy_vision_main:
   rules: 
     - if: $CI_COMMIT_BRANCH == "main"
   variables:
-    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_CORE_BRANCH: main
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/vision
@@ -213,7 +240,8 @@ deploy_laser_main:
   rules: 
     - if: $CI_COMMIT_BRANCH == "main"
   variables:
-    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_CORE_BRANCH: main
+    LASERSCANUTILS_BRANCH: $LASERSCANUTILS_BRANCH
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/laser
@@ -225,7 +253,8 @@ deploy_apriltag_main:
   rules: 
     - if: $CI_COMMIT_BRANCH == "main"
   variables:
-    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_CORE_BRANCH: main
+    WOLF_VISION_BRANCH: main
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/apriltag
@@ -237,7 +266,8 @@ deploy_bodydynamics_main:
   rules: 
     - if: $CI_COMMIT_BRANCH == "main"
   variables:
-    WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+    WOLF_CORE_BRANCH: main
+    WOLF_IMU_BRANCH: main
     DEPLOY_CI_ROS: "false"
   trigger: 
     project: mobile_robotics/wolf_projects/wolf_lib/plugins/bodydynamics

demos/hello_wolf/factor_bearing.h

@@ -36,18 +36,28 @@ namespace wolf
 
 using namespace Eigen;
 
-class FactorBearing : public FactorAutodiff<FactorBearing, 1, 2, 1, 2>
+class FactorBearing : public FactorAutodiff<FactorBearing, 1, 2, 1, 2, 1, 2>
 {
     public:
-        FactorBearing(const LandmarkBasePtr& _landmark_other_ptr,
+        FactorBearing(const FeatureBasePtr& _feature_ptr,
+                      const LandmarkBasePtr& _landmark_other_ptr,
                       const ProcessorBasePtr& _processor_ptr,
                       bool _apply_loss_function, FactorStatus _status) :
-                FactorAutodiff<FactorBearing, 1, 2, 1, 2>("BEARING", TOP_LMK, nullptr, nullptr, nullptr,
-                                                          _landmark_other_ptr, _processor_ptr,
-                                                          _apply_loss_function, _status,
-                                                          getCapture()->getFrame()->getP(),
-                                                          getCapture()->getFrame()->getO(),
-                                                          _landmark_other_ptr->getP())
+                FactorAutodiff<FactorBearing, 1, 2, 1, 2, 1, 2>("BEARING", 
+                                                        TOP_LMK, 
+                                                        _feature_ptr,
+                                                        nullptr, 
+                                                        nullptr, 
+                                                        nullptr,
+                                                        _landmark_other_ptr, 
+                                                        _processor_ptr,
+                                                        _apply_loss_function, 
+                                                        _status,
+                                                        _feature_ptr->getCapture()->getFrame()->getP(),
+                                                        _feature_ptr->getCapture()->getFrame()->getO(),
+                                                        _feature_ptr->getCapture()->getSensor()->getP(),
+                                                        _feature_ptr->getCapture()->getSensor()->getO(),
+                                                        _landmark_other_ptr->getP())
         {
             //
         }
@@ -58,9 +68,11 @@ class FactorBearing : public FactorAutodiff<FactorBearing, 1, 2, 1, 2>
         }
 
         template<typename T>
-        bool operator ()(const T* const _p1,
-                         const T* const _o1,
-                         const T* const _p2,
+        bool operator ()(const T* const _p_w_r,
+                         const T* const _o_w_r,
+                         const T* const _p_r_s, // sensor position
+                         const T* const _o_r_s, // sensor orientation
+                         const T* const _lmk,
                          T* _res) const;
 
 };
@@ -73,24 +85,30 @@ namespace wolf
 {
 
 template<typename T>
-inline bool FactorBearing::operator ()(const T* const _p1, const T* const _o1,
-                                                                    const T* const _p2, T* _res) const
+inline bool FactorBearing::operator ()( const T* const _p_w_r, 
+                                        const T* const _o_w_r,
+                                        const T* const _p_r_s, // sensor position
+                                        const T* const _o_r_s, // sensor orientation
+                                        const T* const _lmk, 
+                                        T* _res) const
 {
 
     // 1. produce a transformation matrix to transform from robot frame to world frame
-    Transform<T, 2, Isometry> H_w_r = Translation<T,2>(_p1[0], _p1[1]) * Rotation2D<T>(*_o1) ; // Robot frame = robot-to-world transform
+    Transform<T, 2, Isometry> H_w_r = Translation<T,2>(_p_w_r[0], _p_w_r[1]) * Rotation2D<T>(*_o_w_r) ; // Robot frame = robot-to-world transform
+    Transform<T, 2, Isometry> H_r_s = Translation<T,2>(_p_r_s[0], _p_r_s[1]) * Rotation2D<T>(*_o_r_s) ; // Robot frame = robot-to-world transform
+    
     // Map input pointers into meaningful Eigen elements
-    Map<const Matrix<T, 2, 1>>      point_w(_p2);
+    Map<const Matrix<T, 2, 1>>      point_w(_lmk);
     Map<const Matrix<T, 1, 1>>      res(_res);
 
-    // 2. Transform world point to robot-referenced point
-    Matrix<T, 2, 1> point_r = H_w_r.inverse() * point_w;
+    // 2. Transform world point to sensor-referenced point
+    Matrix<T, 2, 1> point_s = (H_w_r * H_r_s).inverse() * point_w;
 
     // 3. Get the expected bearing of the point
-    T bearing   = atan2(point_r(1), point_r(0));
+    T bearing   = atan2(point_s(1), point_s(0));
 
     // 4. Get the measured range-and-bearing to the point
-    Matrix<T, 2, 1> range_bearing       = getMeasurement();
+    Matrix<T, 2, 1> range_bearing = getMeasurement().cast<T>();
 
     // 5. Get the bearing error by comparing against the bearing measurement
     T er   = range_bearing(1) - bearing;
@@ -100,7 +118,7 @@ inline bool FactorBearing::operator ()(const T* const _p1, const T* const _o1,
         er -= T(2*M_PI);
 
     // 6. Compute the residual by scaling according to the standard deviation of the bearing part
-    *_res   = er * T(getMeasurementSquareRootInformationUpper()(1,1));
+    *_res   = er * getMeasurementSquareRootInformationUpper()(1,1);
 
     return true;
 }

include/core/frame/frame_base.h

@@ -154,20 +154,20 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
         CaptureBaseConstPtrList getCapturesOfType(const std::string& type) const;
         CaptureBasePtrList getCapturesOfType(const std::string& type);
 
-        CaptureBaseConstPtr getCaptureOf(const SensorBaseConstPtr _sensor_ptr) const;
-        CaptureBasePtr getCaptureOf(const SensorBaseConstPtr _sensor_ptr);
+        CaptureBaseConstPtr getCaptureOf(SensorBaseConstPtr _sensor_ptr) const;
+        CaptureBasePtr getCaptureOf(SensorBaseConstPtr _sensor_ptr);
 
-        CaptureBaseConstPtr getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type) const;
-        CaptureBasePtr getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type);
+        CaptureBaseConstPtr getCaptureOf(SensorBaseConstPtr _sensor_ptr, const std::string& type) const;
+        CaptureBasePtr getCaptureOf(SensorBasePtr _sensor_ptr, const std::string& type);
         
-        CaptureBaseConstPtrList getCapturesOf(const SensorBasePtr _sensor_ptr) const;
-        CaptureBasePtrList getCapturesOf(const SensorBasePtr _sensor_ptr);
+        CaptureBaseConstPtrList getCapturesOf(SensorBaseConstPtr _sensor_ptr) const;
+        CaptureBasePtrList getCapturesOf(SensorBasePtr _sensor_ptr);
 
-        FactorBaseConstPtr getFactorOf(const ProcessorBasePtr _processor_ptr) const;
-        FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr);
+        FactorBaseConstPtr getFactorOf(ProcessorBaseConstPtr _processor_ptr) const;
+        FactorBasePtr getFactorOf(ProcessorBasePtr _processor_ptr);
         
-        FactorBaseConstPtr getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type) const;
-        FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type);
+        FactorBaseConstPtr getFactorOf(ProcessorBaseConstPtr _processor_ptr, const std::string& type) const;
+        FactorBasePtr getFactorOf(ProcessorBasePtr _processor_ptr, const std::string& type);
         
         unsigned int getHits() const;
 

include/core/problem/problem.h

@@ -37,13 +37,11 @@ struct ParamsProcessorBase;
 
 //wolf includes
 #include "core/common/wolf.h"
+#include "core/utils/params_server.h"
 #include "core/frame/frame_base.h"
 #include "core/state_block/state_block.h"
-#include "core/utils/params_server.h"
-#include "core/sensor/factory_sensor.h"
-#include "core/processor/factory_processor.h"
-#include <core/processor/motion_provider.h>
 #include "core/state_block/state_composite.h"
+#include "core/processor/motion_provider.h"
 
 // std includes
 #include <mutex>

include/core/processor/motion_provider.h

@@ -80,7 +80,7 @@ class MotionProvider
         void setStatePriority(int);
 
     public:
-        const StateStructure& getStateStructure ( ) { return state_structure_; };
+        const StateStructure& getStateStructure ( ) const { return state_structure_; };
         void setStateStructure(std::string _state_structure) { state_structure_ = _state_structure; };
         void addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion_ptr);
 

include/core/processor/processor_base.h

@@ -92,6 +92,7 @@ class Buffer
 public:
 
     typedef typename std::map<TimeStamp,T>::iterator Iterator; // buffer iterator
+    typedef typename std::map<TimeStamp,T>::const_iterator ConstIterator; // buffer iterator
 
     Buffer(){};
     ~Buffer(void){};
@@ -101,27 +102,28 @@ public:
     *  Select from the buffer the closest element (w.r.t. time stamp),
     * respecting a defined time tolerances
     */
-    T select(const TimeStamp& _time_stamp, const double& _time_tolerance);
+    T select(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
     
     /**\brief Select an element iterator from the buffer
     *
     *  Select from the buffer the iterator pointing to the closest element (w.r.t. time stamp),
     * respecting a defined time tolerances
     */
+    ConstIterator selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
     Iterator selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance);
 
-    T selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance);
+    T selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
     
-    T selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance);
+    T selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance) const;
 
-    T selectFirst();
+    T selectFirst() const;
 
-    T selectLast();
+    T selectLast() const;
 
     /**\brief Buffer size
     *
     */
-    SizeStd size(void);
+    SizeStd size(void) const;
 
     /**\brief Add a element to the buffer
     *
@@ -132,6 +134,7 @@ public:
     *
     * elements are ordered from most recent to oldest
     */
+    const std::map<TimeStamp,T>& getContainer() const;
     std::map<TimeStamp,T>& getContainer();
 
     /**\brief Remove all elements in the buffer with a time stamp older than the specified
@@ -152,7 +155,7 @@ public:
     /**\brief is the buffer empty ?
     *
     */
-    bool empty();
+    bool empty() const;
 
 protected:
     /**\brief Check time tolerance
@@ -160,14 +163,19 @@ protected:
     * Check if the time distance between two time stamps is smaller than
     * the time tolerance.
     */
-    static bool checkTimeTolerance(const TimeStamp& _time_stamp1, const TimeStamp& _time_stamp2, const double& _time_tolerance);
+    static bool checkTimeTolerance(const TimeStamp& _time_stamp1, 
+                                   const TimeStamp& _time_stamp2,
+                                   const double& _time_tolerance);
 
     /**\brief Check time tolerance
     *
     * Check if the time distance between two time stamps is smaller than
     * the minimum time tolerance of the two frames.
     */
-    static bool doubleCheckTimeTolerance(const TimeStamp& _time_stamp1, const double& _time_tolerance1, const TimeStamp& _time_stamp2, const double& _time_tolerance2);
+    static bool doubleCheckTimeTolerance(const TimeStamp& _time_stamp1,
+                                         const double& _time_tolerance1, 
+                                         const TimeStamp& _time_stamp2, 
+                                         const double& _time_tolerance2);
 
 protected:
 
@@ -454,6 +462,42 @@ std::shared_ptr<classType> ProcessorBase::emplace(SensorBasePtr _sen_ptr, T&&...
 
 /////////////////////////////////////////////////////////////////////////////////////////
 
+template <typename T>
+typename Buffer<T>::ConstIterator Buffer<T>::selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance) const
+{
+    Buffer<T>::ConstIterator post = container_.upper_bound(_time_stamp);
+
+    bool prev_exists = (post != container_.begin());
+    bool post_exists = (post != container_.end());
+
+    bool post_ok = post_exists && checkTimeTolerance(post->first, _time_stamp, _time_tolerance);
+
+    if (prev_exists)
+    {
+        Buffer<T>::ConstIterator prev = std::prev(post);
+
+        bool prev_ok = checkTimeTolerance(prev->first, _time_stamp, _time_tolerance);
+
+        if (prev_ok && !post_ok)
+            return prev;
+
+        else if (!prev_ok && post_ok)
+            return post;
+
+        else if (prev_ok && post_ok)
+        {
+            if (std::fabs(post->first - _time_stamp) < std::fabs(prev->first - _time_stamp))
+                return post;
+            else
+                return prev;
+        }
+    }
+    else if (post_ok)
+        return post;
+
+    return container_.end();
+}
+
 template <typename T>
 typename Buffer<T>::Iterator Buffer<T>::selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance)
 {
@@ -491,12 +535,12 @@ typename Buffer<T>::Iterator Buffer<T>::selectIterator(const TimeStamp& _time_st
 }
 
 template <typename T>
-T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance)
+T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance) const
 {
     if (container_.empty())
         return nullptr;
 
-    Buffer<T>::Iterator it = selectIterator(_time_stamp, _time_tolerance);
+    auto it = selectIterator(_time_stamp, _time_tolerance);
 
     // end is returned from selectIterator if an element of the buffer complying with the time stamp
     // and time tolerance has not been found
@@ -508,7 +552,7 @@ T Buffer<T>::select(const TimeStamp& _time_stamp, const double& _time_tolerance)
 }
 
 template <typename T>
-T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance)
+T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const double& _time_tolerance) const
 {
     // There is no element
     if (container_.empty())
@@ -529,7 +573,7 @@ T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const double& _time
 
 
 template <typename T>
-T Buffer<T>::selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance)
+T Buffer<T>::selectLastAfter(const TimeStamp& _time_stamp, const double& _time_tolerance) const
 {
     // There is no element
     if (container_.empty())
@@ -549,7 +593,7 @@ T Buffer<T>::selectLastAfter(const TimeStamp& _time_stamp, const double& _time_t
 }
 
 template <typename T>
-T Buffer<T>::selectFirst()
+T Buffer<T>::selectFirst() const
 {
     // There is no element
     if (container_.empty())
@@ -560,7 +604,7 @@ T Buffer<T>::selectFirst()
 }
 
 template <typename T>
-T Buffer<T>::selectLast()
+T Buffer<T>::selectLast() const
 {
     // There is no element
     if (container_.empty())
@@ -576,6 +620,12 @@ void Buffer<T>::emplace(const TimeStamp& _time_stamp, const T& _element)
     container_.emplace(_time_stamp, _element);
 }
 
+template <typename T>
+const std::map<TimeStamp,T>& Buffer<T>::getContainer() const
+{
+    return container_;
+}
+
 template <typename T>
 std::map<TimeStamp,T>& Buffer<T>::getContainer()
 {
@@ -589,13 +639,13 @@ inline void Buffer<T>::clear()
 }
 
 template <typename T>
-inline bool Buffer<T>::empty()
+inline bool Buffer<T>::empty() const
 {
     return container_.empty();
 }
 
 template <typename T>
-inline SizeStd Buffer<T>::size(void)
+inline SizeStd Buffer<T>::size(void) const
 {
     return container_.size();
 }
@@ -628,8 +678,8 @@ inline bool Buffer<T>::doubleCheckTimeTolerance(const TimeStamp& _time_stamp1,
 
 template <typename T>
 inline bool Buffer<T>::checkTimeTolerance(const TimeStamp& _time_stamp1,
-                                                const TimeStamp& _time_stamp2,
-                                                const double& _time_tolerance)
+                                          const TimeStamp& _time_stamp2,
+                                          const double& _time_tolerance)
 {
     double time_diff = std::fabs(_time_stamp1 - _time_stamp2);
     bool pass = time_diff <= _time_tolerance;

include/core/processor/processor_tracker.h

@@ -136,7 +136,7 @@ class ProcessorTracker : public ProcessorBase
                          ParamsProcessorTrackerPtr _params_tracker);
         ~ProcessorTracker() override;
 
-        StateStructure getStateStructure() const;
+        const StateStructure& getStateStructure() const;
 
         virtual CaptureBaseConstPtr getOrigin() const;
         virtual CaptureBasePtr getOrigin();
@@ -313,7 +313,7 @@ inline FeatureBasePtrList& ProcessorTracker::getNewFeaturesListIncoming()
     return new_features_incoming_;
 }
 
-inline StateStructure ProcessorTracker::getStateStructure ( ) const
+inline const StateStructure& ProcessorTracker::getStateStructure ( ) const
 {
     return state_structure_;
 }

src/frame/frame_base.cpp

@@ -230,7 +230,7 @@ CaptureBasePtrList FrameBase::getCapturesOfType(const std::string& type)
     return captures;
 }
 
-CaptureBaseConstPtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type) const
+CaptureBaseConstPtr FrameBase::getCaptureOf(SensorBaseConstPtr _sensor_ptr, const std::string& type) const
 {
     for (auto capture_ptr : getCaptureList())
         if (capture_ptr->getSensor() == _sensor_ptr && capture_ptr->getType() == type)
@@ -238,7 +238,7 @@ CaptureBaseConstPtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, con
     return nullptr;
 }
 
-CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type)
+CaptureBasePtr FrameBase::getCaptureOf(SensorBasePtr _sensor_ptr, const std::string& type)
 {
     for (auto capture_ptr : getCaptureList())
         if (capture_ptr->getSensor() == _sensor_ptr && capture_ptr->getType() == type)
@@ -246,7 +246,7 @@ CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, const st
     return nullptr;
 }
 
-CaptureBaseConstPtr FrameBase::getCaptureOf(const SensorBaseConstPtr _sensor_ptr) const
+CaptureBaseConstPtr FrameBase::getCaptureOf(SensorBaseConstPtr _sensor_ptr) const
 {
     if (_sensor_ptr)
         for (auto capture_ptr : getCaptureList())
@@ -255,7 +255,7 @@ CaptureBaseConstPtr FrameBase::getCaptureOf(const SensorBaseConstPtr _sensor_ptr
     return nullptr;
 }
 
-CaptureBasePtr FrameBase::getCaptureOf(const SensorBaseConstPtr _sensor_ptr)
+CaptureBasePtr FrameBase::getCaptureOf(SensorBaseConstPtr _sensor_ptr)
 {
     if (_sensor_ptr)
         for (auto capture_ptr : getCaptureList())
@@ -264,7 +264,7 @@ CaptureBasePtr FrameBase::getCaptureOf(const SensorBaseConstPtr _sensor_ptr)
     return nullptr;
 }
 
-CaptureBaseConstPtrList FrameBase::getCapturesOf(const SensorBasePtr _sensor_ptr) const
+CaptureBaseConstPtrList FrameBase::getCapturesOf(SensorBaseConstPtr _sensor_ptr) const
 {
     CaptureBaseConstPtrList captures;
     for (auto capture_ptr : getCaptureList())
@@ -273,7 +273,7 @@ CaptureBaseConstPtrList FrameBase::getCapturesOf(const SensorBasePtr _sensor_ptr
     return captures;
 }
 
-CaptureBasePtrList FrameBase::getCapturesOf(const SensorBasePtr _sensor_ptr)
+CaptureBasePtrList FrameBase::getCapturesOf(SensorBasePtr _sensor_ptr)
 {
     CaptureBasePtrList captures;
     for (auto capture_ptr : getCaptureList())
@@ -282,7 +282,7 @@ CaptureBasePtrList FrameBase::getCapturesOf(const SensorBasePtr _sensor_ptr)
     return captures;
 }
 
-FactorBaseConstPtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type) const
+FactorBaseConstPtr FrameBase::getFactorOf(ProcessorBaseConstPtr _processor_ptr, const std::string& type) const
 {
     for (auto factor_ptr : getConstrainedByList())
         if (factor_ptr->getProcessor() == _processor_ptr && factor_ptr->getType() == type)
@@ -295,7 +295,7 @@ FactorBaseConstPtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr,
     return nullptr;
 }
 
-FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type) 
+FactorBasePtr FrameBase::getFactorOf(ProcessorBasePtr _processor_ptr, const std::string& type) 
 {
     for (auto factor_ptr : getConstrainedByList())
         if (factor_ptr->getProcessor() == _processor_ptr && factor_ptr->getType() == type)
@@ -308,7 +308,7 @@ FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr, cons
     return nullptr;
 }
 
-FactorBaseConstPtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr) const
+FactorBaseConstPtr FrameBase::getFactorOf(ProcessorBaseConstPtr _processor_ptr) const
 {
     for (auto factor_ptr : getConstrainedByList())
         if (factor_ptr->getProcessor() == _processor_ptr)
@@ -321,7 +321,7 @@ FactorBaseConstPtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr)
     return nullptr;
 }
 
-FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr)
+FactorBasePtr FrameBase::getFactorOf(ProcessorBasePtr _processor_ptr)
 {
     for (auto factor_ptr : getConstrainedByList())
         if (factor_ptr->getProcessor() == _processor_ptr)

src/map/map_base.cpp

@@ -23,7 +23,7 @@
 // wolf
 #include "core/map/map_base.h"
 #include "core/landmark/landmark_base.h"
-//#include "core/common/factory.h"
+#include "core/common/factory.h"
 
 // YAML
 #include <yaml-cpp/yaml.h>

src/problem/problem.cpp

@@ -21,40 +21,18 @@
 //--------LICENSE_END--------
 // wolf includes
 #include "core/problem/problem.h"
-#include "core/hardware/hardware_base.h"
-#include "core/trajectory/trajectory_base.h"
-#include "core/map/map_base.h"
 #include "core/map/factory_map.h"
-#include "core/sensor/sensor_base.h"
 #include "core/sensor/factory_sensor.h"
 #include "core/processor/factory_processor.h"
-#include "core/processor/processor_motion.h"
-#include "core/processor/processor_tracker.h"
-#include "core/capture/capture_pose.h"
 #include "core/capture/capture_void.h"
-#include "core/factor/factor_base.h"
 #include "core/factor/factor_block_absolute.h"
 #include "core/factor/factor_quaternion_absolute.h"
-#include "core/state_block/state_block.h"
 #include "core/state_block/state_quaternion.h"
 #include "core/state_block/state_angle.h"
 #include "core/tree_manager/factory_tree_manager.h"
 #include "core/tree_manager/tree_manager_base.h"
-
-#include "core/utils/params_server.h"
 #include "core/utils/loader.h"
-#include "core/utils/check_log.h"
-#include "core/math/covariance.h"
-#include "core/state_block/factory_state_block.h"
-
-// C++ includes
-#include <algorithm>
-#include <map>
-#include <sstream>
-#include <stdexcept>
-#include <string>
-#include <vector>
-#include <unordered_set>
+
 
 namespace wolf
 {

src/processor/processor_tracker.cpp

@@ -216,10 +216,6 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
                 last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
                 last_ptr_->getFrame()->link(getProblem());
 
-                // // make F; append incoming to new F
-                // FrameBasePtr frm = FrameBase::createNonKeyFrame<FrameBase>(incoming_ptr_->getTimeStamp());
-                // incoming_ptr_->link(frm);
-
                 // Establish factors
                 establishFactors();
 
@@ -232,7 +228,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
                 // make F; append incoming to new F
                 FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
                                                                  getProblem()->getFrameStructure(),
-                                                                 last_ptr_->getFrame()->getState());
+                                                                 getProblem()->getState(incoming_ptr_->getTimeStamp()));
                 incoming_ptr_   ->link(frame);
                 origin_ptr_     = last_ptr_;
                 last_ptr_       = incoming_ptr_;
@@ -250,7 +246,7 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
                 // Replace last frame for a new one in incoming
                 FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
                                                                  getProblem()->getFrameStructure(),
-                                                                 last_ptr_->getFrame()->getState());
+                                                                 getProblem()->getState(incoming_ptr_->getTimeStamp()));
                 incoming_ptr_->link(frame);
                 last_ptr_->unlink(); // unlink last (destroying the frame) instead of frame destruction that would implicitly destroy last
 

src/yaml/processor_odom_3d_yaml.cpp

@@ -31,7 +31,7 @@
 
 // wolf
 #include "core/processor/processor_odom_3d.h"
-#include "core/common/factory.h"
+#include "core/processor/factory_processor.h"
 
 // yaml-cpp library
 #include <yaml-cpp/yaml.h>

src/yaml/sensor_odom_2d_yaml.cpp

@@ -33,6 +33,7 @@
 
 // wolf
 #include "core/sensor/sensor_odom_2d.h"
+#include "core/sensor/factory_sensor.h"
 
 // yaml-cpp library
 #include <yaml-cpp/yaml.h>

src/yaml/sensor_odom_3d_yaml.cpp

@@ -31,7 +31,7 @@
 
 // wolf
 #include "core/sensor/sensor_odom_3d.h"
-#include "core/common/factory.h"
+#include "core/sensor/factory_sensor.h"
 
 // yaml-cpp library
 #include <yaml-cpp/yaml.h>

src/yaml/sensor_pose_yaml.cpp

@@ -31,7 +31,7 @@
 
 // wolf
 #include "core/sensor/sensor_pose.h"
-#include "core/common/factory.h"
+#include "core/sensor/factory_sensor.h"
 
 // yaml-cpp library
 #include <yaml-cpp/yaml.h>

test/gtest_factor_diff_drive.cpp

@@ -626,7 +626,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
 
     // right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
     int N = 6;
-    Vector3d data;
+    Vector2d data;
     Matrix2d data_cov; data_cov.setIdentity();
 
     data(0) = 0.50*intr->ticks_per_wheel_revolution / N;

test/gtest_processor_odom_3d.cpp

@@ -149,7 +149,7 @@ TEST(ProcessorOdom3d, deltaPlusDelta_Jac)
 
     MatrixXd J_D(7,7), J_d(7,7);
 
-    JAC_NUMERIC(prc_ptr, D, d, dt, J_D, J_d, Constants::EPS);
+    JAC_NUMERIC(prc_ptr, D, d, dt, J_D, J_d, 1000*Constants::EPS);
     WOLF_DEBUG("J_D:\n ", J_D);
     WOLF_DEBUG("J_d:\n ", J_d);