diff --git a/include/gnss/processor/processor_gnss_fix.h b/include/gnss/processor/processor_gnss_fix.h
index ce27d06339b96c1fbcea291e5efedfdb3ab356d9..d4f9b8ac08309ae17bd1f7ff246630cfefa2b0c4 100644
--- a/include/gnss/processor/processor_gnss_fix.h
+++ b/include/gnss/processor/processor_gnss_fix.h
@@ -18,11 +18,13 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorGnssFix);
struct ParamsProcessorGnssFix : public ParamsProcessorBase
{
- bool fix_from_raw, init_enu_map;
+ bool fix_from_raw, init_enu_map, remove_outliers;
GnssUtils::Options compute_pos_opt;
double max_time_span;
double dist_traveled;
double enu_map_init_dist_min;
+ double enu_map_fix_time;
+ double outlier_residual_th;
ParamsProcessorGnssFix() = default;
ParamsProcessorGnssFix(std::string _unique_name, const ParamsServer& _server):
@@ -33,7 +35,10 @@ struct ParamsProcessorGnssFix : public ParamsProcessorBase
init_enu_map = _server.getParam<bool> (prefix + _unique_name + "/init_enu_map");
if (init_enu_map)
enu_map_init_dist_min = _server.getParam<double> (prefix + _unique_name + "/enu_map_init_dist_min");
+ enu_map_fix_time = _server.getParam<double> (prefix + _unique_name + "/enu_map_fix_time");
fix_from_raw = _server.getParam<bool> (prefix + _unique_name + "/fix_from_raw");
+ remove_outliers = _server.getParam<bool> (prefix + _unique_name + "/remove_outliers");
+ outlier_residual_th = _server.getParam<double> (prefix + _unique_name + "/outlier_residual_th");
// COMPUTE POS PARAMS (only if compute fix from yaw)
if (fix_from_raw)
@@ -67,7 +72,9 @@ struct ParamsProcessorGnssFix : public ParamsProcessorBase
"init_enu_map: " + std::to_string(init_enu_map) + "\n" +
(init_enu_map ?
"enu_map_init_dist_min: "+ std::to_string(enu_map_init_dist_min) + "\n" : "") +
- "enu_map_init_dist_min: " + std::to_string(enu_map_init_dist_min) + "\n" +
+ "enu_map_fix_time: " + std::to_string(enu_map_fix_time) + "\n" +
+ "remove_outliers: " + std::to_string(remove_outliers) + "\n" +
+ "outlier_residual_th: " + std::to_string(outlier_residual_th) + "\n" +
"keyframe_vote/max_time_span: " + std::to_string(max_time_span) + "\n" +
(fix_from_raw ?
"gnss/sateph: " + std::to_string(compute_pos_opt.sateph) + "\n" +
@@ -149,7 +156,7 @@ class ProcessorGnssFix : public ProcessorBase
private:
FactorBasePtr emplaceFactor(FeatureBasePtr _ftr_ptr);
- bool rejectOutlier(FactorBasePtr ctr_ptr);
+ bool detectOutlier(FactorBasePtr ctr_ptr);
};
diff --git a/include/gnss/processor/processor_tracker_gnss.h b/include/gnss/processor/processor_tracker_gnss.h
index 2339456f70b1205d0f5437e1d3ddb9f3df48572b..aa3a19a7d0cb1b15ef9d3e5cd18379d19f956a8c 100644
--- a/include/gnss/processor/processor_tracker_gnss.h
+++ b/include/gnss/processor/processor_tracker_gnss.h
@@ -20,6 +20,7 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
bool remove_outliers, remove_outliers_tdcp, remove_outliers_with_fix;
double outlier_residual_th;
bool init_frames;
+ double enu_map_fix_time;
ParamsProcessorTrackerGnss() = default;
ParamsProcessorTrackerGnss(std::string _unique_name, const ParamsServer& _server):
@@ -30,6 +31,7 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
outlier_residual_th = _server.getParam<double> (prefix + _unique_name + "/outlier_residual_th");
init_frames = _server.getParam<bool> (prefix + _unique_name + "/init_frames");
max_time_span = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/max_time_span");
+ enu_map_fix_time = _server.getParam<double> (prefix + _unique_name + "/enu_map_fix_time");
// GNSS OPTIONS (see rtklib.h)
gnss_opt.sateph = _server.getParam<int> (prefix + _unique_name + "/gnss/sateph"); // satellite ephemeris/clock (0:broadcast ephemeris,1:precise ephemeris,2:broadcast + SBAS,3:ephemeris option: broadcast + SSR_APC,4:broadcast + SSR_COM,5: QZSS LEX ephemeris
@@ -74,6 +76,7 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
+ "remove_outliers: " + std::to_string(remove_outliers) + "\n"
+ "outlier_residual_th: " + std::to_string(outlier_residual_th) + "\n"
+ "init_frames: " + std::to_string(init_frames) + "\n"
+ + "enu_map_fix_time: " + std::to_string(enu_map_fix_time) + "\n"
+ "keyframe_vote/max_time_span: " + std::to_string(max_time_span) + "\n"
+ "gnss/sateph: " + std::to_string(gnss_opt.sateph) + "\n"
+ "gnss/ionoopt: " + std::to_string(gnss_opt.ionoopt) + "\n"
@@ -125,6 +128,7 @@ class ProcessorTrackerGnss : public ProcessorTrackerFeature
GnssUtils::ComputePosOutput fix_incoming_, fix_last_;
unsigned int outliers_pseudorange_, outliers_tdcp_, inliers_pseudorange_, inliers_tdcp_;
std::map<int, unsigned int> sat_outliers_;
+ TimeStamp first_ts_;
/** \brief Track provided features in \b _capture
* \param _features_in input list of features in \b last to track
diff --git a/src/processor/processor_gnss_fix.cpp b/src/processor/processor_gnss_fix.cpp
index 13322f533e7a2fa3cd293f8ba1ae0918f7406c43..86cbd606326c71b30891ebcd379e69645ab77b18 100644
--- a/src/processor/processor_gnss_fix.cpp
+++ b/src/processor/processor_gnss_fix.cpp
@@ -48,29 +48,6 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture)
FrameBasePtr new_frame = nullptr;
FactorBasePtr new_fac = nullptr;
- // ALREADY CREATED KF
- PackKeyFramePtr KF_pack = buffer_pack_kf_.selectPack( incoming_capture_, params_gnss_->time_tolerance);
- if (KF_pack and (last_KF_capture_==nullptr or KF_pack->key_frame != last_KF_capture_->getFrame()))
- {
- WOLF_DEBUG( "PR ", getName()," - capture ", incoming_capture_->id(), " appended to existing KF " , KF_pack->key_frame->id() , " TS: ", KF_pack->key_frame->getTimeStamp());
- new_frame = KF_pack->key_frame;
- }
- // MAKE KF
- else if (permittedKeyFrame() && voteForKeyFrame())
- {
- WOLF_DEBUG("PR ", getName()," emplacing KF TS = ", incoming_capture_->getTimeStamp());
- new_frame = getProblem()->emplaceFrame(KEY, incoming_capture_->getTimeStamp());
- KF_created = true;
- }
-
- // ESTABLISH FACTOR
- if (new_frame == nullptr)
- return;
-
- // ESTABLISH FACTOR
- // link capture
- incoming_capture_->link(new_frame);
-
// emplace features
if (israw)
{
@@ -102,18 +79,43 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture)
}
auto incoming_feature = FeatureBase::emplace<FeatureGnssFix>(incoming_capture_, incoming_pos_out_);
+ // ALREADY CREATED KF
+ PackKeyFramePtr KF_pack = buffer_pack_kf_.selectPack( incoming_capture_, params_gnss_->time_tolerance);
+ if (KF_pack and last_KF_capture_ and KF_pack->key_frame == last_KF_capture_->getFrame())
+ KF_pack = nullptr;
+ if (KF_pack)
+ {
+ WOLF_DEBUG("PR ", getName()," - capture ", incoming_capture_->id(), " appended to existing KF " , KF_pack->key_frame->id() , " TS: ", KF_pack->key_frame->getTimeStamp());
+ new_frame = KF_pack->key_frame;
+ }
+ // MAKE KF
+ else if (permittedKeyFrame() && voteForKeyFrame())
+ {
+ WOLF_DEBUG("PR ", getName()," emplacing KF TS = ", incoming_capture_->getTimeStamp());
+ new_frame = getProblem()->emplaceFrame(KEY, incoming_capture_->getTimeStamp());
+ KF_created = true;
+ }
+ // OTHERWISE return
+ else
+ return;
+ assert(new_frame);
+
+ // ESTABLISH FACTOR
// emplace factor
new_fac = emplaceFactor(incoming_feature);
// outlier rejection (only can be evaluated if ENU defined and ENU-MAP initialized)
WOLF_DEBUG("ProcessorGnssFix: outlier rejection");
- if (sensor_gnss_->isEnuDefined() && sensor_gnss_->isEnuMapInitialized())
- if (rejectOutlier(new_fac))
+ if (params_gnss_->remove_outliers and sensor_gnss_->isEnuDefined() and sensor_gnss_->isEnuMapInitialized())
+ if (detectOutlier(new_fac))
{
- new_fac->remove();
+ new_frame->remove();
return;
}
+ // link capture
+ incoming_capture_->link(new_frame);
+
// store last KF
last_KF_capture_ = incoming_capture_;
last_KF_feature_ = incoming_feature;
@@ -146,6 +148,15 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture)
}
}
+ // Fix ENU-MAP
+ if (incoming_capture_->getTimeStamp() - first_capture_->getTimeStamp() > params_gnss_->enu_map_fix_time)
+ {
+ sensor_gnss_->getEnuMapTranslation()->fix();
+ sensor_gnss_->getEnuMapRoll()->fix();
+ sensor_gnss_->getEnuMapPitch()->fix();
+ sensor_gnss_->getEnuMapYaw()->fix();
+ }
+
// Notify if KF created
if (KF_created)
getProblem()->keyFrameCallback(new_frame, shared_from_this(), params_gnss_->time_tolerance);
@@ -163,37 +174,6 @@ FactorBasePtr ProcessorGnssFix::emplaceFactor(FeatureBasePtr _ftr)
return FactorBase::emplace<FactorGnssFix3d>(_ftr, _ftr, sensor_gnss_, shared_from_this(), params_->apply_loss_function, FAC_ACTIVE);
}
-bool ProcessorGnssFix::rejectOutlier(FactorBasePtr fac)
-{
- //WOLF_DEBUG( "PR ", getName()," rejectOutlier...");
- // Cast feature
- auto gnss_ftr = std::static_pointer_cast<FeatureGnssFix>(fac->getFeature());
- // copy states
- Eigen::VectorXd x(gnss_ftr->getCapture()->getFrame()->getP()->getState());
- Eigen::VectorXd o(gnss_ftr->getCapture()->getFrame()->getO()->getState());
- Eigen::VectorXd x_antena(sensor_gnss_->getP()->getState());
- Eigen::VectorXd t_ENU_map(sensor_gnss_->getEnuMapTranslation()->getState());
- Eigen::VectorXd roll_ENU_map(sensor_gnss_->getEnuMapRoll()->getState());
- Eigen::VectorXd pitch_ENU_map(sensor_gnss_->getEnuMapPitch()->getState());
- Eigen::VectorXd yaw_ENU_map(sensor_gnss_->getEnuMapYaw()->getState());
- // create double* array of a copy of the state
- double* parameters[7] = {x.data(), o.data(), x_antena.data(), t_ENU_map.data(), roll_ENU_map.data(),
- pitch_ENU_map.data(), yaw_ENU_map.data()};
- // create residuals pointer
- Eigen::VectorXd residuals(3);
- // evaluate the factor in this state
- fac->evaluate(parameters, residuals.data(), nullptr);
- // discard if residual too high evaluated at the current estimation
- if (residuals.norm() > 1e3)
- {
- WOLF_WARN("Discarding GNSS FIX Factor, considered OUTLIER");
- WOLF_TRACE("Feature: ", fac->getMeasurement().transpose(),"\nError: ",(fac->getMeasurementSquareRootInformationUpper().inverse()*residuals).transpose());
- fac->remove();
- return true;
- }
- return false;
-}
-
bool ProcessorGnssFix::voteForKeyFrame() const
{
//WOLF_DEBUG("voteForKeyFrame...");
@@ -214,7 +194,7 @@ bool ProcessorGnssFix::voteForKeyFrame() const
// ENU not defined
if (!sensor_gnss_->isEnuDefined())
{
- WOLF_DEBUG("KF because of enu not defined");
+ WOLF_INFO("KF because of enu not defined");
return true;
}
@@ -225,7 +205,7 @@ bool ProcessorGnssFix::voteForKeyFrame() const
!first_capture_->isRemoving() and
(first_feature_->getMeasurement()-incoming_pos_out_.pos).norm() > params_gnss_->enu_map_init_dist_min)
{
- WOLF_DEBUG("KF because of enu map not initialized");
+ WOLF_INFO("KF because of enu map not initialized");
assert(first_capture_ != nullptr);
return true;
}
@@ -233,7 +213,7 @@ bool ProcessorGnssFix::voteForKeyFrame() const
// Distance criterion (ENU defined and ENU-MAP initialized)
if (last_KF_capture_ != nullptr && (incoming_pos_out_.pos - last_KF_feature_->getMeasurement()).norm() > params_gnss_->dist_traveled)
{
- WOLF_DEBUG("KF because of distance criterion: ", (incoming_pos_out_.pos - last_KF_feature_->getMeasurement()).norm());
+ WOLF_INFO("KF because of distance criterion: ", (incoming_pos_out_.pos - last_KF_feature_->getMeasurement()).norm());
return true;
}
// TODO: more alternatives?
@@ -242,6 +222,39 @@ bool ProcessorGnssFix::voteForKeyFrame() const
return false;
}
+bool ProcessorGnssFix::detectOutlier(FactorBasePtr fac)
+{
+ //WOLF_DEBUG( "PR ", getName()," rejectOutlier...");
+ // Cast feature
+ auto gnss_ftr = std::static_pointer_cast<FeatureGnssFix>(fac->getFeature());
+ // copy states
+ Eigen::VectorXd x(gnss_ftr->getCapture()->getFrame()->getP()->getState());
+ Eigen::VectorXd o(gnss_ftr->getCapture()->getFrame()->getO()->getState());
+ Eigen::VectorXd x_antena(sensor_gnss_->getP()->getState());
+ Eigen::VectorXd t_ENU_map(sensor_gnss_->getEnuMapTranslation()->getState());
+ Eigen::VectorXd roll_ENU_map(sensor_gnss_->getEnuMapRoll()->getState());
+ Eigen::VectorXd pitch_ENU_map(sensor_gnss_->getEnuMapPitch()->getState());
+ Eigen::VectorXd yaw_ENU_map(sensor_gnss_->getEnuMapYaw()->getState());
+ // create double* array of a copy of the state
+ double* parameters[7] = {x.data(), o.data(), x_antena.data(), t_ENU_map.data(), roll_ENU_map.data(),
+ pitch_ENU_map.data(), yaw_ENU_map.data()};
+ // create residuals pointer
+ Eigen::Vector3d residual;
+ // evaluate the factor in this state
+ fac->evaluate(parameters, residual.data(), nullptr);
+ // discard if residual too high evaluated at the current estimation
+ if (residual.norm() > params_gnss_->outlier_residual_th)
+ {
+ WOLF_WARN("Discarding GNSS FIX Factor, considered OUTLIER");
+ WOLF_TRACE("Feature: ", fac->getMeasurement().transpose(),
+ "\n\tError: ",(fac->getMeasurementSquareRootInformationUpper().inverse()*residual).transpose(),
+ "\n\tResidual: ",residual.transpose(),
+ "\n\tResidual norm: ",residual.norm(), "(max: ", params_gnss_->outlier_residual_th, ")");
+ return true;
+ }
+ return false;
+}
+
bool ProcessorGnssFix::storeKeyFrame(FrameBasePtr _frame_ptr)
{
return true;
diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp
index 68991ea0d3a61355a171f126297fac0d2683300c..47a8d4a5e77bcb773a4e42edcf7e420395df85f3 100644
--- a/src/processor/processor_tracker_gnss.cpp
+++ b/src/processor/processor_tracker_gnss.cpp
@@ -14,7 +14,8 @@ ProcessorTrackerGnss::ProcessorTrackerGnss(ParamsProcessorTrackerGnssPtr _params
outliers_pseudorange_(0),
outliers_tdcp_(0),
inliers_pseudorange_(0),
- inliers_tdcp_(0)
+ inliers_tdcp_(0),
+ first_ts_() //invalid timestamp
{
}
@@ -59,6 +60,14 @@ void ProcessorTrackerGnss::preProcess()
WOLF_INFO("setting ECEF-ENU: ", fix_incoming_.pos.transpose());
sensor_gnss_->setEcefEnu(fix_incoming_.pos, true);
}
+ // Fix ENU-MAP
+ if (incoming_ptr_->getTimeStamp() - first_ts_ > params_tracker_gnss_->enu_map_fix_time)
+ {
+ sensor_gnss_->getEnuMapTranslation()->fix();
+ sensor_gnss_->getEnuMapRoll()->fix();
+ sensor_gnss_->getEnuMapPitch()->fix();
+ sensor_gnss_->getEnuMapYaw()->fix();
+ }
WOLF_DEBUG("TS: ", incoming_ptr_->getTimeStamp(), " - Fix solution (ECEF): ", fix_incoming_.pos.transpose(), " - Fix solution (GEO): ", fix_incoming_.lat_lon.transpose());
@@ -88,6 +97,10 @@ void ProcessorTrackerGnss::preProcess()
untracked_incoming_features_[feat->satNumber()] = feat;
}
+ // store as first timestamp (if any not-filtered satellite)
+ if (!untracked_incoming_features_.empty() and !first_ts_.ok())
+ first_ts_ = incoming_ptr_->getTimeStamp();
+
WOLF_INFO("ProcessorTrackerGnss::preProcess()",
"\n\tinitial observations: ", n_initial,
"\n\tRTKLIB discarded: ", fix_incoming_.discarded_sats.size(),