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