From c6715d0b1e1ca00dfa3424c86e6b352661a3fd8b Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Wed, 30 Apr 2025 15:14:12 +0200
Subject: [PATCH] adapted to new PB constructor and P::getState API
---
include/gnss/processor/processor_gnss_fix.h | 3 ++-
include/gnss/processor/processor_gnss_tdcp.h | 1 +
schema/processor/ProcessorGnssFix.schema | 6 ++++++
schema/processor/ProcessorGnssTdcp.schema | 6 ++++++
src/processor/processor_gnss_fix.cpp | 11 ++++++++---
src/processor/processor_gnss_tdcp.cpp | 9 +++++++--
src/processor/processor_tracker_gnss.cpp | 2 +-
7 files changed, 31 insertions(+), 7 deletions(-)
diff --git a/include/gnss/processor/processor_gnss_fix.h b/include/gnss/processor/processor_gnss_fix.h
index c132a2022..90f99804a 100644
--- a/include/gnss/processor/processor_gnss_fix.h
+++ b/include/gnss/processor/processor_gnss_fix.h
@@ -36,6 +36,7 @@ WOLF_PTR_TYPEDEFS(ProcessorGnssFix);
class ProcessorGnssFix : public ProcessorBase
{
protected:
+ unsigned int dim_;
SensorGnssPtr sensor_gnss_;
CaptureBasePtr last_KF_capture_, incoming_capture_;
FeatureBasePtr last_KF_feature_, incoming_feature_;
@@ -62,7 +63,7 @@ class ProcessorGnssFix : public ProcessorBase
*
* The ProcessorTracker only processes incoming captures (it is not called).
*/
- void processKeyFrame(FrameBasePtr _keyframe_ptr) override{};
+ void processKeyFrame(FrameBasePtr _keyframe_ptr) override {};
/** \brief trigger in capture
*
diff --git a/include/gnss/processor/processor_gnss_tdcp.h b/include/gnss/processor/processor_gnss_tdcp.h
index b0f6cc815..4c222d4e4 100644
--- a/include/gnss/processor/processor_gnss_tdcp.h
+++ b/include/gnss/processor/processor_gnss_tdcp.h
@@ -40,6 +40,7 @@ WOLF_PTR_TYPEDEFS(ProcessorGnssTdcp);
class ProcessorGnssTdcp : public ProcessorBase
{
protected:
+ unsigned int dim_;
SensorGnssPtr sensor_gnss_;
CaptureGnssPtr incoming_capture_;
FrameBasePtr last_KF_;
diff --git a/schema/processor/ProcessorGnssFix.schema b/schema/processor/ProcessorGnssFix.schema
index f2b79d5df..b165fe3c5 100644
--- a/schema/processor/ProcessorGnssFix.schema
+++ b/schema/processor/ProcessorGnssFix.schema
@@ -1,5 +1,11 @@
follow: ProcessorBase.schema
+dimension:
+ _mandatory: true
+ _type: unsigned int
+ _options: [2, 3]
+ _doc: "The dimension of the problem representation: 2D or 3D"
+
keyframe_vote:
max_time_span:
_mandatory: true
diff --git a/schema/processor/ProcessorGnssTdcp.schema b/schema/processor/ProcessorGnssTdcp.schema
index 3fb8aa599..d85132af3 100644
--- a/schema/processor/ProcessorGnssTdcp.schema
+++ b/schema/processor/ProcessorGnssTdcp.schema
@@ -1,5 +1,11 @@
follow: ProcessorGnssFix.schema
+dimension:
+ _mandatory: true
+ _type: unsigned int
+ _options: [2, 3]
+ _doc: "The dimension of the problem representation: 2D or 3D"
+
tdcp:
min_common_sats:
_mandatory: true
diff --git a/src/processor/processor_gnss_fix.cpp b/src/processor/processor_gnss_fix.cpp
index 2a37f0635..939413de7 100644
--- a/src/processor/processor_gnss_fix.cpp
+++ b/src/processor/processor_gnss_fix.cpp
@@ -27,8 +27,13 @@
namespace wolf
{
-ProcessorGnssFix::ProcessorGnssFix(const YAML::Node& _params) : ProcessorBase("ProcessorGnssFix", 0, _params)
+ProcessorGnssFix::ProcessorGnssFix(const YAML::Node& _params)
+ : ProcessorBase("ProcessorGnssFix",
+ {{'P', _params["dimension"].as<unsigned int>() == 2 ? "StatePoint2d" : "StatePoint3d"},
+ {'O', _params["dimension"].as<unsigned int>() == 2 ? "StateAngle" : "StateQuaternion"}},
+ _params)
{
+ dim_ = _params["dimension"].as<unsigned int>();
max_time_span_ = _params["keyframe_vote"]["max_time_span"].as<double>();
dist_traveled_ = _params["keyframe_vote"]["dist_traveled"].as<double>();
init_enu_map_ = _params["enu_map"]["init_enabled"].as<bool>();
@@ -218,7 +223,7 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture)
else if (permittedKeyFrame() && voteForKeyFrame())
{
WOLF_DEBUG("PR ", getName(), " emplacing KF TS = ", incoming_capture_->getTimeStamp());
- new_frame = getProblem()->emplaceFrame(incoming_capture_->getTimeStamp());
+ new_frame = getProblem()->emplaceFrame(incoming_capture_->getTimeStamp(), "PO");
KF_created = true;
}
// OTHERWISE store capture
@@ -298,7 +303,7 @@ FactorBasePtr ProcessorGnssFix::emplaceFactor(FeatureBasePtr _ftr)
{
// WOLF_DEBUG("creating the factor...");
// 2d
- if (getProblem()->getDim() == 2)
+ if (dim_ == 2)
return FactorBase::emplace<FactorGnssFix2d>(
_ftr, _ftr, sensor_gnss_, shared_from_this(), applyLossFunction(), FAC_ACTIVE);
// 3d
diff --git a/src/processor/processor_gnss_tdcp.cpp b/src/processor/processor_gnss_tdcp.cpp
index 585860027..5fcd51990 100644
--- a/src/processor/processor_gnss_tdcp.cpp
+++ b/src/processor/processor_gnss_tdcp.cpp
@@ -26,8 +26,13 @@
namespace wolf
{
-ProcessorGnssTdcp::ProcessorGnssTdcp(const YAML::Node& _params) : ProcessorBase("ProcessorGnssTdcp", 0, _params)
+ProcessorGnssTdcp::ProcessorGnssTdcp(const YAML::Node& _params)
+ : ProcessorBase("ProcessorGnssTdcp",
+ {{'P', _params["dimension"].as<unsigned int>() == 2 ? "StatePoint2d" : "StatePoint3d"},
+ {'O', _params["dimension"].as<unsigned int>() == 2 ? "StateAngle" : "StateQuaternion"}},
+ _params)
{
+ dim_ = _params["dimension"].as<unsigned int>();
params_tdcp_.min_common_sats = _params["tdcp"]["min_common_sats"].as<double>();
params_tdcp_.raim_n = _params["tdcp"]["raim_n"].as<double>();
params_tdcp_.max_residual_ci = _params["tdcp"]["max_residual_ci"].as<double>();
@@ -212,7 +217,7 @@ FactorBasePtr ProcessorGnssTdcp::emplaceFactor(FeatureBasePtr _ftr, FrameBasePtr
{
// WOLF_DEBUG("creating the factor...");
// 2D
- if (getProblem()->getDim() == 2)
+ if (dim_ == 2)
return FactorBase::emplace<FactorGnssDisplacement2d>(
_ftr, _ftr, _frm_ref, sensor_gnss_, shared_from_this(), applyLossFunction());
// 3D
diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp
index d7aa0e8a4..f1bb938b3 100644
--- a/src/processor/processor_tracker_gnss.cpp
+++ b/src/processor/processor_tracker_gnss.cpp
@@ -33,7 +33,7 @@
namespace wolf
{
ProcessorTrackerGnss::ProcessorTrackerGnss(const YAML::Node& _params)
- : ProcessorTrackerFeature("ProcessorTrackerGnss", "PO", 3, _params),
+ : ProcessorTrackerFeature("ProcessorTrackerGnss", {{'P', "StatePoint3d"}, {'O', "StateQuaternion"}}, _params),
outliers_code_(0),
outliers_tdcp_(0),
inliers_code_(0),
--
GitLab