From c72ef7f2295e74ea1eed1c6e26bfa4caf61b618f Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Tue, 6 Feb 2024 11:38:02 +0100
Subject: [PATCH] hotfixes
---
include/core/landmark/landmark_external.h | 24 ++-
.../processor/processor_landmark_external.h | 4 +-
src/landmark/landmark_base.cpp | 192 +++++++++++++++++-
src/landmark/landmark_external.cpp | 9 +-
src/processor/processor_landmark_external.cpp | 2 +-
test/gtest_processor_landmark_external.cpp | 36 ++--
6 files changed, 242 insertions(+), 25 deletions(-)
diff --git a/include/core/landmark/landmark_external.h b/include/core/landmark/landmark_external.h
index 3ff6df805..a96c9bc5e 100644
--- a/include/core/landmark/landmark_external.h
+++ b/include/core/landmark/landmark_external.h
@@ -56,12 +56,32 @@ class LandmarkExternal : public LandmarkBase
* Caution: This creator does not set the landmark's anchor frame and sensor.
* These need to be set afterwards.
*/
- static LandmarkExternalPtr create(const YAML::Node& _node);
+ static LandmarkBasePtr create(const YAML::Node& _node);
void setExternalId(const int& _external_id);
int getExternalId() const;
- void setExternalType(const int& _external_id);
+ void setExternalType(const int& _external_type);
int getExternalType() const;
};
+void LandmarkExternal::setExternalId(const int& _external_id)
+{
+ external_id_ = _external_id;
+}
+
+int LandmarkExternal::getExternalId() const
+{
+ return external_id_;
+}
+
+void LandmarkExternal::setExternalType(const int& _external_type)
+{
+ external_type_ = _external_type;
+}
+
+int LandmarkExternal::getExternalType() const
+{
+ return external_type_;
+}
+
} // namespace wolf
diff --git a/include/core/processor/processor_landmark_external.h b/include/core/processor/processor_landmark_external.h
index 917b53fab..06d0f1103 100644
--- a/include/core/processor/processor_landmark_external.h
+++ b/include/core/processor/processor_landmark_external.h
@@ -129,13 +129,13 @@ class ProcessorLandmarkExternal : public ProcessorTracker
/** \brief Emplaces a landmark or modifies (if needed) a landmark
* \param _feature_ptr pointer to the Feature
*/
- LandmarkExternalPtr emplaceLandmark(FeatureExternalPtr _feature_ptr);
+ LandmarkExternalPtr emplaceLandmark(FeatureLandmarkExternalPtr _feature_ptr);
/** \brief Modifies (if needed) a landmark
* \param _landmark pointer to the landmark
* \param _feature pointer to the Feature.
*/
- void modifyLandmark(LandmarkExternalPtr _landmark, FeatureExternalPtr _feature);
+ void modifyLandmark(LandmarkExternalPtr _landmark, FeatureLandmarkExternalPtr _feature);
/** Post-process
*
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 4ce1c2c26..7bec4d89f 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -51,6 +51,44 @@ LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, State
}
+LandmarkBase::~LandmarkBase()
+{
+ removeStateBlocks(getProblem());
+}
+
+void LandmarkBase::remove(bool viral_remove_empty_parent)
+{
+ /* 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
+
+ // remove constrained by
+ while (!constrained_by_list_.empty())
+ {
+ constrained_by_list_.front()->remove(viral_remove_empty_parent);
+ }
+
+ // Remove State Blocks
+ removeStateBlocks(getProblem());
+
+ // remove from upstream
+ auto M = map_ptr_.lock();
+ if (M)
+ M->removeLandmark(this_L);
+ }
+}
+
+bool LandmarkBase::getCovariance(Eigen::MatrixXd& _cov) const
+{
+ return getProblem()->getLandmarkCovariance(shared_from_this(), _cov);
+}
+
YAML::Node LandmarkBase::saveToYaml() const
{
YAML::Node node;
@@ -70,6 +108,70 @@ YAML::Node LandmarkBase::saveToYaml() const
return node;
}
+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)
+ {
+ this->setMap(_map_ptr);
+ _map_ptr->addLandmark(shared_from_this());
+ this->setProblem(_map_ptr->getProblem());
+ }
+ else
+ {
+ WOLF_WARN("Linking with nullptr");
+ }
+}
+
+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);
+}
+
+bool LandmarkBase::isConstrainedBy(const FactorBaseConstPtr &_factor) const
+{
+ return std::find(constrained_by_list_.begin(),
+ constrained_by_list_.end(),
+ _factor) != constrained_by_list_.end();
+}
+
+void LandmarkBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ _stream << _tabs << "Lmk" << id() << " " << getType();
+ if (_constr_by)
+ {
+ _stream << "\t<-- ";
+ for (auto cby : getConstrainedByList())
+ if (cby)
+ _stream << "Fac" << cby->id() << " \t";
+ }
+ _stream << std::endl;
+
+ printState(_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
+{
+ printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
+}
+
LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node)
{
unsigned int id = _node["id"] .as< unsigned int >();
@@ -96,10 +198,98 @@ LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node)
return lmk;
}
+CheckLog LandmarkBase::localCheck(bool _verbose, LandmarkBaseConstPtr _lmk_ptr, std::ostream& _stream, std::string _tabs) const
+{
+ 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;
+ }
+ }
+
+ 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";
+
+ 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();
+ auto map_lmk_list = map_ptr->getLandmarkList();
+ auto map_has_lmk = std::find(map_lmk_list.begin(),
+ map_lmk_list.end(),
+ _lmk_ptr);
+ log.assertTrue(map_has_lmk != map_lmk_list.end(), inconsistency_explanation);
+
+ return log;
+}
+bool LandmarkBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, 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);
+ _log.compose(local_log);
+
+ return _log.is_consistent_;
+}
+
// Register landmark creator
namespace
{
-const bool WOLF_UNUSED registered_lmk_external = FactoryLandmark::registerCreator("LandmarkExternal", LandmarkExternal::create);
+const bool WOLF_UNUSED registered_lmk_base = FactoryLandmark::registerCreator("LandmarkBase", LandmarkBase::create);
}
} // namespace wolf
diff --git a/src/landmark/landmark_external.cpp b/src/landmark/landmark_external.cpp
index 3dd8334a5..8bbd6eaa6 100644
--- a/src/landmark/landmark_external.cpp
+++ b/src/landmark/landmark_external.cpp
@@ -21,6 +21,11 @@
//--------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
{
@@ -28,7 +33,7 @@ namespace wolf
LandmarkExternal::LandmarkExternal(const int& _external_id,
const int& _external_type,
StateBlockPtr _p_ptr,
- StateBlockPtr _o_ptr = nullptr)
+ 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);
@@ -70,7 +75,7 @@ LandmarkBasePtr LandmarkExternal::create(const YAML::Node& _node)
// Register landmark creator
namespace
{
-const bool WOLF_UNUSED registered_lmk_base =
+const bool WOLF_UNUSED registered_lmk_external =
FactoryLandmark::registerCreator("LandmarkExternal", LandmarkExternal::create);
}
diff --git a/src/processor/processor_landmark_external.cpp b/src/processor/processor_landmark_external.cpp
index fe9f4f6bf..f12dffc60 100644
--- a/src/processor/processor_landmark_external.cpp
+++ b/src/processor/processor_landmark_external.cpp
@@ -497,7 +497,7 @@ FactorBasePtr ProcessorLandmarkExternal::emplaceFactor(FeatureBasePtr _feature,
return nullptr; // not going to happen
}
-LandmarkExternalPtr ProcessorLandmarkExternal::emplaceLandmark(FeatureExternalPtr _feature)
+LandmarkExternalPtr ProcessorLandmarkExternal::emplaceLandmark(FeatureLandmarkExternalPtr _feature)
{
assert(_feature);
assert(_feature->getCapture());
diff --git a/test/gtest_processor_landmark_external.cpp b/test/gtest_processor_landmark_external.cpp
index 1e43f1618..b3278fc86 100644
--- a/test/gtest_processor_landmark_external.cpp
+++ b/test/gtest_processor_landmark_external.cpp
@@ -23,7 +23,7 @@
#include "core/utils/utils_gtest.h"
#include "core/problem/problem.h"
#include "core/capture/capture_landmarks_external.h"
-#include "core/landmark/landmark_base.h"
+#include "core/landmark/landmark_external.h"
#include "core/sensor/sensor_odom_2d.h"
#include "core/sensor/sensor_odom_3d.h"
#include "core/processor/processor_odom_2d.h"
@@ -51,12 +51,12 @@ class ProcessorLandmarkExternalTest : public testing::Test
TimeStamp t;
double dt;
- ProblemPtr problem;
- SolverManagerPtr solver;
- SensorBasePtr sensor, sensor_odom;
- ProcessorLandmarkExternalPtr processor;
- ProcessorMotionPtr processor_motion;
- std::vector<LandmarkBasePtr> landmarks;
+ ProblemPtr problem;
+ SolverManagerPtr solver;
+ SensorBasePtr sensor, sensor_odom;
+ ProcessorLandmarkExternalPtr processor;
+ ProcessorMotionPtr processor_motion;
+ std::vector<LandmarkExternalPtr> landmarks;
// Groundtruth states
VectorComposite state_robot, state_sensor;
@@ -171,21 +171,22 @@ void ProcessorLandmarkExternalTest::initProblem(int _dim,
// Emplace 3 random landmarks
for (auto i = 0; i < 3; i++)
{
- LandmarkBasePtr lmk;
+ LandmarkExternalPtr lmk;
if (dim == 2)
- lmk = LandmarkBase::emplace<LandmarkBase>(
+ lmk = LandmarkBase::emplace<LandmarkExternal>(
_init_landmarks ? problem->getMap() : nullptr,
- (orientation ? "LandmarkPose2d" : "LandmarkPosition2d"),
+ i + 1,
+ i + 1,
std::make_shared<StatePoint2d>(Vector2d::Random() * 10),
(orientation ? std::make_shared<StateAngle>(Vector1d::Random() * M_PI) : nullptr));
else
- lmk = LandmarkBase::emplace<LandmarkBase>(
+ lmk = LandmarkBase::emplace<LandmarkExternal>(
_init_landmarks ? problem->getMap() : nullptr,
- (orientation ? "LandmarkPose3d" : "LandmarkPosition3d"),
+ i + 1,
+ i + 1,
std::make_shared<StatePoint3d>(Vector3d::Random() * 10),
(orientation ? std::make_shared<StateQuaternion>(Vector4d::Random().normalized()) : nullptr));
- lmk->setExternalId(i + 1); // TODO LandmarkExternal
landmarks.push_back(lmk);
state_landmarks.push_back(lmk->getState());
}
@@ -373,9 +374,9 @@ void ProcessorLandmarkExternalTest::testConfiguration(int _dim,
// check state correctly initialized
for (auto lmk_map : landmarks_map)
{
- ASSERT_TRUE(lmk_map->id() <= landmarks.size());
- ASSERT_EQ(lmk_map->id(), landmarks.at(lmk_map->id() - 1)->id());
- assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_map->id() - 1));
+ auto lmk_ext = std::static_pointer_cast<LandmarkExternal>(lmk_map);
+ ASSERT_EQ(lmk_ext->getExternalId(), landmarks.at(lmk_ext->getExternalId() - 1)->getExternalId());
+ assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_ext->getExternalId() - 1));
}
}
}
@@ -409,7 +410,8 @@ void ProcessorLandmarkExternalTest::testConfiguration(int _dim,
// check values
for (auto lmk_map : problem->getMap()->getLandmarkList())
{
- assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_map->id() - 1));
+ auto lmk_ext = std::static_pointer_cast<LandmarkExternal>(lmk_map);
+ assertVectorComposite(lmk_map->getState(), state_landmarks.at(lmk_ext->getExternalId() - 1));
}
}
}
--
GitLab