From 3861af97eae532f2ee32c68f311174c0314d7ec1 Mon Sep 17 00:00:00 2001 From: joanvallve <jvallve@iri.upc.edu> Date: Wed, 3 Jun 2020 19:05:04 +0200 Subject: [PATCH] gtest passing --- include/gnss/factor/factor_gnss_fix_2d.h | 29 ++++++++-------- include/gnss/factor/factor_gnss_fix_3d.h | 29 ++++++++-------- .../gnss/factor/factor_gnss_pseudo_range.h | 33 ++++++++++--------- .../gnss/factor/factor_gnss_single_diff_2d.h | 1 + include/gnss/factor/factor_gnss_tdcp.h | 1 + include/gnss/factor/factor_gnss_tdcp_2d.h | 31 ++++++++--------- include/gnss/factor/factor_gnss_tdcp_3d.h | 1 + 7 files changed, 66 insertions(+), 59 deletions(-) diff --git a/include/gnss/factor/factor_gnss_fix_2d.h b/include/gnss/factor/factor_gnss_fix_2d.h index 4965cb14b..5098fe3cb 100644 --- a/include/gnss/factor/factor_gnss_fix_2d.h +++ b/include/gnss/factor/factor_gnss_fix_2d.h @@ -21,20 +21,21 @@ class FactorGnssFix2d : public FactorAutodiff<FactorGnssFix2d, 3, 2, 1, 3, 3, 1, FactorGnssFix2d(FeatureBasePtr& _ftr_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) : FactorAutodiff<FactorGnssFix2d, 3, 2, 1, 3, 3, 1, 1, 1>("FactorGnssFix2d", - 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, + 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()), sensor_gnss_ptr_(_sensor_gnss_ptr) { WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating an active GNSS Fix 2d factor without initializing ENU"); diff --git a/include/gnss/factor/factor_gnss_fix_3d.h b/include/gnss/factor/factor_gnss_fix_3d.h index beaf7808b..fb65f5d32 100644 --- a/include/gnss/factor/factor_gnss_fix_3d.h +++ b/include/gnss/factor/factor_gnss_fix_3d.h @@ -20,20 +20,21 @@ class FactorGnssFix3d : public FactorAutodiff<FactorGnssFix3d, 3, 3, 4, 3, 3, 1, FactorGnssFix3d(FeatureBasePtr& _ftr_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) : FactorAutodiff<FactorGnssFix3d, 3, 3, 4, 3, 3, 1, 1, 1>("FactorGnssFix3d", - 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, + 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()), 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"); diff --git a/include/gnss/factor/factor_gnss_pseudo_range.h b/include/gnss/factor/factor_gnss_pseudo_range.h index c2287849c..85e64ee53 100644 --- a/include/gnss/factor/factor_gnss_pseudo_range.h +++ b/include/gnss/factor/factor_gnss_pseudo_range.h @@ -30,26 +30,27 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) : FactorAutodiff<FactorGnssPseudoRange, 1, 3, 4, 1, 1, 3, 3, 1, 1, 1>("FactorGnssPseudoRange", - nullptr, - nullptr, - nullptr, - nullptr, - _processor_ptr, - _apply_loss_function, - _status, - _ftr_ptr->getFrame()->getP(), - _ftr_ptr->getFrame()->getO(), - _ftr_ptr->getCapture()->getStateBlock("T"), - (_ftr_ptr->getSatellite().sys == SYS_GLO ? + _ftr_ptr, + nullptr, + nullptr, + nullptr, + nullptr, + _processor_ptr, + _apply_loss_function, + _status, + _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->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_ECEF_(_ftr_ptr->getSatellite().pos), diff --git a/include/gnss/factor/factor_gnss_single_diff_2d.h b/include/gnss/factor/factor_gnss_single_diff_2d.h index 44a6dbdff..07feddd0f 100644 --- a/include/gnss/factor/factor_gnss_single_diff_2d.h +++ b/include/gnss/factor/factor_gnss_single_diff_2d.h @@ -21,6 +21,7 @@ class FactorGnssSingleDiff2d : public FactorAutodiff<FactorGnssSingleDiff2d, 3, FactorGnssSingleDiff2d(FeatureBasePtr& _ftr_ptr, const FrameBasePtr& _frame_other_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) : FactorAutodiff<FactorGnssSingleDiff2d, 3, 2, 1, 2, 1, 3, 1, 1, 1>("GNSS SINGLE DIFFERENCES 2d", + _ftr_ptr, _frame_other_ptr, nullptr, nullptr, diff --git a/include/gnss/factor/factor_gnss_tdcp.h b/include/gnss/factor/factor_gnss_tdcp.h index 9a44da6af..3f9625d97 100644 --- a/include/gnss/factor/factor_gnss_tdcp.h +++ b/include/gnss/factor/factor_gnss_tdcp.h @@ -31,6 +31,7 @@ class FactorGnssTdcp : public FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1 bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) : FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1, 3, 3, 1, 1, 1>("FactorGnssTdcp", + _ftr_k, nullptr, _ftr_r->getCapture(), _ftr_r, diff --git a/include/gnss/factor/factor_gnss_tdcp_2d.h b/include/gnss/factor/factor_gnss_tdcp_2d.h index 4bd939d1e..1662ad0ed 100644 --- a/include/gnss/factor/factor_gnss_tdcp_2d.h +++ b/include/gnss/factor/factor_gnss_tdcp_2d.h @@ -21,21 +21,22 @@ class FactorGnssTdcp2d : public FactorAutodiff<FactorGnssTdcp2d, 3, 2, 1, 2, 1, FactorGnssTdcp2d(FeatureBasePtr& _ftr_ptr, const FrameBasePtr& _frame_other_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) : FactorAutodiff<FactorGnssTdcp2d, 3, 2, 1, 2, 1, 3, 1, 1, 1>("FactorGnssTdcp2d", - _frame_other_ptr, - nullptr, - nullptr, - nullptr, - _processor_ptr, - _apply_loss_function, - _status, - _frame_other_ptr->getP(), - _frame_other_ptr->getO(), - _ftr_ptr->getFrame()->getP(), - _ftr_ptr->getFrame()->getO(), - _sensor_gnss_ptr->getP(), - _sensor_gnss_ptr->getEnuMapRoll(), - _sensor_gnss_ptr->getEnuMapPitch(), - _sensor_gnss_ptr->getEnuMapYaw()), + _ftr_ptr, + _frame_other_ptr, + nullptr, + nullptr, + nullptr, + _processor_ptr, + _apply_loss_function, + _status, + _frame_other_ptr->getP(), + _frame_other_ptr->getO(), + _ftr_ptr->getFrame()->getP(), + _ftr_ptr->getFrame()->getO(), + _sensor_gnss_ptr->getP(), + _sensor_gnss_ptr->getEnuMapRoll(), + _sensor_gnss_ptr->getEnuMapPitch(), + _sensor_gnss_ptr->getEnuMapYaw()), sensor_gnss_ptr_(_sensor_gnss_ptr) { WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined(), "Creating a GNSS SingleDiff 2D factor without initializing ENU"); diff --git a/include/gnss/factor/factor_gnss_tdcp_3d.h b/include/gnss/factor/factor_gnss_tdcp_3d.h index e9a0a5914..d034b22ba 100644 --- a/include/gnss/factor/factor_gnss_tdcp_3d.h +++ b/include/gnss/factor/factor_gnss_tdcp_3d.h @@ -21,6 +21,7 @@ class FactorGnssTdcp3d : public FactorAutodiff<FactorGnssTdcp3d, 3, 3, 4, 3, 4, FactorGnssTdcp3d(FeatureBasePtr& _ftr_ptr, const FrameBasePtr& _frame_other_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) : FactorAutodiff<FactorGnssTdcp3d, 3, 3, 4, 3, 4, 3, 1, 1, 1>("FactorGnssTdcp3d", + _ftr_ptr, _frame_other_ptr, nullptr, nullptr, -- GitLab