diff --git a/include/gnss/processor/processor_gnss_fix.h b/include/gnss/processor/processor_gnss_fix.h index c132a20225a41f8b77af3675342f947e229ab0d9..90f99804a25aa5996ce781b56be8453ecdeccb3a 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 b0f6cc815cf662e63284e30f2eff2502544946e3..4c222d4e4199d66d86bbdef09f47d8bef9c1ca5e 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 f2b79d5df6b21d09eba8c17864434a97993425f0..b165fe3c5022d631abd4dcc557f3accdaba6f266 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 3fb8aa59919240240759bbe660d77ef8865d6ede..d85132af36b0489f47a175c8a8032bf30f918f76 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 2a37f06359400fcfcd46d94f96dd2b2056e56a4f..939413de7d79914e4e2975f8b4a1a96b0ce00e31 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 585860027e81684e5c906200d481b99277bbda1d..5fcd519900f4df6c5559dd44ae0949e8109228fb 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 d7aa0e8a41e06c936bac7889d224549212a11e81..f1bb938b34e87625b46fe0f311edc1629220911f 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),