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

working on processor tracker gnss

parent dface027
No related branches found
No related tags found
2 merge requests!28release after RAL,!27After 2nd RAL submission
......@@ -87,6 +87,7 @@ class ProcessorTrackerGnss : public ProcessorTrackerFeature
ParamsProcessorTrackerGnssPtr params_tracker_gnss_;
SensorGnssPtr sensor_gnss_;
std::map<int, FeatureGnssSatellitePtr> untracked_incoming_features_, untracked_last_features_;
Eigen::Vector4d fix_incoming_, fix_last_;
/** \brief Track provided features in \b _capture
* \param _features_in input list of features in \b last to track
......@@ -173,10 +174,12 @@ class ProcessorTrackerGnss : public ProcessorTrackerFeature
* - resetting and/or clearing variables and/or algorithms at the end of processing
* - drawing / printing / logging the results of the processing
*/
virtual void postProcess() override{};
virtual void postProcess() override;
void advanceDerived() override;
void resetDerived() override;
void removeOutliers(FactorBasePtrList fac_list, CaptureBasePtr cap) const;
};
inline ProcessorTrackerGnss::ProcessorTrackerGnss(ParamsProcessorTrackerGnssPtr _params_tracker_gnss) :
......
......@@ -16,14 +16,23 @@ void ProcessorTrackerGnss::preProcess()
if (!inc_snapshot ->satellitesPositionsComputed())
inc_snapshot ->computeSatellitesPositions(params_tracker_gnss_->gnss_opt.sateph);
// auxiliar fix (for computing elevation, if error: resulting zero position -> elevation not filtered)
Eigen::Vector3d inc_pos = GnssUtils::computePos(*inc_snapshot->getObservations(),
*inc_snapshot->getNavigation(),
prcopt_default).pos; // take default (less restrictive?)
// Compute (and store) auxiliar fix (for computing elevation)
GnssUtils::ComputePosOutput fix_output = GnssUtils::computePos(*inc_snapshot->getObservations(),
*inc_snapshot->getNavigation(),
prcopt_default); // take default (less restrictive?)
fix_incoming_.head<3>() = fix_output.pos;
fix_incoming_(3) = fix_output.rcv_bias(0);
// Set ECEF-ENU
if (!sensor_gnss_->isEnuDefined())
sensor_gnss_->setEcefEnu(Eigen::Vector3d::Zero(),true);
WOLF_INFO("TS: ", incoming_ptr_->getTimeStamp(), " - Fix solution (ECEF): ", fix_incoming_.transpose());
// filter observations (available ephemeris, constellations and elevation&SNR)
inc_snapshot->filterObservations(std::set<int>(), // discarded sats
inc_pos,
fix_incoming_.head<3>(),
false, // check code
false, // check carrier phase
params_tracker_gnss_->gnss_opt);
......@@ -118,6 +127,8 @@ void ProcessorTrackerGnss::establishFactors()
{
WOLF_INFO("ProcessorTrackerGnss::establishFactors: Frame = ", last_ptr_->getFrame()->getState().transpose());
FactorBasePtrList new_factors;
if (last_ptr_->getStateBlock("T") == nullptr)
{
WOLF_DEBUG("last clock offset state block nullptr, adding it");
......@@ -131,62 +142,76 @@ void ProcessorTrackerGnss::establishFactors()
if (ftr_sat == nullptr)
continue;
// check valid measurement
if (ftr_sat->getObservation().P[0] < 1e-12) // TODO: move to GnssUtils::isValid(double, Combination)
continue;
// emplace a pseudo range factor
FactorBase::emplace<FactorGnssPseudoRange>(ftr_sat,
GnssUtils::Combination::CODE_L1,//TODO: from params
0.1,// TODO: compute from params and dt
ftr_sat,
sensor_gnss_,
shared_from_this(),
params_->apply_loss_function);
auto new_fac = FactorBase::emplace<FactorGnssPseudoRange>(ftr_sat,
GnssUtils::Combination::CODE_L1,//TODO: from params
0.1,// TODO: compute from params and dt
ftr_sat,
sensor_gnss_,
shared_from_this(),
params_->apply_loss_function);
new_factors.push_back(new_fac);
WOLF_DEBUG("FactorGnssPseudoRange emplaced for satellite: ", ftr_sat->satNumber());
}
if (origin_ptr_ == last_ptr_)
return;
// TDCP FACTORS (tracked sats)
// iterate over tracked features of last
// for (auto ftr_k_pair : track_matrix_.snapshot(last_ptr_))
// if (origin_ptr_ != last_ptr_)
// {
// // current feature
// auto ftr_k = std::dynamic_pointer_cast<FeatureGnssSatellite>(ftr_k_pair.second);
// assert(ftr_k != nullptr);
//
// WOLF_DEBUG("FeatureGnssSatellite sat: ", ftr_k->satNumber(), " id: ", ftr_k->id());
//
// // emplace a tdcp factor from last to each KF
// for (auto ftr_r_pair : track_matrix_.trackAtKeyframes(ftr_k_pair.first))
// // iterate over tracked features of last
// for (auto ftr_k_pair : track_matrix_.snapshot(last_ptr_))
// {
// // reference feature
// auto ftr_r = std::dynamic_pointer_cast<FeatureGnssSatellite>(ftr_r_pair.second);
// assert(ftr_r != nullptr);
// // current feature
// auto ftr_k = std::dynamic_pointer_cast<FeatureGnssSatellite>(ftr_k_pair.second);
// assert(ftr_k != nullptr);
//
// WOLF_DEBUG("FeatureGnssSatellite sat: ", ftr_k->satNumber(), " id: ", ftr_k->id());
//
// if (ftr_k == ftr_r)
// // emplace a tdcp factor from last to each KF
// for (auto ftr_r_pair : track_matrix_.trackAtKeyframes(ftr_k_pair.first))
// {
// WOLF_DEBUG("same KF and feature, skipping...");
// break;
// }
// // reference feature
// auto ftr_r = std::dynamic_pointer_cast<FeatureGnssSatellite>(ftr_r_pair.second);
// assert(ftr_r != nullptr);
//
// // check valid measurement
// if (ftr_sat->getObservation().L[0] < 1e-12) // TODO: move to GnssUtils::isValid(double, Combination)
// continue;
//
// if (ftr_k == ftr_r)
// {
// WOLF_DEBUG("same KF and feature, skipping...");
// break;
// }
//
// WOLF_DEBUG("previous feature at KF: ", ftr_r->getCapture()->getFrame()->id(), " sat: ", ftr_r->satNumber(), " id: ", ftr_r->id());
// WOLF_DEBUG("previous feature at KF: ", ftr_r->getCapture()->getFrame()->id(), " sat: ", ftr_r->satNumber(), " id: ", ftr_r->id());
//
// // emplace tdcp factor
// FactorBase::emplace<FactorGnssTdcp>(ftr_k,
// GnssUtils::Combination::CARRIER_L1,//TODO: from params
// 0.1,// TODO: compute from params and dt
// ftr_r,
// ftr_k,
// sensor_gnss_,
// shared_from_this(),
// params_->apply_loss_function);
// // emplace tdcp factor
// auto new_fac = FactorBase::emplace<FactorGnssTdcp>(ftr_k,
// GnssUtils::Combination::CARRIER_L1,//TODO: from params
// 0.1,// TODO: compute from params and dt
// ftr_r,
// ftr_k,
// sensor_gnss_,
// shared_from_this(),
// params_->apply_loss_function);
// new_factors.push_back(new_fac);
//
// // WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(),
// // " origin: " , feature_in_origin->id() ,
// // " from last: " , feature_in_last->id() );
// // WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(),
// // " origin: " , feature_in_origin->id() ,
// // " from last: " , feature_in_last->id() );
// }
// WOLF_DEBUG("All TDCP factors emplaced!");
// }
// WOLF_DEBUG("All TDCP factors emplaced!");
// }
// remove outliers
removeOutliers(new_factors, last_ptr_);
}
void ProcessorTrackerGnss::advanceDerived()
......@@ -194,6 +219,7 @@ void ProcessorTrackerGnss::advanceDerived()
ProcessorTrackerFeature::advanceDerived();
untracked_last_features_ = std::move(untracked_incoming_features_);
fix_last_ = fix_incoming_;
}
void ProcessorTrackerGnss::resetDerived()
......@@ -201,6 +227,66 @@ void ProcessorTrackerGnss::resetDerived()
ProcessorTrackerFeature::resetDerived();
untracked_last_features_ = std::move(untracked_incoming_features_);
fix_last_ = fix_incoming_;
}
void ProcessorTrackerGnss::postProcess()
{
getProblem()->print(4, 1, 1, 1);
}
void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBasePtr cap) const
{
FactorBasePtrList remove_fac;
//WOLF_DEBUG( "PR ", getName()," rejectOutlier...");
// copy states
Eigen::VectorXd x(cap->getFrame()->getP()->getState());
Eigen::VectorXd o(cap->getFrame()->getO()->getState());
Eigen::VectorXd clock_drift(cap->getStateBlock("T")->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 for pseudo range factors
double* parameters_pr[8] = {x.data(),
o.data(),
clock_drift.data(),
x_antena.data(),
t_ENU_map.data(),
roll_ENU_map.data(),
pitch_ENU_map.data(),
yaw_ENU_map.data()};
// create residuals
Eigen::VectorXd residual_pr(1);
for (auto fac : fac_list)
{
// PSEUDO RANGE FACTORS
auto fac_pr = std::dynamic_pointer_cast<FactorGnssPseudoRange>(fac);
if (fac_pr)
{
// evaluate the factor in this state
fac_pr->evaluate(parameters_pr, residual_pr.data(), nullptr);
WOLF_INFO("FactorGnssPseudoRange with residual = ", residual_pr);
// discard if residual too high evaluated at the current estimation
if (residual_pr(0) > 1e3)
{
WOLF_WARN("Discarding FactorGnssPseudoRange, considered OUTLIER");
WOLF_TRACE("Feature: ", fac->getMeasurement().transpose(),"\nError: ",(fac->getMeasurementSquareRootInformationUpper().inverse()*residual_pr).transpose());
remove_fac.push_back(fac_pr);
}
}
// TODO: TDCP FACTORS
}
// remove outliers
for (auto fac : remove_fac)
fac->remove();
}
} // namespace wolf
......
#include "gnss/sensor/sensor_gnss.h"
#include "core/state_block/state_block.h"
#include "core/state_block/state_quaternion.h"
#include "core/state_block/state_angle.h"
#include "gnss_utils/gnss_utils.h"
#include "gnss_utils/utils/transformations.h"
......@@ -22,9 +22,9 @@ SensorGnss::SensorGnss(const Eigen::VectorXd& _extrinsics,
assert(_extrinsics.size() == 3 && "Bad extrinsics size");
addStateBlock("t", std::make_shared<StateBlock>(3, params_->translation_fixed));
addStateBlock("r", std::make_shared<StateBlock>(1, params_->roll_fixed));
addStateBlock("p", std::make_shared<StateBlock>(1, params_->pitch_fixed));
addStateBlock("y", std::make_shared<StateBlock>(1, params_->yaw_fixed));
addStateBlock("r", std::make_shared<StateAngle>(0.0, params_->roll_fixed));
addStateBlock("p", std::make_shared<StateAngle>(0.0, params_->pitch_fixed));
addStateBlock("y", std::make_shared<StateAngle>(0.0, params_->yaw_fixed));
if (params_->set_ENU)
setEcefEnu(params_->ENU_latlonalt, false);
......
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