From d170f8eae31d67a11022ec1b3c0e7c04b80c4993 Mon Sep 17 00:00:00 2001 From: joanvallve <jvallve@iri.upc.edu> Date: Tue, 12 May 2020 21:30:21 +0200 Subject: [PATCH] ProcessorTrackerGnss: pseudoranges working --- .../gnss/factor/factor_gnss_pseudo_range.h | 55 +++++-- .../gnss/processor/processor_tracker_gnss.h | 12 +- src/processor/processor_tracker_gnss.cpp | 155 +++++++++++++----- test/gtest_factor_gnss_fix_2d.cpp | 5 +- test/gtest_factor_gnss_pseudo_range.cpp | 8 +- 5 files changed, 170 insertions(+), 65 deletions(-) diff --git a/include/gnss/factor/factor_gnss_pseudo_range.h b/include/gnss/factor/factor_gnss_pseudo_range.h index 8212bc7e4..510105781 100644 --- a/include/gnss/factor/factor_gnss_pseudo_range.h +++ b/include/gnss/factor/factor_gnss_pseudo_range.h @@ -13,11 +13,13 @@ namespace wolf { WOLF_PTR_TYPEDEFS(FactorGnssPseudoRange); -class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3, 4, 1, 3, 3, 1, 1, 1> +class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3, 4, 1, 1, 3, 3, 1, 1, 1> { protected: SensorGnssPtr sensor_gnss_ptr_; - Eigen::Vector3d satellite_ENU_; + Eigen::Vector3d satellite_ENU_, satellite_ECEF_; + double sagnac_correction_; + bool not_GPS_; public: @@ -26,7 +28,7 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) : - FactorAutodiff<FactorGnssPseudoRange, 1, 3, 4, 1, 3, 3, 1, 1, 1>("FactorGnssPseudoRange", + FactorAutodiff<FactorGnssPseudoRange, 1, 3, 4, 1, 1, 3, 3, 1, 1, 1>("FactorGnssPseudoRange", nullptr, nullptr, nullptr, @@ -37,16 +39,31 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _ftr_ptr->getCapture()->getStateBlock("T"), + (_ftr_ptr->getSatellite().sys == SYS_GLO ? + _ftr_ptr->getCapture()->getStateBlock("TG") : + (_ftr_ptr->getSatellite().sys == SYS_GAL ? + _ftr_ptr->getCapture()->getStateBlock("TE") : + _ftr_ptr->getCapture()->getStateBlock("TC"))),//in case GPS, TC is set but anyway not used _sensor_gnss_ptr->getP(), _sensor_gnss_ptr->getEnuMapTranslation(), _sensor_gnss_ptr->getEnuMapRoll(), _sensor_gnss_ptr->getEnuMapPitch(), _sensor_gnss_ptr->getEnuMapYaw()), sensor_gnss_ptr_(_sensor_gnss_ptr), - satellite_ENU_(sensor_gnss_ptr_->getREnuEcef() * _ftr_ptr->getSatellite().pos + sensor_gnss_ptr_->gettEnuEcef()) + satellite_ENU_(sensor_gnss_ptr_->getREnuEcef() * _ftr_ptr->getSatellite().pos + sensor_gnss_ptr_->gettEnuEcef()), + satellite_ECEF_(_ftr_ptr->getSatellite().pos), + not_GPS_(_ftr_ptr->getSatellite().sys != SYS_GPS) { //WOLF_INFO("FactorPseudoRange: sat ", _ftr_ptr->getSatellite().sat,"\n\tpos = ", _ftr_ptr->getSatellite().pos.transpose(), "\n\tprange = ", _ftr_ptr->getMeasurement()); WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating FactorGnssPseudoRange without initializing ENU"); + + /* SAGNAC EFFECT CORRECTION + * It should be recomputed with the antenna position in ECEF coordinates, + * but it is multiplied by a factor of 1e-14 (earth rotation / CLIGHT). + * We use instead a precomputed sagnac effect taking ENU origin as antenna position. + */ + Eigen::Vector3d t_ECEF_ENU = -sensor_gnss_ptr_->getREnuEcef().transpose() * sensor_gnss_ptr_->gettEnuEcef(); + sagnac_correction_ = OMGE*(satellite_ECEF_(0)*t_ECEF_ENU(1)-satellite_ECEF_(1)*t_ECEF_ENU(0))/CLIGHT;// change to antenna_ECEF } virtual ~FactorGnssPseudoRange() = default; @@ -60,6 +77,7 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3, bool operator ()(const T* const _x, const T* const _o, const T* const _clock_bias, + const T* const _clock_bias_constellation, const T* const _x_antena, const T* const _t_ENU_map, const T* const _roll_ENU_map, @@ -73,6 +91,7 @@ template<typename T> inline bool FactorGnssPseudoRange::operator ()(const T* const _x, const T* const _o, const T* const _clock_bias, + const T* const _clock_bias_constellation, const T* const _x_antena, const T* const _t_ENU_map, const T* const _roll_ENU_map, @@ -90,17 +109,27 @@ inline bool FactorGnssPseudoRange::operator ()(const T* const _x, // Antenna position in ENU coordinates Eigen::Matrix<T,3,1> antena_ENU = t_ENU_map + R_ENU_map * (t_map_base + q_map_base * t_base_antena); - //std::cout << "sat " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getSatellite().sat << std::endl; - //std::cout << std::setprecision(10) << "x = " << antena_ENU(0) << " "<< antena_ENU(1) << " "<< antena_ENU(2) << "\n"; - // Expected pseudo-range - T exp = (antena_ENU-satellite_ENU_.cast<T>()).norm() - _clock_bias[0]; + T exp = (antena_ENU-satellite_ENU_.cast<T>()).norm() + // norm + sagnac_correction_ + // sagnac correction (precomputed) + _clock_bias[0] + // receiver clock bias (w.r.t. GPS clock) + (not_GPS_ ? _clock_bias_constellation[0] : T(0)); // interconstellation clock bias - //std::cout << "clock_bias = " << _clock_bias[0] << "\n"; - //std::cout << "norm = " << (antena_ENU-satellite_ENU_.cast<T>()).norm() << "\n"; - //std::cout << "exp = " << exp << "\n"; - //std::cout << "meas = " << getMeasurement() << "\n"; - //std::cout << "error = " << exp - getMeasurement()(0) << "\n"; + + //std::cout << "sat " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getSatellite().sat << std::endl; + //std::cout << "\tsys " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getSatellite().sys << std::endl; + //std::cout << std::setprecision(13) <<"\tsat_pos= " << satellite_ENU_.transpose() << "\n"; + //std::cout << "\tx = " << antena_ENU(0) << " "<< antena_ENU(1) << " "<< antena_ENU(2) << "\n"; + //std::cout << "\tprange = " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getPseudoRange().p << "\n"; + //std::cout << "\tiono_corr = " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getPseudoRange().iono_corr << "\n"; + //std::cout << "\ttropo_corr = " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getPseudoRange().tropo_corr << "\n"; + //std::cout << "\tsat_clock_corr = " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getPseudoRange().sat_clock_corr << "\n"; + //std::cout << "\tcorrected prange = " << getMeasurement() << "\n"; + //std::cout << "\tclock_bias = " << _clock_bias[0] << " " << _clock_bias_constellation[0] << "\n"; + //std::cout << "\tnorm = " << (antena_ENU-satellite_ENU_.cast<T>()).norm() + sagnac_correction_ << "\n"; + //std::cout << "\texp_wo = " << (antena_ENU-satellite_ENU_.cast<T>()).norm() + sagnac_correction_ + _clock_bias[0] << "\n"; + //std::cout << "\texp = " << exp << "\n"; + //std::cout << "\terror = " << exp - getMeasurement()(0) << "\n"; // Residual _residual[0] = (exp - getMeasurement()(0)) / getMeasurementSquareRootInformationUpper()(0,0); diff --git a/include/gnss/processor/processor_tracker_gnss.h b/include/gnss/processor/processor_tracker_gnss.h index c1fa0a82d..3a4cecfac 100644 --- a/include/gnss/processor/processor_tracker_gnss.h +++ b/include/gnss/processor/processor_tracker_gnss.h @@ -18,6 +18,7 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature double enu_map_init_dist_min; double max_time_span; bool remove_outliers; + bool init_frames; ParamsProcessorTrackerGnss() = default; ParamsProcessorTrackerGnss(std::string _unique_name, const ParamsServer& _server): @@ -25,7 +26,8 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature { enu_map_init_dist_min = _server.getParam<double> (prefix + _unique_name + "/enu_map_init_dist_min"); remove_outliers = _server.getParam<bool> (prefix + _unique_name + "/remove_outliers"); - max_time_span = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/max_time_span"); + init_frames = _server.getParam<bool> (prefix + _unique_name + "/init_frames"); + remove_outliers = _server.getParam<bool> (prefix + _unique_name + "/remove_outliers"); // GNSS OPTIONS (see rtklib.h) gnss_opt = prcopt_default; @@ -56,6 +58,7 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature return "\n" + ParamsProcessorBase::print() + "\n" + "enu_map_init_dist_min: " + std::to_string(enu_map_init_dist_min) + "\n" + "remove_outliers: " + std::to_string(remove_outliers) + "\n" + + "init_frames: " + std::to_string(init_frames) + "\n" + "max_time_span: " + std::to_string(max_time_span) + "\n" + "mode: " + std::to_string(gnss_opt.mode) + "\n" + "soltype: " + std::to_string(gnss_opt.soltype) + "\n" @@ -186,13 +189,6 @@ class ProcessorTrackerGnss : public ProcessorTrackerFeature void removeOutliers(FactorBasePtrList fac_list, CaptureBasePtr cap) const; }; -inline ProcessorTrackerGnss::ProcessorTrackerGnss(ParamsProcessorTrackerGnssPtr _params_tracker_gnss) : - ProcessorTrackerFeature("ProcessorTrackerGnss", 3, _params_tracker_gnss), - params_tracker_gnss_(_params_tracker_gnss) -{ - // -} - inline ProcessorTrackerGnss::~ProcessorTrackerGnss() { // diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp index bd4118018..8bfc7ee49 100644 --- a/src/processor/processor_tracker_gnss.cpp +++ b/src/processor/processor_tracker_gnss.cpp @@ -8,8 +8,16 @@ namespace wolf { +ProcessorTrackerGnss::ProcessorTrackerGnss(ParamsProcessorTrackerGnssPtr _params_tracker_gnss) : + ProcessorTrackerFeature("ProcessorTrackerGnss", 3, _params_tracker_gnss), + params_tracker_gnss_(_params_tracker_gnss) +{ +} + void ProcessorTrackerGnss::preProcess() { + WOLF_INFO("ProcessorTrackerGnss::preProcess"); + GnssUtils::SnapshotPtr inc_snapshot = std::static_pointer_cast<CaptureGnss>(incoming_ptr_)->getSnapshot(); // compute satellites positions @@ -24,12 +32,11 @@ void ProcessorTrackerGnss::preProcess() // Set ECEF-ENU if (!sensor_gnss_->isEnuDefined() and fix_incoming_.stat != 0) { - WOLF_DEBUG("setting ECEF-ENU: ", fix_incoming_.pos.transpose()); + WOLF_INFO("setting ECEF-ENU: ", fix_incoming_.pos.transpose()); sensor_gnss_->setEcefEnu(fix_incoming_.pos,true); - WOLF_DEBUG("ECEF-ENU set: ", sensor_gnss_->gettEnuEcef().transpose(), "\n", sensor_gnss_->getREnuEcef()); } - WOLF_DEBUG("TS: ", incoming_ptr_->getTimeStamp(), " - Fix solution (ECEF): ", fix_incoming_.pos.transpose(), " - Fix solution (GEO): ", fix_incoming_.lat_lon.transpose()); + WOLF_INFO("TS: ", incoming_ptr_->getTimeStamp(), " - Fix solution (ECEF): ", fix_incoming_.pos.transpose(), " - Fix solution (GEO): ", fix_incoming_.lat_lon.transpose()); // filter observations (available ephemeris, constellations and elevation&SNR) inc_snapshot->filterObservations(std::set<int>(), // discarded sats @@ -38,13 +45,15 @@ void ProcessorTrackerGnss::preProcess() false, // check carrier phase params_tracker_gnss_->gnss_opt); - WOLF_DEBUG("preprocess: filtered observations: ", inc_snapshot->getObservations()->size()); + WOLF_INFO("preprocess: filtered observations: ", inc_snapshot->getObservations()->size()); // compute corrected pseudoranges inc_snapshot->computePseudoRanges(fix_incoming_.sat_azel, fix_incoming_.lat_lon, params_tracker_gnss_->gnss_opt); + WOLF_INFO("preprocess: pseudo ranges computed"); + // create features pseudorange for (auto obs_sat : inc_snapshot->getObservations()->getObservations()) { @@ -56,7 +65,7 @@ void ProcessorTrackerGnss::preProcess() untracked_incoming_features_[feat->satNumber()] = feat; } - WOLF_DEBUG("preprocess: untracked incoming features: ", untracked_incoming_features_.size()); + WOLF_INFO("preprocess: untracked incoming features: ", untracked_incoming_features_.size()); } unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _features_in, @@ -64,7 +73,7 @@ unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _feat FeatureBasePtrList& _features_out, FeatureMatchMap& _feature_correspondences) { - WOLF_DEBUG("tracking " , _features_in.size() , " features..."); + WOLF_INFO("tracking " , _features_in.size() , " features..."); assert(_capture == incoming_ptr_); @@ -81,17 +90,19 @@ unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _feat _features_out.push_back(ftr); _feature_correspondences[ftr] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,0})); - WOLF_DEBUG("track: " , feat_in->trackId() , " last: " , feat_in->id() , " inc: " , ftr->id() , " !" ); + WOLF_INFO("track: " , feat_in->trackId() , " last: " , feat_in->id() , " inc: " , ftr->id() , " !" ); } } - WOLF_DEBUG("After tracking:\n\ttracked features: " , _features_out.size()); + WOLF_INFO("After tracking:\n\ttracked features: " , _features_out.size()); return _features_out.size(); } bool ProcessorTrackerGnss::voteForKeyFrame() const { + //WOLF_INFO("ProcessorTrackerGnss::voteForKeyFrame"); + // too old origin if (origin_ptr_== nullptr or (last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp()) > params_tracker_gnss_->max_time_span ) { @@ -100,7 +111,7 @@ bool ProcessorTrackerGnss::voteForKeyFrame() const } // known features - WOLF_DEBUG("Nbr. of active feature tracks: " , known_features_incoming_.size() ); + WOLF_INFO("Nbr. of active feature tracks: " , known_features_incoming_.size() ); if (known_features_incoming_.size() < params_tracker_feature_->min_features_for_keyframe) { WOLF_INFO( "Vote for KF because of too less known_features_incoming" ); @@ -117,6 +128,8 @@ unsigned int ProcessorTrackerGnss::detectNewFeatures(const int& _max_new_feature const CaptureBasePtr& _capture, FeatureBasePtrList& _features_out) { + WOLF_INFO("ProcessorTrackerGnss::detectNewFeatures"); + assert(_capture == last_ptr_); for (auto feat_pair : untracked_last_features_) @@ -125,15 +138,22 @@ unsigned int ProcessorTrackerGnss::detectNewFeatures(const int& _max_new_feature break; _features_out.push_back(feat_pair.second); - WOLF_DEBUG("feature " , feat_pair.second->id() , " detected!" ); + WOLF_INFO("feature " , feat_pair.second->id() , " detected!" ); } - WOLF_DEBUG(_features_out.size() , " features detected!"); + WOLF_INFO(_features_out.size() , " features detected!"); return _features_out.size(); } void ProcessorTrackerGnss::establishFactors() { + WOLF_INFO("ProcessorTrackerGnss::establishFactors"); + + // initialize frame state with antenna position in map coordinates + // (since we don't have orientation for removing extrinsics) + if (params_tracker_gnss_->init_frames) + last_ptr_->getFrame()->getP()->setState(sensor_gnss_->getREnuMap().transpose() * (sensor_gnss_->getREnuEcef() * fix_last_.pos + sensor_gnss_->gettEnuEcef() - sensor_gnss_->gettEnuMap())); + FactorBasePtrList new_factors; bool last_clock_bias_set = false; @@ -145,7 +165,7 @@ void ProcessorTrackerGnss::establishFactors() if (ftr_sat == nullptr) continue; - // check valid measurement + // Check valid measurement if (ftr_sat->getMeasurement()(0) < 1e-12) { WOLF_INFO("Feature with not valid pseudorange"); @@ -156,25 +176,40 @@ void ProcessorTrackerGnss::establishFactors() if (!last_clock_bias_set) { if (last_ptr_->getStateBlock("T") == nullptr) - last_ptr_->addStateBlock("T", std::make_shared<StateBlock>(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(0))), getProblem()); + { + last_ptr_->addStateBlock("T", std::make_shared<StateBlock>(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(0)), false), getProblem()); + last_ptr_->addStateBlock("TG", std::make_shared<StateBlock>(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(1)), true), getProblem()); + last_ptr_->addStateBlock("TE", std::make_shared<StateBlock>(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(2)), true), getProblem()); + last_ptr_->addStateBlock("TC", std::make_shared<StateBlock>(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(3)), true), getProblem()); + } else - last_ptr_->getStateBlock("T")->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(0))); - - //WOLF_INFO("last clock bias initialized: ", last_ptr_->getStateBlock("T")->getState().transpose()); + { + last_ptr_->getStateBlock("T") ->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(0))); + last_ptr_->getStateBlock("TG")->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(1))); + last_ptr_->getStateBlock("TE")->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(2))); + last_ptr_->getStateBlock("TC")->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(3))); + } + //WOLF_INFO("last clock bias initialized: ", last_ptr_->getStateBlock("T")->getState()); last_clock_bias_set = true; } - - // emplace a pseudo range factor + // unfix clock bias inter-constellation + if (ftr_sat->getSatellite().sys == SYS_GLO) + last_ptr_->getStateBlock("TG")->unfix(); + if (ftr_sat->getSatellite().sys == SYS_GAL) + last_ptr_->getStateBlock("TE")->unfix(); + if (ftr_sat->getSatellite().sys == SYS_CMP) + last_ptr_->getStateBlock("TC")->unfix(); + + // Emplace a pseudo range factor auto new_fac = FactorBase::emplace<FactorGnssPseudoRange>(ftr_sat, 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()); + WOLF_INFO("FactorGnssPseudoRange emplaced for satellite: ", ftr_sat->satNumber()); } // TDCP FACTORS (tracked sats) @@ -187,7 +222,7 @@ void ProcessorTrackerGnss::establishFactors() // 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()); +// WOLF_INFO("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)) @@ -202,11 +237,11 @@ void ProcessorTrackerGnss::establishFactors() // // if (ftr_k == ftr_r) // { -// WOLF_DEBUG("same KF and feature, skipping..."); +// WOLF_INFO("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_INFO("previous feature at KF: ", ftr_r->getCapture()->getFrame()->id(), " sat: ", ftr_r->satNumber(), " id: ", ftr_r->id()); // // // Initialize (and create) clock bias stateblock in capture // if (!last_clock_bias_set) @@ -229,17 +264,21 @@ void ProcessorTrackerGnss::establishFactors() // params_->apply_loss_function); // new_factors.push_back(new_fac); // -// // WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(), +// // WOLF_INFO( "Factor: track: " , feature_in_last->trackId(), // // " origin: " , feature_in_origin->id() , // // " from last: " , feature_in_last->id() ); // } -// WOLF_DEBUG("All TDCP factors emplaced!"); +// WOLF_INFO("All TDCP factors emplaced!"); // } // } // remove outliers if (!new_factors.empty() and params_tracker_gnss_->remove_outliers) removeOutliers(new_factors, last_ptr_); + + //getProblem()->print(4, 1, 1, 1); + + WOLF_INFO("ProcessorTrackerGnss::establishFactors done"); } void ProcessorTrackerGnss::advanceDerived() @@ -260,19 +299,23 @@ void ProcessorTrackerGnss::resetDerived() void ProcessorTrackerGnss::postProcess() { - if (last_ptr_ && last_ptr_->getFrame() && last_ptr_->getFrame()->isKey()) - getProblem()->print(4, 1, 1, 1); + } void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBasePtr cap) const { + WOLF_INFO("ProcessorTrackerGnss::removeOutliers"); + FactorBasePtrList remove_fac; - //WOLF_DEBUG( "PR ", getName()," rejectOutlier..."); + //WOLF_INFO( "PR ", getName()," rejectOutlier..."); // copy states Eigen::Vector3d x(cap->getFrame()->getP()->getState()); Eigen::Vector4d o(cap->getFrame()->getO()->getState()); - Eigen::Vector1d clock_bias(-cap->getStateBlock("T")->getState()); + Eigen::Vector1d clock_bias(cap->getStateBlock("T")->getState()); + Eigen::Vector1d clock_bias_glo(cap->getStateBlock("TG")->getState()); + Eigen::Vector1d clock_bias_gal(cap->getStateBlock("TE")->getState()); + Eigen::Vector1d clock_bias_cmp(cap->getStateBlock("TC")->getState()); Eigen::Vector3d x_antena(sensor_gnss_->getP()->getState()); Eigen::Vector3d t_ENU_map(sensor_gnss_->getEnuMapTranslation()->getState()); Eigen::Vector1d roll_ENU_map(sensor_gnss_->getEnuMapRoll()->getState()); @@ -289,14 +332,34 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas //std::cout << "yaw_ENU_map: " << yaw_ENU_map << std::endl; // create double* array of a copy of the state for pseudo range factors - double* parameters_pr[8] = {x.data(), - o.data(), - clock_bias.data(), - x_antena.data(), - t_ENU_map.data(), - roll_ENU_map.data(), - pitch_ENU_map.data(), - yaw_ENU_map.data()}; + double* parameters_glo_pr[9] = {x.data(), + o.data(), + clock_bias.data(), + clock_bias_glo.data(), + x_antena.data(), + t_ENU_map.data(), + roll_ENU_map.data(), + pitch_ENU_map.data(), + yaw_ENU_map.data()}; + double* parameters_gal_pr[9] = {x.data(), + o.data(), + clock_bias.data(), + clock_bias_gal.data(), + x_antena.data(), + t_ENU_map.data(), + roll_ENU_map.data(), + pitch_ENU_map.data(), + yaw_ENU_map.data()}; + double* parameters_cmp_pr[9] = {x.data(), + o.data(), + clock_bias.data(), + clock_bias_cmp.data(), + x_antena.data(), + t_ENU_map.data(), + roll_ENU_map.data(), + pitch_ENU_map.data(), + yaw_ENU_map.data()}; + // create residuals double residual_pr; @@ -307,7 +370,21 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas if (fac_pr) { // evaluate the factor in this state - fac_pr->evaluate(parameters_pr, &residual_pr, nullptr); + switch (std::static_pointer_cast<FeatureGnssSatellite>(fac->getFeature())->getSatellite().sys) + { + case SYS_GLO: + fac_pr->evaluate(parameters_glo_pr, &residual_pr, nullptr); + break; + case SYS_GAL: + fac_pr->evaluate(parameters_gal_pr, &residual_pr, nullptr); + break; + case SYS_CMP: + fac_pr->evaluate(parameters_cmp_pr, &residual_pr, nullptr); + break; + default: + fac_pr->evaluate(parameters_glo_pr, &residual_pr, nullptr); + break; + } //WOLF_INFO("FactorGnssPseudoRange with residual = ", residual_pr); @@ -325,6 +402,8 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas // remove outliers for (auto fac : remove_fac) fac->remove(); + + WOLF_INFO("ProcessorTrackerGnss::removeOutliers done"); } } // namespace wolf diff --git a/test/gtest_factor_gnss_fix_2d.cpp b/test/gtest_factor_gnss_fix_2d.cpp index b2d3df14a..b3a1ec3f3 100644 --- a/test/gtest_factor_gnss_fix_2d.cpp +++ b/test/gtest_factor_gnss_fix_2d.cpp @@ -5,15 +5,14 @@ * \author: jvallve */ -#include "gnss/factor/factor_gnss_fix_2d.h" #include <core/utils/utils_gtest.h> #include "core/problem/problem.h" +#include "core/ceres_wrapper/ceres_manager.h" #include "gnss/sensor/sensor_gnss.h" #include "gnss/processor/processor_gnss_fix.h" #include "gnss/capture/capture_gnss_fix.h" - -#include "core/ceres_wrapper/ceres_manager.h" +#include "gnss/factor/factor_gnss_fix_2d.h" using namespace Eigen; using namespace wolf; diff --git a/test/gtest_factor_gnss_pseudo_range.cpp b/test/gtest_factor_gnss_pseudo_range.cpp index a5a47b37d..aaafe7765 100644 --- a/test/gtest_factor_gnss_pseudo_range.cpp +++ b/test/gtest_factor_gnss_pseudo_range.cpp @@ -1,11 +1,10 @@ -#include "gnss/factor/factor_gnss_pseudo_range.h" #include <core/utils/utils_gtest.h> #include "core/problem/problem.h" +#include "core/ceres_wrapper/ceres_manager.h" #include "gnss/sensor/sensor_gnss.h" #include "gnss/capture/capture_gnss.h" - -#include "core/ceres_wrapper/ceres_manager.h" +#include "gnss/factor/factor_gnss_pseudo_range.h" using namespace Eigen; using namespace wolf; @@ -110,6 +109,9 @@ void setUpProblem() // capture cap = CaptureBase::emplace<CaptureGnss>(frm, TimeStamp(0), gnss_sensor, nullptr); cap->addStateBlock("T", std::make_shared<StateBlock>(clock_drift, false), prb); + cap->addStateBlock("TG", std::make_shared<StateBlock>(Eigen::Vector1d::Zero(), true), prb); + cap->addStateBlock("TE", std::make_shared<StateBlock>(Eigen::Vector1d::Zero(), true), prb); + cap->addStateBlock("TC", std::make_shared<StateBlock>(Eigen::Vector1d::Zero(), true), prb); // features ftr1 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obsd_t(), sat1, prange1); // obsd_t data is not used in pseudo range factors -- GitLab