From 68fcc731c3decfd1ed16c66ac11fa8762e9dd983 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Fri, 12 Jul 2019 14:19:41 +0200
Subject: [PATCH] added distance_traveled param and corresponding voting for KF
condition
---
include/gnss/processor/processor_gnss_fix.h | 3 +++
include/gnss/processor/processor_gnss_single_diff.h | 3 +++
src/processor/processor_gnss_fix.cpp | 4 ++++
src/processor/processor_gnss_single_diff.cpp | 6 ++++++
4 files changed, 16 insertions(+)
diff --git a/include/gnss/processor/processor_gnss_fix.h b/include/gnss/processor/processor_gnss_fix.h
index 6a2cba34e..c33561169 100644
--- a/include/gnss/processor/processor_gnss_fix.h
+++ b/include/gnss/processor/processor_gnss_fix.h
@@ -18,18 +18,21 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsGnssFix);
struct ProcessorParamsGnssFix : public ProcessorParamsBase
{
Scalar time_th;
+ Scalar dist_traveled;
Scalar enu_map_init_dist_min;
ProcessorParamsGnssFix() = default;
ProcessorParamsGnssFix(std::string _unique_name, const paramsServer& _server):
ProcessorParamsBase(_unique_name, _server)
{
time_th = _server.getParam<Scalar>(_unique_name + "/time_th");
+ dist_traveled = _server.getParam<Scalar>(_unique_name + "/dist_traveled");
enu_map_init_dist_min = _server.getParam<Scalar>(_unique_name + "/enu_map_init_dist_min");
}
std::string print()
{
return "\n" + ProcessorParamsBase::print()
+ "time_th: " + std::to_string(time_th) + "\n";
+ + "dist_traveled: " + std::to_string(dist_traveled) + "\n";
+ "enu_map_init_dist_min: " + std::to_string(enu_map_init_dist_min) + "\n";
}
};
diff --git a/include/gnss/processor/processor_gnss_single_diff.h b/include/gnss/processor/processor_gnss_single_diff.h
index ebd286c8e..940b759d6 100644
--- a/include/gnss/processor/processor_gnss_single_diff.h
+++ b/include/gnss/processor/processor_gnss_single_diff.h
@@ -18,18 +18,21 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsGnssSingleDiff);
struct ProcessorParamsGnssSingleDiff : public ProcessorParamsBase
{
Scalar time_th;
+ Scalar dist_traveled;
Scalar enu_map_init_dist_min;
ProcessorParamsGnssSingleDiff() = default;
ProcessorParamsGnssSingleDiff(std::string _unique_name, const paramsServer& _server):
ProcessorParamsBase(_unique_name, _server)
{
time_th = _server.getParam<Scalar>(_unique_name + "/time_th");
+ dist_traveled = _server.getParam<Scalar>(_unique_name + "/dist_traveled");
enu_map_init_dist_min = _server.getParam<Scalar>(_unique_name + "/enu_map_init_dist_min");
}
std::string print()
{
return "\n" + ProcessorParamsBase::print()
+ "time_th: " + std::to_string(time_th) + "\n";
+ + "dist_traveled: " + std::to_string(dist_traveled) + "\n";
+ "enu_map_init_dist_min: " + std::to_string(enu_map_init_dist_min) + "\n";
}
};
diff --git a/src/processor/processor_gnss_fix.cpp b/src/processor/processor_gnss_fix.cpp
index fca7b52b3..b4efaea1f 100644
--- a/src/processor/processor_gnss_fix.cpp
+++ b/src/processor/processor_gnss_fix.cpp
@@ -145,6 +145,10 @@ bool ProcessorGnssFix::voteForKeyFrame()
if (!last_KF_ || (incoming_capture_->getTimeStamp() - last_KF_->getTimeStamp()) > params_gnss_->time_th)
return true;
+ // Distance criterion
+ if ((incoming_capture_->getFrame()->getP()->getState() - last_KF_->getP()->getState()).norm() > params_gnss_->dist_traveled)
+ return true;
+
// TODO: more alternatives?
// otherwise
diff --git a/src/processor/processor_gnss_single_diff.cpp b/src/processor/processor_gnss_single_diff.cpp
index ff31924f1..cb37515ef 100644
--- a/src/processor/processor_gnss_single_diff.cpp
+++ b/src/processor/processor_gnss_single_diff.cpp
@@ -101,6 +101,12 @@ bool ProcessorGnssSingleDiff::voteForKeyFrame()
if (last_KF_==nullptr || (last_KF_->getTimeStamp() - incoming_capture_->getTimeStamp()) > params_gnss_->time_th)
return true;
+ // Distance criterion
+ std::cout << "params_gnss_->dist_traveled" << params_gnss_->dist_traveled << std::endl;
+ std::cout << "incoming_capture_->getData().norm()" << incoming_capture_->getData().norm() << std::endl;
+ if (incoming_capture_->getData().norm() > params_gnss_->dist_traveled)
+ return true;
+
// TODO: more alternatives?
// otherwise
--
GitLab