[skip ci] wip

CMakeLists.txt

@@ -171,6 +171,7 @@ SET(HDRS_MAP
   )
 SET(HDRS_PROBLEM
   include/${PROJECT_NAME}/problem/problem.h
+  # include/${PROJECT_NAME}/problem/prior_problem.h
   )
 SET(HDRS_PROCESSOR
   include/${PROJECT_NAME}/processor/factory_processor.h
@@ -191,6 +192,7 @@ SET(HDRS_PROCESSOR
   )
 SET(HDRS_SENSOR
   include/${PROJECT_NAME}/sensor/factory_sensor.h
+  include/${PROJECT_NAME}/sensor/prior_sensor.h
   include/${PROJECT_NAME}/sensor/sensor_base.h
   include/${PROJECT_NAME}/sensor/sensor_diff_drive.h
   include/${PROJECT_NAME}/sensor/sensor_motion_model.h
@@ -280,6 +282,7 @@ SET(SRCS_MAP
   src/map/map_base.cpp
   )
 SET(SRCS_PROBLEM
+  # src/problem/prior_problem.cpp
   src/problem/problem.cpp
   )
 SET(SRCS_PROCESSOR
@@ -299,6 +302,7 @@ SET(SRCS_PROCESSOR
   src/processor/track_matrix.cpp
   )
 SET(SRCS_SENSOR
+  src/sensor/prior_sensor.cpp
   src/sensor/sensor_base.cpp
   src/sensor/sensor_diff_drive.cpp
   src/sensor/sensor_motion_model.cpp

demos/hello_wolf/hello_wolf.cpp

@@ -130,8 +130,8 @@ int main()
 
     // sensor odometer 2d
     ParamsSensorOdomPtr intrinsics_odo      = std::make_shared<ParamsSensorOdom>();
-    Priors priors_odo                       = {{'P',Prior("P", Vector2d::Zero())},
-                                               {'O',Prior("O", Vector1d::Zero())}};
+    PriorSensorMap priors_odo                       = {{'P',PriorSensor("P", Vector2d::Zero())},
+                                               {'O',PriorSensor("O", Vector1d::Zero())}};
     SensorBasePtr sensor_odo                = problem->installSensor("SensorOdom", "sensor odo", intrinsics_odo, priors_odo);
 
     // processor odometer 2d
@@ -147,8 +147,8 @@ int main()
 
     // sensor Range and Bearing
     ParamsSensorRangeBearingPtr intrinsics_rb = std::make_shared<ParamsSensorRangeBearing>();
-    Priors priors_rb                          = {{'P',Prior("P", Vector2d(1,1))},
-                                                 {'O',Prior("O", Vector1d::Zero())}};
+    PriorSensorMap priors_rb                          = {{'P',PriorSensor("P", Vector2d(1,1))},
+                                                 {'O',PriorSensor("O", Vector1d::Zero())}};
     intrinsics_rb->noise_range_metres_std     = 0.1;
     intrinsics_rb->noise_bearing_degrees_std  = 1.0;
     SensorBasePtr sensor_rb                   = problem->installSensor("SensorRangeBearing", "sensor RB", intrinsics_rb, priors_rb);

demos/hello_wolf/sensor_range_bearing.cpp

@@ -27,7 +27,7 @@ WOLF_PTR_TYPEDEFS(SensorRangeBearing);
 
 SensorRangeBearing::SensorRangeBearing(const SizeEigen& _dim,
                                        ParamsSensorRangeBearingPtr _params,
-                                       const Priors& _priors) :
+                                       const PriorSensorMap& _priors) :
         SensorBase("SensorRangeBearing", _dim, _params, _priors),
         params_rb_(_params)
 {

demos/hello_wolf/sensor_range_bearing.h

@@ -58,7 +58,7 @@ class SensorRangeBearing : public SensorBase
     public:
         SensorRangeBearing(const SizeEigen& _dim,
                            ParamsSensorRangeBearingPtr _params,
-                           const Priors& _priors);
+                           const PriorSensorMap& _priors);
         WOLF_SENSOR_CREATE(SensorRangeBearing, ParamsSensorRangeBearing);
         
         ~SensorRangeBearing() override;

include/core/problem/prior_problem.h

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#pragma once
+
+#include "core/state_block/prior.h"
+
+namespace wolf
+{
+
+class PriorProblem : public Prior
+{
+    protected:
+        double time_tolerance_;    // time tolerance of the first frame
+
+    public:
+        PriorProblem() = default;
+        PriorProblem(const std::string&     _type,
+                     const Eigen::VectorXd& _state,
+                     const double& _time_tolerance,
+                     const std::string&     _mode      = "fix",
+                     const Eigen::VectorXd& _noise_std = Eigen::VectorXd(0));
+
+        PriorProblem(const YAML::Node& _n);
+
+        virtual ~PriorProblem() = default;
+
+        double getTimeTolerance() const;
+
+        using Prior::check;
+
+        std::string print() const override;
+};
+
+typedef std::unordered_map<char, PriorProblem> StdMapPriorProblem;
+
+class PriorProblemMap : public StdMapPriorProblem
+{
+    public:
+        using StdMapPriorProblem::StdMapPriorProblem;
+
+        PriorProblemMap(const YAML::Node& _n);
+        virtual ~PriorProblemMap() = default;
+};
+
+inline double PriorProblem::getTimeTolerance() const { return time_tolerance_; }
+
+}  // namespace wolf
\ No newline at end of file

include/core/problem/problem.h

@@ -57,14 +57,6 @@ enum Notification
     REMOVE
 };
 
-struct PriorOptions
-{
-    std::string mode = "";
-    VectorComposite state;
-    MatrixComposite cov;
-};
-WOLF_STRUCT_PTR_TYPEDEFS(PriorOptions);
-
 /** \brief Wolf problem node element in the Wolf Tree
  */
 class Problem : public std::enable_shared_from_this<Problem>
@@ -87,7 +79,8 @@ class Problem : public std::enable_shared_from_this<Problem>
         mutable std::mutex mut_notifications_;
         mutable std::mutex mut_covariances_;
         StateStructure frame_structure_;
-        PriorOptionsPtr prior_options_;
+        PriorMap prior_options_;
+        bool prior_applied_;
 
         std::atomic_bool transformed_;
         VectorComposite  transformation_;
@@ -196,10 +189,8 @@ class Problem : public std::enable_shared_from_this<Problem>
         TrajectoryBasePtr getTrajectory();
 
         // Prior
-        bool isPriorSet() const;
-        void setPriorOptions(const std::string& _mode,
-                             const VectorComposite& _state = VectorComposite(),
-                             const VectorComposite& _cov   = VectorComposite());
+        bool isPriorApplied() const;
+        void setPriorOptions(const PriorMap& priors);
         FrameBasePtr applyPriorOptions(const TimeStamp& _ts);
         FrameBasePtr setPriorFactor(const VectorComposite &_state,
                                     const VectorComposite &_sigma,
@@ -443,9 +434,9 @@ inline TreeManagerBasePtr Problem::getTreeManager()
     return tree_manager_;
 }
 
-inline bool Problem::isPriorSet() const
+inline bool Problem::isPriorApplied() const
 {
-    return prior_options_ == nullptr;
+    return prior_applied_;
 }
 
 inline std::map<int,MotionProviderConstPtr> Problem::getMotionProviderMap() const

include/core/sensor/prior_sensor.h

+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#pragma once
+
+#include "core/state_block/prior.h"
+
+namespace wolf
+{
+
+class PriorSensor : public Prior
+{
+    protected:
+        bool            dynamic_;    // State dynamic
+        Eigen::VectorXd drift_std_;  // OPTIONAL std of the state drift [in units/sqrt(s)] diagonal elements (ONLY IF if dynamic)
+
+    public:
+        PriorSensor() = default;
+        PriorSensor(const std::string&     _type,
+                    const Eigen::VectorXd& _state,
+                    const std::string&     _mode      = "fix",
+                    const Eigen::VectorXd& _noise_std = Eigen::VectorXd(0),
+                    bool                   _dynamic   = false,
+                    const Eigen::VectorXd& _drift_std = Eigen::VectorXd(0));
+
+        PriorSensor(const YAML::Node& _n);
+
+        virtual ~PriorSensor() = default;
+
+        bool                   isDynamic() const;
+        const Eigen::VectorXd& getDriftStd() const;
+
+        void check() const override;
+
+        std::string print() const override;
+};
+
+typedef std::unordered_map<char, PriorSensor> StdMapPriorSensor;
+
+class PriorSensorMap : public StdMapPriorSensor
+{
+    public:
+        using StdMapPriorSensor::StdMapPriorSensor;
+
+        PriorSensorMap(const YAML::Node& _n);
+        virtual ~PriorSensorMap() = default;
+};
+
+inline bool PriorSensor::isDynamic() const { return dynamic_; }
+
+inline const Eigen::VectorXd& PriorSensor::getDriftStd() const { return drift_std_; }
+
+}  // namespace wolf
\ No newline at end of file

include/core/sensor/sensor_base.h

@@ -31,7 +31,7 @@ class ParamsServer;
 
 //Wolf includes
 #include "core/common/wolf.h"
-#include "core/state_block/prior.h"
+#include "core/sensor/prior_sensor.h"
 #include "core/common/node_base.h"
 #include "core/common/time_stamp.h"
 #include "core/common/params_base.h"
@@ -54,7 +54,7 @@ namespace wolf {
  *   SensorClass(const std::string& _unique_name, 
  *               SizeEigen _dim,
  *               ParamsSensorClassPtr _params, 
- *               const Priors& _priors)
+ *               const PriorSensorMap& _priors)
  * 
  * Also, there should be the schema file 'SensorClass.schema' containing the specifications 
  * of the user input yaml file.
@@ -64,7 +64,7 @@ static SensorBasePtr create(const YAML::Node& _input_node)
 {                                                                               \
     auto params = std::make_shared<ParamsSensorClass>(_input_node);             \
                                                                                 \
-    auto priors = Priors(_input_node["states"]);                                \
+    auto priors = PriorSensorMap(_input_node["states"]);                        \
                                                                                 \
     return std::make_shared<SensorClass>(params, priors);                       \
 }                                                                               \
@@ -80,7 +80,7 @@ static SensorBasePtr create(const std::string& _schema,
     }                                                                           \
     auto params = std::make_shared<ParamsSensorClass>(server.getNode());        \
                                                                                 \
-    auto priors = Priors(server.getNode()["states"]);                           \
+    auto priors = PriorSensorMap(server.getNode()["states"]);                   \
                                                                                 \
     return std::make_shared<SensorClass>(params, priors);                       \
 }                                                                               \
@@ -125,7 +125,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
 
         std::map<char, bool> state_block_dynamic_; // mark dynamic state blocks
 
-        std::map<char, FeatureBasePtr> params_prior_map_; // Priors (value and covariance) of extrinsic & intrinsic state blocks (by key in state_block_map_)
+        std::map<char, FeatureBasePtr> features_prior_map_; // Priors (value and covariance) of extrinsic & intrinsic state blocks (by key in state_block_map_)
 
         CaptureBasePtr last_capture_; // last capture of the sensor (in the WOLF tree)
 
@@ -135,7 +135,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
         int dim_; ///< Dimension compatibility of the sensor: 2: 2D, 3: 3D, -1: both
 
         void setProblem(ProblemPtr _problem) override final;
-        virtual void loadPriors(const Priors& _priors);
+        virtual void loadPriors(const PriorSensorMap& _priors);
 
     public:
 
@@ -151,7 +151,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
         SensorBase(const std::string& _type,
                    const int& _dim,
                    ParamsSensorBasePtr _params,
-                   const Priors& _priors);
+                   const PriorSensorMap& _priors);
 
         ~SensorBase() override;
 
@@ -333,7 +333,7 @@ inline CaptureBasePtr SensorBase::getLastCapture()
 
 inline StateBlockPtr SensorBase::addStateBlock(const char& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic)
 {
-    assert((params_prior_map_.find(_key) == params_prior_map_.end() || _sb_ptr == nullptr) && "overwriting a state block that has an absolute factor");
+    assert((features_prior_map_.find(_key) == features_prior_map_.end() || _sb_ptr == nullptr) && "overwriting a state block that has an absolute factor");
     HasStateBlocks::addStateBlock(_key, _sb_ptr, getProblem());
     setStateBlockDynamic(_key, _dynamic);
     return _sb_ptr;
@@ -400,29 +400,29 @@ inline void SensorBase::addPriorIntrinsics(const Eigen::VectorXd& _x, const Eige
 inline std::map<char, FeatureBaseConstPtr> SensorBase::getPriorFeatures() const
 {
     std::map<char, FeatureBaseConstPtr> map_const;
-    for (auto&& pair : params_prior_map_)
+    for (auto&& pair : features_prior_map_)
         map_const[pair.first] = pair.second;
     return map_const;
 }
 
 inline std::map<char, FeatureBasePtr> SensorBase::getPriorFeatures()
 {
-    return params_prior_map_;
+    return features_prior_map_;
 }
 
 
 inline FeatureBaseConstPtr SensorBase::getPriorFeature(char key) const
 {
-    if (params_prior_map_.count(key) == 0)
+    if (features_prior_map_.count(key) == 0)
         return nullptr;
-    return params_prior_map_.at(key);
+    return features_prior_map_.at(key);
 }
 
 inline FeatureBasePtr SensorBase::getPriorFeature(char key)
 {
-    if (params_prior_map_.count(key) == 0)
+    if (features_prior_map_.count(key) == 0)
         return nullptr;
-    return params_prior_map_.at(key);
+    return features_prior_map_.at(key);
 }
 
 } // namespace wolf
\ No newline at end of file

include/core/sensor/sensor_diff_drive.h

@@ -55,7 +55,7 @@ class SensorDiffDrive : public SensorBase
 {
     public:
         SensorDiffDrive(ParamsSensorDiffDrivePtr _params,
-                        const Priors& _priors);
+                        const PriorSensorMap& _priors);
         WOLF_SENSOR_CREATE(SensorDiffDrive, ParamsSensorDiffDrive);
 
         ~SensorDiffDrive() override = default;

include/core/sensor/sensor_motion_model.h

@@ -32,7 +32,7 @@ class SensorMotionModel : public SensorBase
 {
     public:
         SensorMotionModel(ParamsSensorBasePtr _params,
-                          const Priors& _priors);
+                          const PriorSensorMap& _priors);
 
         WOLF_SENSOR_CREATE(SensorMotionModel, ParamsSensorBase);
 

include/core/sensor/sensor_odom.h

@@ -68,7 +68,7 @@ class SensorOdom : public SensorBase
         
 	public:
         SensorOdom(ParamsSensorOdomPtr _params, 
-                   const Priors& _priors) :
+                   const PriorSensorMap& _priors) :
             SensorBase("SensorOdom"+toString(DIM)+"d",
                        DIM,
                        _params,

include/core/sensor/sensor_pose.h

@@ -57,7 +57,7 @@ class SensorPose : public SensorBase
 
     public:
         SensorPose(ParamsSensorPosePtr _params,
-                   const Priors& _priors) :
+                   const PriorSensorMap& _priors) :
             SensorBase("SensorPose"+toString(DIM)+"d", 
                        DIM,
                        _params,

include/core/state_block/prior.h

@@ -31,24 +31,20 @@ namespace wolf
 
 class Prior
 {
-    private:
+    protected:
         std::string     type_;       // State type
         Eigen::VectorXd state_;      // state values
         std::string     mode_;       // Prior mode. Can be 'initial_guess', 'fix' or 'factor'
         Eigen::VectorXd noise_std_;  // factor noise std, sqrt of the diagonal of the covariance mtrix (ONLY IF mode == 'factor')
-        bool            dynamic_;    // State dynamic
-        Eigen::VectorXd drift_std_;  // OPTIONAL std of the state drift [in units/sqrt(s)] diagonal elements (ONLY IF if dynamic)
 
     public:
-        Prior() = default;
+        Prior() = delete;
         Prior(const std::string&     _type,
-            const Eigen::VectorXd& _state,
-            const std::string&     _mode      = "fix",
-            const Eigen::VectorXd& _noise_std = Eigen::VectorXd(0),
-            bool                   _dynamic   = false,
-            const Eigen::VectorXd& _drift_std = Eigen::VectorXd(0));
+              const Eigen::VectorXd& _state,
+              const std::string&     _mode      = "fix",
+              const Eigen::VectorXd& _noise_std = Eigen::VectorXd(0));
 
-        Prior(const YAML::Node& prior_node);
+        Prior(const YAML::Node& _n);
 
         virtual ~Prior() = default;
 
@@ -56,26 +52,26 @@ class Prior
         const std::string&     getMode() const;
         const Eigen::VectorXd& getState() const;
         const Eigen::VectorXd& getNoiseStd() const;
-        bool                   isDynamic() const;
         bool                   isFixed() const;
         bool                   isInitialGuess() const;
         bool                   isFactor() const;
-        const Eigen::VectorXd& getDriftStd() const;
 
-        void check() const;
+        virtual void check() const;
 
-        virtual std::string print() const final;
+        virtual std::string print() const;
 };
 
-typedef std::unordered_map<char, Prior> PriorMap;
+typedef std::unordered_map<char, Prior> StdMapPrior;
 
-class Priors : public PriorMap
+class PriorMap : public StdMapPrior
 {
     public:
-        using PriorMap::PriorMap;
+        using StdMapPrior::StdMapPrior;
 
-        Priors(const YAML::Node& priors_node);
-        virtual ~Priors() = default;
+        PriorMap(const YAML::Node& _n);
+        virtual ~PriorMap() = default;
+
+        VectorComposite getState() const;
 };
 
 inline const std::string& Prior::getType() const { return type_; }
@@ -86,14 +82,10 @@ inline const Eigen::VectorXd& Prior::getState() const { return state_; }
 
 inline const Eigen::VectorXd& Prior::getNoiseStd() const { return noise_std_; }
 
-inline bool Prior::isDynamic() const { return dynamic_; }
-
 inline bool Prior::isFixed() const { return mode_ == "fix"; }
 
 inline bool Prior::isInitialGuess() const { return mode_ == "initial_guess"; }
 
 inline bool Prior::isFactor() const { return mode_ == "factor"; }
 
-inline const Eigen::VectorXd& Prior::getDriftStd() const { return drift_std_; }
-
 }  // namespace wolf
\ No newline at end of file

schema/Prior.schema → schema/problem/Prior.schema

@@ -14,20 +14,9 @@ mode:
   mandatory: true
   options: ["fix", "factor", "initial_guess"]
   doc: The prior mode can be 'factor' to add an absolute factor (requires 'noise_std'), 'fix' to set the values constant or 'initial_guess' to just set the values
-dynamic:
-  type: bool
-  yaml_type: scalar
-  mandatory: true
-  doc: If the state is dynamic, i.e. it changes along time.
 noise_std:
   type: VectorXd
   yaml_type: scalar
   mandatory: false
   default: []
-  doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
-drift_std:
+  doc: A vector containing the stdev values of the noise of the factor, i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file
-  type: VectorXd
-  yaml_type: scalar
-  mandatory: false
-  default: []
-  doc: A vector containing the stdev values of the noise of the drift factor (only if dynamic==true), i.e. the sqrt of the diagonal elements of the covariance matrix.
\ No newline at end of file

schema/problem/Problem2d.schema

+follow: Problem.schema
+problem:
+  prior:
+    P:
+      follow: PriorP2d.schema
+    O:
+      follow: PriorO2d.schema
\ No newline at end of file

schema/problem/Problem3d.schema

+follow: Problem.schema
+problem:
+  prior:
+    P:
+      follow: PriorP3d.schema
+    O:
+      follow: PriorO3d.schema
\ No newline at end of file