Skip to content
Snippets Groups Projects
Commit 5d2eeebc authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

improved processors (more options)

parent 4de5f70d
No related branches found
No related tags found
2 merge requests!28release after RAL,!27After 2nd RAL submission
......@@ -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);
};
......
......@@ -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
......
......@@ -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;
......
......@@ -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(),
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment