From c9fc28f96618656b14951c1bb418896fcef7698f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu> Date: Thu, 11 Jul 2019 17:35:47 +0200 Subject: [PATCH] working! --- include/gnss/processor/processor_gnss_fix.h | 7 +- .../processor/processor_gnss_single_diff.h | 27 ++++- include/gnss/sensor/sensor_gnss.h | 6 +- src/processor/processor_gnss_fix.cpp | 54 ++++----- src/processor/processor_gnss_single_diff.cpp | 49 ++++++-- src/sensor/sensor_gnss.cpp | 112 ++++++++++++++++-- test/gtest_factor_gnss_fix_2D.cpp | 6 +- 7 files changed, 206 insertions(+), 55 deletions(-) diff --git a/include/gnss/processor/processor_gnss_fix.h b/include/gnss/processor/processor_gnss_fix.h index f2f1d4a51..a32fb08fd 100644 --- a/include/gnss/processor/processor_gnss_fix.h +++ b/include/gnss/processor/processor_gnss_fix.h @@ -18,16 +18,19 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsGnssFix); struct ProcessorParamsGnssFix : public ProcessorParamsBase { Scalar time_th; + 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"); + time_th = _server.getParam<Scalar>(_unique_name + "/time_th"); + 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"; + + "enu_map_init_dist_min: " + std::to_string(enu_map_init_dist_min) + "\n"; } }; @@ -36,7 +39,7 @@ class ProcessorGnssFix : public ProcessorBase { protected: SensorGnssPtr sensor_gnss_ptr_; - ProcessorParamsGnssFixPtr params_; + ProcessorParamsGnssFixPtr params_gnss_; CaptureGnssFixPtr first_capture_ptr_, last_capture_ptr_; FrameBasePtr last_gnss_fix_KF_; diff --git a/include/gnss/processor/processor_gnss_single_diff.h b/include/gnss/processor/processor_gnss_single_diff.h index 708b5c280..41a6ef8a2 100644 --- a/include/gnss/processor/processor_gnss_single_diff.h +++ b/include/gnss/processor/processor_gnss_single_diff.h @@ -12,17 +12,38 @@ namespace wolf { WOLF_PTR_TYPEDEFS(ProcessorGnssSingleDiff); +WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsGnssSingleDiff); + + +struct ProcessorParamsGnssSingleDiff : public ProcessorParamsBase +{ + Scalar time_th; + 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"); + 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"; + + "enu_map_init_dist_min: " + std::to_string(enu_map_init_dist_min) + "\n"; + } +}; //class class ProcessorGnssSingleDiff : public ProcessorBase { protected: SensorGnssPtr sensor_gnss_ptr_; - ProcessorParamsBasePtr params_; - CaptureGnssSingleDiffPtr last_capture_ptr_; + ProcessorParamsGnssSingleDiffPtr params_gnss_; + CaptureGnssSingleDiffPtr last_capture_ptr_, incoming_capture_ptr_; public: - ProcessorGnssSingleDiff(ProcessorParamsBasePtr _params, SensorGnssPtr _sensor_gnss_ptr); + ProcessorGnssSingleDiff(ProcessorParamsGnssSingleDiffPtr _params_gnss, SensorGnssPtr _sensor_gnss_ptr); virtual ~ProcessorGnssSingleDiff(); virtual void configure(SensorBasePtr _sensor); diff --git a/include/gnss/sensor/sensor_gnss.h b/include/gnss/sensor/sensor_gnss.h index db9301ce1..e7204f6fa 100644 --- a/include/gnss/sensor/sensor_gnss.h +++ b/include/gnss/sensor/sensor_gnss.h @@ -44,13 +44,14 @@ struct IntrinsicsGnss : public IntrinsicsBase class SensorGnss : public SensorBase { protected: + IntrinsicsGnssPtr params_; bool ENU_defined_, ENU_MAP_initialized_; Eigen::Matrix3s R_ENU_ECEF_; Eigen::Vector3s t_ENU_ECEF_; public: - SensorGnss(StateBlockPtr _p_ptr, StateBlockPtr _bias_ptr); - SensorGnss(StateBlockPtr _p_ptr, StateBlockPtr _bias_ptr, StateBlockPtr _t_ENU_MAP_ptr, StateBlockPtr _roll_ENU_MAP_ptr, StateBlockPtr _pitch_ENU_MAP_ptr, StateBlockPtr _yaw_ENU_MAP_ptr); + SensorGnss(StateBlockPtr _p_ptr, StateBlockPtr _bias_ptr, IntrinsicsGnssPtr params); + SensorGnss(StateBlockPtr _p_ptr, StateBlockPtr _bias_ptr, IntrinsicsGnssPtr params, StateBlockPtr _t_ENU_MAP_ptr, StateBlockPtr _roll_ENU_MAP_ptr, StateBlockPtr _pitch_ENU_MAP_ptr, StateBlockPtr _yaw_ENU_MAP_ptr); virtual ~SensorGnss(); @@ -68,6 +69,7 @@ class SensorGnss : public SensorBase StateBlockPtr getEnuMapYawState() const; void initializeEnuMap(const Eigen::VectorXs& _pose_MAP_frameENU, const Eigen::Vector3s& _t_ECEF_antenaENU, const Eigen::VectorXs& _pose_MAP_frame2, const Eigen::Vector3s& _t_ECEF_antena2); + void setEnuMapTranslationState(const Eigen::Vector3s& t_ENU_MAP); // Gets const Eigen::Matrix3s& getREnuEcef() const; diff --git a/src/processor/processor_gnss_fix.cpp b/src/processor/processor_gnss_fix.cpp index 68becb54e..a12da6a37 100644 --- a/src/processor/processor_gnss_fix.cpp +++ b/src/processor/processor_gnss_fix.cpp @@ -7,10 +7,10 @@ namespace wolf { -ProcessorGnssFix::ProcessorGnssFix(ProcessorParamsGnssFixPtr _params, SensorGnssPtr _sensor_gnss_ptr) : - ProcessorBase("GNSS FIX", _params), +ProcessorGnssFix::ProcessorGnssFix(ProcessorParamsGnssFixPtr _params_gnss, SensorGnssPtr _sensor_gnss_ptr) : + ProcessorBase("GNSS FIX", _params_gnss), sensor_gnss_ptr_(_sensor_gnss_ptr), - params_(_params), + params_gnss_(_params_gnss), first_capture_ptr_(nullptr) { // @@ -34,24 +34,24 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture_ptr) //std::cout << "WOLF: gnss fix in ENU coordinates: " << p_enu.transpose() << std::endl; FrameBasePtr new_frame_ptr = nullptr; - bool new_fac_created = false; + FactorBasePtr new_fac = nullptr; // ALREADY CREATED KF - PackKeyFramePtr KF_pack = buffer_pack_kf_.selectPack( _capture_ptr, params_->time_tolerance); + PackKeyFramePtr KF_pack = buffer_pack_kf_.selectPack( _capture_ptr, params_gnss_->time_tolerance); if (KF_pack && KF_pack->key_frame != last_gnss_fix_KF_) { + WOLF_DEBUG( "PR ", getName()," - capture ", _capture_ptr->id(), " appended to existing KF " , KF_pack->key_frame->id() , " TS: ", KF_pack->key_frame->getTimeStamp()); new_frame_ptr = KF_pack->key_frame; - //WOLF_DEBUG( "PR ", getName()," - capture ", _capture_ptr->id(), " appended to existing KF " , KF_pack->key_frame->id() , " TS: ", KF_pack->key_frame->getTimeStamp()); } // MAKE KF else if (voteForKeyFrame() && permittedKeyFrame()) { new_frame_ptr = getProblem()->emplaceFrame(KEY, _capture_ptr->getTimeStamp()); - getProblem()->keyFrameCallback(new_frame_ptr, shared_from_this(), params_->time_tolerance); - //WOLF_DEBUG( "PR ", getName()," - capture ", _capture_ptr->id(), " appended to new KF " , new_frame_ptr->id() , " TS: ", new_frame_ptr->getTimeStamp()); + getProblem()->keyFrameCallback(new_frame_ptr, shared_from_this(), params_gnss_->time_tolerance); + WOLF_DEBUG( "PR ", getName()," - capture ", _capture_ptr->id(), " appended to new KF " , new_frame_ptr->id() , " TS: ", new_frame_ptr->getTimeStamp()); } - // ESTABLISH CONSTRAINT + // ESTABLISH FACTOR if (new_frame_ptr) { // LINK CAPTURE @@ -62,7 +62,7 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture_ptr) auto ftr_ptr = FeatureBase::emplace<FeatureGnssFix>(last_capture_ptr_, last_capture_ptr_->getData(),last_capture_ptr_->getDataCovariance()); // EMPLACE FACTOR - auto new_fac = emplaceFactor(ftr_ptr); + new_fac = emplaceFactor(ftr_ptr); // outlier rejection if (sensor_gnss_ptr_->isEnuDefined() && sensor_gnss_ptr_->isEnuMapInitialized()) @@ -72,34 +72,34 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture_ptr) // store last KF if (new_fac) last_gnss_fix_KF_= new_frame_ptr; - - //WOLF_DEBUG("Factor established"); } // INITIALIZE ENU IF: // 1 - not initialized // 2 - factor established - if (!sensor_gnss_ptr_->isEnuDefined() && new_fac_created) + if (!sensor_gnss_ptr_->isEnuDefined() && new_fac != nullptr) { sensor_gnss_ptr_->setEcefEnu(last_capture_ptr_->getData()); - WOLF_INFO("ProcessorGnssFix: ECEF-ENU initialized.") + // Store the first capture that established a factor + first_capture_ptr_ = last_capture_ptr_; } - // INITIALIZE ENU_MAP IF 2 NECESSARY CONDITIONS: - // 1 - not initialized - // 2 - two factors established (first and current) - if (!sensor_gnss_ptr_->isEnuMapInitialized() && first_capture_ptr_ && new_fac_created) + // INITIALIZE ENU_MAP IF 4 NECESSARY CONDITIONS: + // 1 - ENU-ECEF defined + // 2 - not initialized + // 3 - current capture in key-frame with factor + // 4 - two factors established (first and current) in frames separated enough ( > enu_map_init_dist_min) + if ( sensor_gnss_ptr_->isEnuDefined() && + !sensor_gnss_ptr_->isEnuMapInitialized() && + new_fac != nullptr && + first_capture_ptr_->getFrame() != nullptr && last_capture_ptr_->getFrame() != nullptr && + (first_capture_ptr_->getFrame()->getState()-last_capture_ptr_->getFrame()->getState()).norm() > params_gnss_->enu_map_init_dist_min) { + std::cout << "initializing enu map\n"; sensor_gnss_ptr_->initializeEnuMap(first_capture_ptr_->getFrame()->getState(), first_capture_ptr_->getData(), last_capture_ptr_->getFrame()->getState(), last_capture_ptr_->getData()); - - WOLF_INFO("ProcessorGnssFix: ENU-MAP initialized.") } - - // Store the first capture that established a factor - if (new_fac_created && first_capture_ptr_ == nullptr) - first_capture_ptr_ = last_capture_ptr_; } FactorBasePtr ProcessorGnssFix::emplaceFactor(FeatureBasePtr& ftr_ptr) @@ -108,10 +108,10 @@ FactorBasePtr ProcessorGnssFix::emplaceFactor(FeatureBasePtr& ftr_ptr) //WOLF_DEBUG("creating the factor..."); // 2D if (getProblem()->getDim() == 2) - return FactorBase::emplace<FactorGnssFix2D>(ftr_ptr, ftr_ptr, sensor_gnss_ptr_, shared_from_this(), false); + return FactorBase::emplace<FactorGnssFix2D>(ftr_ptr, ftr_ptr, sensor_gnss_ptr_, shared_from_this(), false, FAC_ACTIVE); // 3D else - return FactorBase::emplace<FactorGnssFix3D>(ftr_ptr, ftr_ptr, sensor_gnss_ptr_, shared_from_this(), false); + return FactorBase::emplace<FactorGnssFix3D>(ftr_ptr, ftr_ptr, sensor_gnss_ptr_, shared_from_this(), false, FAC_ACTIVE); } bool ProcessorGnssFix::rejectOutlier(FactorBasePtr fac_ptr) @@ -147,7 +147,7 @@ bool ProcessorGnssFix::rejectOutlier(FactorBasePtr fac_ptr) bool ProcessorGnssFix::voteForKeyFrame() { // Depending on time since the last KF with gnssfix capture - if (!last_gnss_fix_KF_ || (last_capture_ptr_->getTimeStamp() - last_gnss_fix_KF_->getTimeStamp()) > params_->time_th) + if (!last_gnss_fix_KF_ || (last_capture_ptr_->getTimeStamp() - last_gnss_fix_KF_->getTimeStamp()) > params_gnss_->time_th) return true; // TODO: more alternatives? diff --git a/src/processor/processor_gnss_single_diff.cpp b/src/processor/processor_gnss_single_diff.cpp index 42b246c6c..c069eeb81 100644 --- a/src/processor/processor_gnss_single_diff.cpp +++ b/src/processor/processor_gnss_single_diff.cpp @@ -6,10 +6,10 @@ namespace wolf { -ProcessorGnssSingleDiff::ProcessorGnssSingleDiff(ProcessorParamsBasePtr _params, SensorGnssPtr _sensor_gnss_ptr) : - ProcessorBase("GNSS SINGLE DIFFERENCES", _params), +ProcessorGnssSingleDiff::ProcessorGnssSingleDiff(ProcessorParamsGnssSingleDiffPtr _params_gnss, SensorGnssPtr _sensor_gnss_ptr) : + ProcessorBase("GNSS SINGLE DIFFERENCES", _params_gnss), sensor_gnss_ptr_(_sensor_gnss_ptr), - params_(_params) + params_gnss_(_params_gnss) { // } @@ -24,13 +24,17 @@ void ProcessorGnssSingleDiff::processCapture(CaptureBasePtr _capture_ptr) // TODO: keep captures in a buffer and deal with KFpacks //WOLF_DEBUG("ProcessorGnssSingleDiff::process()"); - last_capture_ptr_ = std::static_pointer_cast<CaptureGnssSingleDiff>(_capture_ptr); + incoming_capture_ptr_ = std::static_pointer_cast<CaptureGnssSingleDiff>(_capture_ptr); + + // discard capture with null or non-key origin frame + if (incoming_capture_ptr_->getOriginFrame() == nullptr || incoming_capture_ptr_->getOriginFrame()->isKey()) + return; FrameBasePtr new_frame_ptr = nullptr; // ALREADY CREATED KF PackKeyFramePtr KF_pack = buffer_pack_kf_.selectPack( _capture_ptr, params_->time_tolerance); - if (KF_pack && KF_pack->key_frame != last_capture_ptr_->getOriginFrame()) + if (KF_pack && KF_pack->key_frame != incoming_capture_ptr_->getOriginFrame()) { new_frame_ptr = KF_pack->key_frame; WOLF_DEBUG( "PR ",getName()," - capture ", _capture_ptr->id(), " appended to existing KF " , KF_pack->key_frame->id() , " TS: ", KF_pack->key_frame->getTimeStamp()); @@ -51,7 +55,7 @@ void ProcessorGnssSingleDiff::processCapture(CaptureBasePtr _capture_ptr) // EMPLACE FEATURE //WOLF_DEBUG("emplacing feature..."); - auto ftr_ptr = FeatureBase::emplace<FeatureGnssSingleDiff>(last_capture_ptr_, last_capture_ptr_->getData(),last_capture_ptr_->getDataCovariance()); + auto ftr_ptr = FeatureBase::emplace<FeatureGnssSingleDiff>(incoming_capture_ptr_, incoming_capture_ptr_->getData(),incoming_capture_ptr_->getDataCovariance()); // ADD CONSTRAINT //WOLF_DEBUG("emplacing factor..."); @@ -59,6 +63,25 @@ void ProcessorGnssSingleDiff::processCapture(CaptureBasePtr _capture_ptr) //WOLF_DEBUG("Factor established"); } + + // INITIALIZE ENU_MAP IF 4 NECESSARY CONDITIONS: + // 1 - ENU-ECEF defined + // 2 - not initialized + // 3 - current capture in key-frame with factor + // 4 - frames constained by the factor separated enough ( > enu_map_init_dist_min) + if ( sensor_gnss_ptr_->isEnuDefined() && + !sensor_gnss_ptr_->isEnuMapInitialized() && + new_fac != nullptr && + incoming_capture_ptr_->getFrame() != nullptr && incoming_capture_ptr_->getFrame()->isKey() && + incoming_capture_ptr_->getData().norm() > params_gnss_->enu_map_init_dist_min) + { + std::cout << "initializing enu map\n"; + sensor_gnss_ptr_->initializeEnuMap(first_capture_ptr_->getFrame()->getState(), first_capture_ptr_->getData(), + last_capture_ptr_->getFrame()->getState(), last_capture_ptr_->getData()); + } + + last_capture_ptr_ = incoming_capture_ptr_; + incoming_capture_ptr_ = nullptr; } FactorBasePtr ProcessorGnssSingleDiff::emplaceFactor(FeatureBasePtr& ftr_ptr) @@ -67,7 +90,7 @@ FactorBasePtr ProcessorGnssSingleDiff::emplaceFactor(FeatureBasePtr& ftr_ptr) //WOLF_DEBUG("creating the factor..."); // 2D if (getProblem()->getDim() == 2) - return FactorBase::emplace<FactorGnssSingleDiff2D>(ftr_ptr, ftr_ptr, last_capture_ptr_->getOriginFrame(), sensor_gnss_ptr_, shared_from_this()); + return FactorBase::emplace<FactorGnssSingleDiff2D>(ftr_ptr, ftr_ptr, incoming_capture_ptr_->getOriginFrame(), sensor_gnss_ptr_, shared_from_this()); // 3D TODO else std::runtime_error("Single Differences in 3D not implemented yet."); @@ -77,8 +100,14 @@ FactorBasePtr ProcessorGnssSingleDiff::emplaceFactor(FeatureBasePtr& ftr_ptr) bool ProcessorGnssSingleDiff::voteForKeyFrame() { - // TODO - return true; + // Depending on time since the last KF with gnssfix capture + if (last_capture_ptr_==nullptr || (last_capture_ptr_->getTimeStamp() - incoming_capture_ptr_->getTimeStamp()) > params_gnss_->time_th) + return true; + + // TODO: more alternatives? + + // otherwise + return false; } void ProcessorGnssSingleDiff::configure(SensorBasePtr _sensor) @@ -88,7 +117,7 @@ void ProcessorGnssSingleDiff::configure(SensorBasePtr _sensor) ProcessorBasePtr ProcessorGnssSingleDiff::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr) { - ProcessorGnssSingleDiffPtr prc_ptr = std::make_shared<ProcessorGnssSingleDiff>(_params, std::static_pointer_cast<SensorGnss>(sensor_ptr)); + ProcessorGnssSingleDiffPtr prc_ptr = std::make_shared<ProcessorGnssSingleDiff>(std::static_pointer_cast<ProcessorParamsGnssSingleDiff>(_params), std::static_pointer_cast<SensorGnss>(sensor_ptr)); prc_ptr->setName(_unique_name); return prc_ptr; } diff --git a/src/sensor/sensor_gnss.cpp b/src/sensor/sensor_gnss.cpp index 1c61c896f..16f2ca347 100644 --- a/src/sensor/sensor_gnss.cpp +++ b/src/sensor/sensor_gnss.cpp @@ -12,9 +12,11 @@ static Scalar kSecondEccentricitySquared = 6.73949674228 * 0.001; //static Scalar kFlattening = 1 / 298.257223563; SensorGnss::SensorGnss(StateBlockPtr _p_ptr, //GNSS antena 3D position with respect to the car frame (base frame) - StateBlockPtr _bias_ptr) //GNSS sensor time bias + StateBlockPtr _bias_ptr, //GNSS sensor time bias + IntrinsicsGnssPtr params) //sensor params : SensorBase("GNSS", _p_ptr, nullptr, _bias_ptr, 0), // antena orientation has no sense in GNSS + params_(params), ENU_defined_(false), ENU_MAP_initialized_(false) { @@ -22,20 +24,22 @@ SensorGnss::SensorGnss(StateBlockPtr _p_ptr, //GNSS antena 3D positi assert((_bias_ptr == nullptr || _bias_ptr->getSize() == 1 )&& "Bad bias size"); getStateBlockVec().resize(7); - setStateBlockPtrStatic(3, std::make_shared<StateBlock>(3, true)); - setStateBlockPtrStatic(4, std::make_shared<StateBlock>(1, true)); - setStateBlockPtrStatic(5, std::make_shared<StateBlock>(1, true)); - setStateBlockPtrStatic(6, std::make_shared<StateBlock>(1, true)); + setStateBlockPtrStatic(3, std::make_shared<StateBlock>(3, params_->translation_fixed_)); + setStateBlockPtrStatic(4, std::make_shared<StateBlock>(1, params_->roll_fixed_)); + setStateBlockPtrStatic(5, std::make_shared<StateBlock>(1, params_->pitch_fixed_)); + setStateBlockPtrStatic(6, std::make_shared<StateBlock>(1, params_->yaw_fixed_)); } SensorGnss::SensorGnss(StateBlockPtr _p_ptr, //GNSS sensor position with respect to the car frame (base frame) StateBlockPtr _bias_ptr, //GNSS sensor time bias + IntrinsicsGnssPtr params, //sensor params StateBlockPtr _t_ENU_MAP_ptr, //ENU_MAP translation StateBlockPtr _roll_ENU_MAP_ptr, //ENU_MAP Roll StateBlockPtr _pitch_ENU_MAP_ptr, //ENU_MAP Pitch StateBlockPtr _yaw_ENU_MAP_ptr) //ENU_MAP Yaw : SensorBase("GNSS", _p_ptr, nullptr, _bias_ptr, 0),// antena orientation has no sense in GNSS + params_(params), ENU_defined_(false), ENU_MAP_initialized_(false) { @@ -105,6 +109,8 @@ void SensorGnss::setEcefEnu(const Eigen::Vector3s& _t_ECEF_ENU) { computeEnuEcef(_t_ECEF_ENU, R_ENU_ECEF_, t_ENU_ECEF_); ENU_defined_ = true; + + WOLF_INFO("SensorGnss: ECEF-ENU initialized.") } void SensorGnss::setEnuEcef(const Eigen::Matrix3s& _R_ENU_ECEF, const Eigen::Vector3s& _t_ENU_ECEF) @@ -145,7 +151,90 @@ void SensorGnss::initializeEnuMap(const Eigen::VectorXs& _pose_MAP_frameENU, con getEnuMapPitchState()->getState()(0), getEnuMapYawState()->getState()(0)).topLeftCorner<2,2>(); t_ENU_MAP.head<2>() = -R_ENU_MAP * t_MAP_antenaENU; - this->getEnuMapTranslationState()->setState(t_ENU_MAP); + + // set translation state + setEnuMapTranslationState(t_ENU_MAP); + + //std::cout << "-----------------------------------------\n"; + //std::cout << "t_ECEF_antenaENU: " << _t_ECEF_antenaENU.transpose() << std::endl; + //std::cout << "t_ECEF_antena2: " << _t_ECEF_antena2.transpose() << std::endl; + //std::cout << "v_ENU: " << v_ENU.transpose() << std::endl; + //std::cout << "theta_ENU: " << theta_ENU << std::endl; + //std::cout << "t_MAP_antena1: " << t_MAP_antenaENU.transpose() << std::endl; + //std::cout << "t_MAP_antena2: " << t_MAP_antena2.transpose() << std::endl; + //std::cout << "v_MAP: " << v_MAP.transpose() << std::endl; + //std::cout << "theta_MAP: " << theta_MAP << std::endl; + //std::cout << "yaw set: " << yaw << std::endl; + //std::cout << "t_ENU1_origin: " << t_ENU_MAP.transpose() << std::endl; + //std::cout << "-----checks\n"; + //std::cout << "R(-yaw) * v_ENU: " << (Eigen::Rotation2D<Scalar>(-yaw) * v_ENU.head<2>()).transpose() << std::endl; + //std::cout << "should be: " << v_MAP.transpose() << std::endl; + //std::cout << "atan2(R(-yaw) * v_ENU): " << atan2((Eigen::Rotation2D<Scalar>(-yaw) * v_ENU.head<2>())(1), (Eigen::Rotation2D<Scalar>(-yaw) * v_ENU.head<2>())(0)) << std::endl; + //std::cout << "should be: " << atan2(v_MAP(1),v_MAP(0)) << std::endl; + //std::cout << "R(yaw) * v_MAP: " << (Eigen::Rotation2D<Scalar>(yaw) * v_MAP).transpose() << std::endl; + //std::cout << "should be: " << v_ENU.head<2>().transpose() << std::endl; + //std::cout << "atan2(R(yaw) * v_MAP): " << atan2((Eigen::Rotation2D<Scalar>(yaw) * v_MAP)(1),(Eigen::Rotation2D<Scalar>(yaw) * v_MAP)(0)) << std::endl; + //std::cout << "should be: " << atan2(v_ENU(1),v_ENU(0)) << std::endl; + //std::cout << "v_ENU.norm(): " << v_ENU.norm() << std::endl; + //std::cout << "v_MAP.norm(): " << v_MAP.norm() << std::endl; + + WOLF_INFO("SensorGnss: ENU-MAP initialized.") + } + // 3D + else + { + //TODO + std::runtime_error("not implemented yet"); + } +} + +void SensorGnss::initializeEnuMapYaw(const Eigen::VectorXs& _pose_MAP_frame1, const Eigen::VectorXs& _pose_MAP_frame2, + const Eigen::Vector3s& _v_ECEF_antena1_antena2) +{ + assert(ENU_defined_ && "initializing ENU-MAP yaw without ENU defined"); + + Eigen::Vector3s t_MAP_antena1 = _pose_MAP_frame1.head<2>() + Eigen::Rotation2D<Scalar>(_pose_MAP_frame1(2)) * getP()->getState().head<2>(); + Eigen::Vector3s t_MAP_antena2 = _pose_MAP_frame2.head<2>() + Eigen::Rotation2D<Scalar>(_pose_MAP_frame2(2)) * getP()->getState().head<2>(); + Eigen::Vector3s v_MAP_antena1_antena2 = t_MAP_antena2 - t_MAP_antena1; + Eigen::Vector3s v_ENU_antena1_antena2 = R_ENU_ECEF_ * (_v_ECEF_antena1_antena2); + + Scalar theta_ENU = atan2(v_ENU_antena1_antena2(1),v_ENU_antena1_antena2(0)); + Scalar theta_MAP = atan2(v_MAP_antena1_antena2(1),v_MAP_antena1_antena2(0)); + Scalar yaw_ENU_MAP = theta_ENU-theta_MAP; + + this->getEnuMapYawState()->setState(Eigen::Vector1s(yaw_ENU_MAP)); + + Eigen::Matrix3s R_ENU_ECEF;//, R_ENU2_ECEF; + Eigen::Vector3s t_ENU_ECEF;//, t_ENU2_ECEF; + computeEnuEcef(_t_ECEF_antenaENU, R_ENU_ECEF, t_ENU_ECEF); + //computeENU_ECEF(_t_ECEF_antena2, R_ENU2_ECEF, t_ENU2_ECEF); + + // compute fix vector (from 1 to 2) in ENU coordinates + Eigen::Vector3s v_ENU = R_ENU_ECEF * (_t_ECEF_antena2 - _t_ECEF_antenaENU); + + // 2D + if (getProblem()->getDim() == 2) + { + // compute antena vector (from 1 to 2) in MAP + Eigen::Vector2s t_MAP_antenaENU = _pose_MAP_frameENU.head<2>() + Eigen::Rotation2D<Scalar>(_pose_MAP_frameENU(2)) * getP()->getState().head<2>(); + Eigen::Vector2s t_MAP_antena2 = _pose_MAP_frame2.head<2>() + Eigen::Rotation2D<Scalar>(_pose_MAP_frame2(2)) * getP()->getState().head<2>(); + Eigen::Vector2s v_MAP = t_MAP_antena2 - t_MAP_antenaENU; + + // initialize yaw + Scalar theta_ENU = atan2(v_ENU(1),v_ENU(0)); + Scalar theta_MAP = atan2(v_MAP(1),v_MAP(0)); + Scalar yaw = theta_ENU-theta_MAP; + this->getEnuMapYawState()->setState(Eigen::Vector1s(yaw)); + + // initialize translation + Eigen::Vector3s t_ENU_MAP(Eigen::Vector3s::Zero()); + Eigen::Matrix2s R_ENU_MAP = computeREnuMap(getEnuMapRollState()->getState()(0), + getEnuMapPitchState()->getState()(0), + getEnuMapYawState()->getState()(0)).topLeftCorner<2,2>(); + t_ENU_MAP.head<2>() = -R_ENU_MAP * t_MAP_antenaENU; + + // set translation state + setEnuMapTranslationState(t_ENU_MAP); //std::cout << "-----------------------------------------\n"; //std::cout << "t_ECEF_antenaENU: " << _t_ECEF_antenaENU.transpose() << std::endl; @@ -169,6 +258,8 @@ void SensorGnss::initializeEnuMap(const Eigen::VectorXs& _pose_MAP_frameENU, con //std::cout << "should be: " << atan2(v_ENU(1),v_ENU(0)) << std::endl; //std::cout << "v_ENU.norm(): " << v_ENU.norm() << std::endl; //std::cout << "v_MAP.norm(): " << v_MAP.norm() << std::endl; + + WOLF_INFO("SensorGnss: ENU-MAP initialized.") } // 3D else @@ -176,6 +267,11 @@ void SensorGnss::initializeEnuMap(const Eigen::VectorXs& _pose_MAP_frameENU, con //TODO std::runtime_error("not implemented yet"); } +} + +void SensorGnss::setEnuMapTranslationState(const Eigen::Vector3s& t_ENU_MAP) +{ + this->getEnuMapTranslationState()->setState(t_ENU_MAP); ENU_MAP_initialized_ = true; } @@ -191,7 +287,7 @@ SensorBasePtr SensorGnss::create(const std::string& _unique_name, const Eigen::V StateBlockPtr p_ptr = std::make_shared<StateBlock>(_extrinsics.head(3), intrinsics_gnss_ptr->extrinsics_fixed_); if (_extrinsics.size() == 3) - sen = std::make_shared<SensorGnss>(p_ptr, nullptr); + sen = std::make_shared<SensorGnss>(p_ptr, nullptr, intrinsics_gnss_ptr); else { StateBlockPtr p_ptr = std::make_shared<StateBlock>(_extrinsics.head<3>(), intrinsics_gnss_ptr->extrinsics_fixed_); @@ -200,7 +296,7 @@ SensorBasePtr SensorGnss::create(const std::string& _unique_name, const Eigen::V StateBlockPtr pitch_ptr = std::make_shared<StateBlock>(_extrinsics.segment<1>(7), intrinsics_gnss_ptr->pitch_fixed_); StateBlockPtr yaw_ptr = std::make_shared<StateBlock>(_extrinsics.segment<1>(8), intrinsics_gnss_ptr->yaw_fixed_); - sen = std::make_shared<SensorGnss>(p_ptr, nullptr, translation_ptr, roll_ptr, pitch_ptr, yaw_ptr); + sen = std::make_shared<SensorGnss>(p_ptr, nullptr, intrinsics_gnss_ptr, translation_ptr, roll_ptr, pitch_ptr, yaw_ptr); } sen->setName(_unique_name); diff --git a/test/gtest_factor_gnss_fix_2D.cpp b/test/gtest_factor_gnss_fix_2D.cpp index bf783d30e..1f3330a89 100644 --- a/test/gtest_factor_gnss_fix_2D.cpp +++ b/test/gtest_factor_gnss_fix_2D.cpp @@ -100,7 +100,7 @@ TEST(FactorGnssFix2DTest, configure_tree) ceres_mgr_ptr->getSolverOptions().max_num_iterations = 100; // Configure sensor and processor - gnss_sensor_ptr->getEnuMapTranslationState()->setState(t_enu_map); + gnss_sensor_ptr->setEnuMapTranslationState(t_enu_map); gnss_sensor_ptr->getEnuMapYawState()->setState(o_enu_map); gnss_sensor_ptr->getEnuMapRollState()->fix(); gnss_sensor_ptr->getEnuMapPitchState()->fix(); @@ -117,8 +117,8 @@ TEST(FactorGnssFix2DTest, configure_tree) problem_ptr->keyFrameCallback(frame_ptr, nullptr, 1.0); // Create & process GNSS Fix capture - // CaptureGnssFixPtr cap_gnss_ptr = std::make_shared<CaptureGnssFix>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena, 1e-3*Matrix3s::Identity()); - CaptureGnssFixPtr cap_gnss_ptr = std::static_pointer_cast<CaptureGnssFix>(CaptureBase::emplace<CaptureGnssFix>(nullptr, TimeStamp(0), gnss_sensor_ptr, t_ecef_antena, 1e-3*Matrix3s::Identity())); + auto cap = CaptureBase::emplace<CaptureGnssFix>(nullptr, TimeStamp(0), gnss_sensor_ptr, t_ecef_antena, 1e-3*Matrix3s::Identity()); + CaptureGnssFixPtr cap_gnss_ptr = std::static_pointer_cast<CaptureGnssFix>(cap); gnss_sensor_ptr->process(cap_gnss_ptr); // Checks -- GitLab