diff --git a/include/gnss/capture/capture_gnss_fix.h b/include/gnss/capture/capture_gnss_fix.h index c894d3e7f887ee2deaed7241f9a3fc819f0e8ace..cfbf8900d360798b371e7982bb3084f0d6979ed1 100644 --- a/include/gnss/capture/capture_gnss_fix.h +++ b/include/gnss/capture/capture_gnss_fix.h @@ -37,38 +37,66 @@ WOLF_PTR_TYPEDEFS(CaptureGnssFix); class CaptureGnssFix : public CaptureBase { protected: - Eigen::Vector3d position_ECEF_; ///< position in ECEF coordinates. - Eigen::Matrix3d covariance_ECEF_; ///< Noise of the fix in ECEF coordinates. + Eigen::Vector4d fix_ECEF_; ///< position in ECEF coordinates and clock_bias. + Eigen::Matrix4d covariance_ECEF_; ///< Noise of the fix in ECEF coordinates. TimeStamp ts_GPST_; ///< Time stamp in GPS time public: - CaptureGnssFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::Vector3d& _position, const Eigen::Matrix3d& _covariance, bool _ecef_coordinates = true); + CaptureGnssFix(const TimeStamp& _ts, + SensorBasePtr _sensor_ptr, + const Eigen::Vector4d& _fix, + const Eigen::Matrix4d& _covariance, + bool _ecef_coordinates = true); ~CaptureGnssFix() override; - const Eigen::Vector3d& getPositionEcef() const; - const Eigen::Matrix3d& getCovarianceEcef() const; + Eigen::Vector4d getFixEcef() const; + Eigen::Matrix4d getFixCovarianceEcef() const; + double getClockBias() const; + double getClockBiasVariance() const; + Eigen::Vector3d getPositionEcef() const; + Eigen::Matrix3d getPositionCovarianceEcef() const; void getPositionAndCovarianceEcef(Eigen::Vector3d& position, Eigen::Matrix3d& cov) const; - const TimeStamp& getGpsTimeStamp() const; + TimeStamp getGpsTimeStamp() const; void setGpsTimeStamp(const TimeStamp& _ts_GPST); }; -inline const Eigen::Vector3d& CaptureGnssFix::getPositionEcef() const +inline Eigen::Vector4d CaptureGnssFix::getFixEcef() const { - return position_ECEF_; + return fix_ECEF_; } -inline const Eigen::Matrix3d& CaptureGnssFix::getCovarianceEcef() const +inline Eigen::Matrix4d CaptureGnssFix::getFixCovarianceEcef() const { return covariance_ECEF_; } +inline double CaptureGnssFix::getClockBias() const +{ + return fix_ECEF_(3); +} + +inline double CaptureGnssFix::getClockBiasVariance() const +{ + return covariance_ECEF_(3,3); +} + +inline Eigen::Vector3d CaptureGnssFix::getPositionEcef() const +{ + return fix_ECEF_.head<3>(); +} + +inline Eigen::Matrix3d CaptureGnssFix::getPositionCovarianceEcef() const +{ + return covariance_ECEF_.topLeftCorner<3,3>(); +} + inline void CaptureGnssFix::getPositionAndCovarianceEcef(Eigen::Vector3d& position, Eigen::Matrix3d& cov) const { - position = position_ECEF_; - cov = covariance_ECEF_; + position = fix_ECEF_.head<3>(); + cov = covariance_ECEF_.topLeftCorner<3,3>(); } -inline const wolf::TimeStamp& CaptureGnssFix::getGpsTimeStamp() const +inline wolf::TimeStamp CaptureGnssFix::getGpsTimeStamp() const { return ts_GPST_; } diff --git a/include/gnss/factor/factor_gnss_fix_2d.h b/include/gnss/factor/factor_gnss_fix_2d.h index 26a8735be406bfdadb7c62f52dd0c3507adf21ed..0a59456b207a4fa63dc7cd84a70748b72257516e 100644 --- a/include/gnss/factor/factor_gnss_fix_2d.h +++ b/include/gnss/factor/factor_gnss_fix_2d.h @@ -96,7 +96,6 @@ inline bool FactorGnssFix2d::operator ()(const T* const _x, Eigen::Matrix<T,3,1> t_ENU_ECEF = sensor_gnss_ptr_->gettEnuEcef().cast<T>(); Eigen::Matrix<T,2,3> R_map_ENU = sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]).transpose().topRows(2); Eigen::Map<const Eigen::Matrix<T,3,1> > t_ENU_map(_t_ENU_map); - Eigen::Matrix<T,3,1> fix_ECEF = getMeasurement().cast<T>(); //std::cout << "computeREnuMap():\n" << sensor_gnss_ptr_->computeREnuMap((const double) _roll[0], (const double) _pitch[0], (const double) _yaw[0]).transpose().topRows(2) << std::endl; //std::cout << "getREnuEcef():\n" << sensor_gnss_ptr_->gettEnuEcef() << std::endl; @@ -106,7 +105,7 @@ inline bool FactorGnssFix2d::operator ()(const T* const _x, //std::cout << "R_map_ENU:\n\t" << R_map_ENU(0,0) << "\t" << R_map_ENU(0,1) << "\t" << R_map_ENU(0,2) << "\n\t" << R_map_ENU(1,0) << "\t" << R_map_ENU(1,1) << "\t" << R_map_ENU(1,2) << std::endl; //std::cout << "R_ENU_ECEF:\n\t" << R_ENU_ECEF(0,0) << "\t" << R_ENU_ECEF(0,1) << "\t" << R_ENU_ECEF(0,2) << "\n\t" << R_ENU_ECEF(1,0) << "\t" << R_ENU_ECEF(1,1) << "\t" << R_ENU_ECEF(1,2) << "\n\t" << R_ENU_ECEF(2,0) << "\t" << R_ENU_ECEF(2,1) << "\t" << R_ENU_ECEF(2,2) << std::endl; //std::cout << "t_ENU_ECEF:\n\t" << t_ENU_ECEF(0) << "\n\t" << t_ENU_ECEF(1) << "\n\t" << t_ENU_ECEF(2) << std::endl; - Eigen::Matrix<T,2,1> fix_map = R_map_ENU * (R_ENU_ECEF * fix_ECEF + t_ENU_ECEF - t_ENU_map); + Eigen::Matrix<T,2,1> fix_map = R_map_ENU * (R_ENU_ECEF * getMeasurement().head<3>() + t_ENU_ECEF - t_ENU_map); //std::cout << "fix 3d:\n\t" << getMeasurement()(0) << "\n\t" << getMeasurement()(1) << "\n\t" << getMeasurement()(2) << std::endl; //std::cout << "fix_map:\n\t" << fix_map[0] << "\n\t" << fix_map[1] << std::endl; @@ -117,7 +116,7 @@ inline bool FactorGnssFix2d::operator ()(const T* const _x, // Compute residual rotating information matrix to 2d Map coordinates // Covariance & information are rotated by R*S*R' so for the squred root upper (or right) R*L*U*R'->U*R' // In this case R = R_map_ENU * R_ENU_ECEF, then R' = R_ENU_ECEF' * R_map_ENU' - residuals_ECEF = (getMeasurementSquareRootInformationUpper().cast<T>() * R_ENU_ECEF.transpose() * R_map_ENU.transpose()) * (antena_map - fix_map); + residuals_ECEF = (getMeasurementSquareRootInformationUpper().topLeftCorner<3,3>() * R_ENU_ECEF.transpose() * R_map_ENU.transpose()) * (antena_map - fix_map); //std::cout << "residuals_ECEF:\n\t" << _residuals[0] << "\n\t" << _residuals[1] << "\n\t" << _residuals[2]<< std::endl; return true; diff --git a/include/gnss/factor/factor_gnss_fix_3d.h b/include/gnss/factor/factor_gnss_fix_3d.h index 650a1055f52b804bccf1f46352b71b3819a56721..c3b3929e94fedc5db62a509bdb1e5cf826f86978 100644 --- a/include/gnss/factor/factor_gnss_fix_3d.h +++ b/include/gnss/factor/factor_gnss_fix_3d.h @@ -95,10 +95,9 @@ inline bool FactorGnssFix3d::operator ()(const T* const _x, Eigen::Matrix<T,3,1> t_ENU_ECEF = sensor_gnss_ptr_->gettEnuEcef().cast<T>(); Eigen::Matrix<T,3,3> R_map_ENU = sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]).transpose(); Eigen::Map<const Eigen::Matrix<T,3,1> > t_ENU_map(_t_ENU_map); - Eigen::Matrix<T,3,1> fix_ECEF = getMeasurement().cast<T>(); // Transform ECEF 3d Fix Feature to Map coordinates - Eigen::Matrix<T,3,1> fix_map = R_map_ENU * (R_ENU_ECEF * fix_ECEF + t_ENU_ECEF - t_ENU_map); + Eigen::Matrix<T,3,1> fix_map = R_map_ENU * (R_ENU_ECEF * getMeasurement().head<3>() + t_ENU_ECEF - t_ENU_map); // Antena position in Map coordinates Eigen::Matrix<T,3,1> antena_map = q_map_base * t_base_antena + t_map_base; @@ -106,7 +105,7 @@ inline bool FactorGnssFix3d::operator ()(const T* const _x, // Compute residual rotating information matrix in Map coordinates // Covariance & information are rotated by R*S*R' so for the squred root upper (or right) R*L*U*R'->U*R' // In this case R = R_map_ENU * R_ENU_ECEF, then R' = R_ENU_ECEF' * R_map_ENU' - residuals_ECEF = (getMeasurementSquareRootInformationUpper().cast<T>() * R_ENU_ECEF.transpose() * R_map_ENU.transpose()) * (antena_map - fix_map); + residuals_ECEF = (getMeasurementSquareRootInformationUpper().topLeftCorner<3,3>() * R_ENU_ECEF.transpose() * R_map_ENU.transpose()) * (antena_map - fix_map); return true; } diff --git a/include/gnss/factor/factor_gnss_fix_3d_with_clock.h b/include/gnss/factor/factor_gnss_fix_3d_with_clock.h new file mode 100644 index 0000000000000000000000000000000000000000..fe582423e8f35cc62a928caea6ba3bf74a8a038f --- /dev/null +++ b/include/gnss/factor/factor_gnss_fix_3d_with_clock.h @@ -0,0 +1,116 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see <http://www.gnu.org/licenses/>. +// +//--------LICENSE_END-------- + +#pragma once + +//Wolf includes +#include "core/common/wolf.h" +#include "core/factor/factor_autodiff.h" +#include "core/frame/frame_base.h" + +namespace wolf { + +WOLF_PTR_TYPEDEFS(FactorGnssFix3dWithClock); + +class FactorGnssFix3dWithClock : public FactorAutodiff<FactorGnssFix3dWithClock, 4, 3, 4, 3, 3, 1, 1, 1, 1> +{ + protected: + SensorGnssPtr sensor_gnss_ptr_; + + public: + + FactorGnssFix3dWithClock(FeatureBasePtr& _ftr_ptr, + const SensorGnssPtr& _sensor_gnss_ptr, + const ProcessorBasePtr& _processor_ptr, + bool _apply_loss_function, + FactorStatus _status = FAC_ACTIVE) : + FactorAutodiff("FactorGnssFix3dWithClock", + TOP_ABS, + _ftr_ptr, + nullptr, + nullptr, + nullptr, + nullptr, + _processor_ptr, + _apply_loss_function, + _status, + _ftr_ptr->getFrame()->getP(), + _ftr_ptr->getFrame()->getO(), + _sensor_gnss_ptr->getP(), + _sensor_gnss_ptr->getEnuMapTranslation(), + _sensor_gnss_ptr->getEnuMapRoll(), + _sensor_gnss_ptr->getEnuMapPitch(), + _sensor_gnss_ptr->getEnuMapYaw(), + _ftr_ptr->getCapture()->getStateBlock('T')), + sensor_gnss_ptr_(_sensor_gnss_ptr) + { + WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating an ACTIVE GNSS Fix 3d factor without initializing ENU"); + } + + ~FactorGnssFix3dWithClock() override = default; + + template<typename T> + bool operator ()(const T* const _x, + const T* const _o, + const T* const _x_antena, + const T* const _t_ENU_map, + const T* const _roll_ENU_map, + const T* const _pitch_ENU_map, + const T* const _yaw_ENU_map, + const T* const _clock_bias, + T* _residuals) const; + +}; + +template<typename T> +inline bool FactorGnssFix3dWithClock::operator ()(const T* const _x, + const T* const _o, + const T* const _x_antena, + const T* const _t_ENU_map, + const T* const _roll_ENU_map, + const T* const _pitch_ENU_map, + const T* const _yaw_ENU_map, + const T* const _clock_bias, + T* _residuals) const +{ + Eigen::Map<const Eigen::Matrix<T,3,1> > t_map_base(_x); + Eigen::Map<const Eigen::Quaternion<T> > q_map_base(_o); + Eigen::Map<const Eigen::Matrix<T,3,1> > t_base_antena(_x_antena); + Eigen::Map<Eigen::Matrix<T,4,1> > residuals(_residuals); + + Eigen::Matrix<T,3,3> R_ECEF_ENU = sensor_gnss_ptr_->getREnuEcef().cast<T>().transpose(); + Eigen::Matrix<T,3,1> t_ENU_ECEF = sensor_gnss_ptr_->gettEnuEcef().cast<T>(); + Eigen::Matrix<T,3,3> R_ENU_map = sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]); + Eigen::Map<const Eigen::Matrix<T,3,1> > t_ENU_map(_t_ENU_map); + + // Expected antena position in ECEF coordinates + Eigen::Matrix<T,4,1> fix_expected; + fix_expected.head<3>() = R_ECEF_ENU * (R_ENU_map * (q_map_base * t_base_antena + t_map_base) + t_ENU_map - t_ENU_ECEF); + fix_expected(3) = _clock_bias; + + // Compute residual + residuals_ECEF = getMeasurementSquareRootInformationUpper() * (antena_ecef - getMeasurement()); + + return true; +} + +} // namespace wolf \ No newline at end of file diff --git a/include/gnss/feature/feature_gnss_fix.h b/include/gnss/feature/feature_gnss_fix.h index 60f8debf65e8e60e3d58703611cf7e3f42b5bc84..37d2acf42869fbfc67eed01fe316d9fe2cc96db3 100644 --- a/include/gnss/feature/feature_gnss_fix.h +++ b/include/gnss/feature/feature_gnss_fix.h @@ -49,12 +49,14 @@ class FeatureGnssFix : public FeatureBase ~FeatureGnssFix() override{}; private: - GnssUtils::ComputePosOutput gnss_fix_output_; + GnssUtils::ComputePosOutput gnss_fix_output_; }; inline FeatureGnssFix::FeatureGnssFix(const GnssUtils::ComputePosOutput& _gnss_fix_output) : - FeatureBase("FeatureGnssFix", _gnss_fix_output.pos, _gnss_fix_output.pos_covar), + FeatureBase("FeatureGnssFix", + (Eigen::Vector4d() << _gnss_fix_output.pos, _gnss_fix_output.rcv_bias(0)).finished(), + _gnss_fix_output.sol_cov), gnss_fix_output_(_gnss_fix_output) { // diff --git a/src/capture/capture_gnss_fix.cpp b/src/capture/capture_gnss_fix.cpp index 5ddd2bb8a87e12e512a92a7313576f6893463ec3..50a2fa24b3b403170285ea78edf3cbe19e48ab29 100644 --- a/src/capture/capture_gnss_fix.cpp +++ b/src/capture/capture_gnss_fix.cpp @@ -20,16 +20,42 @@ // //--------LICENSE_END-------- #include "gnss/capture/capture_gnss_fix.h" +#include <core/state_block/state_block_derived.h> namespace wolf { -CaptureGnssFix::CaptureGnssFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::Vector3d& _position, const Eigen::Matrix3d& _covariance, bool _ecef_coordinates) : +CaptureGnssFix::CaptureGnssFix(const TimeStamp& _ts, + SensorBasePtr _sensor_ptr, + const Eigen::Vector4d& _fix, + const Eigen::Matrix4d& _covariance, + bool _ecef_coordinates) : CaptureBase("CaptureGnssFix", _ts, _sensor_ptr), - position_ECEF_(_ecef_coordinates ? _position : GnssUtils::latLonAltToEcef(_position)), - covariance_ECEF_(_ecef_coordinates ?_covariance : GnssUtils::enuToEcefCov(_position, _covariance)) + fix_ECEF_(_fix), + covariance_ECEF_(_covariance) { - // + // Convert to ECEF from latlonalt + if (not _ecef_coordinates) + { + fix_ECEF_.head<3>() = GnssUtils::latLonAltToEcef(_fix.head<3>()); + covariance_ECEF_.topLeftCorner<3,3>() = GnssUtils::enuToEcefCov(_fix.head<3>(), + _covariance.topLeftCorner<3,3>()); + } + + // Clock bias + assert(_sensor_ptr->getStateBlock('T') != nullptr and _sensor_ptr->isStateBlockDynamic('T')); + addStateBlock('T', std::make_shared<StateParams1>(Vector1d(),true), nullptr); + + // interconstellation clock bias + assert(_sensor_ptr->getStateBlock('G') != nullptr); + if(_sensor_ptr->isStateBlockDynamic('G')) + addStateBlock('G', std::make_shared<StateParams1>(Vector1d(),true), nullptr); + assert(_sensor_ptr->getStateBlock('E') != nullptr); + if(_sensor_ptr->isStateBlockDynamic('E')) + addStateBlock('E', std::make_shared<StateParams1>(Vector1d(),true), nullptr); + assert(_sensor_ptr->getStateBlock('M') != nullptr); + if(_sensor_ptr->isStateBlockDynamic('M')) + addStateBlock('M', std::make_shared<StateParams1>(Vector1d(),true), nullptr); } CaptureGnssFix::~CaptureGnssFix() diff --git a/src/processor/processor_gnss_fix.cpp b/src/processor/processor_gnss_fix.cpp index 6066c1f0aea1e80fbfb698fda952de48cd9bf872..de9eed7707736db191d2360ca619b765208934cf 100644 --- a/src/processor/processor_gnss_fix.cpp +++ b/src/processor/processor_gnss_fix.cpp @@ -139,12 +139,14 @@ FeatureBasePtr ProcessorGnssFix::emplaceFeature(CaptureBasePtr _capture) { WOLF_DEBUG("ProcessorGnssFix: creating feature from incoming navsatfix. Type:", _capture->getType()); auto fix_capture = std::static_pointer_cast<CaptureGnssFix>(_capture); - pos_output.time = fix_capture->getGpsTimeStamp().getSeconds(); // time_t time; - pos_output.sec = 1e-9 * fix_capture->getGpsTimeStamp().getSeconds(); // double sec; - pos_output.pos = fix_capture->getPositionEcef(); // Eigen::Vector3d pos; // position (m) - pos_output.vel = Eigen::Vector3d::Zero(); // Eigen::Vector3d vel; // velocity (m/s) - pos_output.pos_covar = fix_capture->getCovarianceEcef(); // Eigen::Matrix3d pos_covar; // position covariance (m^2) - pos_output.type = 0; // int type; // coordinates used (0:xyz-ecef,1:enu-baseline) + pos_output.time = fix_capture->getGpsTimeStamp().getSeconds(); // time_t + pos_output.sec = 1e-9 * fix_capture->getGpsTimeStamp().getSeconds(); // double + pos_output.pos = fix_capture->getPositionEcef(); // Vector3d - position (m) + pos_output.vel = Eigen::Vector3d::Zero(); // Vector3d - velocity (m/s) + pos_output.sol_cov = fix_capture->getFixCovarianceEcef(); // Matrix4d - solution covariance [x, y, z, dt] (m², s²) + pos_output.rcv_bias(0) = fix_capture->getClockBias(); // Vector4d - receiver clock bias to time systems (s) 0: receiver clock bias (s), 1: glo-gps time offset (s), 2: gal-gps time offset (s), 3: bds-gps time offset (s) + pos_output.rcv_bias_var(0) = fix_capture->getClockBiasVariance(); // Matrix4d - receiver clock bias variances 0: receiver clock bias (s), 1: glo-gps time offset (s), 2: gal-gps time offset (s), 3: bds-gps time offset (s) + pos_output.type = 0; // int - coordinates used (0:xyz-ecef,1:enu-baseline) } else throw std::runtime_error("ProcessorGnssFix::emplaceFeature wrong capture type"); @@ -238,7 +240,7 @@ void ProcessorGnssFix::handleEnuMap(FeatureBasePtr feature) if (!sensor_gnss_->isEnuDefined()) { WOLF_INFO("Defining ecef enu with first fix"); - sensor_gnss_->setEcefEnu(feature->getMeasurement()); + sensor_gnss_->setEcefEnu(feature->getMeasurement().head<3>()); } // Store the first capture that established a factor (for later initialization ENU-MAP) @@ -246,7 +248,7 @@ void ProcessorGnssFix::handleEnuMap(FeatureBasePtr feature) not sensor_gnss_->isEnuMapFixed()) { first_frame_state_ = feature->getCapture()->getFrame()->getState(); - first_pos_ = feature->getMeasurement(); + first_pos_ = feature->getMeasurement().head<3>(); } // Initialize ENU-MAP if: ENU defined and ENU-MAP not initialized (and not fixed) and far enough @@ -255,7 +257,7 @@ void ProcessorGnssFix::handleEnuMap(FeatureBasePtr feature) sensor_gnss_->isEnuDefined() and not sensor_gnss_->isEnuMapInitialized() and not sensor_gnss_->isEnuMapFixed() and - (first_pos_-feature->getMeasurement()).norm() > params_gnss_->enu_map_init_dist_min) + (first_pos_-feature->getMeasurement().head<3>()).norm() > params_gnss_->enu_map_init_dist_min) { assert(first_frame_state_.count('P') and first_frame_state_.count('O') and @@ -264,14 +266,14 @@ void ProcessorGnssFix::handleEnuMap(FeatureBasePtr feature) sensor_gnss_->initializeEnuMap(first_frame_state_.vector("PO"), first_pos_, feature->getCapture()->getFrame()->getState().vector("PO"), - feature->getMeasurement()); + feature->getMeasurement().head<3>()); // Set as not-initialized if factors not separated enough ( < enu_map_init_dist_max) - if ((first_pos_ - feature->getMeasurement()).norm() < params_gnss_->enu_map_init_dist_max) + if ((first_pos_ - feature->getMeasurement().head<3>()).norm() < params_gnss_->enu_map_init_dist_max) sensor_gnss_->setEnuMapInitialized(false); } // Fix ENU-MAP if separated enough from first pos ( > enu_map_fix_dist ) - if ((first_pos_ - feature->getMeasurement()).norm() > params_gnss_->enu_map_fix_dist) + if ((first_pos_ - feature->getMeasurement().head<3>()).norm() > params_gnss_->enu_map_fix_dist) sensor_gnss_->fixEnuMap(); } diff --git a/test/gtest_factor_gnss_fix_2d.cpp b/test/gtest_factor_gnss_fix_2d.cpp index a5a439cfc4d7ec232122da70552d75fecfc3a875..fca8420cebb3aea6705ee24f502e356916ceb800 100644 --- a/test/gtest_factor_gnss_fix_2d.cpp +++ b/test/gtest_factor_gnss_fix_2d.cpp @@ -25,7 +25,7 @@ * Created on: Aug 1, 2018 * \author: jvallve */ - +#include "gnss/internal/config.h" #include <core/ceres_wrapper/solver_ceres.h> #include <core/utils/utils_gtest.h> @@ -38,16 +38,19 @@ using namespace Eigen; using namespace wolf; -void resetStates(SensorGnssPtr gnss_sensor_ptr, FrameBasePtr frame_ptr, +void resetStates(SensorGnssPtr gnss_sensor_ptr, FrameBasePtr frame_ptr, CaptureBasePtr cap_ptr, const Vector1d& o_enu_map, const Vector3d& t_base_antena, const Vector3d& t_enu_map, const Vector3d& t_map_base, - const Vector1d& o_map_base) + const Vector1d& o_map_base, + const double clock_bias) { + // clock bias + cap_ptr->getStateBlock('T')->setState(Vector1d(clock_bias)); // ENU-MAP - gnss_sensor_ptr->getEnuMapRoll()->setState(Eigen::Vector1d::Zero()); - gnss_sensor_ptr->getEnuMapPitch()->setState(Eigen::Vector1d::Zero()); + gnss_sensor_ptr->getEnuMapRoll()->setState(Vector1d::Zero()); + gnss_sensor_ptr->getEnuMapPitch()->setState(Vector1d::Zero()); gnss_sensor_ptr->getEnuMapYaw()->setState(o_enu_map); gnss_sensor_ptr->getEnuMapTranslation()->setState(t_enu_map); // Antena @@ -106,12 +109,14 @@ Matrix3d R_ecef_enu = (AngleAxis<double>(1.3, Vector3d::UnitX()) * Matrix3d R_enu_map = AngleAxis<double>(o_enu_map(0), Vector3d::UnitZ()).toRotationMatrix(); Matrix3d R_map_base = AngleAxis<double>(o_map_base(0), Vector3d::UnitZ()).toRotationMatrix(); Vector3d t_ecef_antena = R_ecef_enu * (R_enu_map * (R_map_base * t_base_antena + t_map_base) + t_enu_map ) + t_ecef_enu; +double clock_bias = 0.5; // WOLF ProblemPtr problem_ptr = Problem::create("PO", 2); SolverCeresPtr solver_ceres = std::make_shared<SolverCeres>(problem_ptr); SensorGnssPtr gnss_sensor_ptr = std::static_pointer_cast<SensorGnss>(problem_ptr->installSensor("SensorGnss", "gnss", t_base_antena, std::make_shared<ParamsSensorGnss>())); FrameBasePtr frame_ptr; +CaptureGnssFixPtr cap_gnss_ptr; //////////////////////////////////////////////////////// @@ -138,7 +143,10 @@ TEST(FactorGnssFix2dTest, configure_tree) problem_ptr->keyFrameCallback(frame_ptr, nullptr); // Create & process GNSS Fix capture - CaptureGnssFixPtr cap_gnss_ptr = std::make_shared<CaptureGnssFix>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena, 1e-3*Matrix3d::Identity()); + cap_gnss_ptr = std::make_shared<CaptureGnssFix>(TimeStamp(0), + gnss_sensor_ptr, + (Vector4d() << t_ecef_antena, clock_bias).finished(), + 1e-3*Matrix4d::Identity()); gnss_sensor_ptr->process(cap_gnss_ptr); // Checks @@ -156,7 +164,7 @@ TEST(FactorGnssFix2dTest, gnss_1_map_base_position) problem_ptr->print(); // --------------------------- Reset states - resetStates(gnss_sensor_ptr, frame_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base); + resetStates(gnss_sensor_ptr, frame_ptr, cap_gnss_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base, clock_bias); // --------------------------- fixing/unfixing states fixAllStates(gnss_sensor_ptr, frame_ptr); @@ -202,7 +210,7 @@ TEST(FactorGnssFix2dTest, gnss_1_map_base_position) TEST(FactorGnssFix2dTest, gnss_1_map_base_orientation) { // --------------------------- Reset states - resetStates(gnss_sensor_ptr, frame_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base); + resetStates(gnss_sensor_ptr, frame_ptr, cap_gnss_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base, clock_bias); // --------------------------- fixing/unfixing states fixAllStates(gnss_sensor_ptr, frame_ptr); @@ -245,7 +253,7 @@ TEST(FactorGnssFix2dTest, gnss_1_map_base_orientation) TEST(FactorGnssFix2dTest, gnss_1_enu_map_yaw) { // --------------------------- Reset states - resetStates(gnss_sensor_ptr, frame_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base); + resetStates(gnss_sensor_ptr, frame_ptr, cap_gnss_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base, clock_bias); // --------------------------- fixing/unfixing states fixAllStates(gnss_sensor_ptr, frame_ptr); @@ -291,7 +299,7 @@ TEST(FactorGnssFix2dTest, gnss_1_enu_map_yaw) TEST(FactorGnssFix2dTest, gnss_1_enu_map_position) { // --------------------------- Reset states - resetStates(gnss_sensor_ptr, frame_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base); + resetStates(gnss_sensor_ptr, frame_ptr, cap_gnss_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base, clock_bias); // --------------------------- fixing/unfixing states fixAllStates(gnss_sensor_ptr, frame_ptr); @@ -335,7 +343,7 @@ TEST(FactorGnssFix2dTest, gnss_1_enu_map_position) TEST(FactorGnssFix2dTest, gnss_1_base_antena) { // --------------------------- Reset states - resetStates(gnss_sensor_ptr, frame_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base); + resetStates(gnss_sensor_ptr, frame_ptr, cap_gnss_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base, clock_bias); // --------------------------- fixing/unfixing states fixAllStates(gnss_sensor_ptr, frame_ptr);