From c12645297ccd746a9461330aa5833e71832e0aa7 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Mon, 8 Feb 2021 16:58:17 +0100
Subject: [PATCH] working
---
include/gnss/factor/factor_gnss_fix_2d.h | 6 +--
include/gnss/factor/factor_gnss_fix_3d.h | 6 +--
.../gnss/factor/factor_gnss_pseudo_range.h | 6 +--
.../gnss/factor/factor_gnss_single_diff_2d.h | 6 +--
include/gnss/factor/factor_gnss_tdcp.h | 6 +--
include/gnss/factor/factor_gnss_tdcp_2d.h | 6 +--
include/gnss/factor/factor_gnss_tdcp_3d.h | 38 +++++++++----------
include/gnss/factor/factor_gnss_tdcp_batch.h | 6 +--
8 files changed, 24 insertions(+), 56 deletions(-)
diff --git a/include/gnss/factor/factor_gnss_fix_2d.h b/include/gnss/factor/factor_gnss_fix_2d.h
index 2ec190507..6104fa08e 100644
--- a/include/gnss/factor/factor_gnss_fix_2d.h
+++ b/include/gnss/factor/factor_gnss_fix_2d.h
@@ -21,6 +21,7 @@ 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",
+ TOP_ABS,
_ftr_ptr,
nullptr,
nullptr,
@@ -43,11 +44,6 @@ class FactorGnssFix2d : public FactorAutodiff<FactorGnssFix2d, 3, 2, 1, 3, 3, 1,
~FactorGnssFix2d() override = default;
- std::string getTopology() const override
- {
- return std::string("ABS");
- }
-
template<typename T>
bool operator ()(const T* const _x,
const T* const _o,
diff --git a/include/gnss/factor/factor_gnss_fix_3d.h b/include/gnss/factor/factor_gnss_fix_3d.h
index 4980149e3..d934eb3b0 100644
--- a/include/gnss/factor/factor_gnss_fix_3d.h
+++ b/include/gnss/factor/factor_gnss_fix_3d.h
@@ -20,6 +20,7 @@ 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",
+ TOP_ABS,
_ftr_ptr,
nullptr,
nullptr,
@@ -42,11 +43,6 @@ class FactorGnssFix3d : public FactorAutodiff<FactorGnssFix3d, 3, 3, 4, 3, 3, 1,
~FactorGnssFix3d() override = default;
- std::string getTopology() const override
- {
- return std::string("ABS");
- }
-
template<typename T>
bool operator ()(const T* const _x,
const T* const _o,
diff --git a/include/gnss/factor/factor_gnss_pseudo_range.h b/include/gnss/factor/factor_gnss_pseudo_range.h
index a3a8ea1a1..dab37126d 100644
--- a/include/gnss/factor/factor_gnss_pseudo_range.h
+++ b/include/gnss/factor/factor_gnss_pseudo_range.h
@@ -30,6 +30,7 @@ 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",
+ TOP_ABS,
_ftr_ptr,
nullptr,
nullptr,
@@ -64,11 +65,6 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3,
~FactorGnssPseudoRange() override = default;
- std::string getTopology() const override
- {
- return std::string("ABS");
- }
-
template<typename T>
bool operator ()(const T* const _x,
const T* const _o,
diff --git a/include/gnss/factor/factor_gnss_single_diff_2d.h b/include/gnss/factor/factor_gnss_single_diff_2d.h
index 07feddd0f..21ac8e8d2 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",
+ TOP_GEOM,
_ftr_ptr,
_frame_other_ptr,
nullptr,
@@ -44,11 +45,6 @@ class FactorGnssSingleDiff2d : public FactorAutodiff<FactorGnssSingleDiff2d, 3,
virtual ~FactorGnssSingleDiff2d() = default;
- virtual std::string getTopology() const override
- {
- return std::string("MOTION");
- }
-
template<typename T>
bool operator ()(const T* const _x1,
const T* const _o1,
diff --git a/include/gnss/factor/factor_gnss_tdcp.h b/include/gnss/factor/factor_gnss_tdcp.h
index f0351efb3..95f77c76a 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",
+ TOP_GEOM,
_ftr_k,
nullptr,
_ftr_r->getCapture(),
@@ -66,11 +67,6 @@ class FactorGnssTdcp : public FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1
~FactorGnssTdcp() override = default;
- std::string getTopology() const override
- {
- return std::string("GEOM");
- }
-
template<typename T>
bool operator ()(const T* const _x_r,
const T* const _o_r,
diff --git a/include/gnss/factor/factor_gnss_tdcp_2d.h b/include/gnss/factor/factor_gnss_tdcp_2d.h
index 3b78b2f06..c876c412b 100644
--- a/include/gnss/factor/factor_gnss_tdcp_2d.h
+++ b/include/gnss/factor/factor_gnss_tdcp_2d.h
@@ -21,6 +21,7 @@ class FactorGnssTdcp2d : public FactorAutodiff<FactorGnssTdcp2d, 3, 2, 1, 2, 1,
FactorGnssTdcp2d(const 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",
+ TOP_GEOM,
_ftr_ptr,
_frame_other_ptr,
nullptr,
@@ -44,11 +45,6 @@ class FactorGnssTdcp2d : public FactorAutodiff<FactorGnssTdcp2d, 3, 2, 1, 2, 1,
~FactorGnssTdcp2d() override = default;
- std::string getTopology() const override
- {
- return std::string("GEOM");
- }
-
template<typename T>
bool operator ()(const T* const _x1,
const T* const _o1,
diff --git a/include/gnss/factor/factor_gnss_tdcp_3d.h b/include/gnss/factor/factor_gnss_tdcp_3d.h
index 90df1ecfe..df9bfe44d 100644
--- a/include/gnss/factor/factor_gnss_tdcp_3d.h
+++ b/include/gnss/factor/factor_gnss_tdcp_3d.h
@@ -21,22 +21,23 @@ class FactorGnssTdcp3d : public FactorAutodiff<FactorGnssTdcp3d, 3, 3, 4, 3, 4,
FactorGnssTdcp3d(const 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,
- 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()),
+ TOP_GEOM,
+ _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 3D factor without initializing ENU");
@@ -44,11 +45,6 @@ class FactorGnssTdcp3d : public FactorAutodiff<FactorGnssTdcp3d, 3, 3, 4, 3, 4,
~FactorGnssTdcp3d() override = default;
- std::string getTopology() const override
- {
- return std::string("GEOM");
- }
-
template<typename T>
bool operator ()(const T* const _x1,
const T* const _o1,
diff --git a/include/gnss/factor/factor_gnss_tdcp_batch.h b/include/gnss/factor/factor_gnss_tdcp_batch.h
index e525ab995..94a08eca6 100644
--- a/include/gnss/factor/factor_gnss_tdcp_batch.h
+++ b/include/gnss/factor/factor_gnss_tdcp_batch.h
@@ -25,6 +25,7 @@ class FactorGnssTdcpBatch : public FactorAutodiff<FactorGnssTdcpBatch, 4, 3, 4,
bool _apply_loss_function = false,
FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorGnssTdcpBatch, 4, 3, 4, 1, 3, 4, 1, 3, 1, 1, 1>("FactorGnssTdcpBatch",
+ TOP_GEOM,
_ftr_ptr,
_capture_other_ptr->getFrame(),
_capture_other_ptr,
@@ -50,11 +51,6 @@ class FactorGnssTdcpBatch : public FactorAutodiff<FactorGnssTdcpBatch, 4, 3, 4,
~FactorGnssTdcpBatch() override = default;
- std::string getTopology() const override
- {
- return std::string("GEOM");
- }
-
template<typename T>
bool operator ()(const T* const _x1,
const T* const _o1,
--
GitLab