demos/hello_wolf/feature_range_bearing.cpp

-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// WOLF - Copyright (C) 2020-2025
+// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
-// This file is part of WOLF
+// This file is part of WOLF: http://www.iri.upc.edu/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.
+// it under the terms of the GNU General Public License version 3
+// as published by the Free Software Foundation.
 //
 // 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/>.
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
 //
-//--------LICENSE_END--------
-/*
- * FeatureRangeBearing2d.cpp
- *
- *  Created on: Nov 30, 2017
- *      Author: jsola
- */
-
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
 #include "feature_range_bearing.h"
 
 namespace wolf
 {
-
-FeatureRangeBearing::FeatureRangeBearing(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance) :
-        FeatureBase("FeatureRangeBearing", _measurement, _meas_covariance)
+FeatureRangeBearing::FeatureRangeBearing(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance)
+    : FeatureBase("FeatureRangeBearing", _measurement, _meas_covariance)
 {
     //
 }
@@ -42,5 +31,4 @@ FeatureRangeBearing::~FeatureRangeBearing()
     //
 }
 
-} // namespace wolf
-
+}  // namespace wolf

demos/hello_wolf/feature_range_bearing.h

-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// WOLF - Copyright (C) 2020-2025
+// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
-// This file is part of WOLF
+// This file is part of WOLF: http://www.iri.upc.edu/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.
+// it under the terms of the GNU General Public License version 3
+// as published by the Free Software Foundation.
 //
 // 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/>.
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
 //
-//--------LICENSE_END--------
-/*
- * FeatureRangeBearing2d.h
- *
- *  Created on: Nov 30, 2017
- *      Author: jsola
- */
-
-#ifndef HELLO_WOLF_FEATURE_RANGE_BEARING_H_
-#define HELLO_WOLF_FEATURE_RANGE_BEARING_H_
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+#pragma once
 
 #include "core/feature/feature_base.h"
 
 namespace wolf
 {
-
 WOLF_PTR_TYPEDEFS(FeatureRangeBearing);
 
 class FeatureRangeBearing : public FeatureBase
 {
-    public:
-        FeatureRangeBearing(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance);
-        ~FeatureRangeBearing() override;
+  public:
+    FeatureRangeBearing(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance);
+    ~FeatureRangeBearing() override;
 };
 
-} // namespace wolf
-
-#endif /* HELLO_WOLF_FEATURE_RANGE_BEARING_H_ */
+}  // namespace wolf

demos/hello_wolf/landmark_point_2d.cpp

-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 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--------
-/**
- * \file landmark_point_2d.cpp
- *
- *  Created on: Dec 4, 2017
- *      \author: jsola
- */
-
-#include "landmark_point_2d.h"
-#include "core/state_block/state_block_derived.h"
-
-namespace wolf
-{
-
-LandmarkPoint2d::LandmarkPoint2d(int _id, const Eigen::Vector2d& _xy) :
-        LandmarkBase("LandmarkPoint2d", std::make_shared<StatePoint2d>(_xy))
-{
-    setId(_id);
-}
-
-LandmarkPoint2d::~LandmarkPoint2d()
-{
-    //
-}
-
-} /* namespace wolf */

demos/hello_wolf/landmark_point_2d.h

-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 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--------
-/**
- * \file landmark_point_2d.h
- *
- *  Created on: Dec 4, 2017
- *      \author: jsola
- */
-
-#ifndef HELLO_WOLF_LANDMARK_POINT_2d_H_
-#define HELLO_WOLF_LANDMARK_POINT_2d_H_
-
-#include "core/landmark/landmark_base.h"
-
-namespace wolf
-{
-
-WOLF_PTR_TYPEDEFS(LandmarkPoint2d);
-
-class LandmarkPoint2d : public LandmarkBase
-{
-    public:
-        LandmarkPoint2d(int _id, const Eigen::Vector2d& _xy);
-        ~LandmarkPoint2d() override;
-};
-
-} /* namespace wolf */
-
-#endif /* HELLO_WOLF_LANDMARK_POINT_2d_H_ */

demos/hello_wolf/processor_range_bearing.cpp

-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// WOLF - Copyright (C) 2020-2025
+// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
-// This file is part of WOLF
+// This file is part of WOLF: http://www.iri.upc.edu/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.
+// it under the terms of the GNU General Public License version 3
+// as published by the Free Software Foundation.
 //
 // 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/>.
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
 //
-//--------LICENSE_END--------
-/*
- * processor_range_bearing.cpp
- *
- *  Created on: Nov 30, 2017
- *      Author: jsola
- */
-
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
 #include "processor_range_bearing.h"
 #include "capture_range_bearing.h"
-#include "landmark_point_2d.h"
+#include "core/landmark/landmark.h"
+#include "core/state_block/state_block_derived.h"
 #include "feature_range_bearing.h"
 #include "factor_range_bearing.h"
 
 namespace wolf
 {
-
-ProcessorRangeBearing::ProcessorRangeBearing(ParamsProcessorBasePtr _params) :
-        ProcessorBase("ProcessorRangeBearing", 2, _params)
+ProcessorRangeBearing::ProcessorRangeBearing(const YAML::Node& _params)
+    : ProcessorBase("ProcessorRangeBearing", {{'P', "StatePoint2d"}, {'O', "StateAngle"}}, _params)
 {
     //
 }
@@ -51,21 +41,22 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
 
     // 1. get KF
     FrameBasePtr keyframe(nullptr);
-    if ( !buffer_frame_.empty() )
+    if (!buffer_frame_.empty())
     {
         // KeyFrame Callback received
-        keyframe = buffer_frame_.select( _capture->getTimeStamp(), params_->time_tolerance );
+        keyframe = buffer_frame_.select(_capture->getTimeStamp(), getTimeTolerance());
 
-        buffer_frame_.removeUpTo( _capture->getTimeStamp() );
+        buffer_frame_.removeUpTo(_capture->getTimeStamp());
 
-        assert( keyframe && "Callback KF is not close enough to _capture!");
+        assert(keyframe && "Callback KF is not close enough to _capture!");
     }
 
     if (!keyframe)
     {
         // No KeyFrame callback received -- we assume a KF is available to hold this _capture (checked in assert below)
         keyframe = getProblem()->closestFrameToTimeStamp(_capture->getTimeStamp());
-        assert( (fabs(keyframe->getTimeStamp() - _capture->getTimeStamp()) < params_->time_tolerance) && "Could not find a KF close enough to _capture!");
+        assert((fabs(keyframe->getTimeStamp() - _capture->getTimeStamp()) < getTimeTolerance()) &&
+               "Could not find a KF close enough to _capture!");
     }
 
     // 2. cast incoming capture to the range-and-bearing type, add it to the keyframe
@@ -76,49 +67,42 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
     for (SizeEigen i = 0; i < capture_rb->getIds().size(); i++)
     {
         // extract info
-        int     id      = capture_rb->getId     (i);
-        double  range   = capture_rb->getRange  (i);
-        double  bearing = capture_rb->getBearing(i);
+        int    id      = capture_rb->getId(i);
+        double range   = capture_rb->getRange(i);
+        double bearing = capture_rb->getBearing(i);
 
         // 4. create or recover landmark
-        LandmarkPoint2dPtr lmk;
-        auto lmk_it = known_lmks.find(id);
-        if ( lmk_it != known_lmks.end() )
+        Landmark2dPtr lmk;
+        auto          lmk_it = known_lmks.find(id);
+        if (lmk_it != known_lmks.end())
         {
             // known landmarks : recover landmark
-            lmk = std::static_pointer_cast<LandmarkPoint2d>(lmk_it->second);
+            lmk = std::static_pointer_cast<Landmark2d>(lmk_it->second);
             WOLF_TRACE("known lmk(", id, "): ", lmk->getP()->getState().transpose());
         }
         else
         {
             // new landmark:
             // - create landmark
-            lmk = LandmarkBase::emplace<LandmarkPoint2d>(getProblem()->getMap(), id, invObserve(range, bearing));
+            lmk = LandmarkBase::emplace<Landmark2d>(
+                getProblem()->getMap(), VectorComposite{{'P', invObserve(range, bearing)}}, PriorComposite());
+            lmk->setId(id);
             WOLF_TRACE("new   lmk(", id, "): ", lmk->getP()->getState().transpose());
             // - add to known landmarks
             known_lmks.emplace(id, lmk);
         }
 
         // 5. create feature
-        Vector2d measurement_rb(range,bearing);
-        auto ftr = FeatureBase::emplace<FeatureRangeBearing>(capture_rb,
-                                                             measurement_rb,
-                                                             getSensor()->getNoiseCov());
+        Vector2d measurement_rb(range, bearing);
+        auto     ftr = FeatureBase::emplace<FeatureRangeBearing>(
+            capture_rb, measurement_rb, getSensor()->computeNoiseCov(measurement_rb));
 
         // 6. create factor
         auto prc = shared_from_this();
-        auto fac = FactorBase::emplace<FactorRangeBearing>(ftr,
-                                                           capture_rb,
-                                                           ftr,
-                                                           lmk,
-                                                           prc,
-                                                           false,
-                                                           FAC_ACTIVE);
+        auto fac = FactorBase::emplace<FactorRangeBearing>(ftr, capture_rb, ftr, lmk, prc, false, FAC_ACTIVE);
     }
-
 }
 
-
 Eigen::Vector2d ProcessorRangeBearing::observe(const Eigen::Vector2d& lmk_w) const
 {
     return polar(toSensor(lmk_w));
@@ -131,27 +115,26 @@ Eigen::Vector2d ProcessorRangeBearing::invObserve(double r, double b) const
 
 ProcessorRangeBearing::Trf ProcessorRangeBearing::transform(const Eigen::Vector3d& _pose) const
 {
-    Trf H = Eigen::Translation<double,2>(_pose(0), _pose(1)) * Eigen::Rotation2D<double>(_pose(2)) ;
+    Trf H = Eigen::Translation<double, 2>(_pose(0), _pose(1)) * Eigen::Rotation2D<double>(_pose(2));
     return H;
 }
 
 ProcessorRangeBearing::Trf ProcessorRangeBearing::transform(const Eigen::Vector2d& _position,
                                                             const Eigen::Vector1d& _orientation) const
 {
-    Trf H = Eigen::Translation<double,2>(_position(0), _position(1)) * Eigen::Rotation2D<double>(_orientation(0)) ;
+    Trf H = Eigen::Translation<double, 2>(_position(0), _position(1)) * Eigen::Rotation2D<double>(_orientation(0));
     return H;
 }
 
 Eigen::Vector2d ProcessorRangeBearing::fromSensor(const Eigen::Vector2d& lmk_s) const
 {
-    Trf H_w_r = transform(getProblem()->getState().vector("PO"));
+    Trf H_w_r = transform(getProblem()->getState("PO").vector("PO"));
     return H_w_r * H_r_s * lmk_s;
 }
 
 Eigen::Vector2d ProcessorRangeBearing::toSensor(const Eigen::Vector2d& lmk_w) const
 {
-//    Trf H_w_r = transform(getSensor()->getP()->getState(), getSensor()->getO()->getState());
-    Trf H_w_r = transform(getProblem()->getState().vector("PO"));
+    Trf H_w_r = transform(getProblem()->getState("PO").vector("PO"));
     return (H_w_r * H_r_s).inverse() * lmk_w;
 }
 
@@ -170,19 +153,12 @@ Eigen::Vector2d ProcessorRangeBearing::rect(double range, double bearing) const
 
 bool ProcessorRangeBearing::storeKeyFrame(FrameBasePtr _frame_ptr)
 {
-  return true;
+    return true;
 }
 bool ProcessorRangeBearing::storeCapture(CaptureBasePtr _cap_ptr)
 {
-  return false;
+    return false;
 }
-} /* namespace wolf */
 
-// Register in the SensorFactory
-#include "core/processor/factory_processor.h"
-namespace wolf
-{
 WOLF_REGISTER_PROCESSOR(ProcessorRangeBearing)
-WOLF_REGISTER_PROCESSOR_AUTO(ProcessorRangeBearing)
-} // namespace wolf
-
+}  // namespace wolf

demos/hello_wolf/processor_range_bearing.h

-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// WOLF - Copyright (C) 2020-2025
+// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
-// This file is part of WOLF
+// This file is part of WOLF: http://www.iri.upc.edu/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.
+// it under the terms of the GNU General Public License version 3
+// as published by the Free Software Foundation.
 //
 // 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/>.
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
 //
-//--------LICENSE_END--------
-/*
- * ProcessorRangeBearing.h
- *
- *  Created on: Nov 30, 2017
- *      Author: jsola
- */
-
-#ifndef HELLO_WOLF_PROCESSOR_RANGE_BEARING_H_
-#define HELLO_WOLF_PROCESSOR_RANGE_BEARING_H_
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+#pragma once
 
 #include "core/processor/processor_base.h"
 #include "sensor_range_bearing.h"
@@ -37,79 +26,72 @@
 
 namespace wolf
 {
-
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorRangeBearing);
-
-struct ParamsProcessorRangeBearing : public ParamsProcessorBase
-{
-        // We do not need special parameters, but in case you need they should be defined here.
-        ParamsProcessorRangeBearing()
-        {
-            //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
-        }
-        ParamsProcessorRangeBearing(std::string _unique_name, const ParamsServer& _server) :
-                ParamsProcessorBase(_unique_name, _server)
-        {
-            //
-        }
-        std::string print() const
-        {
-            return ParamsProcessorBase::print();
-        }
-};
-
 using namespace Eigen;
 WOLF_PTR_TYPEDEFS(ProcessorRangeBearing);
 
 class ProcessorRangeBearing : public ProcessorBase
 {
-    public:
-        typedef Eigen::Transform<double, 2, Eigen::Isometry> Trf;
-
-        ProcessorRangeBearing(ParamsProcessorBasePtr _params);
-        ~ProcessorRangeBearing() override {/* empty */}
-        void configure(SensorBasePtr _sensor) override;
-
-        // Factory method for high level API
-        WOLF_PROCESSOR_CREATE(ProcessorRangeBearing, ParamsProcessorRangeBearing);
-
-    protected:
-        // Implementation of pure virtuals from ProcessorBase
-        void processCapture     (CaptureBasePtr _capture) override;
-        void processKeyFrame    (FrameBasePtr _keyframe_ptr) override {};
-        bool triggerInCapture   (CaptureBasePtr) const override { return true;};
-        bool triggerInKeyFrame  (FrameBasePtr _keyframe_ptr) const override {return false;}
-        bool voteForKeyFrame    () const override {return false;}
-
-        /** \brief store key frame
-        *
-        * Returns true if the key frame should be stored
-        */
-        bool storeKeyFrame(FrameBasePtr) override;
-
-        /** \brief store capture
-        *
-        * Returns true if the capture should be stored
-        */
-        bool storeCapture(CaptureBasePtr) override;
-
-    private:
-        // control variables
-        Trf H_r_s; // transformation matrix, robot to sensor
-        std::map<int, LandmarkBasePtr> known_lmks; // all observed lmks so far
-
-    protected:
-        // helper methods -- to be used only here -- they would be better off in a separate library e.g. range_bearing_tools.h
-        Eigen::Vector2d observe     (const Eigen::Vector2d& lmk_w) const;
-        Eigen::Vector2d invObserve  (double r, double b) const;
-    private:
-        // helper methods -- to be used only here -- they would be better off in a separate library e.g. range_bearing_tools.h
-        Trf             transform   (const Eigen::Vector3d& _pose) const;
-        Trf             transform   (const Eigen::Vector2d& _position, const Eigen::Vector1d& _orientation) const;
-        Eigen::Vector2d fromSensor  (const Eigen::Vector2d& lmk_s) const;
-        Eigen::Vector2d toSensor    (const Eigen::Vector2d& lmk_w) const;
-        Eigen::Vector2d polar       (const Eigen::Vector2d& rect) const;
-        Eigen::Vector2d rect        (double range, double bearing) const;
+  public:
+    typedef Eigen::Transform<double, 2, Eigen::Isometry> Trf;
+
+    ProcessorRangeBearing(const YAML::Node& _params);
+    ~ProcessorRangeBearing() override
+    { /* empty */
+    }
+    void configure(SensorBasePtr _sensor) override;
+
+    // Factory method for high level API
+    WOLF_PROCESSOR_CREATE(ProcessorRangeBearing);
+
+  protected:
+    // Implementation of pure virtuals from ProcessorBase
+    void processCapture(CaptureBasePtr _capture) override;
+    void processKeyFrame(FrameBasePtr _keyframe_ptr) override{};
+    bool triggerInCapture(CaptureBasePtr) const override
+    {
+        return true;
+    };
+    bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override
+    {
+        return false;
+    }
+    bool voteForKeyFrame() const override
+    {
+        return false;
+    }
+
+    /** \brief store key frame
+     *
+     * Returns true if the key frame should be stored
+     */
+    bool storeKeyFrame(FrameBasePtr) override;
+
+    /** \brief store capture
+     *
+     * Returns true if the capture should be stored
+     */
+    bool storeCapture(CaptureBasePtr) override;
+
+  private:
+    // control variables
+    Trf                            H_r_s;       // transformation matrix, robot to sensor
+    std::map<int, LandmarkBasePtr> known_lmks;  // all observed lmks so far
+
+  protected:
+    // helper methods -- to be used only here -- they would be better off in a separate library e.g.
+    // range_bearing_tools.h
+    Eigen::Vector2d observe(const Eigen::Vector2d& lmk_w) const;
+    Eigen::Vector2d invObserve(double r, double b) const;
+
+  private:
+    // helper methods -- to be used only here -- they would be better off in a separate library e.g.
+    // range_bearing_tools.h
+    Trf             transform(const Eigen::Vector3d& _pose) const;
+    Trf             transform(const Eigen::Vector2d& _position, const Eigen::Vector1d& _orientation) const;
+    Eigen::Vector2d fromSensor(const Eigen::Vector2d& lmk_s) const;
+    Eigen::Vector2d toSensor(const Eigen::Vector2d& lmk_w) const;
+    Eigen::Vector2d polar(const Eigen::Vector2d& rect) const;
+    Eigen::Vector2d rect(double range, double bearing) const;
 };
 
 inline void ProcessorRangeBearing::configure(SensorBasePtr _sensor)
@@ -118,5 +100,3 @@ inline void ProcessorRangeBearing::configure(SensorBasePtr _sensor)
 }
 
 } /* namespace wolf */
-
-#endif /* HELLO_WOLF_PROCESSOR_RANGE_BEARING_H_ */

demos/hello_wolf/schema/ProcessorRangeBearing.schema

+follow: ProcessorBase.schema
\ No newline at end of file

demos/hello_wolf/schema/SensorRangeBearing.schema

+follow: SensorBase.schema
+
+noise_range_metres_std:
+  _mandatory: true
+  _type: double
+  _doc: Standard deviation of the noise of the range measurements (meters)
+
+noise_bearing_degrees_std:
+  _mandatory: true
+  _type: double
+  _doc: Standard deviation of the noise of the bearing measurements (degrees)
+
+states:
+  P:
+    follow: StateSensorP2d.schema
+  O:
+    follow: StateSensorO2d.schema
\ No newline at end of file

demos/hello_wolf/sensor_range_bearing.cpp

-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// WOLF - Copyright (C) 2020-2025
+// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
-// This file is part of WOLF
+// This file is part of WOLF: http://www.iri.upc.edu/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.
+// it under the terms of the GNU General Public License version 3
+// as published by the Free Software Foundation.
 //
 // 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/>.
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
 //
-//--------LICENSE_END--------
-/*
- * SensorRangeBearing.cpp
- *
- *  Created on: Nov 30, 2017
- *      Author: jsola
- */
-
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
 #include "sensor_range_bearing.h"
-#include "core/state_block/state_angle.h"
-#include "core/state_block/state_block_derived.h"
 
 namespace wolf
 {
-
 WOLF_PTR_TYPEDEFS(SensorRangeBearing);
 
-SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const Eigen::Vector2d& _noise_std) :
-        SensorBase("SensorRangeBearing",
-                   std::make_shared<StatePoint2d>(_extrinsics.head<2>(), true),
-                   std::make_shared<StateAngle>(_extrinsics(2), true),
-                   nullptr,
-                   _noise_std)
+SensorRangeBearing::SensorRangeBearing(const YAML::Node& _params)
+    : SensorBase("SensorRangeBearing", {{'P', "StatePoint2d"}, {'O', "StateAngle"}}, _params),
+      noise_range_metres_std_(_params["noise_range_metres_std"].as<double>()),
+      noise_bearing_degrees_std_(_params["noise_bearing_degrees_std"].as<double>())
 {
-    assert(_extrinsics.size() == 3 && "Bad extrinsics vector size. Must be 3 for 2d");
 }
 
-SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const ParamsSensorRangeBearingPtr _intr) :
-        SensorRangeBearing(_extrinsics, Eigen::Vector2d(_intr->noise_range_metres_std, toRad(_intr->noise_bearing_degrees_std)))
+SensorRangeBearing::~SensorRangeBearing()
 {
     //
 }
 
-SensorRangeBearing::~SensorRangeBearing()
+Eigen::MatrixXd SensorRangeBearing::computeNoiseCov(const Eigen::VectorXd& _data) const
 {
-    //
+    return (Eigen::Vector2d() << noise_range_metres_std_, noise_bearing_degrees_std_)
+        .finished()
+        .cwiseAbs2()
+        .asDiagonal();
 }
 
-
-} /* namespace wolf */
-
-// Register in the SensorFactory
-#include "core/sensor/factory_sensor.h"
-namespace wolf
-{
 WOLF_REGISTER_SENSOR(SensorRangeBearing)
-WOLF_REGISTER_SENSOR_AUTO(SensorRangeBearing)
-} // namespace wolf
+}  // namespace wolf

demos/hello_wolf/sensor_range_bearing.h

-//--------LICENSE_START--------
-//
-// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// WOLF - Copyright (C) 2020-2025
+// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
-// This file is part of WOLF
+// This file is part of WOLF: http://www.iri.upc.edu/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.
+// it under the terms of the GNU General Public License version 3
+// as published by the Free Software Foundation.
 //
 // 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/>.
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
 //
-//--------LICENSE_END--------
-/*
- * SensorRangeBearing.h
- *
- *  Created on: Nov 30, 2017
- *      Author: jsola
- */
-
-#ifndef HELLO_WOLF_SENSOR_RANGE_BEARING_H_
-#define HELLO_WOLF_SENSOR_RANGE_BEARING_H_
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+#pragma once
 
 #include "core/sensor/sensor_base.h"
-#include "core/utils/params_server.h"
+#include "yaml-cpp/yaml.h"
 
 namespace wolf
 {
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorRangeBearing);
-
-struct ParamsSensorRangeBearing : public ParamsSensorBase
-{
-        double noise_range_metres_std       = 0.05;
-        double noise_bearing_degrees_std    = 0.5;
-    ParamsSensorRangeBearing()
-    {
-        //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
-    }
-    ParamsSensorRangeBearing(std::string _unique_name, const ParamsServer& _server):
-        ParamsSensorBase(_unique_name, _server)
-    {
-        noise_range_metres_std      = _server.getParam<double>(prefix + _unique_name + "/noise_range_metres_std");
-        noise_bearing_degrees_std   = _server.getParam<double>(prefix + _unique_name + "/noise_bearing_degrees_std");
-    }
-    std::string print() const
-    {
-        return ParamsSensorBase::print()                                         + "\n"
-        + "noise_range_metres_std: "    + std::to_string(noise_range_metres_std)    + "\n"
-        + "noise_bearing_degrees_std: " + std::to_string(noise_bearing_degrees_std) + "\n";
-    }
-};
-
 WOLF_PTR_TYPEDEFS(SensorRangeBearing)
 
 class SensorRangeBearing : public SensorBase
 {
-    public:
-        SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const Eigen::Vector2d& _noise_std);
-        SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const ParamsSensorRangeBearingPtr _intr);
-        WOLF_SENSOR_CREATE(SensorRangeBearing, ParamsSensorRangeBearing, 3);
-        ~SensorRangeBearing() override;
+  private:
+    double noise_range_metres_std_    = 0.05;
+    double noise_bearing_degrees_std_ = 0.5;
+
+    SensorRangeBearing(const YAML::Node& _params);
+
+  public:
+    WOLF_SENSOR_CREATE(SensorRangeBearing);
 
+    ~SensorRangeBearing() override;
+
+    Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd& _data) const override;
 };
 
 } /* namespace wolf */
-
-#endif /* HELLO_WOLF_SENSOR_RANGE_BEARING_H_ */

demos/hello_wolf/yaml/hello_wolf_config.yaml

-config:
+problem:
 
-  problem:
+  dimension:            2               # space is 2d
+  frame_structure:      "PO"            # keyframes have position and orientation
+
+  first_frame:
+    P:
+      value: [0,0]
+      prior:
+        mode:               "factor"
+        factor_std: [0.31, 0.31]
+    O:
+      value: [0]
+      prior:
+        mode:               "factor"
+        factor_std: [0.31]
+
+  tree_manager:
+    type: "none"
+
+solver:
+  type: SolverCeres
+  plugin: core
+  max_num_iterations: 100
+  verbose: 0
+  period: 0.2
+  interrupt_on_problem_change: false
+  n_threads: 2
+  compute_cov: false
+  minimizer: levenberg_marquardt
+  use_nonmonotonic_steps: false         # only for levenberg_marquardt and DOGLEG
+  parameter_tolerance: 1e-6
+  function_tolerance: 1e-6
+  gradient_tolerance: 1e-10
   
-    dimension:            2               # space is 2d
-    frame_structure:      "PO"            # keyframes have position and orientation
+sensors:
   
-    prior:
-      mode:               "factor"
-      $state:
-        P: [0,0]
-        O: [0]
-      $sigma:
-        P: [0.31, 0.31]
-        O: [0.31]
-      time_tolerance:     0.5
+  - type:               "SensorOdom2d"
+    plugin:             "core"
+    name:               "sen odom"
+    follow:             "sensor_odom_2d.yaml"         # config parameters in this file
 
-    tree_manager:
-      type: "none"
-      plugin: "core"
+  - type:               "SensorRangeBearing"
+    plugin:             "core"
+    name:               "sen rb"  
+    noise_range_metres_std: 0.2                       # parser only considers first appearence so the following file parsing will not overwrite this param
+    follow:             sensor_range_bearing.yaml     # config parameters in this file
+        
+processors:
   
-  solver:
-    max_num_iterations: 100
-    verbose: 0
-    period: 0.2
-    interrupt_on_problem_change: false
-    n_threads: 2
-    compute_cov: false
-    minimizer: LEVENBERG_MARQUARDT
-    use_nonmonotonic_steps: false         # only for LEVENBERG_MARQUARDT and DOGLEG
-    function_tolerance: 0.000001
-    gradient_tolerance: 0.0000000001
-    
-  sensors:
-    
-    - type:               "SensorOdom2d"
-      plugin:             "core"
-      name:               "sen odom"
-      extrinsic: 
-        pose:             [0,0, 0]
-      follow:             "demos/hello_wolf/yaml/sensor_odom_2d.yaml"         # config parameters in this file
-
-    - type:               "SensorRangeBearing"
-      plugin:             "core"
-      name:               "sen rb"  
-      extrinsic:
-        pose:             [1,1, 0]
-      noise_range_metres_std: 0.2                                       # parser only considers first appearence so the following file parsing will not overwrite this param
-      follow:             demos/hello_wolf/yaml/sensor_range_bearing.yaml     # config parameters in this file
-          
-  processors:
-    
-    - type:               "ProcessorOdom2d"
-      plugin:             "core"
-      name:               "prc odom"
-      sensor_name:        "sen odom"                                    # attach processor to this sensor
-      follow:             demos/hello_wolf/yaml/processor_odom_2d.yaml        # config parameters in this file
-    
-    - type:               "ProcessorRangeBearing"
-      plugin:             "core"
-      name:               "prc rb"
-      sensor_name:        "sen rb"                                      # attach processor to this sensor
-      follow:             demos/hello_wolf/yaml/processor_range_bearing.yaml  # config parameters in this file
+  - type:               "ProcessorOdom2d"
+    plugin:             "core"
+    name:               "prc odom"
+    sensor_name:        "sen odom"                    # attach processor to this sensor
+    follow:             processor_odom_2d.yaml        # config parameters in this file
+  
+  - type:               "ProcessorRangeBearing"
+    plugin:             "core"
+    name:               "prc rb"
+    sensor_name:        "sen rb"                      # attach processor to this sensor
+    follow:             processor_range_bearing.yaml  # config parameters in this file

demos/hello_wolf/yaml/processor_odom_2d.yaml

@@ -5,11 +5,11 @@ unmeasured_perturbation_std: 0.001
 keyframe_vote:
   voting_active:      true
   max_time_span:      999
-  dist_traveled:      0.95
-  angle_turned:       999
+  max_dist_traveled:  0.95
+  max_angle_turned:   999
   max_buff_length:    999
-  cov_det:            999
+  max_cov_det:        999
 apply_loss_function: true
 
-state_getter: true
-state_priority: 1
+state_provider: true
+state_provider_order: 1

demos/hello_wolf/yaml/sensor_odom_2d.yaml

-#type: "SensorOdom2d"              # This must match the KEY used in the FactorySensor. Otherwise it is an error.
+#type: "SensorOdom2d"              # This must match the KEY used in the FactorySensorNode. Otherwise it is an error.
 
 k_disp_to_disp:   0.1  # m^2   / m
 k_rot_to_rot:     0.1  # rad^2 / rad
+k_disp_to_rot:    0.0  # m^2   / rad
+min_disp_var:     0.0
+min_rot_var:      0.0
 apply_loss_function: true
+
+states:
+  P:
+    prior:
+      mode: fix
+    value: [0,0]
+    dynamic: false
+  O:
+    prior:
+      mode: fix
+    value: [0]
+    dynamic: false
\ No newline at end of file

demos/hello_wolf/yaml/sensor_range_bearing.yaml

@@ -3,3 +3,15 @@
 noise_range_metres_std:     0.1
 noise_bearing_degrees_std:  0.5
 apply_loss_function: true
+
+states:
+  P:
+    prior:
+      mode: fix
+    value: [1,1]
+    dynamic: false
+  O:
+    prior:
+      mode: fix
+    value: [0]
+    dynamic: false
\ No newline at end of file

demos/sensor_odom_3d.yaml

-type: "SensorOdom3d"              # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-
-k_disp_to_disp:   0.02  # m^2   / m
-k_disp_to_rot:    0.02  # rad^2 / m
-k_rot_to_rot:     0.01  # rad^2 / rad
-min_disp_var:     0.01  # m^2
-min_rot_var:      0.01  # rad^2

demos/sensor_odom_3d_HQ.yaml

-type: "SensorOdom3d"              # This must match the KEY used in the SensorFactory. Otherwise it is an error.
-
-k_disp_to_disp:   0.000001  # m^2   / m
-k_disp_to_rot:    0.000001  # rad^2 / m
-k_rot_to_rot:     0.000001  # rad^2 / rad
-min_disp_var:     0.00000001  # m^2
-min_rot_var:      0.00000001  # rad^2

demos/solver/CMakeLists.txt

+ADD_EXECUTABLE(demo_wolf_imported_graph demo_wolf_imported_graph.cpp)
+TARGET_LINK_LIBRARIES(demo_wolf_imported_graph ${PLUGIN_NAME})
+
+ADD_EXECUTABLE(demo_analytic_autodiff_comparison demo_analytic_autodiff_comparison.cpp)
+TARGET_LINK_LIBRARIES(demo_analytic_autodiff_comparison ${PLUGIN_NAME})

demos/solver/demo_analytic_autodiff_comparison.cpp

+// WOLF - Copyright (C) 2020-2025
+// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
+// Joan Vallvé Navarro (jvallve@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF: http://www.iri.upc.edu/wolf
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License version 3
+// as published by the Free Software Foundation.
+//
+// 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 General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+// Testing creating wolf tree from imported .graph file
+
+// C includes for sleep, time and main args
+#include "unistd.h"
+
+// std includes
+#include <iostream>
+#include <memory>
+#include <random>
+#include <cmath>
+#include <queue>
+
+// Wolf includes
+#include "core/common/wolf.h"
+#include "core/ceres_wrapper/solver_ceres.h"
+#include "core/capture/capture_odom_2d.h"
+#include "core/feature/feature_odom_2d.h"
+#include "core/factor/factor_relative_pose_2d.h"
+#include "../test/dummy/factor_relative_pose_2d_autodiff.h"
+#include "core/problem/problem.h"
+#include "load_graph.h"
+
+std::string wolf_dir = _WOLF_CODE_DIR;
+
+int main(int argc, char** argv)
+{
+    using namespace wolf;
+
+    // Welcome message
+    std::cout << std::endl
+              << " ========= WOLF ANALITIC vs. AUTODIFF FACTORS TEST with loaded graph ===========" << std::endl
+              << std::endl;
+
+    if (argc != 3 || atoi(argv[2]) < 0)
+    {
+        std::cout << "Please call me with: [./test_wolf_imported_graph FILE_PATH MAX_VERTICES], where:" << std::endl;
+        std::cout << "    FILE_PATH is the .g2o or .toro file path" << std::endl;
+        std::cout << "    MAX_VERTICES max edges to be loaded (0: ALL)" << std::endl;
+        std::cout << "EXIT due to bad user input" << std::endl << std::endl;
+        return -1;
+    }
+
+    // load graph arguments
+    std::string  file_path  = argv[1];
+    unsigned int max_vertex = atoi(argv[2]);
+
+    // Create problem 2D
+    ProblemPtr problem_analitic = Problem::create(2);
+    ProblemPtr problem_autodiff = Problem::create(2);
+
+    // Ceres solver
+    SolverManagerPtr solver_analitic = FactorySolverFile::create(
+        "SolverCeres", problem_analitic, wolf_dir + "/demos/solver/yaml/solver_ceres.yaml", {wolf_dir});
+    SolverManagerPtr solver_autodiff = FactorySolverFile::create(
+        "SolverCeres", problem_autodiff, wolf_dir + "/demos/solver/yaml/solver_ceres.yaml", {wolf_dir});
+
+    // load graph from .txt
+    loadGraph<FactorRelativePose2d>(problem_analitic, file_path, max_vertex);
+    loadGraph<FactorRelativePose2dAutodiff>(problem_autodiff, file_path, max_vertex);
+
+    // UPDATE
+    WOLF_INFO("updating solver_analitic...");
+    auto t1 = clock();
+    solver_analitic->update();
+    double t_update_analitic = ((double)clock() - t1) / CLOCKS_PER_SEC;
+    WOLF_INFO("updated in ", t_update_analitic, " s")
+    WOLF_INFO("updating solver_autodiff...");
+    t1 = clock();
+    solver_autodiff->update();
+    double t_update_autodiff = ((double)clock() - t1) / CLOCKS_PER_SEC;
+    WOLF_INFO("updated in ", t_update_autodiff, " s")
+
+    // SOLVE
+    WOLF_INFO("solving analitic...");
+    t1                           = clock();
+    std::string report_analitic  = solver_analitic->solve(SolverManager::ReportVerbosity::FULL);
+    double      t_solve_analitic = ((double)clock() - t1) / CLOCKS_PER_SEC;
+    WOLF_INFO(report_analitic);
+    WOLF_INFO("solved in ", t_solve_analitic, " s")
+    WOLF_INFO("solving autodiff...");
+    t1                           = clock();
+    std::string report_autodiff  = solver_autodiff->solve(SolverManager::ReportVerbosity::FULL);
+    double      t_solve_autodiff = ((double)clock() - t1) / CLOCKS_PER_SEC;
+    WOLF_INFO(report_autodiff);
+    WOLF_INFO("solved in ", t_solve_autodiff, " s")
+
+    // COMPUTE COVARIANCES
+    WOLF_INFO("computing marginal covariances analitic...");
+    t1 = clock();
+    solver_analitic->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+    double t_cov_analitic = ((double)clock() - t1) / CLOCKS_PER_SEC;
+    WOLF_INFO("computed in ", t_cov_analitic, " s")
+    WOLF_INFO("computing marginal covariances autodiff...");
+    t1 = clock();
+    solver_autodiff->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+    double t_cov_autodiff = ((double)clock() - t1) / CLOCKS_PER_SEC;
+    WOLF_INFO("computed in ", t_cov_autodiff, " s")
+
+    // End message
+    std::cout << " =========================== END ===============================" << std::endl << std::endl;
+
+    // exit
+    return 0;
+}