src/hardware/hardware_base.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--------
+// 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 "core/hardware/hardware_base.h"
 #include "core/sensor/sensor_base.h"
 
-namespace wolf {
-
-HardwareBase::HardwareBase() :
-        NodeBase("HARDWARE", "HardwareBase")
+namespace wolf
+{
+HardwareBase::HardwareBase() : NodeBase("HARDWARE", "HardwareBase")
 {
-//    std::cout << "constructed H" << std::endl;
+    //    std::cout << "constructed H" << std::endl;
 }
 
 HardwareBase::~HardwareBase()
 {
-//	std::cout << "destructed -H" << std::endl;
+    //	std::cout << "destructed -H" << std::endl;
 }
 
 SensorBasePtr HardwareBase::addSensor(SensorBasePtr _sensor_ptr)
@@ -43,71 +39,67 @@ SensorBasePtr HardwareBase::addSensor(SensorBasePtr _sensor_ptr)
 
 SensorBaseConstPtr HardwareBase::getSensor(const std::string& _sensor_name) const
 {
-    auto sen_it = std::find_if(sensor_list_.begin(),
-                               sensor_list_.end(),
-                               [&](SensorBaseConstPtr sb)
-                               {
-                                   return sb->getName() == _sensor_name;
-                               }); // lambda function for the find_if
+    auto sen_it = std::find_if(sensor_list_.cbegin(), sensor_list_.cend(), [&](SensorBaseConstPtr sb) {
+        return sb->getName() == _sensor_name;
+    });  // lambda function for the find_if
 
-    if (sen_it == sensor_list_.end())
-        return nullptr;
-
-    return (*sen_it);
+    return sen_it != sensor_list_.cend() ? *sen_it : nullptr;
 }
 
 SensorBasePtr HardwareBase::getSensor(const std::string& _sensor_name)
 {
-    auto sen_it = std::find_if(sensor_list_.begin(),
-                               sensor_list_.end(),
-                               [&](SensorBasePtr sb)
-                               {
-                                   return sb->getName() == _sensor_name;
-                               }); // lambda function for the find_if
-
-    if (sen_it == sensor_list_.end())
-        return nullptr;
+    auto sen_it = std::find_if(sensor_list_.cbegin(), sensor_list_.cend(), [&](SensorBasePtr sb) {
+        return sb->getName() == _sensor_name;
+    });  // lambda function for the find_if
 
-    return (*sen_it);
+    return sen_it != sensor_list_.cend() ? *sen_it : nullptr;
 }
 
-void HardwareBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+void HardwareBase::printHeader(int           _depth,
+                               bool          _factored_by,
+                               bool          _metric,
+                               bool          _state_blocks,
+                               std::ostream& _stream,
+                               std::string   _tabs) const
 {
-    _stream << _tabs << "Hardware" << ((_depth < 1) ? ("   -- " + std::to_string(getSensorList().size()) + "S") : "")  << std::endl;
-
+    _stream << _tabs << "Hardware" << ((_depth < 1) ? ("   -- " + std::to_string(getSensorList().size()) + "S") : "")
+            << std::endl;
 }
-void HardwareBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+void HardwareBase::print(int           _depth,
+                         bool          _factored_by,
+                         bool          _metric,
+                         bool          _state_blocks,
+                         std::ostream& _stream,
+                         std::string   _tabs) const
 {
-    printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
+    printHeader(_depth, _factored_by, _metric, _state_blocks, _stream, _tabs);
     if (_depth >= 1)
         for (auto S : getSensorList())
-            if (S)
-                S->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + "  ");
+            if (S) S->print(_depth, _factored_by, _metric, _state_blocks, _stream, _tabs + "  ");
 }
 
-CheckLog HardwareBase::localCheck(bool _verbose, HardwareBaseConstPtr _hwd_ptr, std::ostream& _stream, std::string _tabs) const
+CheckLog HardwareBase::localCheck(bool _verbose, std::ostream& _stream, std::string _tabs) const
 {
-    CheckLog log;
+    CheckLog          log;
     std::stringstream inconsistency_explanation;
     if (_verbose)
     {
-        _stream << _tabs << "Hrw @ " << _hwd_ptr.get() << std::endl;
+        _stream << _tabs << "Hrw @ " << this << std::endl;
     }
 
     // check pointer to Problem
-    inconsistency_explanation << "Hwd->getProblem() [" << getProblem().get()
-                              << "] -> " << " Prb->getHardware() [" << getProblem()->getHardware().get() << "] -> Hwd [" << _hwd_ptr.get() << "] Mismatch!\n";
-    log.assertTrue((_hwd_ptr->getProblem()->getHardware().get() == _hwd_ptr.get()), inconsistency_explanation);
+    inconsistency_explanation << "Hwd->getProblem() [" << getProblem() << "] -> "
+                              << " Prb->getHardware() [" << getProblem()->getHardware() << "] -> Hwd [" << this
+                              << "] Mismatch!\n";
+    log.assertTrue((getProblem()->getHardware() == shared_from_this()), inconsistency_explanation);
 
     return log;
 }
-bool HardwareBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+bool HardwareBase::check(CheckLog& _log, bool _verbose, std::ostream& _stream, std::string _tabs) const
 {
-    auto hwd_ptr = std::static_pointer_cast<const HardwareBase>(_node_ptr);
-    auto local_log = localCheck(_verbose, hwd_ptr, _stream, _tabs);
+    auto local_log = localCheck(_verbose, _stream, _tabs);
     _log.compose(local_log);
-    for (auto S : getSensorList())
-        S->check(_log, S, _verbose, _stream, _tabs + "  ");
+    for (auto S : getSensorList()) S->check(_log, _verbose, _stream, _tabs + "  ");
     return _log.is_consistent_;
 }
-} // namespace wolf
+}  // namespace wolf

src/landmark/landmark.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/>.
+#include "core/landmark/landmark.h"
+
+namespace wolf
+{
+// Register in the FactoryLandmark
+WOLF_REGISTER_LANDMARK(Landmark2d);
+WOLF_REGISTER_LANDMARK(Landmark3d);
+
+}  // namespace wolf

src/landmark/landmark_base.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--------
-
+// 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 "core/landmark/landmark_base.h"
 #include "core/factor/factor_base.h"
 #include "core/map/map_base.h"
-#include "core/state_block/state_block_derived.h"
-#include "core/state_block/state_angle.h"
-#include "core/state_block/state_quaternion.h"
-#include "core/state_block/factory_state_block.h"
-#include "core/common/factory.h"
-#include "core/yaml/yaml_conversion.h"
-
-namespace wolf {
+#include "yaml-schema-cpp/yaml_conversion.hpp"
 
+namespace wolf
+{
 unsigned int LandmarkBase::landmark_id_count_ = 0;
 
-LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr) :
-        NodeBase("LANDMARK", _type),
-        HasStateBlocks(""),
-        map_ptr_(),
-        landmark_id_(++landmark_id_count_)
+LandmarkBase::LandmarkBase(const std::string&     _type,
+                           const TypeComposite&   _state_types,
+                           const VectorComposite& _state_vectors,
+                           const PriorComposite&  _state_priors,
+                           const int              _external_id,
+                           const int              _external_type)
+    : NodeStateBlocks("LANDMARK", _type, _state_types, _state_vectors, _state_priors),
+      map_ptr_(),
+      landmark_id_(++landmark_id_count_),
+      track_id_(0),
+      external_id_(_external_id),
+      external_type_(_external_type)
 {
-    if (_p_ptr)
-    {
-        addStateBlock('P', _p_ptr, nullptr);
-    }
-    if (_o_ptr)
-    {
-        addStateBlock('O', _o_ptr, nullptr);
-    }
-
 }
 
-LandmarkBase::~LandmarkBase()
+LandmarkBase::LandmarkBase(const std::string& _type, const TypeComposite& _state_types, const YAML::Node& _n)
+    : NodeStateBlocks("LANDMARK",
+                      _type,
+                      _state_types,
+                      VectorComposite(_n["states"], "value", false, _state_types.getKeys()),
+                      PriorComposite(_n["states"], "prior", false, _state_types.getKeys())),
+      map_ptr_(),
+      landmark_id_(_n["id"].as<int>()),
+      track_id_(_n["track_id"] ? _n["track_id"].as<unsigned int>() : 0),
+      external_id_(_n["external_id"] ? _n["external_id"].as<int>() : -1),
+      external_type_(_n["external_type"] ? _n["external_type"].as<int>() : -1)
 {
-    removeStateBlocks(getProblem());
 }
 
-void LandmarkBase::remove(bool viral_remove_empty_parent)
+void LandmarkBase::remove(bool viral_remove_parent_without_children)
 {
     /* Order of removing:
      * Factors are removed first, and afterwards the rest of nodes containing state blocks.
      * In case multi-threading, solver can be called while removing.
      * This order avoids SolverManager to ignore notifications (efficiency)
      */
-    if (!is_removing_)
-    {
-        is_removing_ = true;
-        LandmarkBasePtr this_L = shared_from_this(); // keep this alive while removing it
+    if (is_removing_) return;
 
-        // remove constrained by
-        while (!constrained_by_list_.empty())
-        {
-            constrained_by_list_.front()->remove(viral_remove_empty_parent);
-        }
+    is_removing_           = true;
+    LandmarkBasePtr this_L = shared_from_this_landmark();  // keep this alive while removing it
 
-        // Remove State Blocks
-        removeStateBlocks(getProblem());
+    // remove from upstream
+    auto M = map_ptr_.lock();
+    if (M) M->removeLandmark(this_L);
 
-        // remove from upstream
-        auto M = map_ptr_.lock();
-        if (M)
-            M->removeLandmark(this_L);
-    }
+    // Remove State Blocks
+    NodeStateBlocks::remove(viral_remove_parent_without_children);
 }
 
-bool LandmarkBase::getCovariance(Eigen::MatrixXd& _cov) const
+YAML::Node LandmarkBase::toYaml() const
 {
-    return getProblem()->getLandmarkCovariance(shared_from_this(), _cov);
-}
+    YAML::Node node = NodeStateBlocks::toYaml();
+
+    node["plugin"]        = "core";  // TO BE overwritten in derived classes
+    node["id"]            = landmark_id_;
+    node["track_id"]      = track_id_;
+    node["external_id"]   = external_id_;
+    node["external_type"] = external_type_;
 
-YAML::Node LandmarkBase::saveToYaml() const
-{
-    YAML::Node node;
-    node["id"] = landmark_id_;
-    node["type"] = node_type_;
-    if (getP() != nullptr)
-    {
-        node["position"] = getP()->getState();
-        node["position fixed"] = getP()->isFixed();
-    }
-    if (getO() != nullptr)
-    {
-        node["orientation"] = getO()->getState();
-        node["orientation fixed"] = getO()->isFixed();
-    }
-    node["transformable"] = getP()->isTransformable();
     return node;
 }
 
@@ -113,10 +92,10 @@ void LandmarkBase::link(MapBasePtr _map_ptr)
     assert(!is_removing_ && "linking a removed landmark");
     assert(this->getMap() == nullptr && "linking an already linked landmark");
 
-    if(_map_ptr)
+    if (_map_ptr)
     {
         this->setMap(_map_ptr);
-        _map_ptr->addLandmark(shared_from_this());
+        _map_ptr->addLandmark(shared_from_this_landmark());
         this->setProblem(_map_ptr->getProblem());
     }
     else
@@ -127,169 +106,77 @@ void LandmarkBase::link(MapBasePtr _map_ptr)
 
 void LandmarkBase::setProblem(ProblemPtr _problem)
 {
-    if (_problem == nullptr || _problem == this->getProblem())
-        return;
-
-    NodeBase::setProblem(_problem);
-    registerNewStateBlocks(_problem);
-}
-
-FactorBasePtr LandmarkBase::addConstrainedBy(FactorBasePtr _fac_ptr)
-{
-    constrained_by_list_.push_back(_fac_ptr);
-    return _fac_ptr;
-}
-
-void LandmarkBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
-{
-    constrained_by_list_.remove(_fac_ptr);
-}
+    if (_problem == nullptr || _problem == this->getProblem()) return;
 
-bool LandmarkBase::isConstrainedBy(const FactorBaseConstPtr &_factor) const
-{
-    return std::find(constrained_by_list_.begin(),
-                     constrained_by_list_.end(),
-                     _factor) != constrained_by_list_.end();
+    NodeStateBlocks::setProblem(_problem);
 }
 
-void LandmarkBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+void LandmarkBase::printHeader(int           _depth,
+                               bool          _factored_by,
+                               bool          _metric,
+                               bool          _state_blocks,
+                               std::ostream& _stream,
+                               std::string   _tabs) const
 {
     _stream << _tabs << "Lmk" << id() << " " << getType();
-    if (_constr_by)
+    if (_factored_by)
     {
         _stream << "\t<-- ";
-        for (auto cby : getConstrainedByList())
-            if (cby)
-                _stream << "Fac" << cby->id() << " \t";
+        for (auto fac : getFactoredBySet())
+            if (fac) _stream << "Fac" << fac->id() << " \t";
     }
     _stream << std::endl;
 
-    printState(_metric, _state_blocks, _stream, _tabs);
+    printState(_factored_by, _metric, _state_blocks, _stream, _tabs);
 }
 
-void LandmarkBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+void LandmarkBase::print(int           _depth,
+                         bool          _factored_by,
+                         bool          _metric,
+                         bool          _state_blocks,
+                         std::ostream& _stream,
+                         std::string   _tabs) const
 {
-    printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
-}
-
-LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node)
-{
-    unsigned int    id          = _node["id"]               .as< unsigned int     >();
-    Eigen::VectorXd pos         = _node["position"]         .as< Eigen::VectorXd  >();
-    bool            pos_fixed   = _node["position fixed"]   .as< bool >();
-
-    StateBlockPtr pos_sb = FactoryStateBlock::create("P", pos, pos_fixed);
-    StateBlockPtr ori_sb = nullptr;
-
-    if (_node["orientation"])
-    {
-        Eigen::VectorXd ori       = _node["orientation"].as< Eigen::VectorXd >();
-        bool            ori_fixed = _node["orientation fixed"].as< bool >();
-
-        if (ori.size() == 4)
-            ori_sb = std::make_shared<StateQuaternion>(ori, ori_fixed);
-        else
-            ori_sb = std::make_shared<StateAngle>(ori(0), ori_fixed);
-    }
-
-    LandmarkBasePtr lmk = std::make_shared<LandmarkBase>("LandmarkBase", pos_sb, ori_sb);
-    lmk->setId(id);
-
-    return lmk;
+    printHeader(_depth, _factored_by, _metric, _state_blocks, _stream, _tabs);
 }
 
-CheckLog LandmarkBase::localCheck(bool _verbose, LandmarkBaseConstPtr _lmk_ptr, std::ostream& _stream, std::string _tabs) const
+CheckLog LandmarkBase::localCheck(bool _verbose, std::ostream& _stream, std::string _tabs) const
 {
-    CheckLog log;
+    CheckLog          log;
     std::stringstream inconsistency_explanation;
 
     if (_verbose)
     {
-        _stream << _tabs << "Lmk" << id() << " @ " << _lmk_ptr.get() << std::endl;
-        _stream << _tabs << "  -> Prb @ " << getProblem().get() << std::endl;
-        _stream << _tabs << "  -> Map @ " << getMap().get() << std::endl;
-        for (auto pair : getStateBlockMap())
-        {
-            auto sb = pair.second;
-            _stream << _tabs << "  " << pair.first << " sb @ " << sb.get();
-            if (sb)
-            {
-                auto lp = sb->getLocalParametrization();
-                if (lp)
-                    _stream <<  " (lp @ " << lp.get() << ")";
-            }
-            _stream << std::endl;
-        }
+        _stream << _tabs << "Lmk" << id() << " @ " << this << std::endl;
+        _stream << _tabs << "  -> Prb @ " << getProblem() << std::endl;
+        _stream << _tabs << "  -> Map @ " << getMap() << std::endl;
     }
 
-    auto map_ptr = getMap();
     //  check problem and map pointers
-    inconsistency_explanation << "Landmarks' problem ptr "
-                              << getProblem().get() << " different from Map's problem ptr "
-                              << map_ptr->getProblem().get() << "\n";
+    auto map_ptr = getMap();
+    inconsistency_explanation << "Landmarks' problem ptr " << getProblem() << " different from Map's problem ptr "
+                              << map_ptr->getProblem() << "\n";
 
     log.assertTrue((getProblem() == map_ptr->getProblem()), inconsistency_explanation);
 
-    // check constrained-by factors
-    for (auto cby : getConstrainedByList())
-    {
-        if (_verbose)
-        {
-            _stream << _tabs << "    " << "<- Fac" << cby->id() << " ->";
-
-            for (auto Low : cby->getLandmarkOtherList())
-            {
-                if (!Low.expired())
-                {
-                    auto Lo = Low.lock();
-                    _stream << " Lmk" << Lo->id();
-                }
-            }
-            _stream << std::endl;
-        }
-
-        inconsistency_explanation << "constrained-by landmark " << id() << " @ " << _lmk_ptr.get()
-                                  << " not found among constrained-by factors\n";
-        log.assertTrue((cby->hasLandmarkOther(_lmk_ptr)), inconsistency_explanation);
-
-        for (auto sb : cby->getStateBlockPtrVector()) {
-            if (_verbose) {
-                _stream << _tabs << "      " << "sb @ " << sb.get();
-                if (sb) {
-                    auto lp = sb->getLocalParametrization();
-                    if (lp)
-                        _stream << " (lp @ " << lp.get() << ")";
-                }
-                _stream << std::endl;
-            }
-        }
-    }
-
     // check map
-    inconsistency_explanation << "Lmk" << id() << " @ " << _lmk_ptr
-                                << " ---> Map" << map_ptr
-                                << " -X-> Lmk" << id();
+    inconsistency_explanation << "Lmk" << id() << " @ " << this << " ---> Map" << map_ptr << " -X-> Lmk" << id();
     auto map_lmk_list = map_ptr->getLandmarkList();
-    auto map_has_lmk = std::find(map_lmk_list.begin(), 
-                                 map_lmk_list.end(), 
-                                 _lmk_ptr);
+    auto map_has_lmk  = std::find(map_lmk_list.begin(), map_lmk_list.end(), shared_from_this_landmark());
     log.assertTrue(map_has_lmk != map_lmk_list.end(), inconsistency_explanation);
 
+    // check NodeStateBlocks
+    auto node_log = NodeStateBlocks::localCheck(_verbose, _stream, _tabs);
+    log.compose(node_log);
+
     return log;
 }
-bool LandmarkBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+bool LandmarkBase::check(CheckLog& _log, bool _verbose, std::ostream& _stream, std::string _tabs) const
 {
-    auto lmk_ptr = std::static_pointer_cast<const LandmarkBase>(_node_ptr);
-    auto local_log = localCheck(_verbose, lmk_ptr, _stream, _tabs);
+    auto local_log = localCheck(_verbose, _stream, _tabs);
     _log.compose(local_log);
 
     return _log.is_consistent_;
 }
 
-// Register landmark creator
-namespace
-{
-const bool WOLF_UNUSED registered_lmk_base = FactoryLandmark::registerCreator("LandmarkBase", LandmarkBase::create);
-}
-
-} // namespace wolf
+}  // namespace wolf

src/landmark/landmark_external.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--------
-
-#include "core/landmark/landmark_external.h"
-#include "core/state_block/state_block_derived.h"
-#include "core/state_block/state_angle.h"
-#include "core/state_block/state_quaternion.h"
-#include "core/state_block/factory_state_block.h"
-#include "core/yaml/yaml_conversion.h"
-
-namespace wolf
-{
-
-LandmarkExternal::LandmarkExternal(const int&    _external_id,
-                                   const int&    _external_type,
-                                   StateBlockPtr _p_ptr,
-                                   StateBlockPtr _o_ptr)
-    : LandmarkBase("LandmarkExternal", _p_ptr, _o_ptr), external_id_(_external_id), external_type_(_external_type)
-{
-    assert(external_id_ >= 0 or external_id_ == -1);
-    assert(external_type_ >= 0 or external_type_ == -1);
-}
-
-YAML::Node LandmarkExternal::saveToYaml() const
-{
-    YAML::Node node       = LandmarkBase::saveToYaml();
-    node["external_id"]   = external_id_;
-    node["external_type"] = external_type_;
-    return node;
-}
-
-LandmarkBasePtr LandmarkExternal::create(const YAML::Node& _node)
-{
-    unsigned int    external_id   = _node["external_id"].as<int>();
-    unsigned int    external_type = _node["external_type"].as<int>();
-    Eigen::VectorXd pos           = _node["position"].as<Eigen::VectorXd>();
-    bool            pos_fixed     = _node["position fixed"].as<bool>();
-
-    StateBlockPtr pos_sb = FactoryStateBlock::create("P", pos, pos_fixed);
-    StateBlockPtr ori_sb = nullptr;
-
-    if (_node["orientation"])
-    {
-        Eigen::VectorXd ori       = _node["orientation"].as<Eigen::VectorXd>();
-        bool            ori_fixed = _node["orientation fixed"].as<bool>();
-
-        if (ori.size() == 4)
-            ori_sb = std::make_shared<StateQuaternion>(ori, ori_fixed);
-        else
-            ori_sb = std::make_shared<StateAngle>(ori(0), ori_fixed);
-    }
-
-    return std::make_shared<LandmarkExternal>(external_id, external_type, pos_sb, ori_sb);
-}
-
-// Register landmark creator
-namespace
-{
-const bool WOLF_UNUSED registered_lmk_external =
-    FactoryLandmark::registerCreator("LandmarkExternal", LandmarkExternal::create);
-}
-
-}  // namespace wolf

src/map/map_base.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--------
-
+// You should have received a copy of the GNU General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
 // wolf
 #include "core/map/map_base.h"
 #include "core/landmark/landmark_base.h"
-#include "core/common/factory.h"
+#include "core/landmark/factory_landmark.h"
 
 // YAML
 #include <yaml-cpp/yaml.h>
 
-// stl
-#include <fstream>
-#include <ctime>
-#include <iomanip>
-#include <iostream>
-#include <sstream>
-
-namespace wolf {
-
-MapBase::MapBase() :
-    NodeBase("MAP", "Base")
+namespace wolf
 {
-//    std::cout << "constructed M"<< std::endl;
-}
+MapBase::MapBase() : NodeBase("MAP", "MapBase") {}
 
-MapBase::MapBase(ParamsMapBasePtr _params, const std::string& _type) :
-    NodeBase("MAP", _type)
-{
-//    std::cout << "constructed M"<< std::endl;
-}
+MapBase::MapBase(const YAML::Node& _params) : NodeBase("MAP", "MapBase") {}
+
+MapBase::MapBase(const std::string& _type, const YAML::Node& _params) : NodeBase("MAP", _type) {}
 
-MapBase::~MapBase()
+bool MapBase::hasChildren() const
 {
-//	std::cout << "destructed -M" << std::endl;
+    return not landmark_list_.empty();
 }
 
 LandmarkBasePtr MapBase::addLandmark(LandmarkBasePtr _landmark_ptr)
@@ -67,139 +50,129 @@ void MapBase::removeLandmark(LandmarkBasePtr _landmark_ptr)
 
 LandmarkBaseConstPtr MapBase::getLandmark(const unsigned int& _id) const
 {
-    auto lmk_it = std::find_if(landmark_list_.begin(),
-                               landmark_list_.end(),
-                               [&](LandmarkBaseConstPtr lmk)
-                               {
-                                   return lmk->id() == _id;
-                               }); // lambda function for the find_if
+    auto lmk_it = std::find_if(landmark_list_.cbegin(), landmark_list_.cend(), [&](LandmarkBaseConstPtr lmk) {
+        return lmk->id() == _id;
+    });  // lambda function for the find_if
 
-    if (lmk_it == landmark_list_.end())
-        return nullptr;
-
-    return (*lmk_it);
+    return lmk_it != landmark_list_.cend() ? *lmk_it : nullptr;
 }
 
 LandmarkBasePtr MapBase::getLandmark(const unsigned int& _id)
 {
-    auto lmk_it = std::find_if(landmark_list_.begin(),
-                               landmark_list_.end(),
-                               [&](LandmarkBasePtr lmk)
-                               {
-                                   return lmk->id() == _id;
-                               }); // lambda function for the find_if
-
-    if (lmk_it == landmark_list_.end())
-        return nullptr;
+    auto lmk_it = std::find_if(landmark_list_.cbegin(), landmark_list_.cend(), [&](LandmarkBasePtr lmk) {
+        return lmk->id() == _id;
+    });  // lambda function for the find_if
 
-    return (*lmk_it);
+    return lmk_it != landmark_list_.cend() ? *lmk_it : nullptr;
 }
 
 void MapBase::load(std::string _map_file_dot_yaml)
 {
     // change ~ with HOME using environment variable
     if (_map_file_dot_yaml.at(0) == '~')
-        _map_file_dot_yaml = std::string(std::getenv("HOME")) + _map_file_dot_yaml.substr(1);
+        _map_file_dot_yaml = std::string(std::getenv("HOME")) + "/" + _map_file_dot_yaml.substr(1);
 
     try
     {
         YAML::Node map = YAML::LoadFile(_map_file_dot_yaml);
         for (unsigned int i = 0; i < map["landmarks"].size(); i++)
         {
-            YAML::Node lmk_node = map["landmarks"][i];
-            LandmarkBasePtr lmk_ptr = FactoryLandmark::create(lmk_node["type"].as<std::string>(), lmk_node);
+            YAML::Node      lmk_node = map["landmarks"][i];
+            LandmarkBasePtr lmk_ptr  = FactoryLandmark::create(lmk_node["type"].as<std::string>(), lmk_node, {});
             lmk_ptr->link(shared_from_this());
         }
     }
-    catch(const std::exception& e)
+    catch (const std::exception& e)
     {
         std::cerr << "MapBase::load: " << e.what() << '\n';
-    }    
+    }
 }
 
-void MapBase::save(std::string _map_file_yaml, const std::string& _map_name) const
+bool MapBase::save(std::string _map_file_yaml, const std::string& _map_name) const
 {
     // change ~ with HOME using environment variable
     if (_map_file_yaml.at(0) == '~')
-        _map_file_yaml = std::string(std::getenv("HOME")) + _map_file_yaml.substr(1);
+        _map_file_yaml = std::string(std::getenv("HOME")) + "/" + _map_file_yaml.substr(1);
 
     YAML::Emitter emitter;
 
     emitter << YAML::BeginMap;
-    emitter << "map_name"   << _map_name;
-    emitter << "date_time" << dateTimeNow(); // Get date and time for archiving purposes
+    emitter << "type" << getType();
+    emitter << "plugin"
+            << "core";
+    if (not _map_name.empty()) emitter << "name" << _map_name;
+    emitter << "date_time" << dateTimeNow();  // Get date and time for archiving purposes
 
-    emitter << "landmarks"  << YAML::BeginSeq;
+    emitter << "landmarks" << YAML::BeginSeq;
 
     for (LandmarkBaseConstPtr lmk_ptr : getLandmarkList())
     {
-        emitter << YAML::Flow << lmk_ptr->saveToYaml();
+        emitter << /*YAML::Flow <<*/ lmk_ptr->toYaml();
     }
     emitter << YAML::EndSeq << YAML::EndMap;
 
     std::ofstream fout(_map_file_yaml);
-    fout << emitter.c_str();
-    fout.close();
-}
-
-std::string MapBase::dateTimeNow() const
-{
-    // Get date and time for archiving purposes
-    std::time_t rawtime;
-    std::time(&rawtime);
-    const std::tm* timeinfo = std::localtime(&rawtime);
-    char time_char[30];
-    std::strftime(time_char, sizeof(time_char), "%d/%m/%Y at %H:%M:%S", timeinfo);
-    std::string date_time(time_char);
-    return date_time;
+    if (fout.is_open())
+    {
+        fout << emitter.c_str();
+        fout.close();
+    }
+    else
+    {
+        WOLF_ERROR("Could not create the file: ", _map_file_yaml);
+        return false;
+    }
+    return true;
 }
 
-void MapBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+void MapBase::printHeader(int           _depth,
+                          bool          _factored_by,
+                          bool          _metric,
+                          bool          _state_blocks,
+                          std::ostream& _stream,
+                          std::string   _tabs) const
 {
-    _stream << _tabs << "Map" << ((_depth < 1) ? ("        -- " + std::to_string(getLandmarkList().size()) + "L") : "") << std::endl;
+    _stream << _tabs << "Map" << ((_depth < 1) ? ("        -- " + std::to_string(getLandmarkList().size()) + "L") : "")
+            << std::endl;
 }
 
-void MapBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+void MapBase::print(int           _depth,
+                    bool          _factored_by,
+                    bool          _metric,
+                    bool          _state_blocks,
+                    std::ostream& _stream,
+                    std::string   _tabs) const
 {
-    printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
+    printHeader(_depth, _factored_by, _metric, _state_blocks, _stream, _tabs);
     if (_depth >= 1)
         for (auto L : getLandmarkList())
-            if (L)
-                L->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + "  ");
+            if (L) L->print(_depth, _factored_by, _metric, _state_blocks, _stream, _tabs + "  ");
 }
 
-CheckLog MapBase::localCheck(bool _verbose, MapBaseConstPtr _map_ptr, std::ostream& _stream, std::string _tabs) const
+CheckLog MapBase::localCheck(bool _verbose, std::ostream& _stream, std::string _tabs) const
 {
-    CheckLog log;
+    CheckLog          log;
     std::stringstream inconsistency_explanation;
 
-    if (_verbose)
-        _stream << _tabs << "Map @ " << _map_ptr.get() << std::endl;
+    if (_verbose) _stream << _tabs << "Map @ " << this << std::endl;
 
     // check pointer to Problem
-    inconsistency_explanation << "Map->getProblem() [" << getProblem().get()
-                              << "] -> " << " Prb->getMap() [" << getProblem()->getMap().get() << "] -> Map [" << _map_ptr.get() << "] Mismatch!\n";
-    log.assertTrue((_map_ptr->getProblem()->getMap().get() == _map_ptr.get()), inconsistency_explanation);
+    inconsistency_explanation << "Map->getProblem() [" << getProblem() << "] -> "
+                              << " Prb->getMap() [" << getProblem()->getMap() << "] -> Map [" << this
+                              << "] Mismatch!\n";
+    log.assertTrue((getProblem()->getMap() == shared_from_this()), inconsistency_explanation);
     return log;
 }
 
-bool MapBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
+bool MapBase::check(CheckLog& _log, bool _verbose, std::ostream& _stream, std::string _tabs) const
 {
-    auto map_ptr = std::static_pointer_cast<const MapBase>(_node_ptr);
-    auto local_log = localCheck(_verbose, map_ptr, _stream, _tabs);
+    auto local_log = localCheck(_verbose, _stream, _tabs);
     _log.compose(local_log);
-    for (auto L : getLandmarkList())
-        L->check(_log, L, _verbose, _stream, _tabs + "  ");
+    for (auto L : getLandmarkList()) L->check(_log, _verbose, _stream, _tabs + "  ");
     return _log.is_consistent_;
 }
 
-} // namespace wolf
-
-
-// Register in the FactorySensor
-#include "core/map/factory_map.h"
-namespace wolf {
+// Register in the FactorySensorNode
 WOLF_REGISTER_MAP(MapBase);
-WOLF_REGISTER_MAP_AUTO(MapBase);
-} // namespace wolf
 
+}  // namespace wolf