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

gtest passing

parent 26fbdcfe
No related branches found
No related tags found
3 merge requests!28release after RAL,!27After 2nd RAL submission,!16Resolve "Copying measurement and sqrtinfo in FactorBase (multi-threading)"
Hide whitespace changes
Inline Side-by-side
Showing
with 66 additions and 59 deletions
include/gnss/factor/factor_gnss_fix_2d.h
+ 15
− 14
View file @ 3861af97
...@@ -21,20 +21,21 @@ class FactorGnssFix2d : public FactorAutodiff<FactorGnssFix2d, 3, 2, 1, 3, 3, 1, ...@@ -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) : 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", FactorAutodiff<FactorGnssFix2d, 3, 2, 1, 3, 3, 1, 1, 1>("FactorGnssFix2d",
nullptr, _ftr_ptr,
nullptr, nullptr,
nullptr, nullptr,
nullptr, nullptr,
_processor_ptr, nullptr,
_apply_loss_function, _processor_ptr,
_status, _apply_loss_function,
_ftr_ptr->getFrame()->getP(), _status,
_ftr_ptr->getFrame()->getO(), _ftr_ptr->getFrame()->getP(),
_sensor_gnss_ptr->getP(), _ftr_ptr->getFrame()->getO(),
_sensor_gnss_ptr->getEnuMapTranslation(), _sensor_gnss_ptr->getP(),
_sensor_gnss_ptr->getEnuMapRoll(), _sensor_gnss_ptr->getEnuMapTranslation(),
_sensor_gnss_ptr->getEnuMapPitch(), _sensor_gnss_ptr->getEnuMapRoll(),
_sensor_gnss_ptr->getEnuMapYaw()), _sensor_gnss_ptr->getEnuMapPitch(),
_sensor_gnss_ptr->getEnuMapYaw()),
sensor_gnss_ptr_(_sensor_gnss_ptr) 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"); WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating an active GNSS Fix 2d factor without initializing ENU");
......
include/gnss/factor/factor_gnss_fix_3d.h
+ 15
− 14
View file @ 3861af97
...@@ -20,20 +20,21 @@ class FactorGnssFix3d : public FactorAutodiff<FactorGnssFix3d, 3, 3, 4, 3, 3, 1, ...@@ -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) : 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", FactorAutodiff<FactorGnssFix3d, 3, 3, 4, 3, 3, 1, 1, 1>("FactorGnssFix3d",
nullptr, _ftr_ptr,
nullptr, nullptr,
nullptr, nullptr,
nullptr, nullptr,
_processor_ptr, nullptr,
_apply_loss_function, _processor_ptr,
_status, _apply_loss_function,
_ftr_ptr->getFrame()->getP(), _status,
_ftr_ptr->getFrame()->getO(), _ftr_ptr->getFrame()->getP(),
_sensor_gnss_ptr->getP(), _ftr_ptr->getFrame()->getO(),
_sensor_gnss_ptr->getEnuMapTranslation(), _sensor_gnss_ptr->getP(),
_sensor_gnss_ptr->getEnuMapRoll(), _sensor_gnss_ptr->getEnuMapTranslation(),
_sensor_gnss_ptr->getEnuMapPitch(), _sensor_gnss_ptr->getEnuMapRoll(),
_sensor_gnss_ptr->getEnuMapYaw()), _sensor_gnss_ptr->getEnuMapPitch(),
_sensor_gnss_ptr->getEnuMapYaw()),
sensor_gnss_ptr_(_sensor_gnss_ptr) 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"); WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating an ACTIVE GNSS Fix 3d factor without initializing ENU");
......
include/gnss/factor/factor_gnss_pseudo_range.h
+ 17
− 16
View file @ 3861af97
...@@ -30,26 +30,27 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3, ...@@ -30,26 +30,27 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3,
bool _apply_loss_function, bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) : FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorGnssPseudoRange, 1, 3, 4, 1, 1, 3, 3, 1, 1, 1>("FactorGnssPseudoRange", FactorAutodiff<FactorGnssPseudoRange, 1, 3, 4, 1, 1, 3, 3, 1, 1, 1>("FactorGnssPseudoRange",
nullptr, _ftr_ptr,
nullptr, nullptr,
nullptr, nullptr,
nullptr, nullptr,
_processor_ptr, nullptr,
_apply_loss_function, _processor_ptr,
_status, _apply_loss_function,
_ftr_ptr->getFrame()->getP(), _status,
_ftr_ptr->getFrame()->getO(), _ftr_ptr->getFrame()->getP(),
_ftr_ptr->getCapture()->getStateBlock("T"), _ftr_ptr->getFrame()->getO(),
(_ftr_ptr->getSatellite().sys == SYS_GLO ? _ftr_ptr->getCapture()->getStateBlock("T"),
(_ftr_ptr->getSatellite().sys == SYS_GLO ?
_ftr_ptr->getCapture()->getStateBlock("TG") : _ftr_ptr->getCapture()->getStateBlock("TG") :
(_ftr_ptr->getSatellite().sys == SYS_GAL ? (_ftr_ptr->getSatellite().sys == SYS_GAL ?
_ftr_ptr->getCapture()->getStateBlock("TE") : _ftr_ptr->getCapture()->getStateBlock("TE") :
_ftr_ptr->getCapture()->getStateBlock("TC"))),//in case GPS, TC is set but anyway not used _ftr_ptr->getCapture()->getStateBlock("TC"))),//in case GPS, TC is set but anyway not used
_sensor_gnss_ptr->getP(), _sensor_gnss_ptr->getP(),
_sensor_gnss_ptr->getEnuMapTranslation(), _sensor_gnss_ptr->getEnuMapTranslation(),
_sensor_gnss_ptr->getEnuMapRoll(), _sensor_gnss_ptr->getEnuMapRoll(),
_sensor_gnss_ptr->getEnuMapPitch(), _sensor_gnss_ptr->getEnuMapPitch(),
_sensor_gnss_ptr->getEnuMapYaw()), _sensor_gnss_ptr->getEnuMapYaw()),
sensor_gnss_ptr_(_sensor_gnss_ptr), 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), satellite_ECEF_(_ftr_ptr->getSatellite().pos),
......
include/gnss/factor/factor_gnss_single_diff_2d.h
+ 1
− 0
View file @ 3861af97
...@@ -21,6 +21,7 @@ class FactorGnssSingleDiff2d : public FactorAutodiff<FactorGnssSingleDiff2d, 3, ...@@ -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) : 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", FactorAutodiff<FactorGnssSingleDiff2d, 3, 2, 1, 2, 1, 3, 1, 1, 1>("GNSS SINGLE DIFFERENCES 2d",
_ftr_ptr,
_frame_other_ptr, _frame_other_ptr,
nullptr, nullptr,
nullptr, nullptr,
......
include/gnss/factor/factor_gnss_tdcp.h
+ 1
− 0
View file @ 3861af97
...@@ -31,6 +31,7 @@ class FactorGnssTdcp : public FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1 ...@@ -31,6 +31,7 @@ class FactorGnssTdcp : public FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1
bool _apply_loss_function, bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) : FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1, 3, 3, 1, 1, 1>("FactorGnssTdcp", FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1, 3, 3, 1, 1, 1>("FactorGnssTdcp",
_ftr_k,
nullptr, nullptr,
_ftr_r->getCapture(), _ftr_r->getCapture(),
_ftr_r, _ftr_r,
......
include/gnss/factor/factor_gnss_tdcp_2d.h
+ 16
− 15
View file @ 3861af97
...@@ -21,21 +21,22 @@ class FactorGnssTdcp2d : public FactorAutodiff<FactorGnssTdcp2d, 3, 2, 1, 2, 1, ...@@ -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) : 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", FactorAutodiff<FactorGnssTdcp2d, 3, 2, 1, 2, 1, 3, 1, 1, 1>("FactorGnssTdcp2d",
_frame_other_ptr, _ftr_ptr,
nullptr, _frame_other_ptr,
nullptr, nullptr,
nullptr, nullptr,
_processor_ptr, nullptr,
_apply_loss_function, _processor_ptr,
_status, _apply_loss_function,
_frame_other_ptr->getP(), _status,
_frame_other_ptr->getO(), _frame_other_ptr->getP(),
_ftr_ptr->getFrame()->getP(), _frame_other_ptr->getO(),
_ftr_ptr->getFrame()->getO(), _ftr_ptr->getFrame()->getP(),
_sensor_gnss_ptr->getP(), _ftr_ptr->getFrame()->getO(),
_sensor_gnss_ptr->getEnuMapRoll(), _sensor_gnss_ptr->getP(),
_sensor_gnss_ptr->getEnuMapPitch(), _sensor_gnss_ptr->getEnuMapRoll(),
_sensor_gnss_ptr->getEnuMapYaw()), _sensor_gnss_ptr->getEnuMapPitch(),
_sensor_gnss_ptr->getEnuMapYaw()),
sensor_gnss_ptr_(_sensor_gnss_ptr) sensor_gnss_ptr_(_sensor_gnss_ptr)
{ {
WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined(), "Creating a GNSS SingleDiff 2D factor without initializing ENU"); WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined(), "Creating a GNSS SingleDiff 2D factor without initializing ENU");
......
include/gnss/factor/factor_gnss_tdcp_3d.h
+ 1
− 0
View file @ 3861af97
...@@ -21,6 +21,7 @@ class FactorGnssTdcp3d : public FactorAutodiff<FactorGnssTdcp3d, 3, 3, 4, 3, 4, ...@@ -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) : 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", FactorAutodiff<FactorGnssTdcp3d, 3, 3, 4, 3, 4, 3, 1, 1, 1>("FactorGnssTdcp3d",
_ftr_ptr,
_frame_other_ptr, _frame_other_ptr,
nullptr, nullptr,
nullptr, nullptr,
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment